Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: added logging of UAVCAN actuator::Status messages 

this allows for logging of CAN servo status
This commit is contained in:
Andrew Tridgell 2020-01-04 14:52:59 +11:00
parent 2666853100
commit b4966f21d8
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -51,6 +51,7 @@
#include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
#include <AP_ADSB/AP_ADSB.h>
#include "AP_UAVCAN_DNA_Server.h"
#include <AP_Logger/AP_Logger.h>
#define LED_DELAY_US 50000
@ -119,6 +120,10 @@ static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_butto
UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS];
// handler actuator status
UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status);
static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
AP_UAVCAN::AP_UAVCAN() :
_node_allocator()
@ -272,6 +277,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
if (traffic_report_listener[driver_index]) {
traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report));
}
actuator_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb>(*_node);
if (actuator_status_listener[driver_index]) {
actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
}
_led_conf.devices_count = 0;
if (enable_filters) {
@ -712,4 +722,18 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
adsb->handle_adsb_vehicle(vehicle);
}
/*
handle actuator status message
*/
void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb)
{
// log as CSRV message
AP::logger().Write_ServoStatus(AP_HAL::micros64(),
cb.msg->actuator_id,
cb.msg->position,
cb.msg->force,
cb.msg->speed,
cb.msg->power_rating_pct);
}
#endif // HAL_WITH_UAVCAN

2
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -49,6 +49,7 @@
// fwd-declare callback classes
class ButtonCb;
class TrafficReportCb;
class ActuatorStatusCb;
/*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@ -226,6 +227,7 @@ private:
// safety button handling
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);
};
#endif /* AP_UAVCAN_H_ */