fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-07 04:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: ESC publish esc_status in callback instead of timer

Browse Source 
- this ensures every ESC status gets published with minimal latency
 - also prevents publishing ORB_ID(esc_status) when there's no actual
data (bug)
This commit is contained in:
Daniel Agar
2022-01-14 12:17:49 -05:00
parent 25099ce11f
commit 81ecd130fc
2 changed files with 11 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/actuators/esc.cpp
+10 -29
View File
@@ -48,16 +48,11 @@ using namespace time_literals;
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
_uavcan_sub_status(node),
_orb_timer(node)
_uavcan_sub_status(node)
{
_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
}
UavcanEscController::~UavcanEscController()
{
}
int
UavcanEscController::init()
{
@@ -69,11 +64,6 @@ UavcanEscController::init()
return res;
}
// ESC status will be relayed from UAVCAN bus into ORB at this rate
_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
_esc_status_pub.advertise();
return res;
}
@@ -136,10 +126,6 @@ void
UavcanEscController::set_rotor_count(uint8_t count)
{
_rotor_count = count;
if (_rotor_count != 0u) {
_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
}
}
void
@@ -148,29 +134,24 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
auto &ref = _esc_status.esc[msg.esc_index];
ref.esc_address = msg.getSrcNodeID().get();
ref.timestamp = hrt_absolute_time();
ref.esc_address = msg.getSrcNodeID().get();
ref.esc_voltage = msg.voltage;
ref.esc_current = msg.current;
ref.esc_temperature = msg.temperature;
ref.esc_rpm = msg.rpm;
ref.esc_errorcount = msg.error_count;
_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
_esc_status.timestamp = hrt_absolute_time();
_esc_status_pub.publish(_esc_status);
}
}
void
UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.timestamp = hrt_absolute_time();
_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
_esc_status_pub.publish(_esc_status);
}
uint8_t
UavcanEscController::check_escs_status()
{
src/drivers/uavcan/actuators/esc.hpp
+1 -8
View File
@@ -65,7 +65,7 @@ public:
UavcanEscController(uavcan::INode &node);
~UavcanEscController();
~UavcanEscController() = default;
int init();
@@ -84,17 +84,11 @@ private:
*/
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
/**
* ESC status will be published to ORB from this callback (fixed rate).
*/
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
/**
* Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
*/
uint8_t check_escs_status();
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5; ///< 0..31, inclusive, 0 - highest, 31 - lowest
typedef uavcan::MethodBinder<UavcanEscController *,
@@ -116,7 +110,6 @@ private:
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states