forked from Archive/PX4-Autopilot
[UAVCANv1] Subscriber multi topic subscription
reg.drone.bms full support legacy.equipment.bms support
This commit is contained in:
parent
5e7ebbe47c
commit
399815469f
|
@ -56,7 +56,7 @@
|
||||||
url = https://github.com/UAVCAN/libcanard.git
|
url = https://github.com/UAVCAN/libcanard.git
|
||||||
[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
|
[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
|
||||||
path = src/drivers/uavcan_v1/public_regulated_data_types
|
path = src/drivers/uavcan_v1/public_regulated_data_types
|
||||||
url = https://github.com/UAVCAN/public_regulated_data_types.git
|
url = https://github.com/PX4/public_regulated_data_types.git
|
||||||
[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"]
|
[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"]
|
||||||
path = src/drivers/uavcannode_gps_demo/public_regulated_data_types
|
path = src/drivers/uavcannode_gps_demo/public_regulated_data_types
|
||||||
url = https://github.com/UAVCAN/public_regulated_data_types.git
|
url = https://github.com/UAVCAN/public_regulated_data_types.git
|
||||||
|
|
|
@ -40,6 +40,7 @@ px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR})
|
||||||
find_program(NNVG_PATH nnvg)
|
find_program(NNVG_PATH nnvg)
|
||||||
if(NNVG_PATH)
|
if(NNVG_PATH)
|
||||||
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg)
|
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg)
|
||||||
|
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/legacy)
|
||||||
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
|
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
|
||||||
else()
|
else()
|
||||||
message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
|
message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
|
||||||
|
|
|
@ -63,14 +63,18 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
const UavcanParamBinder _uavcan_params[8] {
|
const UavcanParamBinder _uavcan_params[10] {
|
||||||
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"},
|
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"},
|
||||||
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
|
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
|
||||||
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"},
|
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"},
|
||||||
{"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID"},
|
{"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID"},
|
||||||
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"},
|
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"},
|
||||||
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"},
|
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"},
|
||||||
{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"},
|
{"uavcan.sub.energy_source.0.id", "UCAN1_BMS_ES_PID"},
|
||||||
{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
|
{"uavcan.sub.battery_status.0.id", "UCAN1_BMS_BS_PID"},
|
||||||
|
{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"},
|
||||||
|
{"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_PID"},
|
||||||
|
//{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"}, //FIXME instancing
|
||||||
|
//{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|
|
@ -59,7 +59,8 @@ public:
|
||||||
// Update the uORB Subscription and broadcast a UAVCAN message
|
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||||
virtual void update() override
|
virtual void update() override
|
||||||
{
|
{
|
||||||
if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
|
if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET
|
||||||
|
&& _port_id != 0) { //FIXME either make default param UNSET or handle 0 in base class
|
||||||
sensor_gps_s gps {};
|
sensor_gps_s gps {};
|
||||||
_gps_sub.update(&gps);
|
_gps_sub.update(&gps);
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,102 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Readiness.hpp
|
||||||
|
*
|
||||||
|
* Defines the UAVCAN v1 readiness publisher
|
||||||
|
* readiness state is used to command or report the availability status
|
||||||
|
*
|
||||||
|
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
// DS-15 Specification Messages
|
||||||
|
#include <reg/drone/service/common/Readiness_0_1.h>
|
||||||
|
|
||||||
|
#include "Publisher.hpp"
|
||||||
|
|
||||||
|
class UavcanReadinessPublisher : public UavcanPublisher
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||||
|
UavcanPublisher(ins, pmgr, "readiness", instance)
|
||||||
|
{
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
// Update the uORB Subscription and broadcast a UAVCAN message
|
||||||
|
virtual void update() override
|
||||||
|
{
|
||||||
|
// Not sure if actuator_armed is a good indication of readiness but seems close to it
|
||||||
|
if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
|
||||||
|
actuator_armed_s armed {};
|
||||||
|
_actuator_armed_sub.update(&armed);
|
||||||
|
|
||||||
|
reg_drone_service_common_Readiness_0_1 readiness {};
|
||||||
|
|
||||||
|
if (armed.armed) {
|
||||||
|
readiness.value = reg_drone_service_common_Readiness_0_1_ENGAGED;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
readiness.value = reg_drone_service_common_Readiness_0_1_STANDBY;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t readiness_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
|
||||||
|
|
||||||
|
CanardTransfer transfer = {
|
||||||
|
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
|
||||||
|
.priority = CanardPriorityNominal,
|
||||||
|
.transfer_kind = CanardTransferKindMessage,
|
||||||
|
.port_id = _port_id, // This is the subject-ID.
|
||||||
|
.remote_node_id = CANARD_NODE_ID_UNSET,
|
||||||
|
.transfer_id = _transfer_id,
|
||||||
|
.payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||||
|
.payload = &readiness_payload_buffer,
|
||||||
|
};
|
||||||
|
|
||||||
|
int32_t result = reg_drone_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer,
|
||||||
|
&transfer.payload_size);
|
||||||
|
|
||||||
|
if (result == 0) {
|
||||||
|
// set the data ready in the buffer and chop if needed
|
||||||
|
++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
|
||||||
|
result = canardTxPush(&_canard_instance, &transfer);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
private:
|
||||||
|
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
|
||||||
|
};
|
|
@ -66,9 +66,7 @@ public:
|
||||||
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
||||||
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
_port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -66,9 +66,7 @@ public:
|
||||||
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
|
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
|
||||||
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
_port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -65,9 +65,7 @@ public:
|
||||||
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
||||||
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
_port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -65,9 +65,7 @@ public:
|
||||||
uavcan_register_List_1_0_FIXED_PORT_ID_,
|
uavcan_register_List_1_0_FIXED_PORT_ID_,
|
||||||
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
|
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
_port_id = uavcan_register_List_1_0_FIXED_PORT_ID_;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -53,28 +53,78 @@ public:
|
||||||
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
|
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
|
||||||
|
|
||||||
UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) :
|
UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) :
|
||||||
_canard_instance(ins), _subject_name(subject_name), _instance(instance) { };
|
_canard_instance(ins), _instance(instance)
|
||||||
|
{
|
||||||
|
_subj_sub._subject_name = subject_name;
|
||||||
|
}
|
||||||
|
|
||||||
virtual void subscribe() = 0;
|
virtual void subscribe() = 0;
|
||||||
virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); };
|
virtual void unsubscribe()
|
||||||
|
{
|
||||||
|
SubjectSubscription *curSubj = &_subj_sub;
|
||||||
|
|
||||||
|
while (curSubj != NULL) {
|
||||||
|
canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub._port_id);
|
||||||
|
curSubj = curSubj->next;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
virtual void callback(const CanardTransfer &msg) = 0;
|
virtual void callback(const CanardTransfer &msg) = 0;
|
||||||
|
|
||||||
CanardPortID id() { return _port_id; };
|
CanardPortID id(uint32_t id = 0)
|
||||||
|
{
|
||||||
|
uint32_t i = 0;
|
||||||
|
SubjectSubscription *curSubj = &_subj_sub;
|
||||||
|
|
||||||
|
while (curSubj != NULL) {
|
||||||
|
if (id == i) {
|
||||||
|
return curSubj->_canard_sub._port_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
curSubj = curSubj->next;
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
|
||||||
|
return CANARD_PORT_ID_UNSET; // Wrong id return unset
|
||||||
|
}
|
||||||
|
|
||||||
|
bool hasPortID(CanardPortID port_id)
|
||||||
|
{
|
||||||
|
SubjectSubscription *curSubj = &_subj_sub;
|
||||||
|
|
||||||
|
while (curSubj != NULL) {
|
||||||
|
if (port_id == curSubj->_canard_sub._port_id) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
curSubj = curSubj->next;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
void printInfo()
|
void printInfo()
|
||||||
{
|
{
|
||||||
if (_port_id != CANARD_PORT_ID_UNSET) {
|
SubjectSubscription *curSubj = &_subj_sub;
|
||||||
PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id);
|
|
||||||
|
while (curSubj != NULL) {
|
||||||
|
if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
|
||||||
|
PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
curSubj = curSubj->next;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
struct SubjectSubscription {
|
||||||
|
CanardRxSubscription _canard_sub;
|
||||||
|
const char *_subject_name;
|
||||||
|
struct SubjectSubscription *next {NULL};
|
||||||
|
};
|
||||||
|
|
||||||
CanardInstance &_canard_instance;
|
CanardInstance &_canard_instance;
|
||||||
CanardRxSubscription _canard_sub;
|
SubjectSubscription _subj_sub;
|
||||||
const char *_subject_name;
|
|
||||||
uint8_t _instance {0};
|
uint8_t _instance {0};
|
||||||
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
|
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
|
||||||
|
|
||||||
CanardPortID _port_id {CANARD_PORT_ID_UNSET};
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -41,43 +41,149 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/battery_status.h>
|
||||||
|
#include <uORB/PublicationMulti.hpp>
|
||||||
|
|
||||||
// DS-15 Specification Messages
|
// DS-15 Specification Messages
|
||||||
#include <reg/drone/service/battery/Parameters_0_1.h>
|
#include <reg/drone/physics/electricity/SourceTs_0_1.h>
|
||||||
#include <reg/drone/service/battery/Status_0_1.h>
|
#include <reg/drone/service/battery/Parameters_0_3.h>
|
||||||
|
#include <reg/drone/service/battery/Status_0_2.h>
|
||||||
|
|
||||||
#include "DynamicPortSubscriber.hpp"
|
#include "DynamicPortSubscriber.hpp"
|
||||||
|
|
||||||
|
#define KELVIN_OFFSET 273.15f
|
||||||
|
#define WH_TO_JOULE 3600
|
||||||
|
|
||||||
class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
|
class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||||
UavcanDynamicPortSubscriber(ins, pmgr, "bms", instance) { };
|
UavcanDynamicPortSubscriber(ins, pmgr, "energy_source", instance)
|
||||||
|
{
|
||||||
|
_subj_sub.next = &_status_sub;
|
||||||
|
_status_sub._subject_name = _status_name;
|
||||||
|
_status_sub.next = &_parameters_sub;
|
||||||
|
_parameters_sub._subject_name = _parameters_name;
|
||||||
|
_parameters_sub.next = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
void subscribe() override
|
void subscribe() override
|
||||||
{
|
{
|
||||||
// Subscribe to messages reg.drone.service.battery.Status.0.1
|
// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
|
||||||
canardRxSubscribe(&_canard_instance,
|
canardRxSubscribe(&_canard_instance,
|
||||||
CanardTransferKindMessage,
|
CanardTransferKindMessage,
|
||||||
_port_id,
|
_subj_sub._canard_sub._port_id,
|
||||||
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
|
reg_drone_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
|
// Subscribe to messages reg.drone.service.battery.Status.0.2
|
||||||
|
canardRxSubscribe(&_canard_instance,
|
||||||
|
CanardTransferKindMessage,
|
||||||
|
_status_sub._canard_sub._port_id,
|
||||||
|
reg_drone_service_battery_Status_0_2_EXTENT_BYTES_,
|
||||||
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
|
&_status_sub._canard_sub);
|
||||||
|
|
||||||
|
// Subscribe to messages reg.drone.service.battery.Parameters.0.3
|
||||||
|
canardRxSubscribe(&_canard_instance,
|
||||||
|
CanardTransferKindMessage,
|
||||||
|
_parameters_sub._canard_sub._port_id,
|
||||||
|
reg_drone_service_battery_Parameters_0_3_EXTENT_BYTES_,
|
||||||
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
|
&_parameters_sub._canard_sub);
|
||||||
};
|
};
|
||||||
|
|
||||||
void callback(const CanardTransfer &receive) override
|
void callback(const CanardTransfer &receive) override
|
||||||
{
|
{
|
||||||
PX4_INFO("BmsCallback");
|
PX4_INFO("BmsCallback");
|
||||||
|
|
||||||
reg_drone_service_battery_Status_0_1 bat {};
|
if (receive.port_id == _subj_sub._canard_sub._port_id) {
|
||||||
size_t bat_size_in_bytes = receive.payload_size;
|
reg_drone_physics_electricity_SourceTs_0_1 source_ts {};
|
||||||
reg_drone_service_battery_Status_0_1_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
|
size_t source_ts_size_in_bytes = receive.payload_size;
|
||||||
|
reg_drone_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
|
||||||
|
(const uint8_t *)receive.payload,
|
||||||
|
&source_ts_size_in_bytes);
|
||||||
|
bat_status.timestamp = hrt_absolute_time(); //FIXME timesyncronization source_ts.timestamp.microsecond;
|
||||||
|
bat_status.voltage_v = source_ts.value.power.voltage.volt;
|
||||||
|
bat_status.current_a = source_ts.value.power.current.ampere;
|
||||||
|
|
||||||
uavcan_si_unit_voltage_Scalar_1_0 V_Min = bat.cell_voltage_min_max[0];
|
bat_status.connected = true; // Based on some threshold or an error??
|
||||||
uavcan_si_unit_voltage_Scalar_1_0 V_Max = bat.cell_voltage_min_max[1];
|
|
||||||
double vmin = static_cast<double>(V_Min.volt);
|
|
||||||
double vmax = static_cast<double>(V_Max.volt);
|
|
||||||
PX4_INFO("Min voltage: %f, Max Voltage: %f", vmin, vmax);
|
|
||||||
/// do something with the data
|
|
||||||
};
|
|
||||||
|
|
||||||
|
// Estimate discharged mah from Joule
|
||||||
|
if (_nominal_voltage != NAN) {
|
||||||
|
bat_status.discharged_mah = (source_ts.value.full_energy.joule - source_ts.value.energy.joule)
|
||||||
|
/ (_nominal_voltage * WH_TO_JOULE);
|
||||||
|
}
|
||||||
|
|
||||||
|
bat_status.remaining = source_ts.value.energy.joule / source_ts.value.full_energy.joule;
|
||||||
|
|
||||||
|
// TODO uORB publication rate limiting
|
||||||
|
_battery_status_pub.publish(bat_status);
|
||||||
|
print_message(bat_status);
|
||||||
|
|
||||||
|
|
||||||
|
} else if (receive.port_id == _status_sub._canard_sub._port_id) {
|
||||||
|
reg_drone_service_battery_Status_0_2 bat {};
|
||||||
|
size_t bat_size_in_bytes = receive.payload_size;
|
||||||
|
reg_drone_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
|
||||||
|
|
||||||
|
bat_status.scale = -1; // What does the mean?
|
||||||
|
|
||||||
|
bat_status.temperature = bat.temperature_min_max[1].kelvin - KELVIN_OFFSET; // PX4 uses degC we assume
|
||||||
|
|
||||||
|
bat_status.cell_count = bat.cell_voltages.count;
|
||||||
|
|
||||||
|
uint32_t cell_count = bat_status.cell_count;
|
||||||
|
|
||||||
|
if (cell_count > (sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]))) {
|
||||||
|
cell_count = sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]);
|
||||||
|
}
|
||||||
|
|
||||||
|
float voltage_cell_min = FLT_MAX_EXP;
|
||||||
|
float voltage_cell_max = FLT_MIN_EXP;
|
||||||
|
|
||||||
|
for (uint32_t i = 0; i < cell_count; i++) {
|
||||||
|
bat_status.voltage_cell_v[i] = bat.cell_voltages.elements[i];
|
||||||
|
|
||||||
|
if (bat_status.voltage_cell_v[i] > voltage_cell_max) {
|
||||||
|
voltage_cell_max = bat_status.voltage_cell_v[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (bat_status.voltage_cell_v[i] < voltage_cell_min) {
|
||||||
|
voltage_cell_min = bat_status.voltage_cell_v[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time?
|
||||||
|
|
||||||
|
} else if (receive.port_id == _parameters_sub._canard_sub._port_id) {
|
||||||
|
reg_drone_service_battery_Parameters_0_3 parameters {};
|
||||||
|
size_t parameters_size_in_bytes = receive.payload_size;
|
||||||
|
reg_drone_service_battery_Parameters_0_3_deserialize_(¶meters,
|
||||||
|
(const uint8_t *)receive.payload,
|
||||||
|
¶meters_size_in_bytes);
|
||||||
|
|
||||||
|
bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
|
||||||
|
bat_status.cycle_count = parameters.cycle_count;
|
||||||
|
bat_status.serial_number = parameters.unique_id & 0xFFFF;
|
||||||
|
bat_status.state_of_health = parameters.state_of_health_pct;
|
||||||
|
bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
|
||||||
|
bat_status.id = 0; //TODO instancing
|
||||||
|
_nominal_voltage = parameters.nominal_voltage.volt;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
float _nominal_voltage = NAN;
|
||||||
|
|
||||||
|
uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
|
||||||
|
|
||||||
|
SubjectSubscription _status_sub;
|
||||||
|
SubjectSubscription _parameters_sub;
|
||||||
|
|
||||||
|
const char *_status_name = "battery_status";
|
||||||
|
const char *_parameters_name = "battery_parameters";
|
||||||
|
|
||||||
|
battery_status_s bat_status {0};
|
||||||
};
|
};
|
||||||
|
|
|
@ -60,30 +60,37 @@ public:
|
||||||
|
|
||||||
void updateParam()
|
void updateParam()
|
||||||
{
|
{
|
||||||
char uavcan_param[90];
|
SubjectSubscription *curSubj = &_subj_sub;
|
||||||
sprintf(uavcan_param, "uavcan.sub.%s.%d.id", _subject_name, _instance);
|
|
||||||
|
|
||||||
// Set _port_id from _uavcan_param
|
while (curSubj != NULL) {
|
||||||
uavcan_register_Value_1_0 value;
|
char uavcan_param[90];
|
||||||
_param_manager.GetParamByName(uavcan_param, value);
|
sprintf(uavcan_param, "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance);
|
||||||
int32_t new_id = value.integer32.value.elements[0];
|
|
||||||
|
|
||||||
if (_port_id != new_id) {
|
// Set _port_id from _uavcan_param
|
||||||
if (new_id == CANARD_PORT_ID_UNSET) {
|
uavcan_register_Value_1_0 value;
|
||||||
// Cancel subscription
|
_param_manager.GetParamByName(uavcan_param, value);
|
||||||
unsubscribe();
|
int32_t new_id = value.integer32.value.elements[0];
|
||||||
|
|
||||||
} else {
|
/* FIXME how about partial subscribing */
|
||||||
if (_port_id != CANARD_PORT_ID_UNSET) {
|
if (curSubj->_canard_sub._port_id != new_id) {
|
||||||
// Already active; unsubscribe first
|
if (new_id == CANARD_PORT_ID_UNSET) {
|
||||||
|
// Cancel subscription
|
||||||
unsubscribe();
|
unsubscribe();
|
||||||
}
|
|
||||||
|
|
||||||
// Subscribe on the new port ID
|
} else {
|
||||||
_port_id = (CanardPortID)new_id;
|
if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
|
||||||
PX4_INFO("Subscribing %s.%d on port %d", _subject_name, _instance, _port_id);
|
// Already active; unsubscribe first
|
||||||
subscribe();
|
unsubscribe();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Subscribe on the new port ID
|
||||||
|
curSubj->_canard_sub._port_id = (CanardPortID)new_id;
|
||||||
|
PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
|
||||||
|
subscribe();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
curSubj = curSubj->next;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -62,15 +62,15 @@ public:
|
||||||
// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
|
// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
|
||||||
canardRxSubscribe(&_canard_instance,
|
canardRxSubscribe(&_canard_instance,
|
||||||
CanardTransferKindMessage,
|
CanardTransferKindMessage,
|
||||||
_port_id,
|
_subj_sub._canard_sub._port_id,
|
||||||
reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
|
reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
// Subscribe to messages reg.drone.service.common.Readiness.0.1
|
// Subscribe to messages reg.drone.service.common.Readiness.0.1
|
||||||
canardRxSubscribe(&_canard_instance,
|
canardRxSubscribe(&_canard_instance,
|
||||||
CanardTransferKindMessage,
|
CanardTransferKindMessage,
|
||||||
static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1),
|
static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub._port_id) + 1),
|
||||||
reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_,
|
reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub_readiness);
|
&_canard_sub_readiness);
|
||||||
|
|
|
@ -57,10 +57,10 @@ public:
|
||||||
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
|
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
|
||||||
canardRxSubscribe(&_canard_instance,
|
canardRxSubscribe(&_canard_instance,
|
||||||
CanardTransferKindMessage,
|
CanardTransferKindMessage,
|
||||||
_port_id,
|
_subj_sub._canard_sub._port_id,
|
||||||
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
|
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
/** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan):
|
/** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan):
|
||||||
* # A compliant implementation of this service should publish the following subjects:
|
* # A compliant implementation of this service should publish the following subjects:
|
||||||
|
|
|
@ -0,0 +1,129 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Battery.hpp
|
||||||
|
*
|
||||||
|
* Defines basic functionality of UAVCAN legacy Battery subscription
|
||||||
|
*
|
||||||
|
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/battery_status.h>
|
||||||
|
#include <uORB/PublicationMulti.hpp>
|
||||||
|
|
||||||
|
// Legacy message from UAVCANv0
|
||||||
|
#include <legacy/equipment/power/BatteryInfo_1_0.h>
|
||||||
|
|
||||||
|
#include "DynamicPortSubscriber.hpp"
|
||||||
|
|
||||||
|
class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
UavcanLegacyBatteryInfoSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
|
||||||
|
UavcanDynamicPortSubscriber(ins, pmgr, "legacy_bms", instance) { };
|
||||||
|
|
||||||
|
void subscribe() override
|
||||||
|
{
|
||||||
|
// Subscribe to messages reg.drone.service.battery.Status.0.1
|
||||||
|
canardRxSubscribe(&_canard_instance,
|
||||||
|
CanardTransferKindMessage,
|
||||||
|
_subj_sub._canard_sub._port_id,
|
||||||
|
legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_,
|
||||||
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler
|
||||||
|
&_subj_sub._canard_sub);
|
||||||
|
};
|
||||||
|
|
||||||
|
void callback(const CanardTransfer &receive) override
|
||||||
|
{
|
||||||
|
PX4_INFO("Legacy BmsCallback");
|
||||||
|
|
||||||
|
legacy_equipment_power_BatteryInfo_1_0 bat_info {};
|
||||||
|
size_t bat_info_size_in_bytes = receive.payload_size;
|
||||||
|
legacy_equipment_power_BatteryInfo_1_0_deserialize_(&bat_info, (const uint8_t *)receive.payload,
|
||||||
|
&bat_info_size_in_bytes);
|
||||||
|
|
||||||
|
battery_status_s bat_status {0};
|
||||||
|
bat_status.timestamp = hrt_absolute_time();
|
||||||
|
bat_status.voltage_filtered_v = bat_info.voltage;
|
||||||
|
bat_status.current_filtered_a = bat_info.current;
|
||||||
|
bat_status.average_current_a = bat_info.average_power_10sec;
|
||||||
|
bat_status.remaining = bat_info.state_of_charge_pct / 100.0f;
|
||||||
|
bat_status.scale = -1;
|
||||||
|
|
||||||
|
if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_HOT) {
|
||||||
|
bat_status.temperature = 100;
|
||||||
|
|
||||||
|
} else if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_COLD) {
|
||||||
|
bat_status.temperature = -30;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
bat_status.temperature = 20; // Temp okay ?
|
||||||
|
}
|
||||||
|
|
||||||
|
bat_status.cell_count = 0; // Unknown
|
||||||
|
bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
|
||||||
|
bat_status.source = 1; // External
|
||||||
|
bat_status.capacity = bat_info.full_charge_capacity_wh;
|
||||||
|
bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
|
||||||
|
bat_status.state_of_health = bat_info.state_of_health_pct; // External
|
||||||
|
bat_status.id = bat_info.battery_id;
|
||||||
|
|
||||||
|
/* Missing fields in UAVCANv0 legacy message
|
||||||
|
* temperature (partly)
|
||||||
|
* cell_count
|
||||||
|
* connected (partly)
|
||||||
|
* priority
|
||||||
|
* cycle_count
|
||||||
|
* run_time_to_empty
|
||||||
|
* average_time_to_empty
|
||||||
|
* manufacture_date
|
||||||
|
* max_error
|
||||||
|
* interface_error
|
||||||
|
* voltage_cell_v
|
||||||
|
* max_cell_voltage_delta
|
||||||
|
* is_powering_off
|
||||||
|
* warning
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
_battery_status_pub.publish(bat_status);
|
||||||
|
print_message(bat_status);
|
||||||
|
};
|
||||||
|
|
||||||
|
private:
|
||||||
|
uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
|
||||||
|
|
||||||
|
};
|
|
@ -69,9 +69,7 @@ public:
|
||||||
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
|
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
|
||||||
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
|
(_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_canard_sub);
|
&_subj_sub._canard_sub);
|
||||||
|
|
||||||
_port_id = _canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -168,13 +168,6 @@ void UavcanNode::init()
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
&_heartbeat_subscription);
|
&_heartbeat_subscription);
|
||||||
|
|
||||||
canardRxSubscribe(&_canard_instance,
|
|
||||||
CanardTransferKindMessage,
|
|
||||||
bms_port_id,
|
|
||||||
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
|
|
||||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
|
||||||
&_drone_srv_battery_subscription);
|
|
||||||
|
|
||||||
canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet
|
canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet
|
||||||
CanardTransferKindMessage,
|
CanardTransferKindMessage,
|
||||||
gps_port_id,
|
gps_port_id,
|
||||||
|
@ -281,6 +274,8 @@ void UavcanNode::Run()
|
||||||
CanardFrame received_frame{};
|
CanardFrame received_frame{};
|
||||||
received_frame.payload = &data;
|
received_frame.payload = &data;
|
||||||
|
|
||||||
|
/* FIXME this flawed we've to go through the whole loop to get the next frame in the buffer */
|
||||||
|
|
||||||
if (_can_interface->receive(&received_frame) > 0) {
|
if (_can_interface->receive(&received_frame) > 0) {
|
||||||
|
|
||||||
CanardTransfer receive{};
|
CanardTransfer receive{};
|
||||||
|
@ -297,22 +292,19 @@ void UavcanNode::Run()
|
||||||
// A transfer has been received, process it.
|
// A transfer has been received, process it.
|
||||||
// PX4_INFO("received Port ID: %d", receive.port_id);
|
// PX4_INFO("received Port ID: %d", receive.port_id);
|
||||||
|
|
||||||
if (receive.port_id == bms_port_id) {
|
if (receive.port_id == gps_port_id) {
|
||||||
result = handleBMSStatus(receive);
|
|
||||||
|
|
||||||
} else if (receive.port_id == gps_port_id) {
|
|
||||||
result = handleUORBSensorGPS(receive);
|
result = handleUORBSensorGPS(receive);
|
||||||
|
|
||||||
} else if (receive.port_id > 0) {
|
} else if (receive.port_id > 0) {
|
||||||
// If not a fixed port ID, check any subscribers which may have registered it
|
// If not a fixed port ID, check any subscribers which may have registered it
|
||||||
for (auto &subscriber : _subscribers) {
|
for (auto &subscriber : _subscribers) {
|
||||||
if (receive.port_id == subscriber->id()) {
|
if (subscriber->hasPortID(receive.port_id)) {
|
||||||
subscriber->callback(receive);
|
subscriber->callback(receive);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (auto &subscriber : _dynsubscribers) {
|
for (auto &subscriber : _dynsubscribers) {
|
||||||
if (receive.port_id == subscriber->id()) {
|
if (subscriber->hasPortID(receive.port_id)) {
|
||||||
subscriber->callback(receive);
|
subscriber->callback(receive);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -449,22 +441,6 @@ void UavcanNode::sendHeartbeat()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int UavcanNode::handleBMSStatus(const CanardTransfer &receive)
|
|
||||||
{
|
|
||||||
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
|
|
||||||
//TODO deserialize
|
|
||||||
/*
|
|
||||||
battery_status_s battery_status{};
|
|
||||||
battery_status.id = bms_status.battery_id;
|
|
||||||
battery_status.voltage_v = bms_status.voltage;
|
|
||||||
//battery_status.remaining = bms_status.remaining_capacity;
|
|
||||||
battery_status.timestamp = hrt_absolute_time();
|
|
||||||
_battery_status_pub.publish(battery_status);
|
|
||||||
*/
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
|
int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
|
||||||
{
|
{
|
||||||
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
|
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
|
||||||
|
|
|
@ -76,6 +76,7 @@
|
||||||
#include "Subscribers/Esc.hpp"
|
#include "Subscribers/Esc.hpp"
|
||||||
#include "Subscribers/Gnss.hpp"
|
#include "Subscribers/Gnss.hpp"
|
||||||
#include "Subscribers/NodeIDAllocationData.hpp"
|
#include "Subscribers/NodeIDAllocationData.hpp"
|
||||||
|
#include "Subscribers/LegacyBatteryInfo.hpp"
|
||||||
|
|
||||||
#include "ServiceClients/GetInfo.hpp"
|
#include "ServiceClients/GetInfo.hpp"
|
||||||
#include "ServiceClients/Access.hpp"
|
#include "ServiceClients/Access.hpp"
|
||||||
|
@ -167,7 +168,6 @@ private:
|
||||||
int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
|
int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
|
||||||
int handleRegisterList(const CanardTransfer &receive);
|
int handleRegisterList(const CanardTransfer &receive);
|
||||||
int handleRegisterAccess(const CanardTransfer &receive);
|
int handleRegisterAccess(const CanardTransfer &receive);
|
||||||
int handleBMSStatus(const CanardTransfer &receive);
|
|
||||||
int handleUORBSensorGPS(const CanardTransfer &receive);
|
int handleUORBSensorGPS(const CanardTransfer &receive);
|
||||||
|
|
||||||
void *_uavcan_heap{nullptr};
|
void *_uavcan_heap{nullptr};
|
||||||
|
@ -209,17 +209,11 @@ private:
|
||||||
* for demo purposes untill we have nice interface (QGC or latter)
|
* for demo purposes untill we have nice interface (QGC or latter)
|
||||||
* to configure the nodes
|
* to configure the nodes
|
||||||
*/
|
*/
|
||||||
const uint16_t bms_port_id = 1234;
|
|
||||||
const uint16_t gps_port_id = 1235;
|
const uint16_t gps_port_id = 1235;
|
||||||
|
|
||||||
CanardTransferID _uavcan_register_list_request_transfer_id{0};
|
CanardTransferID _uavcan_register_list_request_transfer_id{0};
|
||||||
CanardTransferID _uavcan_register_access_request_transfer_id{0};
|
CanardTransferID _uavcan_register_access_request_transfer_id{0};
|
||||||
|
|
||||||
// regulated::drone::sensor::BMSStatus_1_0
|
|
||||||
uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_service_battery_Status_0_1_EXTENT_BYTES_];
|
|
||||||
hrt_abstime _regulated_drone_sensor_bmsstatus_last{0};
|
|
||||||
CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0};
|
|
||||||
|
|
||||||
DEFINE_PARAMETERS(
|
DEFINE_PARAMETERS(
|
||||||
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
|
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
|
||||||
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
|
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
|
||||||
|
@ -243,8 +237,9 @@ private:
|
||||||
UavcanGnssSubscriber _gps0_sub {_canard_instance, _param_manager, 0};
|
UavcanGnssSubscriber _gps0_sub {_canard_instance, _param_manager, 0};
|
||||||
UavcanGnssSubscriber _gps1_sub {_canard_instance, _param_manager, 1};
|
UavcanGnssSubscriber _gps1_sub {_canard_instance, _param_manager, 1};
|
||||||
UavcanBmsSubscriber _bms0_sub {_canard_instance, _param_manager, 0};
|
UavcanBmsSubscriber _bms0_sub {_canard_instance, _param_manager, 0};
|
||||||
UavcanBmsSubscriber _bms1_sub {_canard_instance, _param_manager, 1};
|
//UavcanBmsSubscriber _bms1_sub {_canard_instance, _param_manager, 1};
|
||||||
UavcanEscSubscriber _esc_sub {_canard_instance, _param_manager, 0};
|
UavcanEscSubscriber _esc_sub {_canard_instance, _param_manager, 0};
|
||||||
|
UavcanLegacyBatteryInfoSubscriber _legacybms_sub {_canard_instance, _param_manager, 0};
|
||||||
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
|
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
|
||||||
|
|
||||||
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
|
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
|
||||||
|
@ -254,7 +249,7 @@ private:
|
||||||
UavcanListServiceReply _list_service {_canard_instance, _node_manager};
|
UavcanListServiceReply _list_service {_canard_instance, _node_manager};
|
||||||
|
|
||||||
// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
|
// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
|
||||||
UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
|
UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub}; /// TODO: turn into List<UavcanSubscription*>
|
||||||
UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>
|
UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>
|
||||||
|
|
||||||
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
|
UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
|
||||||
|
|
|
@ -90,8 +90,14 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);
|
||||||
PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
|
PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
|
||||||
PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
|
PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
|
||||||
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
|
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
|
||||||
PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
|
//PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
|
||||||
PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
|
//PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
|
||||||
|
|
||||||
|
PARAM_DEFINE_INT32(UCAN1_BMS_ES_PID, 0);
|
||||||
|
PARAM_DEFINE_INT32(UCAN1_BMS_BS_PID, 0);
|
||||||
|
PARAM_DEFINE_INT32(UCAN1_BMS_BP_PID, 0);
|
||||||
|
|
||||||
|
PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0);
|
||||||
|
|
||||||
// Publication Port IDs
|
// Publication Port IDs
|
||||||
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
|
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e
|
Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e
|
Loading…
Reference in New Issue