From d5c29735d6d52f386302499391a936d0a40f47bf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:11:48 +0000 Subject: [PATCH] AP_DroneCAN: send incomming servo telem data to new `AP_Servo_Telem` lib --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 106 ++++++++++++++++---------- libraries/AP_DroneCAN/AP_DroneCAN.h | 17 ++++- 2 files changed, 78 insertions(+), 45 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index a8c9a531c3..8f07785773 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #if AP_DRONECAN_SERIAL_ENABLED #include "AP_DroneCAN_serial.h" @@ -1379,62 +1380,83 @@ void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const /* handle actuator status message */ +#if AP_SERVO_TELEM_ENABLED void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg) { -#if HAL_LOGGING_ENABLED - // log as CSRV message - AP::logger().Write_ServoStatus(AP_HAL::micros64(), - msg.actuator_id, - msg.position, - msg.force, - msg.speed, - msg.power_rating_pct, - 0, 0, 0, 0, 0, 0); -#endif -} + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } -#if AP_DRONECAN_HIMARK_SERVO_SUPPORT + const AP_Servo_Telem::TelemetryData telem_data { + .measured_position = ToDeg(msg.position), + .force = msg.force, + .speed = msg.speed, + .duty_cycle = msg.power_rating_pct, + .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION | + AP_Servo_Telem::TelemetryData::Types::FORCE | + AP_Servo_Telem::TelemetryData::Types::SPEED | + AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE + }; + + servo_telem->update_telem_data(msg.actuator_id, telem_data); +} +#endif + +#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED /* handle himark ServoInfo message */ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg) { -#if HAL_LOGGING_ENABLED - // log as CSRV message - AP::logger().Write_ServoStatus(AP_HAL::micros64(), - msg.servo_id, - msg.pos_sensor*0.01, - 0, - 0, - 0, - msg.pos_cmd*0.01, - msg.voltage*0.01, - msg.current*0.01, - msg.motor_temp*0.2-40, - msg.pcb_temp*0.2-40, - msg.error_status); -#endif + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } + + const AP_Servo_Telem::TelemetryData telem_data { + .command_position = msg.pos_cmd * 0.01, + .measured_position = msg.pos_sensor * 0.01, + .voltage = msg.voltage * 0.01, + .current = msg.current * 0.01, + .motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100), + .pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100), + .status_flags = msg.error_status, + .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION | + AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | + AP_Servo_Telem::TelemetryData::Types::VOLTAGE | + AP_Servo_Telem::TelemetryData::Types::CURRENT | + AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP | + AP_Servo_Telem::TelemetryData::Types::PCB_TEMP | + AP_Servo_Telem::TelemetryData::Types::STATUS + }; + + servo_telem->update_telem_data(msg.servo_id, telem_data); } #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg) { -#if HAL_LOGGING_ENABLED - AP::logger().WriteStreaming( - "CVOL", - "TimeUS,Id,Pos,Cur,V,Pow,T", - "s#dAv%O", - "F-00000", - "QBfffBh", - AP_HAL::micros64(), - msg.actuator_id, - ToDeg(msg.actual_position), - msg.current * 0.025f, - msg.voltage * 0.2f, - uint8_t(msg.motor_pwm * (100.0/255.0)), - int16_t(msg.motor_temperature) - 50); -#endif + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } + + const AP_Servo_Telem::TelemetryData telem_data { + .measured_position = ToDeg(msg.actual_position), + .voltage = msg.voltage * 0.2, + .current = msg.current * 0.025, + .duty_cycle = msg.motor_pwm * (100.0/255.0), + .motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50)) * 100, + .valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | + AP_Servo_Telem::TelemetryData::Types::VOLTAGE | + AP_Servo_Telem::TelemetryData::Types::CURRENT | + AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE | + AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP + }; + + servo_telem->update_telem_data(msg.actuator_id, telem_data); } #endif diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 9a7ae54c66..3e594bac07 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -36,6 +36,7 @@ #include #include #include +#include #ifndef DRONECAN_SRV_NUMBER #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS @@ -329,8 +330,10 @@ private: Canard::ObjCallback traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report}; Canard::Subscriber traffic_report_listener{traffic_report_cb, _driver_index}; +#if AP_SERVO_TELEM_ENABLED Canard::ObjCallback actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status}; Canard::Subscriber actuator_status_listener{actuator_status_cb, _driver_index}; +#endif Canard::ObjCallback esc_status_cb{this, &AP_DroneCAN::handle_ESC_status}; Canard::Subscriber esc_status_listener{esc_status_cb, _driver_index}; @@ -343,7 +346,11 @@ private: Canard::ObjCallback debug_cb{this, &AP_DroneCAN::handle_debug}; Canard::Subscriber debug_listener{debug_cb, _driver_index}; -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED +#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED + Canard::ObjCallback himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo}; + Canard::Subscriber himark_servo_ServoInfo_cb_listener{himark_servo_ServoInfo_cb, _driver_index}; +#endif +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED Canard::ObjCallback volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz}; Canard::Subscriber volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index}; #endif @@ -401,15 +408,19 @@ private: void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg); #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT -#if AP_DRONECAN_HIMARK_SERVO_SUPPORT +#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg); #endif - + // incoming button handling void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg); void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg); +#if AP_SERVO_TELEM_ENABLED void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg); +#endif +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg); +#endif void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg); #if AP_EXTENDED_ESC_TELEM_ENABLED void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);