bdshot per channel, use PWM_TIM param for BDSHOT instead of single parameter to allow per timer bdshot, clean up logic and fix minor bugs, re-init dshot in mixerChanged

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

@@ -424,8 +424,9 @@ void up_dshot_trigger()
 			io_timer_update_dma_req(timer_index, false);
 
 			// Trigger DMA (DShot Outputs). Only capture compare after the system has had time to boot.
-			if (timer->bidirectional && (hrt_absolute_time() > 3000000)) {
+			static const uint64_t ESC_BOOT_DELAY_US = 3000000;
 
+			if (timer->bidirectional && (hrt_absolute_time() > ESC_BOOT_DELAY_US)) {
 				perf_begin(capture_cycle_perf);
 				stm32_dmastart(timer->dma_handle, dma_burst_finished_callback, &timer->timer_index, false);
 

src/drivers/dshot/DShot.cpp

@@ -72,7 +72,6 @@ int DShot::init()
 	update_params();
 
 	_serial_telemetry_enabled = _param_dshot_tel_cfg.get();
-	_bdshot_telemetry_enabled = _param_dshot_bidir_en.get();
 	_bdshot_edt_enabled = _param_dshot_bidir_edt.get();
 
 	if (initialize_dshot()) {
@@ -172,16 +171,13 @@ void DShot::select_next_command()
 	// NOTE: only update when we're not actively programming an ESC
 	if (!_dshot_programming_active) {
 
-		// Get the next update from the queue?
 		if (_esc_eeprom_write_sub.updated()) {
-
-			// TODO: because uORB can't queue?? (what is the point of the ORB_QUEUE_LENGTH then??)
-			auto last = _esc_eeprom_write_sub.get_last_generation();
+			auto last_generation = _esc_eeprom_write_sub.get_last_generation();
 			_esc_eeprom_write_sub.copy(&_esc_eeprom_write);
-			auto current = _esc_eeprom_write_sub.get_last_generation();
+			auto current_generation = _esc_eeprom_write_sub.get_last_generation();
 
-			if (current != last + 1) {
-				PX4_ERR("esc_eeprom_write lost, generation %u -> %u", last, current);
+			if (current_generation != last_generation + 1) {
+				PX4_ERR("esc_eeprom_write lost, generation %u -> %u", last_generation, current_generation);
 			}
 
 			PX4_INFO("ESC%u: starting programming mode", _esc_eeprom_write.index + 1);
@@ -204,12 +200,14 @@ void DShot::select_next_command()
 
 	_current_command.clear();
 
-	if (_bdshot_telemetry_enabled && _bdshot_edt_enabled && needs_edt_request_mask) {
+	if (_bdshot_output_mask && _bdshot_edt_enabled && needs_edt_request_mask) {
 		// EDT Request first
 		int next_motor_index = 0;
 
 		for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) {
-			if (needs_edt_request_mask & (1 << i)) {
+			bool is_bdshot = _bdshot_output_mask & (1 << i);
+
+			if (is_bdshot && (needs_edt_request_mask & (1 << i))) {
 				next_motor_index = i;
 				break;
 			}
@@ -222,12 +220,14 @@ void DShot::select_next_command()
 		_bdshot_edt_requested_mask |= (1 << next_motor_index);
 		PX4_INFO("ESC%d: requesting EDT at time %.2fs", next_motor_index + 1, (double)now / 1000000.);
 
-	} else if (_serial_telemetry_enabled && _bdshot_telemetry_enabled && needs_settings_request_mask && serial_telem_delay_elapsed) {
+	} else if (_serial_telemetry_enabled && _bdshot_output_mask && needs_settings_request_mask && serial_telem_delay_elapsed) {
 		// Settings Request next
 		int next_motor_index = 0;
 
 		for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) {
-			if (needs_settings_request_mask & (1 << i)) {
+			bool is_bdshot = _bdshot_output_mask & (1 << i);
+
+			if (is_bdshot && (needs_settings_request_mask & (1 << i))) {
 				next_motor_index = i;
 				break;
 			}
@@ -546,7 +546,7 @@ bool DShot::set_next_telemetry_index()
 
 bool DShot::process_bdshot_telemetry()
 {
-	if (!_bdshot_telemetry_enabled) {
+	if (!_bdshot_output_mask) {
 		return false;
 	}
 
@@ -563,7 +563,10 @@ bool DShot::process_bdshot_telemetry()
 	}
 
 	for (uint8_t output_channel = 0; output_channel < DSHOT_MAXIMUM_CHANNELS; output_channel++) {
-		if (!_mixing_output.isMotor(output_channel)) {
+		bool is_motor = _mixing_output.isMotor(output_channel);
+		bool is_bdshot = _bdshot_output_mask & (1 << output_channel);
+
+		if (!is_motor || !is_bdshot) {
 			continue;
 		}
 
@@ -606,14 +609,14 @@ bool DShot::process_bdshot_telemetry()
 					}
 
 					if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_VOLTAGE, &value) == PX4_OK) {
-						esc.voltage = value * 0.25f; // BDShot voltage is in 0.25V
+						esc.voltage = value / 4.f; // BDShot voltage is in 0.25V
 
 					} else {
 						esc.voltage = _esc_status.esc[motor_index].esc_voltage; // use previous
 					}
 
 					if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_CURRENT, &value) == PX4_OK) {
-						esc.current = value * 0.5f; // BDShot current is in 0.5V
+						esc.current = value / 2.f; // BDShot current is in 0.5A
 
 					} else {
 						esc.current = _esc_status.esc[motor_index].esc_current;  // use previous
@@ -641,10 +644,12 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source)
 		return;
 	}
 
+	bool is_bdshot = _bdshot_output_mask & (1 << esc.motor_index);
+
 	// Require both sources online when enabled
 	uint8_t online_mask = 0xFF;
 
-	if (_bdshot_telemetry_enabled) {
+	if (is_bdshot) {
 		online_mask &= _bdshot_telem_online_mask;
 	}
 
@@ -660,7 +665,7 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source)
 
 	if (source == TelemetrySource::Serial) {
 		// Only use SerialTelemetry eRPM when BDShot is disabled
-		if (!_bdshot_telemetry_enabled) {
+		if (!is_bdshot) {
 			_esc_status.esc[esc.motor_index].timestamp = esc.timestamp;
 			_esc_status.esc[esc.motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2);
 		}
@@ -730,6 +735,7 @@ void DShot::handle_vehicle_commands()
 		case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
 			PX4_INFO("VEHICLE_CMD_REQUEST_MESSAGE: param1: %f", (double)command.param1);
 			handle_esc_request_eeprom(command);
+			break;
 
 		default:
 			break;
@@ -759,7 +765,7 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command)
 	command_ack.target_component = command.source_component;
 	command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 
-	if ((motor_index >= 0) || (motor_index < DSHOT_MAXIMUM_CHANNELS)) {
+	if ((motor_index >= 0) && (motor_index < DSHOT_MAXIMUM_CHANNELS)) {
 		int type = (int)(command.param1 + 0.5f);
 		PX4_INFO("motor_index: %i type: %i", motor_index, type);
 		_current_command.clear();
@@ -882,7 +888,8 @@ void DShot::mixerChanged()
 		}
 	}
 
-	// TODO: re-init dshot.c output channels and stuff
+	// TODO: verify that this works
+	initialize_dshot();
 }
 
 bool DShot::initialize_dshot()
@@ -891,38 +898,42 @@ bool DShot::initialize_dshot()
 	uint32_t dshot_frequency_param = 0;
 
 	_output_mask = 0;
+	_bdshot_output_mask = 0;
 
-	for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+	for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
 
 		// Get mask of actuator channels associated with this timer group
-		uint32_t channels = io_timer_get_group(timer);
+		uint32_t channels = io_timer_get_group(timer_index);
 
 		if (channels == 0) {
 			continue;
 		}
 
-		// TODO: allow BDSHOT options
 		char param_name[17];
-		snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+		snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer_index);
 
 		int32_t tim_config = 0;
 		param_t handle = param_find(param_name);
 		param_get(handle, &tim_config);
 		unsigned int dshot_frequency_request = 0;
 
-		if (tim_config == -5) {
+		if (tim_config == -5 || tim_config == -8) {
 			dshot_frequency_request = DSHOT150;
 			_output_mask |= channels;
 
-		} else if (tim_config == -4) {
+		} else if (tim_config == -4 || tim_config == -7) {
 			dshot_frequency_request = DSHOT300;
 			_output_mask |= channels;
 
-		} else if (tim_config == -3) {
+		} else if (tim_config == -3 || tim_config == -6) {
 			dshot_frequency_request = DSHOT600;
 			_output_mask |= channels;
 		}
 
+		if (tim_config < -5) {
+			_bdshot_output_mask |= io_timer_get_group(timer_index);
+		}
+
 		if (dshot_frequency_request != 0) {
 			if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
 				PX4_WARN("Only supporting a single frequency (%u), adjusting param %s", dshot_frequency, param_name);
@@ -935,8 +946,8 @@ bool DShot::initialize_dshot()
 		}
 	}
 
-	// TODO: use bdshot_channel_mask
-	int ret = up_dshot_init(_output_mask, _output_mask, dshot_frequency, _bdshot_edt_enabled);
+	_bdshot_output_mask &= _output_mask;
+	int ret = up_dshot_init(_output_mask, _bdshot_output_mask, dshot_frequency, _bdshot_edt_enabled);
 
 	if (ret < 0) {
 		PX4_ERR("up_dshot_init failed (%i)", ret);
@@ -1006,7 +1017,7 @@ int DShot::print_status()
 	// Configuration
 	PX4_INFO("Configuration:");
 	PX4_INFO("  Output Mask:        0x%02lx (%d channels)", (unsigned long)_output_mask, count_set_bits(_output_mask));
-	PX4_INFO("  Bidirectional:      %s", _bdshot_telemetry_enabled ? "Enabled" : "Disabled");
+	PX4_INFO("  BDShot Telemetry:   %s", _bdshot_output_mask ? "Enabled" : "Disabled");
 	PX4_INFO("  Serial Telemetry:   %s%s", _serial_telemetry_enabled ? "Enabled" : "Disabled",
 		 _serial_telemetry_enabled ? (strlen(_telemetry_device) ? "" : " (no device)") : "");
 
@@ -1029,7 +1040,7 @@ int DShot::print_status()
 	PX4_INFO("  3D Mode:            %s", _param_dshot_3d_enable.get() ? "Enabled" : "Disabled");
 
 	// Telemetry Status
-	if (_bdshot_telemetry_enabled || _serial_telemetry_enabled) {
+	if (_bdshot_output_mask || _serial_telemetry_enabled) {
 		print_spacer();
 		PX4_INFO("Telemetry Status:");
 		PX4_INFO("  %-6s %-8s %-8s %-12s %-12s", "Motor", "BDShot", "Serial", "BDShot Err", "Serial Err");
@@ -1042,7 +1053,7 @@ int DShot::print_status()
 			const char *bdshot_status = "-";
 			const char *serial_status = "-";
 
-			if (_bdshot_telemetry_enabled) {
+			if (_bdshot_output_mask) {
 				bdshot_status = (_bdshot_telem_online_mask & (1 << i)) ? "Online" : "Offline";
 			}
 
@@ -1058,7 +1069,7 @@ int DShot::print_status()
 				 (unsigned long)_serial_telem_errors[i]);
 		}
 
-		if (_bdshot_telemetry_enabled && _bdshot_edt_enabled) {
+		if (_bdshot_output_mask && _bdshot_edt_enabled) {
 			PX4_INFO("  EDT Requested Mask: 0x%02x", _bdshot_edt_requested_mask);
 		}
 
@@ -1068,7 +1079,7 @@ int DShot::print_status()
 	}
 
 	// Bidirectional DShot hardware status
-	if (_bdshot_telemetry_enabled) {
+	if (_bdshot_output_mask) {
 		print_spacer();
 		PX4_INFO("Bidirectional DShot Hardware:");
 		up_bdshot_status();
@@ -1089,7 +1100,7 @@ int DShot::print_status()
 	PX4_INFO("Performance Counters:");
 	perf_print_counter(_cycle_perf);
 
-	if (_bdshot_telemetry_enabled) {
+	if (_bdshot_output_mask) {
 		perf_print_counter(_bdshot_success_perf);
 		perf_print_counter(_bdshot_error_perf);
 	}

src/drivers/dshot/DShot.h

@@ -124,6 +124,7 @@ private:
 	// Mixer
 	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _output_mask{0}; // Configured outputs for this (shouldn't this live in OutputModuleInterface?)
+	uint32_t _bdshot_output_mask{0};
 
 	// uORB
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -158,7 +159,7 @@ private:
 
 	// Parameters we must load only at init
 	bool _serial_telemetry_enabled = false;
-	bool _bdshot_telemetry_enabled = false;
+
 	bool _bdshot_edt_enabled = false;
 
 	// Perf counters
@@ -217,7 +218,6 @@ private:
 		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
 		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
 		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
-		(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_dshot_bidir_en,
 		(ParamBool<px4::params::DSHOT_BIDIR_EDT>) _param_dshot_bidir_edt,
 		(ParamBool<px4::params::DSHOT_TEL_CFG>) _param_dshot_tel_cfg
 	)

src/drivers/dshot/DShotTelemetry.cpp

@@ -137,9 +137,14 @@ int DShotTelemetry::parseCommandResponse()
 		return -1;
 	}
 
-	uint8_t buf[COMMAND_RESPONSE_MAX_SIZE];
+	uint8_t buf[COMMAND_RESPONSE_MAX_SIZE] = {};
 	int bytes = _uart.read(buf, sizeof(buf));
 
+	// Handle potential overflow
+	if (_command_response_position + bytes >= COMMAND_RESPONSE_MAX_SIZE) {
+		_command_response_position = 0;
+	}
+
 	// Add bytes to buffer
 	for (int i = 0; i < bytes; i++) {
 		_command_response_buffer[_command_response_position++] = buf[i];
@@ -234,7 +239,7 @@ TelemetryStatus DShotTelemetry::decodeTelemetryResponse(uint8_t *buffer, int len
 				esc_data->timestamp = hrt_absolute_time();
 				esc_data->temperature = (float)temperature;
 				esc_data->voltage = (float)voltage * 0.01f;
-				esc_data->current = (float)current * 0.01f;;
+				esc_data->current = (float)current * 0.01f;
 				esc_data->erpm = erpm * 100;
 
 				++_num_successful_responses;

src/drivers/dshot/module.yaml

@@ -42,16 +42,6 @@ parameters:
                     When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
             type: boolean
             default: 0
-        DSHOT_BIDIR_EN:
-            description:
-                short: Enable bidirectional DShot
-                long: |
-                    This parameter enables bidirectional DShot which provides RPM feedback.
-                    Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32.
-                    This is not the same as DShot telemetry which requires an additional serial connection.
-            type: boolean
-            default: 0
-            reboot_required: true
         DSHOT_BIDIR_EDT:
             description:
                 short: Enable Extended DShot Telemetry

src/drivers/pwm_out/module.yaml

@@ -21,6 +21,9 @@ actuator_output:
         type: enum
         default: 400
         values:
+            -8: BDShot150
+            -7: BDShot300
+            -6: BDShot600
             -5: DShot150
             -4: DShot300
             -3: DShot600