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

AP_DroneCAN: addition of ESC extended status message 

- Conditional compilation definition : AP_EXTENDED_ESC_TELEM_ENABLE
 - ESCX log structure
 - Update functionalities for ESCX status message
 - ESCX DroneCAN callback
This commit is contained in:
Pradeep CK 2024-07-27 20:44:13 +01:00 committed by Andrew Tridgell
parent 54a6344bd3
commit c7216c05b8
2 changed files with 36 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -1457,6 +1457,34 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc
#endif
}
#if AP_EXTENDED_ESC_TELEM_ENABLED
/*
handle Extended ESC status message
*/
void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
{
const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
const uint8_t esc_index = msg.esc_index + esc_offset;
if (!is_esc_data_index_valid(esc_index)) {
return;
}
TelemetryData telemetryData {
.motor_temp_cdeg = (int16_t)(msg.motor_temperature_degC * 100),
.input_duty = msg.input_pct,
.output_duty = msg.output_pct,
.flags = msg.status_flags,
};
update_telem_data(esc_index, telemetryData,
AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE
| AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
| AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
| AP_ESC_Telem_Backend::TelemetryType::FLAGS);
}
#endif // AP_EXTENDED_ESC_TELEM_ENABLED
bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
if (index > DRONECAN_SRV_NUMBER) {
// printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER);

8
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -326,6 +326,11 @@ private:
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};
#if AP_EXTENDED_ESC_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_StatusExtended> esc_status_extended_cb{this, &AP_DroneCAN::handle_esc_ext_status};
Canard::Subscriber<uavcan_equipment_esc_StatusExtended> esc_status_extended_listener{esc_status_extended_cb, _driver_index};
#endif
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};
@ -387,6 +392,9 @@ private:
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
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);
#endif
static bool is_esc_data_index_valid(const uint8_t index);
void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);