dshot: bdshot fix esc offline/online checks

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

@@ -86,8 +86,11 @@ typedef struct dshot_handler_t {
 	uint32_t		crc_error_cnt;
 	uint32_t		frame_error_cnt;
 	uint32_t		no_response_cnt;
+	uint32_t		last_no_response_cnt;
 } dshot_handler_t;
 
+#define BDSHOT_OFFLINE_COUNT 400 // If there are no responses for 400 setpoints ESC is offline
+
 static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {};
 
 static uint32_t dshot_tcmp;
@@ -390,6 +393,7 @@ void up_bdshot_erpm(void)
 				} else {
 					dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF;
 					bdshot_parsed_recv_mask |= (1 << channel);
+					dshot_inst[channel].last_no_response_cnt = dshot_inst[channel].no_response_cnt;
 				}
 
 			} else {
@@ -411,6 +415,14 @@ int up_bdshot_get_erpm(uint8_t channel, int *erpm)
 	return -1;
 }
 
+int up_bdshot_channel_status(uint8_t channel)
+{
+	if (channel < DSHOT_TIMERS) {
+		return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT);
+	}
+
+	return -1;
+}
 
 void up_bdshot_status(void)
 {
@@ -418,7 +430,8 @@ void up_bdshot_status(void)
 	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
 
 		if (dshot_inst[channel].init) {
-			PX4_INFO("Channel %i Last erpm %i value", channel, dshot_inst[channel].erpm);
+			PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline",
+				 dshot_inst[channel].erpm);
 			PX4_INFO("CRC errors Frame error No response");
 			PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt,
 				 dshot_inst[channel].no_response_cnt);

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

@@ -158,6 +158,12 @@ int up_bdshot_get_erpm(uint8_t channel, int *erpm)
 	return -1;
 }
 
+int up_bdshot_channel_status(uint8_t channel)
+{
+	// Not implemented
+	return -1;
+}
+
 void up_bdshot_status(void)
 {
 }

src/drivers/drv_dshot.h

@@ -152,4 +152,13 @@ __EXPORT extern void up_bdshot_status(void);
 __EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm);
 
 
+/**
+ * Get bidrectional dshot status for a channel
+ * @param channel	Dshot channel
+ * @param erpm		pointer to write the erpm value
+ * @return <0 on error / not supported, 0 on offline, 1 on online
+ */
+__EXPORT extern int up_bdshot_channel_status(uint8_t channel);
+
+
 __END_DECLS

src/drivers/dshot/DShot.cpp

@@ -159,6 +159,9 @@ void DShot::enable_dshot_outputs(const bool enabled)
 		for (unsigned i = 0; i < _num_outputs; ++i) {
 			if (((1 << i) & _output_mask) == 0) {
 				_mixing_output.disableFunction(i);
+
+			} else {
+				_dshot_esc_count++;
 			}
 		}
 
@@ -248,9 +251,6 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
 		esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 		esc_status.esc_count = _telemetry->handler.numMotors();
 		++esc_status.counter;
-		// FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout
-		esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
-		esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
 
 		ret = 1; // Indicate we wrapped, so we publish data
 	}
@@ -263,14 +263,31 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
 void DShot::publish_esc_status(void)
 {
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+	uint8_t channel;
 
 	// clear data of the esc that are offline
-	for (uint8_t channel = 0; (channel < _telemetry->last_telemetry_index); channel++) {
+	for (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));
 		}
 	}
 
+	// FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout
+	esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
+	esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
+
+	if (_bdshot) {
+		esc_status.esc_online_flags |= _output_mask;
+		esc_status.esc_armed_flags |= _output_mask;
+		esc_status.esc_count = _dshot_esc_count;
+
+		for (channel = 0; (channel < 8); channel++) {
+			if (up_bdshot_channel_status(channel) == 0) {
+				esc_status.esc_online_flags &= ~(1 << channel);
+			}
+		}
+	}
+
 	// ESC telem wrap around or bdshot update
 	_telemetry->esc_status_pub.update();
 
@@ -302,8 +319,6 @@ int DShot::handle_new_bdshot_erpm(void)
 
 	}
 
-	esc_status.esc_count = num_erpms;
-
 	return num_erpms;
 }
 
@@ -780,7 +795,9 @@ int DShot::print_status()
 	}
 
 	/* Print dshot status */
-	up_bdshot_status();
+	if (_bdshot) {
+		up_bdshot_status();
+	}
 
 	return 0;
 }

src/drivers/dshot/DShot.h

@@ -166,6 +166,7 @@ private:
 
 	static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
 	uint32_t _output_mask{0};
+	uint32_t _dshot_esc_count{0};
 
 	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};