From 937998b739b9ffa0b2dc1aba97d20096a06b45af Mon Sep 17 00:00:00 2001
From: Alexander Lerach <alexander@auterion.com>
Date: Thu, 17 Apr 2025 18:31:57 +0200
Subject: [PATCH] vtol: reduce schedule frequency, which causes DSHOT150
 problems

* vtol: reduce schedule frequency, which causes DSHOT150 problems

* vtol_att_control_main: refactor callback handling

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
---
 .../vtol_att_control_main.cpp                 | 57 +++++++++++--------
 .../vtol_att_control/vtol_att_control_main.h  |  8 ++-
 2 files changed, 40 insertions(+), 25 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 3de8994ec1..de0ce8f78c 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -94,26 +94,7 @@ VtolAttitudeControl::~VtolAttitudeControl()
 bool
 VtolAttitudeControl::init()
 {
-	if (!_vehicle_torque_setpoint_virtual_fw_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	if (!_vehicle_torque_setpoint_virtual_mc_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	if (!_vehicle_thrust_setpoint_virtual_fw_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	if (!_vehicle_thrust_setpoint_virtual_mc_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
+	ScheduleNow();
 	return true;
 }
 
@@ -272,14 +253,38 @@ VtolAttitudeControl::parameters_update()
 	}
 }
 
+void
+VtolAttitudeControl::update_callbacks()
+{
+	mode current_vtol_mode = _vtol_type->get_mode();
+
+	switch (current_vtol_mode) {
+	case mode::TRANSITION_TO_FW:
+	case mode::TRANSITION_TO_MC:
+	case mode::ROTARY_WING:
+		if (_vehicle_torque_setpoint_virtual_mc_sub.registerCallback()) {
+			_vehicle_torque_setpoint_virtual_fw_sub.unregisterCallback();
+		}
+
+		break;
+
+	case mode::FIXED_WING:
+		if (_vehicle_torque_setpoint_virtual_fw_sub.registerCallback()) {
+			_vehicle_torque_setpoint_virtual_mc_sub.unregisterCallback();
+		}
+
+		break;
+	}
+
+	_previous_vtol_mode = current_vtol_mode;
+}
+
 void
 VtolAttitudeControl::Run()
 {
 	if (should_exit()) {
 		_vehicle_torque_setpoint_virtual_fw_sub.unregisterCallback();
 		_vehicle_torque_setpoint_virtual_mc_sub.unregisterCallback();
-		_vehicle_thrust_setpoint_virtual_fw_sub.unregisterCallback();
-		_vehicle_thrust_setpoint_virtual_mc_sub.unregisterCallback();
 		exit_and_cleanup();
 		return;
 	}
@@ -298,6 +303,7 @@ VtolAttitudeControl::Run()
 	if (!_initialized) {
 
 		if (_vtol_type->init()) {
+			update_callbacks();
 			_initialized = true;
 
 		} else {
@@ -315,8 +321,13 @@ VtolAttitudeControl::Run()
 
 	// run on actuator publications corresponding to VTOL mode
 	bool should_run = false;
+	mode current_vtol_mode = _vtol_type->get_mode();
 
-	switch (_vtol_type->get_mode()) {
+	if (current_vtol_mode != _previous_vtol_mode) {
+		update_callbacks();
+	}
+
+	switch (current_vtol_mode) {
 	case mode::TRANSITION_TO_FW:
 	case mode::TRANSITION_TO_MC:
 		should_run = updated_fw_in || updated_mc_in;
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index a69a614743..ad08d23b54 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -87,6 +87,7 @@
 #include "standard.h"
 #include "tailsitter.h"
 #include "tiltrotor.h"
+#include "vtol_type.h"
 
 using namespace time_literals;
 
@@ -149,8 +150,8 @@ private:
 	void Run() override;
 	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_virtual_fw_sub{this, ORB_ID(vehicle_torque_setpoint_virtual_fw)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_virtual_mc_sub{this, ORB_ID(vehicle_torque_setpoint_virtual_mc)};
-	uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_virtual_fw_sub{this, ORB_ID(vehicle_thrust_setpoint_virtual_fw)};
-	uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_virtual_mc_sub{this, ORB_ID(vehicle_thrust_setpoint_virtual_mc)};
+	uORB::Subscription _vehicle_thrust_setpoint_virtual_fw_sub{ORB_ID(vehicle_thrust_setpoint_virtual_fw)};
+	uORB::Subscription _vehicle_thrust_setpoint_virtual_mc_sub{ORB_ID(vehicle_thrust_setpoint_virtual_mc)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -223,6 +224,7 @@ private:
 	uint8_t _nav_state_prev;
 
 	VtolType	*_vtol_type{nullptr};	// base class for different vtol types
+	mode		_previous_vtol_mode;
 
 	bool		_initialized{false};
 
@@ -236,6 +238,8 @@ private:
 
 	void 		parameters_update();
 
+	void		update_callbacks();
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::VT_TYPE>) _param_vt_type,
 		(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld