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

AP_ESC_Telem: 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 c7216c05b8
commit 0e53eb74df
3 changed files with 65 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -492,6 +492,18 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
telemdata.usage_s = new_data.usage_s;
}
#if AP_EXTENDED_ESC_TELEM_ENABLED
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) {
_telem_data[esc_index].input_duty = new_data.input_duty;
}
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY) {
_telem_data[esc_index].output_duty = new_data.output_duty;
}
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) {
_telem_data[esc_index].flags = new_data.flags;
}
#endif //AP_EXTENDED_ESC_TELEM_ENABLED
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) {
telemdata.edt2_status = merge_edt2_status(telemdata.edt2_status, new_data.edt2_status);
@ -603,6 +615,10 @@ void AP_ESC_Telem::update()
if (telemdata.last_update_ms != _last_telem_log_ms[i]
|| rpmdata.last_update_us != _last_rpm_log_us[i]) {
// Update last log timestamps
_last_telem_log_ms[i] = telemdata.last_update_ms;
_last_rpm_log_us[i] = rpmdata.last_update_us;
float rpm = AP::logger().quiet_nanf();
get_rpm(i, rpm);
float raw_rpm = AP::logger().quiet_nanf();
@ -631,8 +647,37 @@ void AP_ESC_Telem::update()
error_rate : rpmdata.error_rate
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
_last_telem_log_ms[i] = telemdata.last_update_ms;
_last_rpm_log_us[i] = rpmdata.last_update_us;
#if AP_EXTENDED_ESC_TELEM_ENABLED
// Write ESC extended status messages
// id: starts from 0
// input duty: duty cycle input to the ESC in percent
// output duty: duty cycle output to the motor in percent
// status flags: manufacurer-specific status flags
const bool has_ext_data = telemdata.types &
(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY |
AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY |
AP_ESC_Telem_Backend::TelemetryType::FLAGS);
if (has_ext_data) {
// @LoggerMessage: ESCX
// @Description: ESC extended telemetry data
// @Field: TimeUS: Time since system startup
// @Field: Instance: starts from 0
// @Field: inpct: input duty cycle in percent
// @Field: outpct: output duty cycle in percent
// @Field: flags: manufacturer-specific status flags
AP::logger().WriteStreaming("ESCX",
"TimeUS,Instance,inpct,outpct,flags",
"s" "#" "%" "%" "-",
"F" "-" "-" "-" "-",
"Q" "B" "B" "B" "I",
AP_HAL::micros64(),
i,
telemdata.input_duty,
telemdata.output_duty,
telemdata.flags);
}
#endif //AP_EXTENDED_ESC_TELEM_ENABLED
}
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED

10
libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
View File

@ -23,6 +23,11 @@ public:
uint16_t edt2_status; // status reported by Extended DShot Telemetry v2
uint16_t edt2_stress; // stress reported in dedicated messages by Extended DShot Telemetry v2
#endif
#if AP_EXTENDED_ESC_TELEM_ENABLED
uint8_t input_duty; // input duty cycle
uint8_t output_duty; // output duty cycle
uint32_t flags; // Status flags
#endif // AP_EXTENDED_ESC_TELEM_ENABLED
// return true if the data is stale
bool stale(uint32_t now_ms=0) const volatile;
@ -50,6 +55,11 @@ public:
EDT2_STATUS = 1 << 8,
EDT2_STRESS = 1 << 9,
#endif
#if AP_EXTENDED_ESC_TELEM_ENABLED
INPUT_DUTY = 1 << 10,
OUTPUT_DUTY = 1 << 11,
FLAGS = 1 << 12
#endif // AP_EXTENDED_ESC_TELEM_ENABLED
};

8
libraries/AP_ESC_Telem/AP_ESC_Telem_config.h
View File

@ -6,3 +6,11 @@
#ifndef HAL_WITH_ESC_TELEM
#define HAL_WITH_ESC_TELEM ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS)))
#endif
#ifndef AP_EXTENDED_ESC_TELEM_ENABLED
#define AP_EXTENDED_ESC_TELEM_ENABLED HAL_ENABLE_DRONECAN_DRIVERS
#endif
#if AP_EXTENDED_ESC_TELEM_ENABLED && !HAL_WITH_ESC_TELEM
#error "AP_EXTENDED_ESC_TELEM_ENABLED requires HAL_WITH_ESC_TELEM"
#endif