[Sponsored by ARK] Bidirectional DShot (#23863)

* Bidirectional DShot

Co-authored-by: Julian Oes <julian@oes.ch>

* f4/f1 support, not supported

* fix f1 build target

* sanity check timer_channel value, fix CCxNP ifdef, debug stuff

* removed debug code, added define for H7 HAVE_GTIM_CCXNP

* round robin sampling for less than 4 DMA

* unlimited esc_status logging

* dshot: fix formatting

* dshot: add define for number of DMA channels to use

This allows individual boards to override the number of DShot channels
and hence avoid round robin capture of the RPM feedback.

* ARK: enable 4 DMA channels for DShot on 6X

* dshot: publish when all channels are updated

This slows down the ESC_STATUS publication in the case of round robin
capture. E.g. for 800 Hz output with one DMA channel, the ESC_STATUS is
now published at 200 Hz.

* dshot: avoid duplicate publications for bidir and telem

Instead of publishing both bidirectional dshot updates as well as
telemetry updates, we now combine the data from both streams, and
publish whenever we get RPM updates, as the latter arrives with higher
rate, e.g. 200 Hz with round robin, or faster otherwise.

When combining the data, we take RPM from bidirectional dshot, and the
rest from telemetry.

When we have only one of the two, either telemetry or bidirectional
dshot, we just publish that one.

* boards: add ark fpv and pi6x BOARD_DMA_NUM_DSHOT_CHANNELS

* dshot: turn off debug build

---------

Co-authored-by: Julian Oes <julian@oes.ch>
Co-authored-by: alexklimaj <alex@arkelectron.com>

src/drivers/drv_dshot.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -96,7 +96,7 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq
 /**
  * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
  */
-__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry);
+__EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
 
 /**
  * Set the current dshot throttle value for a channel (motor).
@@ -143,6 +143,17 @@ __EXPORT extern int up_dshot_arm(bool armed);
 __EXPORT extern void up_bdshot_status(void);
 
 
+/**
+ * Get how many bidirectional erpm channels are ready
+ *
+ * When we get the erpm round-robin style, we need to get
+ * and publish the erpms less often.
+ *
+ * @return <0 on error, OK on succes
+ */
+__EXPORT extern int up_bdshot_num_erpm_ready(void);
+
+
 /**
  * Get bidrectional dshot erpm for a channel
  * @param channel	Dshot channel

src/drivers/dshot/DShot.cpp

@@ -167,10 +167,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
 		}
 
 		_outputs_initialized = true;
-
-		if (_bidirectional_dshot_enabled) {
-			init_telemetry(NULL);
-		}
 	}
 
 	if (_outputs_initialized) {
@@ -179,28 +175,24 @@ void DShot::enable_dshot_outputs(const bool enabled)
 	}
 }
 
-void DShot::update_telemetry_num_motors()
+void DShot::update_num_motors()
 {
-	if (!_telemetry) {
-		return;
-	}
-
 	int motor_count = 0;
 
 	for (unsigned i = 0; i < _num_outputs; ++i) {
 		if (_mixing_output.isFunctionSet(i)) {
-			_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
+			_actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
 			++motor_count;
 		}
 	}
 
-	_telemetry->handler.setNumMotors(motor_count);
+	_num_motors = motor_count;
 }
 
 void DShot::init_telemetry(const char *device)
 {
 	if (!_telemetry) {
-		_telemetry = new Telemetry{};
+		_telemetry = new DShotTelemetry{};
 
 		if (!_telemetry) {
 			PX4_ERR("alloc failed");
@@ -208,32 +200,35 @@ void DShot::init_telemetry(const char *device)
 		}
 	}
 
-	_telemetry->esc_status_pub.advertise();
-
 	if (device != NULL) {
-		int ret = _telemetry->handler.init(device);
+		int ret = _telemetry->init(device);
 
 		if (ret != 0) {
 			PX4_ERR("telemetry init failed (%i)", ret);
 		}
 	}
 
-	update_telemetry_num_motors();
+	update_num_motors();
 }
 
-int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
+int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm)
 {
 	int ret = 0;
 	// fill in new motor data
-	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+	esc_status_s &esc_status = esc_status_pub.get();
 
 	if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
 		esc_status.esc_online_flags |= 1 << telemetry_index;
 
-		esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
-		esc_status.esc[telemetry_index].timestamp       = data.time;
-		esc_status.esc[telemetry_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) /
-				(_param_mot_pole_count.get() / 2);
+		esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index];
+
+		if (!ignore_rpm) {
+			// If we also have bidirectional dshot, we use rpm and timestamps from there.
+			esc_status.esc[telemetry_index].timestamp       = data.time;
+			esc_status.esc[telemetry_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) /
+					(_param_mot_pole_count.get() / 2);
+		}
+
 		esc_status.esc[telemetry_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
 		esc_status.esc[telemetry_index].esc_current     = static_cast<float>(data.current) * 0.01f;
 		esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
@@ -241,34 +236,34 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
 	}
 
 	// publish when motor index wraps (which is robust against motor timeouts)
-	if (telemetry_index <= _telemetry->last_telemetry_index) {
+	if (telemetry_index <= _last_telemetry_index) {
 		esc_status.timestamp = hrt_absolute_time();
 		esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
-		esc_status.esc_count = _telemetry->handler.numMotors();
+		esc_status.esc_count = _num_motors;
 		++esc_status.counter;
 
 		ret = 1; // Indicate we wrapped, so we publish data
 	}
 
-	_telemetry->last_telemetry_index = telemetry_index;
+	_last_telemetry_index = telemetry_index;
 
 	return ret;
 }
 
 void DShot::publish_esc_status(void)
 {
-	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+	esc_status_s &esc_status = esc_status_pub.get();
 	int telemetry_index = 0;
 
 	// clear data of the esc that are offline
-	for (int index = 0; (index < _telemetry->last_telemetry_index); index++) {
+	for (int index = 0; (index < _last_telemetry_index); index++) {
 		if ((esc_status.esc_online_flags & (1 << index)) == 0) {
 			memset(&esc_status.esc[index], 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_count = _telemetry->handler.numMotors();
+	esc_status.esc_count = _num_motors;
 	esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
 	esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
 
@@ -287,8 +282,12 @@ void DShot::publish_esc_status(void)
 		}
 	}
 
-	// ESC telem wrap around or bdshot update
-	_telemetry->esc_status_pub.update();
+	if (!esc_status_pub.advertised()) {
+		esc_status_pub.advertise();
+
+	} else {
+		esc_status_pub.update();
+	}
 
 	// reset esc online flags
 	esc_status.esc_online_flags = 0;
@@ -299,13 +298,18 @@ int DShot::handle_new_bdshot_erpm(void)
 	int num_erpms = 0;
 	int telemetry_index = 0;
 	int erpm;
-	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+	esc_status_s &esc_status = esc_status_pub.get();
 
 	esc_status.timestamp = hrt_absolute_time();
 	esc_status.counter = _esc_status_counter++;
 	esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 	esc_status.esc_armed_flags = _outputs_on;
 
+	// We wait until all are ready.
+	if (up_bdshot_num_erpm_ready() < _num_motors) {
+		return 0;
+	}
+
 	for (unsigned i = 0; i < _num_outputs; i++) {
 		if (_mixing_output.isFunctionSet(i)) {
 			if (up_bdshot_get_erpm(i, &erpm) == 0) {
@@ -313,7 +317,7 @@ int DShot::handle_new_bdshot_erpm(void)
 				esc_status.esc_online_flags |= 1 << telemetry_index;
 				esc_status.esc[telemetry_index].timestamp = hrt_absolute_time();
 				esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2);
-				esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
+				esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index];
 			}
 
 			++telemetry_index;
@@ -389,7 +393,7 @@ void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index)
 
 int DShot::request_esc_info()
 {
-	_telemetry->handler.redirectOutput(*_request_esc_info.load());
+	_telemetry->redirectOutput(*_request_esc_info.load());
 	_waiting_for_esc_info = true;
 
 	int motor_index = _request_esc_info.load()->motor_index;
@@ -405,7 +409,8 @@ int DShot::request_esc_info()
 
 void DShot::mixerChanged()
 {
-	update_telemetry_num_motors();
+	update_num_motors();
+
 }
 
 bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -420,11 +425,11 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	if (_telemetry) {
 		// check for an ESC info request. We only process it when we're not expecting other telemetry data
 		if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors
-		    && !_telemetry->handler.expectingData() && !_current_command.valid()) {
+		    && !_telemetry->expectingData() && !_current_command.valid()) {
 			requested_telemetry_index = request_esc_info();
 
 		} else {
-			requested_telemetry_index = _telemetry->handler.getRequestMotorIndex();
+			requested_telemetry_index = _telemetry->getRequestMotorIndex();
 		}
 	}
 
@@ -534,8 +539,7 @@ void DShot::Run()
 	}
 
 	if (_telemetry) {
-		int telem_update = _telemetry->handler.update();
-		int need_to_publish = 0;
+		const int telem_update = _telemetry->update(_num_motors);
 
 		// Are we waiting for ESC info?
 		if (_waiting_for_esc_info) {
@@ -545,20 +549,24 @@ void DShot::Run()
 			}
 
 		} else if (telem_update >= 0) {
-			need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
-		}
+			const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(),
+						    _bidirectional_dshot_enabled);
 
-		if (_bidirectional_dshot_enabled) {
-			// Add bdshot data to esc status
-			need_to_publish += handle_new_bdshot_erpm();
-		}
-
-		if (need_to_publish > 0) {
-			// ESC telem wrap around or bdshot update
-			publish_esc_status();
+			// We don't want to publish twice, once by telemetry and once by bidirectional dishot.
+			if (!_bidirectional_dshot_enabled && need_to_publish) {
+				publish_esc_status();
+			}
 		}
 	}
 
+	if (_bidirectional_dshot_enabled) {
+		// Add bdshot data to esc status
+		const int need_to_publish = handle_new_bdshot_erpm();
+
+		if (need_to_publish) {
+			publish_esc_status();
+		}
+	}
 
 	if (_parameter_update_sub.updated()) {
 		update_params();
@@ -794,7 +802,7 @@ int DShot::print_status()
 
 	if (_telemetry) {
 		PX4_INFO("telemetry on: %s", _telemetry_device);
-		_telemetry->handler.printStatus();
+		_telemetry->printStatus();
 	}
 
 	/* Print dshot status */

src/drivers/dshot/DShot.h

@@ -118,18 +118,14 @@ private:
 		void clear() { num_repetitions = 0; }
 	};
 
-	struct Telemetry {
-		DShotTelemetry handler{};
-		uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
-		int last_telemetry_index{-1};
-		uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
-	};
+	int _last_telemetry_index{-1};
+	uint8_t _actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
 
 	void enable_dshot_outputs(const bool enabled);
 
 	void init_telemetry(const char *device);
 
-	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
 
 	void publish_esc_status(void);
 
@@ -141,14 +137,16 @@ private:
 
 	void update_params();
 
-	void update_telemetry_num_motors();
+	void update_num_motors();
 
 	void handle_vehicle_commands();
 
 	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _reversible_outputs{};
 
-	Telemetry *_telemetry{nullptr};
+	DShotTelemetry *_telemetry{nullptr};
+
+	uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
 
 	static char _telemetry_device[20];
 	static px4::atomic_bool _request_telemetry_init;
@@ -165,6 +163,8 @@ private:
 	static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
 	uint32_t _output_mask{0};
 
+	int _num_motors{0};
+
 	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
 	Command _current_command{};

src/drivers/dshot/DShotTelemetry.cpp

@@ -87,7 +87,7 @@ int DShotTelemetry::redirectOutput(OutputBuffer &buffer)
 	return 0;
 }
 
