From 118d20943a4f2acc7e47f7aa200784efa878b2db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Aug 2023 17:17:11 +1000 Subject: [PATCH] AP_UAVCAN: support Himark servo protocol --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 75 +++++++++++++++++++++++++++++-- libraries/AP_UAVCAN/AP_UAVCAN.h | 4 ++ 2 files changed, 76 insertions(+), 3 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 41f1ef3ac6..c97b80bf79 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -50,6 +50,8 @@ #endif #include #include +#include +#include #include #include @@ -139,7 +141,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Param: OPTION // @DisplayName: UAVCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), @@ -174,6 +176,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // publisher interfaces static uavcan::Publisher* act_out_array[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* himark_out[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* esc_raw[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS]; @@ -221,6 +224,10 @@ static uavcan::Subscriber *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +// handler himark ServoInfo +UC_REGISTRY_BINDER(HimarkServoInfoCb, com::himark::servo::ServoInfo); +static uavcan::Subscriber *himark_servoinfo_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; + // handler DEBUG UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage); static uavcan::Subscriber *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; @@ -393,6 +400,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + himark_out[driver_index] = new uavcan::Publisher(*_node); + himark_out[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + himark_out[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + esc_raw[driver_index] = new uavcan::Publisher(*_node); esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); @@ -470,6 +481,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status)); } + himark_servoinfo_listener[driver_index] = new uavcan::Subscriber(*_node); + if (himark_servoinfo_listener[driver_index]) { + himark_servoinfo_listener[driver_index]->start(HimarkServoInfoCb(this, &handle_himark_servoinfo)); + } + debug_listener[driver_index] = new uavcan::Subscriber(*_node); if (debug_listener[driver_index]) { debug_listener[driver_index]->start(DebugCb(this, &handle_debug)); @@ -523,7 +539,11 @@ void AP_UAVCAN::loop(void) const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); if (now - _SRV_last_send_us >= servo_period_us) { _SRV_last_send_us = now; - SRV_send_actuator(); + if (option_is_set(Options::USE_HIMARK_SERVO)) { + SRV_send_himark(); + } else { + SRV_send_actuator(); + } sent_servos = true; for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { _SRV_conf[i].servo_pending = false; @@ -624,6 +644,34 @@ void AP_UAVCAN::SRV_send_actuator(void) } while (repeat_send); } +/* + Himark servo output. This uses com.himark.servo.ServoCmd packets + */ +void AP_UAVCAN::SRV_send_himark(void) +{ + WITH_SEMAPHORE(SRV_sem); + + // ServoCmd can hold maximum of 17 commands. First find the highest pending servo < 17 + int8_t highest_to_send = -1; + for (int8_t i = 16; i >= 0; i--) { + if (_SRV_conf[i].servo_pending && ((1U<broadcast(msg); +} + void AP_UAVCAN::SRV_send_esc(void) { static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); @@ -1201,7 +1249,28 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co cb.msg->position, cb.msg->force, cb.msg->speed, - cb.msg->power_rating_pct); + cb.msg->power_rating_pct, + 0, 0, 0, 0, 0, 0); +} + +/* + handle himark ServoInfo message + */ +void AP_UAVCAN::handle_himark_servoinfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HimarkServoInfoCb &cb) +{ + // log as CSRV message + AP::logger().Write_ServoStatus(AP_HAL::native_micros64(), + cb.msg->servo_id, + cb.msg->pos_sensor*0.01, + 0, + 0, + 0, + cb.msg->pos_cmd*0.01, + cb.msg->voltage*0.01, + cb.msg->current*0.01, + cb.msg->motor_temp*0.2-40, + cb.msg->pcb_temp*0.2-40, + cb.msg->error_status); } #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index cf11dcf773..fbac8b9a9f 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -53,6 +53,7 @@ class ButtonCb; class TrafficReportCb; class ActuatorStatusCb; class ActuatorStatusVolzCb; +class HimarkServoInfoCb; class ESCStatusCb; class DebugCb; class ParamGetSetCb; @@ -210,6 +211,7 @@ public: DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), USE_ACTUATOR_PWM = (1U<<4), SEND_GNSS = (1U<<5), + USE_HIMARK_SERVO = (1U<<6), }; // check if a option is set @@ -231,6 +233,7 @@ private: ///// SRV output ///// void SRV_send_actuator(); void SRV_send_esc(); + void SRV_send_himark(); ///// LED ///// void led_out_send(); @@ -366,6 +369,7 @@ private: 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_himark_servoinfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HimarkServoInfoCb &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);