Extend BLH ESC telem packet sep timeout

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());

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<<motor_idx);
 
@@ -1600,12 +1602,13 @@ void AP_BLHeli::update_telemetry(void)
         // if we have more than 10 bytes then we don't know which ESC
         // they are from. Throw them all away
         telem_uart->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) {

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)

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

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
  */

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;
 };