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

uavcan esc: added timeout checks to escs.

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2019-05-31 13:15:20 +02:00 committed by Beat Küng
parent 03637fa6f1
commit d06c679252
2 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/drivers/uavcan/actuators/esc.cpp
View File

@ -208,7 +208,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
auto &ref = _esc_status.esc[msg.esc_index];
ref.esc_address = msg.getSrcNodeID().get();
ref.timestamp = hrt_absolute_time();
ref.esc_voltage = msg.voltage;
ref.esc_current = msg.current;
ref.esc_temperature = msg.temperature;
@ -222,6 +222,7 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
if (_esc_status_pub != nullptr) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
@ -230,3 +231,20 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
}
uint8_t UavcanEscController::check_escs_status()
{
int esc_status_flags = 255;
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
if (_esc_status.esc[index].timestamp == 0) {
esc_status_flags &= ~(1 << index);
} else if (hrt_elapsed_time(&_esc_status.esc[index].timestamp) > 800000.0f) {
esc_status_flags &= ~(1 << index);
}
}
return esc_status_flags;
}

7
src/drivers/uavcan/actuators/esc.hpp
View File

@ -50,6 +50,7 @@
#include <perf/perf_counter.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/actuator_outputs.h>
#include <drivers/drv_hrt.h>
class UavcanEscController
@ -78,6 +79,11 @@ private:
*/
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 MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
@ -95,6 +101,7 @@ private:
esc_status_s _esc_status = {};
orb_advert_t _esc_status_pub = nullptr;
orb_advert_t _actuator_outputs_pub = nullptr;
hrt_abstime _last_received_msg[esc_status_s::CONNECTED_ESC_MAX] {0};
/*
* libuavcan related things