Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: add Volz servo feedback handling

Browse Source
This commit is contained in:
Tom Pittenger 2023-02-13 05:49:17 -08:00 committed by Tom Pittenger
parent 0eb6f49ed6
commit bc555c9082
2 changed files with 41 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -82,6 +82,14 @@ extern const AP_HAL::HAL& hal;
#define UAVCAN_STACK_SIZE 4096
#endif
#ifndef AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
#define AP_DRONECAN_VOLZ_FEEDBACK_ENABLED 0
#endif
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
#include <com/volz/servo/ActuatorStatus.hpp>
#endif
#define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0)
// Translation of all messages from UAVCAN structures into AP structures is done
@ -191,6 +199,11 @@ 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[HAL_MAX_CAN_PROTOCOL_DRIVERS];
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
UC_REGISTRY_BINDER(ActuatorStatusVolzCb, com::volz::servo::ActuatorStatus);
static uavcan::Subscriber<com::volz::servo::ActuatorStatus, ActuatorStatusVolzCb> *actuator_status_Volz_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
#endif
// handler ESC status
UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
@ -412,6 +425,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
}
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
actuator_status_Volz_listener[driver_index] = new uavcan::Subscriber<com::volz::servo::ActuatorStatus, ActuatorStatusVolzCb>(*_node);
if (actuator_status_Volz_listener[driver_index]) {
actuator_status_Volz_listener[driver_index]->start(ActuatorStatusVolzCb(this, &handle_actuator_status_Volz));
}
#endif
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));
@ -992,6 +1012,25 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
cb.msg->power_rating_pct);
}
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
void AP_UAVCAN::handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb)
{
AP::logger().WriteStreaming(
"CVOL",
"TimeUS,Id,Pos,Cur,V,Pow,T",
"s#dAv%O",
"F-00000",
"QBfffBh",
AP_HAL::native_micros64(),
cb.msg->actuator_id,
ToDeg(cb.msg->actual_position),
cb.msg->current * 0.025f,
cb.msg->voltage * 0.2f,
cb.msg->motor_pwm * (100.0/255.0),
int16_t(cb.msg->motor_temperature) - 50);
}
#endif
/*
handle ESC status message
*/

2
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -48,6 +48,7 @@
class ButtonCb;
class TrafficReportCb;
class ActuatorStatusCb;
class ActuatorStatusVolzCb;
class ESCStatusCb;
class DebugCb;
class ParamGetSetCb;
@ -346,6 +347,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_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb);
static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
static bool is_esc_data_index_valid(const uint8_t index);
static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);