aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_UAVCAN: added logging of UAVCAN actuator::Status messages

Browse Source 
this allows for logging of CAN servo status
This commit is contained in:
Andrew Tridgell 2020-01-08 08:35:57 +11:00
parent 30e51c9f64
commit 27937d50ff
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_OpticalFlow/AP_OpticalFlow_HereFlow.h>
#include <AP_ADSB/AP_ADSB.h> #include <AP_ADSB/AP_ADSB.h>
#include "AP_UAVCAN_Server.h" #include "AP_UAVCAN_Server.h"
#include <AP_Logger/AP_Logger.h>
#define LED_DELAY_US 50000 #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); UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS]; 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() : AP_UAVCAN::AP_UAVCAN() :
_node_allocator() _node_allocator()
@ -273,6 +278,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report)); 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; _led_conf.devices_count = 0;
if (enable_filters) { if (enable_filters) {
configureCanAcceptanceFilters(*_node); configureCanAcceptanceFilters(*_node);
@ -712,4 +722,18 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
adsb->handle_adsb_vehicle(vehicle); 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 #endif // HAL_WITH_UAVCAN

2
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -49,6 +49,7 @@
// fwd-declare callback classes // fwd-declare callback classes
class ButtonCb; class ButtonCb;
class TrafficReportCb; class TrafficReportCb;
class ActuatorStatusCb;
/* /*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received, Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@ -226,6 +227,7 @@ private:
// safety button handling // safety button handling
static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb); 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_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_ */ #endif /* AP_UAVCAN_H_ */