diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 96f7de74c5..29745e57f1 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -329,6 +329,60 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const return true; } +#if AP_EXTENDED_ESC_TELEM_ENABLED +// get an individual ESC's input duty cycle if available, returns true on success +bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { + return false; + } + input_duty = telemdata.input_duty; + return true; +} + +// get an individual ESC's output duty cycle if available, returns true on success +bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { + return false; + } + output_duty = telemdata.output_duty; + return true; +} + +// get an individual ESC's status flags if available, returns true on success +bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::FLAGS)) { + return false; + } + flags = telemdata.flags; + return true; +} + +// get an individual ESC's percentage of output power if available, returns true on success +bool AP_ESC_Telem::get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const +{ + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; + if (esc_index >= ESC_TELEM_MAX_ESCS + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE)) { + return false; + } + power_percentage = telemdata.power_percentage; + return true; +} +#endif // AP_EXTENDED_ESC_TELEM_ENABLED + // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) { diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index e862f6bcec..cf4a5a9cc2 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -69,6 +69,20 @@ public: // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const; +#if AP_EXTENDED_ESC_TELEM_ENABLED + // get an individual ESC's input duty cycle if available, returns true on success + bool get_input_duty(uint8_t esc_index, uint8_t& input_duty) const; + + // get an individual ESC's output duty cycle if available, returns true on success + bool get_output_duty(uint8_t esc_index, uint8_t& output_duty) const; + + // get an individual ESC's status flags if available, returns true on success + bool get_flags(uint8_t esc_index, uint32_t& flags) const; + + // get an individual ESC's percentage of output power if available, returns true on success + bool get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const; +#endif + // return the average motor frequency in Hz for dynamic filtering float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };