From 08ce72be9c78cee744d6e41b2e0cf60b32eec311 Mon Sep 17 00:00:00 2001 From: Jordan Leiber Date: Tue, 5 Nov 2024 10:48:03 -0500 Subject: [PATCH 1/3] implement support for the receiving and logging of the dronecan esc status extended message --- msg/CMakeLists.txt | 2 ++ msg/DronecanEscStatusExtended.msg | 5 ++++ msg/DronecanEscStatusExtendedData.msg | 9 +++++++ src/drivers/uavcan/actuators/esc.cpp | 38 ++++++++++++++++++++++++++- src/drivers/uavcan/actuators/esc.hpp | 15 +++++++++++ src/modules/logger/logged_topics.cpp | 1 + 6 files changed, 69 insertions(+), 1 deletion(-) create mode 100644 msg/DronecanEscStatusExtended.msg create mode 100644 msg/DronecanEscStatusExtendedData.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c8c8518bb861..166a10dff99b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -74,6 +74,8 @@ set(msg_files DifferentialPressure.msg DistanceSensor.msg DistanceSensorModeChangeRequest.msg + DronecanEscStatusExtended.msg + DronecanEscStatusExtendedData.msg Ekf2Timestamps.msg EscReport.msg EscStatus.msg diff --git a/msg/DronecanEscStatusExtended.msg b/msg/DronecanEscStatusExtended.msg new file mode 100644 index 000000000000..7c704d8afe55 --- /dev/null +++ b/msg/DronecanEscStatusExtended.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. To be consistent with ESC Status, limit it to 8. + +DronecanEscStatusExtendedData[8] extended_esc_status_data # An array of up to CONNECTED_ESC_MAX DronecanEscStatusExtendedData instances diff --git a/msg/DronecanEscStatusExtendedData.msg b/msg/DronecanEscStatusExtendedData.msg new file mode 100644 index 000000000000..7b060c98cff9 --- /dev/null +++ b/msg/DronecanEscStatusExtendedData.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +# From the StatusExtended.uavcan message +uint8 input_percent # Input command to ESC, in percent, which is commanded using the setpoint messages. Range 0% to 100%. +uint8 output_percent # Output command from ESC to motor, in percent. Range 0% to 100%. +int16 motor_temperature_deg_c # Temperature of connected motor, in Celsius. Range is -256 to +255 C. +uint16 motor_angle # Measured angle of connected angle sensor, in degrees. Range is 0 to 360. +uint32 status_flags # Manufacturer-specific status flags currently active. +uint8 esc_index # Index of currently reporting ESC. diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 12f5d8600bf2..3ac724521951 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -48,7 +48,8 @@ using namespace time_literals; UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), - _uavcan_sub_status(node) + _uavcan_sub_status(node), + _uavcan_sub_status_extended(node) { _uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority } @@ -64,7 +65,16 @@ UavcanEscController::init() return res; } + //ESC Status Extended subscription + res = _uavcan_sub_status_extended.start(StatusExtendedCbBinder(this, &UavcanEscController::esc_status_extended_sub_cb)); + + if (res < 0) { + PX4_ERR("ESC status extended sub failed %i", res); + return res; + } + _esc_status_pub.advertise(); + _status_extended_pub.advertise(); //Make sure people are listening return res; } @@ -154,6 +164,32 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &received_status_extended_msg) +{ + //Make sure it's an ESC we can handle + if (received_status_extended_msg.esc_index < dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) { + //Grab the ESC we're talking about + auto &esc_reference = _status_extended.extended_esc_status_data[received_status_extended_msg.esc_index]; + + //Fill in the data + esc_reference.input_percent = received_status_extended_msg.input_pct; + esc_reference.output_percent = received_status_extended_msg.output_pct; + esc_reference.motor_temperature_deg_c = received_status_extended_msg.motor_temperature_degC; + esc_reference.motor_angle = received_status_extended_msg.motor_angle; + esc_reference.status_flags = received_status_extended_msg.status_flags; + esc_reference.esc_index = received_status_extended_msg.esc_index; + esc_reference.timestamp = hrt_absolute_time(); + + //Make sure to update the timestamp of our top level status + _status_extended.timestamp = hrt_absolute_time(); + + //Put the data into the world + _status_extended_pub.publish(_status_extended); + } +} + uint8_t UavcanEscController::check_escs_status() { diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 516b1472147c..615d0d73511c 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -47,12 +47,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include class UavcanEscController { @@ -86,6 +88,12 @@ class UavcanEscController */ void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + /** + * ESC status extended message reception will be reported via this callback. + */ + void esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure + &received_status_extended_msg); + /** * Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline. */ @@ -94,12 +102,18 @@ class UavcanEscController typedef uavcan::MethodBinder&)> StatusCbBinder; + typedef uavcan::MethodBinder&)> + StatusExtendedCbBinder; + typedef uavcan::MethodBinder TimerCbBinder; esc_status_s _esc_status{}; + dronecan_esc_status_extended_s _status_extended{}; uORB::PublicationMulti _esc_status_pub{ORB_ID(esc_status)}; + uORB::Publication _status_extended_pub{ORB_ID(dronecan_esc_status_extended)}; uint8_t _rotor_count{0}; @@ -110,6 +124,7 @@ class UavcanEscController uavcan::INode &_node; uavcan::Publisher _uavcan_pub_raw_cmd; uavcan::Subscriber _uavcan_sub_status; + uavcan::Subscriber _uavcan_sub_status_extended; /* * ESC states diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..1489a06f5647 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -59,6 +59,7 @@ void LoggedTopics::add_default_topics() add_topic("config_overrides"); add_topic("cpuload"); add_topic("distance_sensor_mode_change_request"); + add_optional_topic("dronecan_esc_status_extended", 250); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); From 68a48f0c6f8e88b286d306f2d98a6195c09b6648 Mon Sep 17 00:00:00 2001 From: Jordan Leiber Date: Thu, 7 Nov 2024 08:59:59 -0500 Subject: [PATCH 2/3] Update esc status extended pub to be a publicationmulti. Remove now unnecessary messge type --- msg/CMakeLists.txt | 1 - msg/DronecanEscStatusExtended.msg | 10 +++++++-- msg/DronecanEscStatusExtendedData.msg | 9 -------- src/drivers/uavcan/actuators/esc.cpp | 31 +++++++++++---------------- src/drivers/uavcan/actuators/esc.hpp | 15 +++++++++++-- src/modules/logger/logged_topics.cpp | 2 +- 6 files changed, 35 insertions(+), 33 deletions(-) delete mode 100644 msg/DronecanEscStatusExtendedData.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 166a10dff99b..abd7188ecc11 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -75,7 +75,6 @@ set(msg_files DistanceSensor.msg DistanceSensorModeChangeRequest.msg DronecanEscStatusExtended.msg - DronecanEscStatusExtendedData.msg Ekf2Timestamps.msg EscReport.msg EscStatus.msg diff --git a/msg/DronecanEscStatusExtended.msg b/msg/DronecanEscStatusExtended.msg index 7c704d8afe55..04e4f9472cde 100644 --- a/msg/DronecanEscStatusExtended.msg +++ b/msg/DronecanEscStatusExtended.msg @@ -1,5 +1,11 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. To be consistent with ESC Status, limit it to 8. -DronecanEscStatusExtendedData[8] extended_esc_status_data # An array of up to CONNECTED_ESC_MAX DronecanEscStatusExtendedData instances +# From the StatusExtended.uavcan message +uint8 esc_index # Index of currently reporting ESC. +uint8 input_percent # Input command to ESC, in percent, which is commanded using the setpoint messages. Range 0% to 100%. +uint8 output_percent # Output command from ESC to motor, in percent. Range 0% to 100%. +int16 motor_temperature_deg_c # Temperature of connected motor, in Celsius. Range is -256 to +255 C. +uint16 motor_angle # Measured angle of connected angle sensor, in degrees. Range is 0 to 360. +uint32 status_flags # Manufacturer-specific status flags currently active. diff --git a/msg/DronecanEscStatusExtendedData.msg b/msg/DronecanEscStatusExtendedData.msg deleted file mode 100644 index 7b060c98cff9..000000000000 --- a/msg/DronecanEscStatusExtendedData.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -# From the StatusExtended.uavcan message -uint8 input_percent # Input command to ESC, in percent, which is commanded using the setpoint messages. Range 0% to 100%. -uint8 output_percent # Output command from ESC to motor, in percent. Range 0% to 100%. -int16 motor_temperature_deg_c # Temperature of connected motor, in Celsius. Range is -256 to +255 C. -uint16 motor_angle # Measured angle of connected angle sensor, in degrees. Range is 0 to 360. -uint32 status_flags # Manufacturer-specific status flags currently active. -uint8 esc_index # Index of currently reporting ESC. diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 3ac724521951..b22331f3dad3 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -74,7 +74,6 @@ UavcanEscController::init() } _esc_status_pub.advertise(); - _status_extended_pub.advertise(); //Make sure people are listening return res; } @@ -170,23 +169,19 @@ UavcanEscController::esc_status_extended_sub_cb(const { //Make sure it's an ESC we can handle if (received_status_extended_msg.esc_index < dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) { - //Grab the ESC we're talking about - auto &esc_reference = _status_extended.extended_esc_status_data[received_status_extended_msg.esc_index]; - - //Fill in the data - esc_reference.input_percent = received_status_extended_msg.input_pct; - esc_reference.output_percent = received_status_extended_msg.output_pct; - esc_reference.motor_temperature_deg_c = received_status_extended_msg.motor_temperature_degC; - esc_reference.motor_angle = received_status_extended_msg.motor_angle; - esc_reference.status_flags = received_status_extended_msg.status_flags; - esc_reference.esc_index = received_status_extended_msg.esc_index; - esc_reference.timestamp = hrt_absolute_time(); - - //Make sure to update the timestamp of our top level status - _status_extended.timestamp = hrt_absolute_time(); - - //Put the data into the world - _status_extended_pub.publish(_status_extended); + //Make a struct to store our data (we'll decide who it belongs to later) + dronecan_esc_status_extended_s status_extended_to_publish{}; + + status_extended_to_publish.timestamp = hrt_absolute_time(); + status_extended_to_publish.esc_index = received_status_extended_msg.esc_index; + status_extended_to_publish.input_percent = received_status_extended_msg.input_pct; + status_extended_to_publish.output_percent = received_status_extended_msg.output_pct; + status_extended_to_publish.motor_temperature_deg_c = received_status_extended_msg.motor_temperature_degC; + status_extended_to_publish.motor_angle = received_status_extended_msg.motor_angle; + status_extended_to_publish.status_flags = received_status_extended_msg.status_flags; + + //Put the data into the world using esc index as our publication index + _status_extended_pub[received_status_extended_msg.esc_index].publish(status_extended_to_publish); } } diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 615d0d73511c..b55489972edc 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -110,10 +110,21 @@ class UavcanEscController void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder; esc_status_s _esc_status{}; - dronecan_esc_status_extended_s _status_extended{}; uORB::PublicationMulti _esc_status_pub{ORB_ID(esc_status)}; - uORB::Publication _status_extended_pub{ORB_ID(dronecan_esc_status_extended)}; + + //This is a PublicationMulti where we know all of the instances live here. So, let's make an array to store as many as we know how to handle (dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) + uORB::PublicationMulti + _status_extended_pub[dronecan_esc_status_extended_s::CONNECTED_ESC_MAX] { + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended), + ORB_ID(dronecan_esc_status_extended) + }; uint8_t _rotor_count{0}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1489a06f5647..cade0aaa9258 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -59,7 +59,6 @@ void LoggedTopics::add_default_topics() add_topic("config_overrides"); add_topic("cpuload"); add_topic("distance_sensor_mode_change_request"); - add_optional_topic("dronecan_esc_status_extended", 250); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); @@ -153,6 +152,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("actuator_outputs", 100, 3); add_optional_topic_multi("airspeed_wind", 1000, 4); add_optional_topic_multi("control_allocator_status", 200, 2); + add_optional_topic_multi("dronecan_esc_status_extended", 250, 8); add_optional_topic_multi("rate_ctrl_status", 200, 2); add_optional_topic_multi("sensor_hygrometer", 500, 4); add_optional_topic_multi("rpm", 200); From 56d18fdc5f0f2d8d96a16a05ca39e9724ec51a93 Mon Sep 17 00:00:00 2001 From: Jordan Leiber Date: Mon, 11 Nov 2024 15:59:35 -0500 Subject: [PATCH 3/3] simplify naming of variables in the Status Extended callback --- src/drivers/uavcan/actuators/esc.cpp | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index b22331f3dad3..b1d3873ffe4a 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -165,23 +165,25 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &received_status_extended_msg) + uavcan::ReceivedDataStructure &msg) { + uint8_t index = msg.esc_index; + //Make sure it's an ESC we can handle - if (received_status_extended_msg.esc_index < dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) { + if (index < dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) { //Make a struct to store our data (we'll decide who it belongs to later) - dronecan_esc_status_extended_s status_extended_to_publish{}; + dronecan_esc_status_extended_s status{}; - status_extended_to_publish.timestamp = hrt_absolute_time(); - status_extended_to_publish.esc_index = received_status_extended_msg.esc_index; - status_extended_to_publish.input_percent = received_status_extended_msg.input_pct; - status_extended_to_publish.output_percent = received_status_extended_msg.output_pct; - status_extended_to_publish.motor_temperature_deg_c = received_status_extended_msg.motor_temperature_degC; - status_extended_to_publish.motor_angle = received_status_extended_msg.motor_angle; - status_extended_to_publish.status_flags = received_status_extended_msg.status_flags; + status.timestamp = hrt_absolute_time(); + status.esc_index = index; + status.input_percent = msg.input_pct; + status.output_percent = msg.output_pct; + status.motor_temperature_deg_c = msg.motor_temperature_degC; + status.motor_angle = msg.motor_angle; + status.status_flags = msg.status_flags; //Put the data into the world using esc index as our publication index - _status_extended_pub[received_status_extended_msg.esc_index].publish(status_extended_to_publish); + _status_extended_pub[index].publish(status); } }