dshot: fix clearing out esc status

platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c

@@ -440,7 +440,7 @@ void up_dshot_trigger(void)
 	}
 
 	// Calc data now since we're not event driven
-	if(bdshot_recv_mask != 0x0) {
+	if (bdshot_recv_mask != 0x0) {
 		up_bdshot_erpm();
 		bdshot_recv_mask = 0x0;
 	}

platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@@ -152,6 +152,12 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
 	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
+{
+	// Not implemented
+	return -1;
+}
+
 void up_bdshot_status(void)
 {
 }

src/drivers/dshot/DShot.cpp

@@ -253,10 +253,6 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
 		esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
 
 		ret = 1; // Indicate we wrapped, so we publish data
-
-		// reset esc data (in case a motor times out, so we won't send stale data)
-		memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
-		esc_status.esc_online_flags = 0;
 	}
 
 	_telemetry->last_telemetry_index = telemetry_index;
@@ -264,6 +260,24 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
 	return ret;
 }
 
+void DShot::publish_esc_status(void)
+{
+	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+
+	// clear data of the esc that are offline
+	for (uint8_t channel = 0; (channel < _telemetry->last_telemetry_index); channel++) {
+		if ((esc_status.esc_online_flags & (1 << channel)) == 0) {
+			memset(&esc_status.esc[channel], 0, sizeof(struct esc_report_s));
+		}
+	}
+
+	// ESC telem wrap around or bdshot update
+	_telemetry->esc_status_pub.update();
+
+	// reset esc online flags
+	esc_status.esc_online_flags = 0;
+}
+
 int DShot::handle_new_bdshot_erpm(void)
 {
 	int num_erpms = 0;
@@ -279,9 +293,11 @@ int DShot::handle_new_bdshot_erpm(void)
 	for (channel = 0; channel < 8; channel++) {
 		if (up_bdshot_get_erpm(channel, &erpm) == 0) {
 			num_erpms++;
+			esc_status.esc_online_flags |= 1 << channel;
 			esc_status.esc[channel].timestamp = hrt_absolute_time();
 			esc_status.esc[channel].esc_rpm = (erpm * 100) /
 							  (_param_mot_pole_count.get() / 2);
+			esc_status.esc[channel].actuator_function = _telemetry->actuator_functions[channel];
 		}
 
 	}
@@ -521,7 +537,7 @@ void DShot::Run()
 
 		if (need_to_publish > 0) {
 			// ESC telem wrap around or bdshot update
-			_telemetry->esc_status_pub.update();
+			publish_esc_status();
 		}
 	}
 

src/drivers/dshot/DShot.h

@@ -133,6 +133,8 @@ private:
 
 	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
 
+	void publish_esc_status(void);
+
 	int handle_new_bdshot_erpm(void);
 
 	int request_esc_info();

src/drivers/dshot/DShotTelemetry.cpp

@@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
 				  _latest_data.erpm);
 			++_num_successful_responses;
 			successful_decoding = true;
+
+		} else {
+			++_num_checksum_errors;
 		}
 
 		return true;
@@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const
 {
 	PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
 	PX4_INFO("Number of timeouts: %i", _num_timeouts);
+	PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
 }
 
 uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)

src/drivers/dshot/DShotTelemetry.h

@@ -140,4 +140,5 @@ private:
 	// statistics
 	int _num_timeouts{0};
 	int _num_successful_responses{0};
+	int _num_checksum_errors{0};
 };