forked from Archive/PX4-Autopilot
[UAVCANv1] Subscriber multi topic subscription
reg.drone.bms full support legacy.equipment.bms support
This commit is contained in:
Peter van der Perk
Lorenz Meier
parent
5e7ebbe47c
commit
399815469f
|
|
@ -56,7 +56,7 @@
|
|||
url = https://github.com/UAVCAN/libcanard.git
|
||||
[submodule "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"]
|
||||
path = src/drivers/uavcannode_gps_demo/public_regulated_data_types
|
||||
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)
|
||||
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}/legacy)
|
||||
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
|
||||
else()
|
||||
message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
|
||||
|
|
|
|||
|
|
@ -63,14 +63,18 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
const UavcanParamBinder _uavcan_params[8] {
|
||||
const UavcanParamBinder _uavcan_params[10] {
|
||||
{"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"},
|
||||
{"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"},
|
||||
{"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"},
|
||||
{"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID"},
|
||||
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"},
|
||||
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"},
|
||||
{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"},
|
||||
{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
|
||||
{"uavcan.sub.energy_source.0.id", "UCAN1_BMS_ES_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
|
||||
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 {};
|
||||
_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_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_;
|
||||
&_subj_sub._canard_sub);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -66,9 +66,7 @@ public:
|
|||
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
|
||||
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_;
|
||||
&_subj_sub._canard_sub);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -65,9 +65,7 @@ public:
|
|||
uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_;
|
||||
&_subj_sub._canard_sub);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -65,9 +65,7 @@ public:
|
|||
uavcan_register_List_1_0_FIXED_PORT_ID_,
|
||||
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = uavcan_register_List_1_0_FIXED_PORT_ID_;
|
||||
&_subj_sub._canard_sub);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -53,28 +53,78 @@ public:
|
|||
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
|
||||
|
||||
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 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;
|
||||
|
||||
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()
|
||||
{
|
||||
if (_port_id != CANARD_PORT_ID_UNSET) {
|
||||
PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id);
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
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:
|
||||
CanardInstance &_canard_instance;
|
||||
struct SubjectSubscription {
|
||||
CanardRxSubscription _canard_sub;
|
||||
const char *_subject_name;
|
||||
struct SubjectSubscription *next {NULL};
|
||||
};
|
||||
|
||||
CanardInstance &_canard_instance;
|
||||
SubjectSubscription _subj_sub;
|
||||
uint8_t _instance {0};
|
||||
/// 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
|
||||
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
|
||||
// DS-15 Specification Messages
|
||||
#include <reg/drone/service/battery/Parameters_0_1.h>
|
||||
#include <reg/drone/service/battery/Status_0_1.h>
|
||||
#include <reg/drone/physics/electricity/SourceTs_0_1.h>
|
||||
#include <reg/drone/service/battery/Parameters_0_3.h>
|
||||
#include <reg/drone/service/battery/Status_0_2.h>
|
||||
|
||||
#include "DynamicPortSubscriber.hpp"
|
||||
|
||||
#define KELVIN_OFFSET 273.15f
|
||||
#define WH_TO_JOULE 3600
|
||||
|
||||
class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
|
||||
{
|
||||
public:
|
||||
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
|
||||
{
|
||||
// Subscribe to messages reg.drone.service.battery.Status.0.1
|
||||
// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindMessage,
|
||||
_port_id,
|
||||
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
|
||||
_subj_sub._canard_sub._port_id,
|
||||
reg_drone_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
|
||||
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
|
||||
{
|
||||
PX4_INFO("BmsCallback");
|
||||
|
||||
reg_drone_service_battery_Status_0_1 bat {};
|
||||
if (receive.port_id == _subj_sub._canard_sub._port_id) {
|
||||
reg_drone_physics_electricity_SourceTs_0_1 source_ts {};
|
||||
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;
|
||||
|
||||
bat_status.connected = true; // Based on some threshold or an error??
|
||||
|
||||
// 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_1_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
|
||||
reg_drone_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
|
||||
|
||||
uavcan_si_unit_voltage_Scalar_1_0 V_Min = bat.cell_voltage_min_max[0];
|
||||
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
|
||||
};
|
||||
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,31 +60,38 @@ public:
|
|||
|
||||
void updateParam()
|
||||
{
|
||||
SubjectSubscription *curSubj = &_subj_sub;
|
||||
|
||||
while (curSubj != NULL) {
|
||||
char uavcan_param[90];
|
||||
sprintf(uavcan_param, "uavcan.sub.%s.%d.id", _subject_name, _instance);
|
||||
sprintf(uavcan_param, "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance);
|
||||
|
||||
// Set _port_id from _uavcan_param
|
||||
uavcan_register_Value_1_0 value;
|
||||
_param_manager.GetParamByName(uavcan_param, value);
|
||||
int32_t new_id = value.integer32.value.elements[0];
|
||||
|
||||
if (_port_id != new_id) {
|
||||
/* FIXME how about partial subscribing */
|
||||
if (curSubj->_canard_sub._port_id != new_id) {
|
||||
if (new_id == CANARD_PORT_ID_UNSET) {
|
||||
// Cancel subscription
|
||||
unsubscribe();
|
||||
|
||||
} else {
|
||||
if (_port_id != CANARD_PORT_ID_UNSET) {
|
||||
if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
|
||||
// Already active; unsubscribe first
|
||||
unsubscribe();
|
||||
}
|
||||
|
||||
// Subscribe on the new port ID
|
||||
_port_id = (CanardPortID)new_id;
|
||||
PX4_INFO("Subscribing %s.%d on port %d", _subject_name, _instance, _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;
|
||||
}
|
||||
};
|
||||
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -62,15 +62,15 @@ public:
|
|||
// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindMessage,
|
||||
_port_id,
|
||||
_subj_sub._canard_sub._port_id,
|
||||
reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
&_subj_sub._canard_sub);
|
||||
|
||||
// Subscribe to messages reg.drone.service.common.Readiness.0.1
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
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_,
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub_readiness);
|
||||
|
|
|
|||
|
|
@ -57,10 +57,10 @@ public:
|
|||
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
|
||||
canardRxSubscribe(&_canard_instance,
|
||||
CanardTransferKindMessage,
|
||||
_port_id,
|
||||
_subj_sub._canard_sub._port_id,
|
||||
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
|
||||
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):
|
||||
* # 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_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
|
||||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_canard_sub);
|
||||
|
||||
_port_id = _canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID;
|
||||
&_subj_sub._canard_sub);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -168,13 +168,6 @@ void UavcanNode::init()
|
|||
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||
&_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
|
||||
CanardTransferKindMessage,
|
||||
gps_port_id,
|
||||
|
|
@ -281,6 +274,8 @@ void UavcanNode::Run()
|
|||
CanardFrame received_frame{};
|
||||
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) {
|
||||
|
||||
CanardTransfer receive{};
|
||||
|
|
@ -297,22 +292,19 @@ void UavcanNode::Run()
|
|||
// A transfer has been received, process it.
|
||||
// PX4_INFO("received Port ID: %d", receive.port_id);
|
||||
|
||||
if (receive.port_id == bms_port_id) {
|
||||
result = handleBMSStatus(receive);
|
||||
|
||||
} else if (receive.port_id == gps_port_id) {
|
||||
if (receive.port_id == gps_port_id) {
|
||||
result = handleUORBSensorGPS(receive);
|
||||
|
||||
} else if (receive.port_id > 0) {
|
||||
// If not a fixed port ID, check any subscribers which may have registered it
|
||||
for (auto &subscriber : _subscribers) {
|
||||
if (receive.port_id == subscriber->id()) {
|
||||
if (subscriber->hasPortID(receive.port_id)) {
|
||||
subscriber->callback(receive);
|
||||
}
|
||||
}
|
||||
|
||||
for (auto &subscriber : _dynsubscribers) {
|
||||
if (receive.port_id == subscriber->id()) {
|
||||
if (subscriber->hasPortID(receive.port_id)) {
|
||||
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)
|
||||
{
|
||||
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@
|
|||
#include "Subscribers/Esc.hpp"
|
||||
#include "Subscribers/Gnss.hpp"
|
||||
#include "Subscribers/NodeIDAllocationData.hpp"
|
||||
#include "Subscribers/LegacyBatteryInfo.hpp"
|
||||
|
||||
#include "ServiceClients/GetInfo.hpp"
|
||||
#include "ServiceClients/Access.hpp"
|
||||
|
|
@ -167,7 +168,6 @@ private:
|
|||
int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
|
||||
int handleRegisterList(const CanardTransfer &receive);
|
||||
int handleRegisterAccess(const CanardTransfer &receive);
|
||||
int handleBMSStatus(const CanardTransfer &receive);
|
||||
int handleUORBSensorGPS(const CanardTransfer &receive);
|
||||
|
||||
void *_uavcan_heap{nullptr};
|
||||
|
|
@ -209,17 +209,11 @@ private:
|
|||
* for demo purposes untill we have nice interface (QGC or latter)
|
||||
* to configure the nodes
|
||||
*/
|
||||
const uint16_t bms_port_id = 1234;
|
||||
const uint16_t gps_port_id = 1235;
|
||||
|
||||
CanardTransferID _uavcan_register_list_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(
|
||||
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
|
||||
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
|
||||
|
|
@ -243,8 +237,9 @@ private:
|
|||
UavcanGnssSubscriber _gps0_sub {_canard_instance, _param_manager, 0};
|
||||
UavcanGnssSubscriber _gps1_sub {_canard_instance, _param_manager, 1};
|
||||
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};
|
||||
UavcanLegacyBatteryInfoSubscriber _legacybms_sub {_canard_instance, _param_manager, 0};
|
||||
UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
|
||||
|
||||
UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
|
||||
|
|
@ -254,7 +249,7 @@ private:
|
|||
UavcanListServiceReply _list_service {_canard_instance, _node_manager};
|
||||
|
||||
// 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*>
|
||||
|
||||
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_GPS0_PID, 0);
|
||||
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
|
||||
PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
|
||||
PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
|
||||
//PARAM_DEFINE_INT32(UCAN1_BMS0_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
|
||||
PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e
|
||||
Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e
|
||||
Loading…
Reference in New Issue