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_Mount/AP_Mount_Xacti.h>
#include <string.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
#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

17
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -36,6 +36,7 @@
#include <dronecan_msgs.h>
#include <AP_SerialManager/AP_SerialManager_config.h>
#include <AP_Relay/AP_Relay_config.h>
#include <AP_Servo_Telem/AP_Servo_Telem_config.h>
#ifndef DRONECAN_SRV_NUMBER
#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::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::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::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::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::Subscriber<com_volz_servo_ActuatorStatus> 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);