Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Support for DroneCAN Status Extended Message #23896

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions msg/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ set(msg_files
DifferentialPressure.msg
DistanceSensor.msg
DistanceSensorModeChangeRequest.msg
DronecanEscStatusExtended.msg
Ekf2Timestamps.msg
EscReport.msg
EscStatus.msg
Expand Down
11 changes: 11 additions & 0 deletions msg/DronecanEscStatusExtended.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
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.

# 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.
35 changes: 34 additions & 1 deletion src/drivers/uavcan/actuators/esc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand All @@ -64,6 +65,14 @@ 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();

return res;
Expand Down Expand Up @@ -154,6 +163,30 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
}
}

void
UavcanEscController::esc_status_extended_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &msg)
{
uint8_t index = msg.esc_index;

//Make sure it's an ESC we can handle
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{};

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[index].publish(status);
}
}

uint8_t
UavcanEscController::check_escs_status()
{
Expand Down
26 changes: 26 additions & 0 deletions src/drivers/uavcan/actuators/esc.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,12 +47,14 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/esc/StatusExtended.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/esc_status.h>
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <uORB/topics/dronecan_esc_status_extended.h>

class UavcanEscController
{
Expand Down Expand Up @@ -86,6 +88,12 @@ class UavcanEscController
*/
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);

/**
* ESC status extended message reception will be reported via this callback.
*/
void esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>
&received_status_extended_msg);

/**
* Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
*/
Expand All @@ -94,13 +102,30 @@ class UavcanEscController
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;

typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)>
StatusExtendedCbBinder;

typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;

esc_status_s _esc_status{};

uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};

//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<dronecan_esc_status_extended_s>
dakejahl marked this conversation as resolved.
Show resolved Hide resolved
_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};

/*
Expand All @@ -110,6 +135,7 @@ class UavcanEscController
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, StatusExtendedCbBinder> _uavcan_sub_status_extended;

/*
* ESC states
Expand Down
1 change: 1 addition & 0 deletions src/modules/logger/logged_topics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -152,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);
Expand Down
Loading