-int DShotTelemetry::update()
+int DShotTelemetry::update(int num_motors)
 {
 	if (_uart_fd < 0) {
 		return -1;
@@ -120,7 +120,7 @@ int DShotTelemetry::update()
 				++_num_timeouts;
 			}
 
-			requestNextMotor();
+			requestNextMotor(num_motors);
 			return -2;
 		}
 
@@ -142,7 +142,7 @@ int DShotTelemetry::update()
 				_redirect_output = nullptr;
 				ret = _current_motor_index_request;
 				_current_motor_index_request = -1;
-				requestNextMotor();
+				requestNextMotor(num_motors);
 			}
 
 		} else {
@@ -153,7 +153,7 @@ int DShotTelemetry::update()
 					ret = _current_motor_index_request;
 				}
 
-				requestNextMotor();
+				requestNextMotor(num_motors);
 			}
 		}
 	}
@@ -225,9 +225,9 @@ uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
 }
 
 
-void DShotTelemetry::requestNextMotor()
+void DShotTelemetry::requestNextMotor(int num_motors)
 {
-	_current_motor_index_request = (_current_motor_index_request + 1) % _num_motors;
+	_current_motor_index_request = (_current_motor_index_request + 1) % num_motors;
 	_current_request_start = 0;
 	_frame_position = 0;
 }

src/drivers/dshot/DShotTelemetry.h

@@ -64,14 +64,12 @@ public:
 
 	void deinit();
 
-	void setNumMotors(int num_motors) { _num_motors = num_motors; }
-	int numMotors() const { return _num_motors; }
-
 	/**
 	 * Read telemetry from the UART (non-blocking) and handle timeouts.
+	 * @param num_motors How many DShot enabled motors
 	 * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data.
 	 */
-	int update();
+	int update(int num_motors);
 
 	/**
 	 * Redirect everything that is read into a different buffer.
@@ -112,7 +110,7 @@ private:
 	 */
 	int setBaudrate(unsigned baud);
 
-	void requestNextMotor();
+	void requestNextMotor(int num_motors);
 
 	/**
 	 * Decode a single byte from an ESC feedback frame
@@ -126,7 +124,6 @@ private:
 	static uint8_t crc8(const uint8_t *buf, uint8_t len);
 
 	int _uart_fd{-1};
-	int _num_motors{0};
 	uint8_t _frame_buffer[ESC_FRAME_SIZE];
 	int _frame_position{0};