Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: added logging of CAN ESC status

Browse Source
This commit is contained in:
Andrew Tridgell 2020-01-04 15:57:01 +11:00
parent 0dea4b2fa5
commit 21cc58e513
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -32,6 +32,7 @@
#include <uavcan/equipment/actuator/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/SingleLightCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp>
@ -125,6 +126,10 @@ static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, T
UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status);
static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
// handler ESC status
UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
AP_UAVCAN::AP_UAVCAN() :
_node_allocator()
@ -283,6 +288,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
if (actuator_status_listener[driver_index]) {
actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
}
esc_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb>(*_node);
if (esc_status_listener[driver_index]) {
esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
}
_led_conf.devices_count = 0;
if (enable_filters) {
@ -737,4 +747,21 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
cb.msg->power_rating_pct);
}
/*
handle ESC status message
*/
void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb)
{
// log as CESC message
AP::logger().Write_ESCStatus(AP_HAL::micros64(),
cb.msg->esc_index,
cb.msg->error_count,
cb.msg->voltage,
cb.msg->current,
cb.msg->temperature - C_TO_KELVIN,
cb.msg->rpm,
cb.msg->power_rating_pct);
}
#endif // HAL_WITH_UAVCAN

2
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -50,6 +50,7 @@
class ButtonCb;
class TrafficReportCb;
class ActuatorStatusCb;
class ESCStatusCb;
/*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@ -228,6 +229,7 @@ private:
static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
};
#endif /* AP_UAVCAN_H_ */