Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DroneCAN: send incomming servo telem data to new `AP_Servo_Telem` lib

This commit is contained in:
Iampete1 2024-11-16 11:11:48 +00:00 committed by Andrew Tridgell
parent 2863dcfb94
commit d5c29735d6
2 changed files with 78 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -50,6 +50,7 @@
#include <AP_OpenDroneID/AP_OpenDroneID.h> #include <AP_OpenDroneID/AP_OpenDroneID.h>
#include <AP_Mount/AP_Mount_Xacti.h> #include <AP_Mount/AP_Mount_Xacti.h>
#include <string.h> #include <string.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
#if AP_DRONECAN_SERIAL_ENABLED #if AP_DRONECAN_SERIAL_ENABLED
#include "AP_DroneCAN_serial.h" #include "AP_DroneCAN_serial.h"
@ -1379,62 +1380,83 @@ void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const
/* /*
handle actuator status message handle actuator status message
*/ */
#if AP_SERVO_TELEM_ENABLED
void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg) void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg)
{ {
#if HAL_LOGGING_ENABLED AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
// log as CSRV message if (servo_telem == nullptr) {
AP::logger().Write_ServoStatus(AP_HAL::micros64(), return;
msg.actuator_id, }
msg.position,
msg.force,
msg.speed,
msg.power_rating_pct,
0, 0, 0, 0, 0, 0);
#endif
}
#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 handle himark ServoInfo message
*/ */
void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg) void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg)
{ {
#if HAL_LOGGING_ENABLED AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
// log as CSRV message if (servo_telem == nullptr) {
AP::logger().Write_ServoStatus(AP_HAL::micros64(), return;
msg.servo_id, }
msg.pos_sensor*0.01,
0, const AP_Servo_Telem::TelemetryData telem_data {
0, .command_position = msg.pos_cmd * 0.01,
0, .measured_position = msg.pos_sensor * 0.01,
msg.pos_cmd*0.01, .voltage = msg.voltage * 0.01,
msg.voltage*0.01, .current = msg.current * 0.01,
msg.current*0.01, .motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100),
msg.motor_temp*0.2-40, .pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100),
msg.pcb_temp*0.2-40, .status_flags = msg.error_status,
msg.error_status); .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
#endif 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 #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg) void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)
{ {
#if HAL_LOGGING_ENABLED AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
AP::logger().WriteStreaming( if (servo_telem == nullptr) {
"CVOL", return;
"TimeUS,Id,Pos,Cur,V,Pow,T", }
"s#dAv%O",
"F-00000", const AP_Servo_Telem::TelemetryData telem_data {
"QBfffBh", .measured_position = ToDeg(msg.actual_position),
AP_HAL::micros64(), .voltage = msg.voltage * 0.2,
msg.actuator_id, .current = msg.current * 0.025,
ToDeg(msg.actual_position), .duty_cycle = msg.motor_pwm * (100.0/255.0),
msg.current * 0.025f, .motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50)) * 100,
msg.voltage * 0.2f, .valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
uint8_t(msg.motor_pwm * (100.0/255.0)), AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
int16_t(msg.motor_temperature) - 50); AP_Servo_Telem::TelemetryData::Types::CURRENT |
#endif 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 #endif

17
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -36,6 +36,7 @@
#include <dronecan_msgs.h> #include <dronecan_msgs.h>
#include <AP_SerialManager/AP_SerialManager_config.h> #include <AP_SerialManager/AP_SerialManager_config.h>
#include <AP_Relay/AP_Relay_config.h> #include <AP_Relay/AP_Relay_config.h>
#include <AP_Servo_Telem/AP_Servo_Telem_config.h>
#ifndef DRONECAN_SRV_NUMBER #ifndef DRONECAN_SRV_NUMBER
#define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS
@ -329,8 +330,10 @@ private:
Canard::ObjCallback<AP_DroneCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report}; Canard::ObjCallback<AP_DroneCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report};
Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index}; Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index};
#if AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status}; Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status};
Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index}; Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index};
#endif
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status}; Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index}; Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
@ -343,7 +346,11 @@ private:
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug}; Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index}; Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED #if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo};
Canard::Subscriber<com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb_listener{himark_servo_ServoInfo_cb, _driver_index};
#endif
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz}; Canard::ObjCallback<AP_DroneCAN, com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz};
Canard::Subscriber<com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index}; Canard::Subscriber<com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index};
#endif #endif
@ -401,15 +408,19 @@ private:
void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg); void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT #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); void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
#endif #endif
// incoming button handling // incoming button handling
void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg); void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& 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); 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); 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); void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
#if AP_EXTENDED_ESC_TELEM_ENABLED #if AP_EXTENDED_ESC_TELEM_ENABLED
void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg); void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);