dshot: convert period to RPM

This calculates the actual RPM from the period transmitted via dshot to based on the pole count. Signed-off-by: Julian Oes <[email protected]>
PX4 · May 18, 2023 · 7eb919a · 7eb919a
1 parent 3fcc64e
commit 7eb919a
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 15 deletions.
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
@@ -113,9 +113,9 @@ static int _channels_init_mask = 0;
 
 // We only support capture on the first timer (usually 4 channels) for now.
 static uint32_t _motor_to_capture = 0;
-static uint32_t _erpms[4] = {};
+static int32_t _erpms[4] = {};
 
-static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL;
+static void(*_erpm_callback)(int32_t[], size_t, void *) = NULL;
 static void *_erpm_callback_context = NULL;
 
 uint8_t nibbles_from_mapped(uint8_t mapped)
@@ -500,9 +500,20 @@ void process_capture_results(void *arg)
 			     (uintptr_t)dshot_capture_buffer +
 			     sizeof(dshot_capture_buffer));
 
-	// TODO: fix order
-	// TODO: convert from period to erpm
-	_erpms[_motor_to_capture] = calculate_period();
+	const unsigned period = calculate_period();
+
+	if (period == 0) {
+		// If the parsing failed, we get 0.
+		_erpms[_motor_to_capture] = 0;
+
+	} else if (period == 65408) {
+		// For still, we get this magic 65408 value.
+		_erpms[_motor_to_capture] = 0;
+
+	} else {
+		// from period in us to eRPM
+		_erpms[_motor_to_capture] = 1000000 * 60 / period;
+	}
 
 	for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
 		if (_channels_init_mask & (1 << channel)) {
@@ -576,7 +587,7 @@ int up_dshot_arm(bool armed)
 				   IO_TIMER_ALL_MODES_CHANNELS);
 }
 
-void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context)
+void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context)
 {
 	_erpm_callback = callback;
 	_erpm_callback_context = context;

diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h
@@ -140,7 +140,7 @@ __EXPORT extern int up_dshot_arm(bool armed);
 
 __EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods);
 
-__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context);
+__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context);
 
 __EXPORT extern void print_driver_stats(void);
 

diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
@@ -614,26 +614,25 @@ void DShot::update_params()
 	}
 }
 
-void DShot::erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context)
+void DShot::erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context)
 {
 	DShot *self = static_cast<DShot *>(context);
 	self->erpm(erpms, num_erpms);
 }
 
-void DShot::erpm(uint32_t erpms[], size_t num_erpms)
+void DShot::erpm(int32_t erpms[], size_t num_erpms)
 {
-	// TODO: this is hard-coded to 4 motors
 	esc_status_s &esc_status = _esc_status_pub.get();
 	esc_status = {};
 	esc_status.timestamp = hrt_absolute_time();
 	esc_status.counter = _esc_status_counter++;
-	esc_status.esc_count = 4;
+	esc_status.esc_count = num_erpms;
 	esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 	esc_status.esc_armed_flags = _outputs_on;
 
-	for (unsigned i = 0; i < 4 && i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
+	for (unsigned i = 0; i < num_erpms && i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
 		esc_status.esc[i].timestamp = hrt_absolute_time();
-		esc_status.esc[i].esc_rpm = erpms[i];
+		esc_status.esc[i].esc_rpm = erpms[i] / (_param_mot_pole_count.get() / 2);
 	}
 
 	_esc_status_pub.update();

diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h
@@ -143,8 +143,8 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 
 	void handle_vehicle_commands();
 
-	static void erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context);
-	void erpm(uint32_t erpms[], size_t num_erpms);
+	static void erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context);
+	void erpm(int32_t erpms[], size_t num_erpms);
 
 	MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _reversible_outputs{};