aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 27937d50ff
commit c5c921d321
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/actuator/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp> #include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/indication/LightsCommand.hpp> #include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/SingleLightCommand.hpp> #include <uavcan/equipment/indication/SingleLightCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp> #include <uavcan/equipment/indication/BeepCommand.hpp>
@ -124,6 +125,10 @@ static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, T
UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::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]; 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() : AP_UAVCAN::AP_UAVCAN() :
_node_allocator() _node_allocator()
@ -282,6 +287,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
if (actuator_status_listener[driver_index]) { if (actuator_status_listener[driver_index]) {
actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status)); 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; _led_conf.devices_count = 0;
if (enable_filters) { if (enable_filters) {
@ -736,4 +746,21 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
cb.msg->power_rating_pct); 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 #endif // HAL_WITH_UAVCAN

2
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -50,6 +50,7 @@
class ButtonCb; class ButtonCb;
class TrafficReportCb; class TrafficReportCb;
class ActuatorStatusCb; class ActuatorStatusCb;
class ESCStatusCb;
/* /*
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,
@ -228,6 +229,7 @@ private:
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); 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_ */ #endif /* AP_UAVCAN_H_ */