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");