diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 30ec6e133b..a4f89765b1 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1705,6 +1705,14 @@ void AP_Periph_FW::esc_telem_update() #endif 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 & 0xFFFFFF) << 8; + } uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout()); diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index c8b1fb6199..e5d6ab7813 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 << 16); 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 && 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; @@ -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 << 8); return; } if (nbytes == telem_packet_size) { 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; };