From 9f531dd7d576f3acfa164f077cbe6c512577d4a6 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 7 Aug 2024 01:14:41 +0100 Subject: [PATCH 1/6] AP_ESC_Telem: add getters for extended status values --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 54 +++++++++++++++++++++++++ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 14 +++++++ 2 files changed, 68 insertions(+) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 96f7de74c59db..29745e57f1f8b 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 e862f6bcec3e1..cf4a5a9cc2d57 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -69,6 +69,20 @@ class AP_ESC_Telem { // 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); }; From 859d53cdfa15945b4b9718b203d49782b3dd0ea6 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 7 Aug 2024 01:15:16 +0100 Subject: [PATCH 2/6] AP_ESC_Telem: SITL: fill in extended status values --- libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp index 25f84fb285c69..cf435a2051c79 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp @@ -71,13 +71,28 @@ void AP_ESC_Telem_SITL::update() .voltage = 16.8f, .current = 0.8f, .consumption_mah = 1.0f, + .motor_temp_cdeg = 3500, +#if AP_EXTENDED_ESC_TELEM_ENABLED + .input_duty = 1, + .output_duty = 2, + .flags = 3, + .power_percentage = 4, +#endif }; update_telem_data(motor, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION - | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); + | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE + | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE +#if AP_EXTENDED_ESC_TELEM_ENABLED + | AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE + | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY + | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY + | AP_ESC_Telem_Backend::TelemetryType::FLAGS +#endif + ); } #endif } From 876c8101af24ce82161f69d720db4ab6056521a6 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 7 Aug 2024 01:16:10 +0100 Subject: [PATCH 3/6] AP_Periph: add support for sending extended ESC status --- Tools/AP_Periph/AP_Periph.h | 6 ++ Tools/AP_Periph/Parameters.cpp | 11 ++++ Tools/AP_Periph/Parameters.h | 4 ++ Tools/AP_Periph/can.cpp | 101 +++++++++++++++++++++++++++++---- Tools/AP_Periph/rc_out.cpp | 3 + 5 files changed, 115 insertions(+), 10 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index b6db8f6a11c2c..d5f73445890d1 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -334,9 +334,15 @@ class AP_Periph_FW { #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM AP_ESC_Telem esc_telem; + uint8_t get_motor_number(const uint8_t esc_number) const; uint32_t last_esc_telem_update_ms; void esc_telem_update(); uint32_t esc_telem_update_period_ms; +#if AP_EXTENDED_ESC_TELEM_ENABLED + void esc_telem_extended_update(const uint32_t &now_ms); + uint32_t last_esc_telem_extended_update; + uint8_t last_esc_telem_extended_sent_id; +#endif #endif SRV_Channels servo_channels; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 79eed58f74ca3..2908c0fddfeb0 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -712,6 +712,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(rpm_msg_rate, "RPM_MSG_RATE", 0), #endif +#if AP_EXTENDED_ESC_TELEM_ENABLED && HAL_WITH_ESC_TELEM + // @Param: ESC_EXT_TLM_RATE + // @DisplayName: ESC Extended telemetry message rate + // @Description: This is the rate at which extended ESC Telemetry will be sent across the CAN bus for each ESC + // @Units: Hz + // @Range: 0 50 + // @Increment: 1 + // @User: Advanced + GSCALAR(esc_extended_telem_rate, "ESC_EXT_TLM_RATE", AP_PERIPH_ESC_TELEM_RATE_DEFAULT / 10), +#endif + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 03730a5ff4d0a..6479b511687b7 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -96,6 +96,7 @@ class Parameters { k_param_options, k_param_rpm_msg_rate, k_param_esc_rate, + k_param_esc_extended_telem_rate, }; AP_Int16 format_version; @@ -183,6 +184,9 @@ class Parameters { #endif #if HAL_WITH_ESC_TELEM AP_Int32 esc_telem_rate; +#if AP_EXTENDED_ESC_TELEM_ENABLED + AP_Int16 esc_extended_telem_rate; +#endif #endif #endif diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 556791c311af0..036cbed6dffe3 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1670,6 +1670,20 @@ void AP_Periph_FW::can_start() #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM +// try to map the ESC number to a motor number. This is needed +// for when we have multiple CAN nodes, one for each ESC +uint8_t AP_Periph_FW::get_motor_number(const uint8_t esc_number) const +{ + const auto *channel = SRV_Channels::srv_channel(esc_number); + // try to map the ESC number to a motor number. This is needed + // for when we have multiple CAN nodes, one for each ESC + if (channel == nullptr) { + return esc_number; + } + const int8_t motor_num = channel->get_motor_num(); + return (motor_num == -1) ? esc_number : motor_num; +} + /* send ESC status packets based on AP_ESC_Telem */ @@ -1681,15 +1695,8 @@ void AP_Periph_FW::esc_telem_update() mask &= ~(1U<get_motor_num(); - pkt.esc_index = motor_num==-1? i:motor_num; - } + pkt.esc_index = get_motor_number(i); + if (!esc_telem.get_voltage(i, pkt.voltage)) { pkt.voltage = nan; } @@ -1708,7 +1715,14 @@ void AP_Periph_FW::esc_telem_update() if (esc_telem.get_raw_rpm(i, rpm)) { pkt.rpm = rpm; } - pkt.power_rating_pct = 0; + +#if AP_EXTENDED_ESC_TELEM_ENABLED + uint8_t power_rating_pct; + if (esc_telem.get_power_percentage(i, power_rating_pct)) { + pkt.power_rating_pct = power_rating_pct; + } +#endif + pkt.error_count = 0; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE]; @@ -1722,6 +1736,73 @@ void AP_Periph_FW::esc_telem_update() } #endif // HAL_WITH_ESC_TELEM +#if AP_EXTENDED_ESC_TELEM_ENABLED +void AP_Periph_FW::esc_telem_extended_update(const uint32_t &now_ms) +{ + if (g.esc_extended_telem_rate <= 0) { + // Not configured to send + return; + } + + uint32_t mask = esc_telem.get_active_esc_mask(); + if (mask == 0) { + // No ESCs to report + return; + } + + // ESCs are sent in turn to minimise used bandwidth, to make the rate param match the status message we multiply + // the period such that the param gives the per-esc rate + const uint32_t update_period_ms = 1000 / constrain_int32(g.esc_extended_telem_rate.get() * __builtin_popcount(mask), 1, 1000); + if (now_ms - last_esc_telem_extended_update < update_period_ms) { + // Too soon! + return; + } + last_esc_telem_extended_update = now_ms; + + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + // Send each ESC in turn + const uint8_t index = (last_esc_telem_extended_sent_id + 1 + i) % ESC_TELEM_MAX_ESCS; + + if ((mask & (1U << index)) == 0) { + // Not enabled + continue; + } + + uavcan_equipment_esc_StatusExtended pkt {}; + + // Only send if we have data + bool have_data = false; + + int16_t motor_temp_cdeg; + if (esc_telem.get_motor_temperature(index, motor_temp_cdeg)) { + // Convert from centi-degrees to degrees + pkt.motor_temperature_degC = motor_temp_cdeg * 0.01; + have_data = true; + } + + have_data |= esc_telem.get_input_duty(index, pkt.input_pct); + have_data |= esc_telem.get_output_duty(index, pkt.output_pct); + have_data |= esc_telem.get_flags(index, pkt.status_flags); + + if (have_data) { + pkt.esc_index = get_motor_number(index); + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_MAX_SIZE]; + const uint16_t total_size = uavcan_equipment_esc_StatusExtended_encode(&pkt, buffer, !canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + + last_esc_telem_extended_sent_id = index; + break; + } +} +#endif + #ifdef HAL_PERIPH_ENABLE_ESC_APD void AP_Periph_FW::apd_esc_telem_update() { diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 4b9f5e97d664a..2dbfbe07f5149 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -165,6 +165,9 @@ void AP_Periph_FW::rcout_update() last_esc_telem_update_ms = now_ms; esc_telem_update(); } +#if AP_EXTENDED_ESC_TELEM_ENABLED + esc_telem_extended_update(now_ms); +#endif #endif } From 1e2631b8ddeb540cabdb16423a0655be382e097e Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 7 Aug 2024 01:16:44 +0100 Subject: [PATCH 4/6] Tools: ardupilotwaf: boards: enable extended esc telem on sitl_periph_universal --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 65452f8d08b74..3d8ac2f20856c 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -941,6 +941,7 @@ def configure_env(self, cfg, env): AP_BATTERY_ESC_ENABLED = 1, HAL_PWM_COUNT = 32, HAL_WITH_ESC_TELEM = 1, + AP_EXTENDED_ESC_TELEM_ENABLED = 1, AP_TERRAIN_AVAILABLE = 1, ) From a43ed4494d3fa0bc93547a9750b0b0b30dfe3a79 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 7 Aug 2024 01:37:49 +0100 Subject: [PATCH 5/6] AP_ESC_Telem: add `telemdata.valid` method to simplify code --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 30 +++++++------------ .../AP_ESC_Telem/AP_ESC_Telem_Backend.cpp | 15 ++++++++-- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 5 +++- 3 files changed, 26 insertions(+), 24 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 29745e57f1f8b..221f6224504e6 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -240,8 +240,7 @@ bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) 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::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)) { return false; } temp = telemdata.temperature_cdeg; @@ -253,8 +252,7 @@ bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) 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::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)) { return false; } temp = telemdata.motor_temp_cdeg; @@ -282,8 +280,7 @@ bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) 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::CURRENT)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { return false; } amps = telemdata.current; @@ -295,8 +292,7 @@ bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) 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::VOLTAGE)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { return false; } volts = telemdata.voltage; @@ -308,8 +304,7 @@ bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah { 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::CONSUMPTION)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { return false; } consumption_mah = telemdata.consumption_mah; @@ -321,8 +316,7 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) 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::USAGE)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::USAGE)) { return false; } usage_s = telemdata.usage_s; @@ -335,8 +329,7 @@ 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)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { return false; } input_duty = telemdata.input_duty; @@ -348,8 +341,7 @@ bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) cons { 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)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { return false; } output_duty = telemdata.output_duty; @@ -361,8 +353,7 @@ 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)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::FLAGS)) { return false; } flags = telemdata.flags; @@ -374,8 +365,7 @@ bool AP_ESC_Telem::get_power_percentage(uint8_t esc_index, uint8_t& power_percen { 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)) { + || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE)) { return false; } power_percentage = telemdata.power_percentage; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index 46f5aa6145996..a63c0191c5c85 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -50,10 +50,19 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele */ bool AP_ESC_Telem_Backend::TelemetryData::stale(uint32_t now_ms) const volatile { - if (now_ms == 0) { - now_ms = AP_HAL::millis(); - } return last_update_ms == 0 || now_ms - last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS; } +/* + return true if the requested types of data are available and not stale + */ +bool AP_ESC_Telem_Backend::TelemetryData::valid(const uint16_t type_mask) const volatile +{ + if ((types & type_mask) == 0) { + // Requested type not available + return false; + } + return !stale(AP_HAL::millis()); +} + #endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index 0764cb79f0c9b..f127c0d71ab92 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -31,7 +31,10 @@ class AP_ESC_Telem_Backend { #endif // AP_EXTENDED_ESC_TELEM_ENABLED // return true if the data is stale - bool stale(uint32_t now_ms=0) const volatile; + bool stale(uint32_t now_ms) const volatile; + + // return true if the requested type of data is available and not stale + bool valid(const uint16_t type_mask) const volatile; }; struct RpmData { From 4cb080a329d3e0935dba17b5d3a5344739ee0297 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 13 Aug 2024 02:56:31 +0100 Subject: [PATCH 6/6] AP_ESC_Telem: getters: check index is in range first --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 70 ++++++++++++++++++------- 1 file changed, 50 insertions(+), 20 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 221f6224504e6..02c2f8f205ed0 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -238,9 +238,12 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const // get an individual ESC's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)) { return false; } temp = telemdata.temperature_cdeg; @@ -250,9 +253,12 @@ bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const // get an individual motor's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)) { return false; } temp = telemdata.motor_temp_cdeg; @@ -278,9 +284,12 @@ bool AP_ESC_Telem::get_highest_temperature(int16_t& temp) const // get an individual ESC's current in Ampere if available, returns true on success bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { return false; } amps = telemdata.current; @@ -290,9 +299,12 @@ bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const // get an individual ESC's voltage in Volt if available, returns true on success bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { return false; } volts = telemdata.voltage; @@ -302,9 +314,12 @@ bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const // get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { return false; } consumption_mah = telemdata.consumption_mah; @@ -314,9 +329,12 @@ bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah // get an individual ESC's usage time in seconds if available, returns true on success bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::USAGE)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::USAGE)) { return false; } usage_s = telemdata.usage_s; @@ -327,9 +345,12 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const // 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 { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { return false; } input_duty = telemdata.input_duty; @@ -339,9 +360,12 @@ bool AP_ESC_Telem::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 AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { return false; } output_duty = telemdata.output_duty; @@ -351,9 +375,12 @@ bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) cons // 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 { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::FLAGS)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::FLAGS)) { return false; } flags = telemdata.flags; @@ -363,9 +390,12 @@ bool AP_ESC_Telem::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 AP_ESC_Telem::get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; - if (esc_index >= ESC_TELEM_MAX_ESCS - || !telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE)) { + if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE)) { return false; } power_percentage = telemdata.power_percentage;