diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 92115d6016..bce7c8b61e 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -21,6 +21,7 @@ #if HAL_WITH_ESC_TELEM #include +#include //#define ESC_TELEM_DEBUG @@ -209,7 +210,7 @@ bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const { if (esc_index >= ESC_TELEM_MAX_ESCS || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)) { + || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { return false; } temp = _telem_data[esc_index].temperature_cdeg; @@ -221,7 +222,7 @@ bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const { if (esc_index >= ESC_TELEM_MAX_ESCS || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE)) { + || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { return false; } temp = _telem_data[esc_index].motor_temp_cdeg; @@ -420,28 +421,23 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem _have_data = true; #if AP_TEMPERATURE_SENSOR_ENABLED - // if it's ever been set externally ignore normal "internal" updates - if (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) { - _temperature_is_external[esc_index] = true; - _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; - } else if ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !_temperature_is_external[esc_index]) { - _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; - } - if (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) { - _motor_temp_is_external[esc_index] = true; - _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; - } else if ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !_motor_temp_is_external[esc_index]) { - _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; - } + // always allow external data. Block "internal" if external has ever its ever been set externally then ignore normal "internal" updates + const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) || + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); + + const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) || + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); #else - if (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) { - _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; - } - if (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) { - _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; - } + const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); + const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE); #endif + if (has_temperature) { + _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; + } + if (has_motor_temperature) { + _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; + } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) { _telem_data[esc_index].voltage = new_data.voltage; } diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 05c225af77..12a4c5eb4b 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -2,7 +2,6 @@ #include #include -#include #include #include "AP_ESC_Telem_Backend.h" @@ -115,11 +114,6 @@ private: // telemetry data volatile AP_ESC_Telem_Backend::TelemetryData _telem_data[ESC_TELEM_MAX_ESCS]; -#if AP_TEMPERATURE_SENSOR_ENABLED - bool _temperature_is_external[ESC_TELEM_MAX_ESCS]; - bool _motor_temp_is_external[ESC_TELEM_MAX_ESCS]; -#endif - uint32_t _last_telem_log_ms[ESC_TELEM_MAX_ESCS]; uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS]; uint8_t next_idx;