From a73012b471995f3d155d744a6a3388c2a19ef908 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 12 Nov 2024 10:56:11 +1100 Subject: [PATCH 1/6] AP_BLHeli: increase packet timeout to 5ms Workaround for APD's mid-packet gaps in their new firmware --- libraries/AP_BLHeli/AP_BLHeli.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index c8b1fb6199..3d1901a495 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1605,7 +1605,7 @@ void AP_BLHeli::update_telemetry(void) if (nbytes > 0 && nbytes < telem_packet_size && (last_telem_byte_read_us == 0 || - now - last_telem_byte_read_us < 1000)) { + now - last_telem_byte_read_us < 5000)) { // wait a bit longer, we don't have enough bytes yet if (last_telem_byte_read_us == 0) { last_telem_byte_read_us = now; From f6ed5ba5540a15ccad2dd83e98850fa032b4e015 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 21 Oct 2024 09:32:22 +1100 Subject: [PATCH 2/6] AP_ESC_Telem: add count and error_count handlers --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 32 +++++++++++++++++++ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 9 ++++++ .../AP_ESC_Telem/AP_ESC_Telem_Backend.cpp | 6 ++++ libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 6 ++++ 4 files changed, 53 insertions(+) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 30239127fb..3163b6c177 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -326,6 +326,26 @@ bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah return true; } +// get an individual ESC's packet count if available, returns true on success +bool AP_ESC_Telem::get_count(uint8_t esc_index, uint16_t& count) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + count = _telem_data[esc_index].count; + return true; +} + +// get an individual ESC's error count if available, returns true on success +bool AP_ESC_Telem::get_error_count(uint8_t esc_index, uint32_t& error_count) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + error_count = _telem_data[esc_index].error_count; + return true; +} + // 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 { @@ -562,6 +582,9 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { telemdata.usage_s = new_data.usage_s; } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::ERROR_COUNT) { + telemdata.error_count = new_data.error_count; + } #if AP_EXTENDED_ESC_TELEM_ENABLED if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) { @@ -583,6 +606,15 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem telemdata.last_update_ms = AP_HAL::millis(); } +// callback to increment the error count in the frontend, should be called by the driver when an error occurs +void AP_ESC_Telem::increment_error_count(const uint8_t esc_index, const uint32_t amount) +{ + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return; + } + _telem_data[esc_index].error_count += amount; +} + // record an update to the RPM together with timestamp, this allows the notch values to be slewed // this should be called by backends when new telemetry values are available void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, const float error_rate) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 85ec050952..ed125c5bb9 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -67,6 +67,12 @@ 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; + // get an individual ESC's packet count if available, returns true on success + bool get_count(uint8_t esc_index, uint16_t& count) const; + + // get an individual ESC's error count if available, returns true on success + bool get_error_count(uint8_t esc_index, uint32_t& error_count) 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; @@ -122,6 +128,9 @@ class AP_ESC_Telem { // callback to update the data in the frontend, should be called by the driver when new data is available void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask); + // callback to increment the error count in the frontend, should be called by the driver when an error occurs + void increment_error_count(const uint8_t esc_index, const uint32_t amount); + #if AP_SCRIPTING_ENABLED /* set RPM scale factor from script diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index a63c0191c5..31909fb8a9 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -45,6 +45,12 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele _frontend->update_telem_data(esc_index, new_data, data_present_mask); } +// callback to increment the error count in the frontend, should be called by the driver when an error occurs +// XXX: we also supply the amount to increment by, which we are using as a hack to encode different error types +void AP_ESC_Telem_Backend::increment_error_count(const uint8_t esc_index, const uint32_t amount) { + _frontend->increment_error_count(esc_index, amount); +} + /* return true if the data is stale */ diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index 9f8da7a7f1..ba98a10f0d 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -19,6 +19,7 @@ class AP_ESC_Telem_Backend { uint32_t last_update_ms; // last update time in milliseconds, determines whether active uint16_t types; // telemetry types present uint16_t count; // number of times updated + uint32_t error_count; // number of errors #if AP_EXTENDED_ESC_TELEM_ENABLED uint8_t input_duty; // input duty cycle uint8_t output_duty; // output duty cycle @@ -51,6 +52,7 @@ class AP_ESC_Telem_Backend { USAGE = 1 << 5, TEMPERATURE_EXTERNAL = 1 << 6, MOTOR_TEMPERATURE_EXTERNAL = 1 << 7, + ERROR_COUNT = 1 << 8, #if AP_EXTENDED_ESC_TELEM_ENABLED INPUT_DUTY = 1 << 10, OUTPUT_DUTY = 1 << 11, @@ -72,6 +74,10 @@ class AP_ESC_Telem_Backend { // callback to update the data in the frontend, should be called by the driver when new data is available void update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask); + // callback to increment the error count in the frontend, should be called by the driver when an error occurs + // XXX: we also supply the amount to increment by, which we are using as a hack to encode different error types + void increment_error_count(const uint8_t esc_index, const uint32_t amount); + private: AP_ESC_Telem* _frontend; }; From f45ef65228b1fb51405835f987053525a29faaf4 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 12 Nov 2024 10:59:01 +1100 Subject: [PATCH 3/6] AP_BLHeli: increment error count on failed reads --- libraries/AP_BLHeli/AP_BLHeli.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 3d1901a495..e94febf0c2 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1454,8 +1454,10 @@ void AP_BLHeli::read_telemetry_packet(void) { #if HAL_WITH_ESC_TELEM uint8_t buf[13]; + const uint8_t motor_idx = motor_map[last_telem_esc]; if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) { // short read, we should have all bytes ready when this function is called + // THIS LINE IS NOT ACTUALLY REACHABLE, SO WE DON'T LOG THIS (the caller already checks) return; } @@ -1468,11 +1470,11 @@ void AP_BLHeli::read_telemetry_packet(void) if (buf[telem_packet_size-1] != crc) { // bad crc debug("Bad CRC on %u", last_telem_esc); + increment_error_count(motor_idx - chan_offset, 1); return; } // record the previous rpm so that we can slew to the new one uint16_t new_rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles; - const uint8_t motor_idx = motor_map[last_telem_esc]; // we have received valid data, mark the ESC as now active hal.rcout->set_active_escs_mask(1<discard_input(); + increment_error_count(motor_map[last_telem_esc] - chan_offset, 1); return; } if (nbytes > 0 && @@ -1615,6 +1618,7 @@ void AP_BLHeli::update_telemetry(void) if (nbytes > 0 && nbytes < telem_packet_size) { // we've waited long enough, discard bytes if we don't have 10 yet telem_uart->discard_input(); + increment_error_count(motor_map[last_telem_esc] - chan_offset, 1); return; } if (nbytes == telem_packet_size) { From cacfe159004ae3d9e8157d76df66959793a7b0de Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 12 Nov 2024 11:03:16 +1100 Subject: [PATCH 4/6] AP_Periph: send ESC error count --- Tools/AP_Periph/can.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 30ec6e133b..822cd716fa 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1705,6 +1705,10 @@ void AP_Periph_FW::esc_telem_update() #endif pkt.error_count = 0; + uint32_t error_count; + if (esc_telem.get_error_count(i, error_count)) { + pkt.error_count = error_count; + } uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); From 51f1cf00f1a1a8d764d121c6eb1f099711fb2290 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 12 Nov 2024 11:01:02 +1100 Subject: [PATCH 5/6] HACK: AP_BLHeli: encode different error types in count Bits: 0-7: long packet 8-15: short packet 16-24: CRC failure --- libraries/AP_BLHeli/AP_BLHeli.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index e94febf0c2..e5d6ab7813 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1470,7 +1470,7 @@ void AP_BLHeli::read_telemetry_packet(void) if (buf[telem_packet_size-1] != crc) { // bad crc debug("Bad CRC on %u", last_telem_esc); - increment_error_count(motor_idx - chan_offset, 1); + increment_error_count(motor_idx - chan_offset, 1 << 16); return; } // record the previous rpm so that we can slew to the new one @@ -1618,7 +1618,7 @@ void AP_BLHeli::update_telemetry(void) if (nbytes > 0 && nbytes < telem_packet_size) { // we've waited long enough, discard bytes if we don't have 10 yet telem_uart->discard_input(); - increment_error_count(motor_map[last_telem_esc] - chan_offset, 1); + increment_error_count(motor_map[last_telem_esc] - chan_offset, 1 << 8); return; } if (nbytes == telem_packet_size) { From 053511c8f19ddd3a1e67c73b3a35fd61d9a5794a Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 12 Nov 2024 11:05:05 +1100 Subject: [PATCH 6/6] HACK: AP_Periph: embed good count in error_count This sends both the count and the error count within the error_count field of the ESC telemetry message. count is held in the lower 8 bits and error count in the upper 24 bits. --- Tools/AP_Periph/can.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 822cd716fa..a4f89765b1 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1706,8 +1706,12 @@ void AP_Periph_FW::esc_telem_update() pkt.error_count = 0; uint32_t error_count; + uint16_t count; + if (esc_telem.get_count(i, count)) { + pkt.error_count = count & 0xFF; + } if (esc_telem.get_error_count(i, error_count)) { - pkt.error_count = error_count; + pkt.error_count += (error_count & 0xFFFFFF) << 8; } uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {};