From 399815469f7443e6d7753d20013614a03e0d7d34 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 12 Apr 2021 16:15:59 +0200 Subject: [PATCH] [UAVCANv1] Subscriber multi topic subscription reg.drone.bms full support legacy.equipment.bms support --- .gitmodules | 2 +- src/drivers/uavcan_v1/CMakeLists.txt | 1 + src/drivers/uavcan_v1/ParamManager.hpp | 22 +-- src/drivers/uavcan_v1/Publishers/Gnss.hpp | 3 +- .../uavcan_v1/Publishers/Readiness.hpp | 102 +++++++++++++ .../uavcan_v1/ServiceClients/Access.hpp | 4 +- .../uavcan_v1/ServiceClients/GetInfo.hpp | 4 +- .../uavcan_v1/Services/AccessReply.hpp | 4 +- src/drivers/uavcan_v1/Services/ListReply.hpp | 4 +- .../uavcan_v1/Subscribers/BaseSubscriber.hpp | 68 +++++++-- src/drivers/uavcan_v1/Subscribers/Battery.hpp | 140 +++++++++++++++--- .../Subscribers/DynamicPortSubscriber.hpp | 43 +++--- src/drivers/uavcan_v1/Subscribers/Esc.hpp | 6 +- src/drivers/uavcan_v1/Subscribers/Gnss.hpp | 4 +- .../Subscribers/LegacyBatteryInfo.hpp | 129 ++++++++++++++++ .../Subscribers/NodeIDAllocationData.hpp | 4 +- src/drivers/uavcan_v1/Uavcan.cpp | 34 +---- src/drivers/uavcan_v1/Uavcan.hpp | 13 +- src/drivers/uavcan_v1/parameters.c | 10 +- .../uavcan_v1/public_regulated_data_types | 2 +- 20 files changed, 483 insertions(+), 116 deletions(-) create mode 100644 src/drivers/uavcan_v1/Publishers/Readiness.hpp create mode 100644 src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp diff --git a/.gitmodules b/.gitmodules index 4316f92406..aef1b8c555 100644 --- a/.gitmodules +++ b/.gitmodules @@ -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 diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index f1eed4a12e..b9d609c6b4 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -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") diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index 6d99174230..81888b930f 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -63,14 +63,18 @@ public: private: - const UavcanParamBinder _uavcan_params[8] { - {"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"}, + 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.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"}, }; }; diff --git a/src/drivers/uavcan_v1/Publishers/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/Gnss.hpp index 1a53f5a452..eea856f549 100644 --- a/src/drivers/uavcan_v1/Publishers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/Gnss.hpp @@ -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); diff --git a/src/drivers/uavcan_v1/Publishers/Readiness.hpp b/src/drivers/uavcan_v1/Publishers/Readiness.hpp new file mode 100644 index 0000000000..624032a825 --- /dev/null +++ b/src/drivers/uavcan_v1/Publishers/Readiness.hpp @@ -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 + */ + +#pragma once + +// DS-15 Specification Messages +#include + +#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)}; +}; diff --git a/src/drivers/uavcan_v1/ServiceClients/Access.hpp b/src/drivers/uavcan_v1/ServiceClients/Access.hpp index 9c64cc4847..372394eb4d 100644 --- a/src/drivers/uavcan_v1/ServiceClients/Access.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/Access.hpp @@ -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); }; diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp index bc24004e2e..11a9420891 100644 --- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp @@ -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); }; diff --git a/src/drivers/uavcan_v1/Services/AccessReply.hpp b/src/drivers/uavcan_v1/Services/AccessReply.hpp index 38a4d83e1d..ed7e5508e8 100644 --- a/src/drivers/uavcan_v1/Services/AccessReply.hpp +++ b/src/drivers/uavcan_v1/Services/AccessReply.hpp @@ -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); }; diff --git a/src/drivers/uavcan_v1/Services/ListReply.hpp b/src/drivers/uavcan_v1/Services/ListReply.hpp index 1b747da316..8232190cbf 100644 --- a/src/drivers/uavcan_v1/Services/ListReply.hpp +++ b/src/drivers/uavcan_v1/Services/ListReply.hpp @@ -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); }; diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp index 1f963197f2..4a536f8c05 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -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: + struct SubjectSubscription { + CanardRxSubscription _canard_sub; + const char *_subject_name; + struct SubjectSubscription *next {NULL}; + }; + CanardInstance &_canard_instance; - CanardRxSubscription _canard_sub; - const char *_subject_name; + 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}; }; diff --git a/src/drivers/uavcan_v1/Subscribers/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/Battery.hpp index fb41ab95e2..e3a6c4ed29 100644 --- a/src/drivers/uavcan_v1/Subscribers/Battery.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Battery.hpp @@ -41,43 +41,149 @@ #pragma once +#include +#include + // DS-15 Specification Messages -#include -#include +#include +#include +#include #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 {}; - 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); + 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; - 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(V_Min.volt); - double vmax = static_cast(V_Max.volt); - PX4_INFO("Min voltage: %f, Max Voltage: %f", vmin, vmax); - /// do something with the data - }; + 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_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_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}; }; diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index 93140fb811..89ae5431f6 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -60,30 +60,37 @@ public: void updateParam() { - char uavcan_param[90]; - sprintf(uavcan_param, "uavcan.sub.%s.%d.id", _subject_name, _instance); + SubjectSubscription *curSubj = &_subj_sub; - // 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]; + while (curSubj != NULL) { + char uavcan_param[90]; + sprintf(uavcan_param, "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance); - if (_port_id != new_id) { - if (new_id == CANARD_PORT_ID_UNSET) { - // Cancel subscription - unsubscribe(); + // 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]; - } else { - if (_port_id != CANARD_PORT_ID_UNSET) { - // Already active; unsubscribe first + /* FIXME how about partial subscribing */ + if (curSubj->_canard_sub._port_id != new_id) { + if (new_id == CANARD_PORT_ID_UNSET) { + // Cancel subscription 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); - subscribe(); + } else { + if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) { + // Already active; unsubscribe first + 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; } }; diff --git a/src/drivers/uavcan_v1/Subscribers/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/Esc.hpp index e323a058e0..941d059033 100644 --- a/src/drivers/uavcan_v1/Subscribers/Esc.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Esc.hpp @@ -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(static_cast(_port_id) + 1), + static_cast(static_cast(_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); diff --git a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp index 83c22cffdf..6c5ee051b3 100644 --- a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp @@ -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: diff --git a/src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp b/src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp new file mode 100644 index 0000000000..172ea1bcfc --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp @@ -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 + */ + +#pragma once + +#include +#include + +// Legacy message from UAVCANv0 +#include + +#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_pub{ORB_ID(battery_status)}; + +}; diff --git a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp b/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp index 0505eb5cc9..0a6c637781 100644 --- a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp +++ b/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp @@ -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); }; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 6437312a7d..8a64e9796c 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -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); diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 6ca66b4d2e..da01b15427 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -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) _param_uavcan_v1_enable, (ParamInt) _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 + UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub}; /// TODO: turn into List UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index 2b0eba1c62..417fe31de5 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -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); diff --git a/src/drivers/uavcan_v1/public_regulated_data_types b/src/drivers/uavcan_v1/public_regulated_data_types index 309b251a7e..36a01e428b 160000 --- a/src/drivers/uavcan_v1/public_regulated_data_types +++ b/src/drivers/uavcan_v1/public_regulated_data_types @@ -1 +1 @@ -Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e +Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e