Update src/modules/offboard_switch/offboard_switch.cpp

Co-authored-by: Matthias Grob <maetugr@gmail.com>
Update src/modules/offboard_switch/CMakeLists.txt
2026-05-28 11:50:06 +08:00 · 2024-01-12 13:36:37 +01:00 · 2024-01-12 13:36:31 +01:00 · 2024-01-12 13:36:25 +01:00 · 2023-12-15 05:52:48 +01:00 · 2023-12-08 21:17:59 +01:00
62 changed files with 2639 additions and 1928 deletions
@@ -79,3 +79,5 @@ mc_pos_control start
 # Start Multicopter Land Detector.
 #
 land_detector start multicopter
+
+offboard_switch start
@@ -52,6 +52,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_MODULES_OFFBOARD_SWITCH=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DYN=y
@@ -234,6 +234,7 @@ set(msg_files
 	VehicleTorqueSetpoint.msg
 	VehicleTrajectoryBezier.msg
 	VehicleTrajectoryWaypoint.msg
+	VelocityLimits.msg
 	VtolVehicleStatus.msg
 	WheelEncoders.msg
 	Wind.msg
@@ -13,3 +13,5 @@ float32[3] jerk # in meters/second^3 (for logging only)

 float32 yaw # euler angle of desired attitude in radians -PI..+PI
 float32 yawspeed # angular velocity around NED frame z-axis in radians/second
+
+# TOPICS trajectory_setpoint offboard_trajectory_setpoint
@@ -17,4 +17,4 @@ bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)

 bool fw_control_yaw_wheel	# control heading with steering wheel (used for auto takeoff on runway)

-# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
+# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint offboard_attitude_setpoint
@@ -10,3 +10,5 @@ float32 yaw		# [rad/s] yaw rate setpoint
 float32[3] thrust_body	# Normalized thrust command in body NED frame [-1,1]

 bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change)
+
+# TOPICS vehicle_rates_setpoint offboard_rates_setpoint
@@ -37,15 +37,17 @@ uint8 NAVIGATION_STATE_POSCTL = 2               # Position control mode
 uint8 NAVIGATION_STATE_AUTO_MISSION = 3         # Auto mission mode
 uint8 NAVIGATION_STATE_AUTO_LOITER = 4          # Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5             # Auto return to launch mode
-uint8 NAVIGATION_STATE_UNUSED3 = 8              # Free slot
-uint8 NAVIGATION_STATE_UNUSED = 9               # Free slot
+uint8 NAVIGATION_STATE_POSITION_SLOW = 6
+uint8 NAVIGATION_STATE_FREE5 = 7
+uint8 NAVIGATION_STATE_FREE4 = 8
+uint8 NAVIGATION_STATE_FREE3 = 9
 uint8 NAVIGATION_STATE_ACRO = 10                # Acro mode
-uint8 NAVIGATION_STATE_UNUSED1 = 11             # Free slot
+uint8 NAVIGATION_STATE_FREE2 = 11
 uint8 NAVIGATION_STATE_DESCEND = 12             # Descend mode (no position control)
 uint8 NAVIGATION_STATE_TERMINATION = 13         # Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
-uint8 NAVIGATION_STATE_UNUSED2 = 16             # Free slot
+uint8 NAVIGATION_STATE_FREE1 = 16
 uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
 uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
@@ -0,0 +1,8 @@
+# Velocity and yaw rate limits for a multicopter position slow mode only
+
+uint64 timestamp # time since system start (microseconds)
+
+# absolute speeds, NAN means use default limit
+float32 horizontal_velocity # [m/s]
+float32 vertical_velocity # [m/s]
+float32 yaw_rate # [rad/s]
@@ -3,22 +3,20 @@ menu "Distance sensors"
        bool "Common distance sensor's"
        default n
        select DRIVERS_DISTANCE_SENSOR_CM8JL65
+        select DRIVERS_DISTANCE_SENSOR_GY_US42
        select DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE
-        select DRIVERS_DISTANCE_SENSOR_LL40LS
-        select DRIVERS_DISTANCE_SENSOR_LL40LS_PWM
-        select DRIVERS_DISTANCE_SENSOR_MAPPYDOT
-        select DRIVERS_DISTANCE_SENSOR_MB12XX
-        select DRIVERS_DISTANCE_SENSOR_PGA460
        select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C
        select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL
+        select DRIVERS_DISTANCE_SENSOR_LL40LS
+        select DRIVERS_DISTANCE_SENSOR_MB12XX
+        select DRIVERS_DISTANCE_SENSOR_PGA460
        select DRIVERS_DISTANCE_SENSOR_SRF02
        select DRIVERS_DISTANCE_SENSOR_TERARANGER
+        select DRIVERS_DISTANCE_SENSOR_TF02PRO
        select DRIVERS_DISTANCE_SENSOR_TFMINI
        select DRIVERS_DISTANCE_SENSOR_ULANDING_RADAR
        select DRIVERS_DISTANCE_SENSOR_VL53L0X
        select DRIVERS_DISTANCE_SENSOR_VL53L1X
-        select DRIVERS_DISTANCE_SENSOR_GY_US42
-        select DRIVERS_DISTANCE_SENSOR_TF02PRO
        ---help---
            Enable default set of distance sensor drivers

@@ -102,18 +102,10 @@ msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
 			display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_RTL");
 			break;

-		case vehicle_status_s::NAVIGATION_STATE_UNUSED:
-			display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED");
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 			display.set(MessageDisplayType::FLIGHT_MODE, "ACRO");
 			break;

-		case vehicle_status_s::NAVIGATION_STATE_UNUSED1:
-			display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED1");
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 			display.set(MessageDisplayType::FLIGHT_MODE, "DESCEND");
 			break;
@@ -130,10 +122,6 @@ msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
 			display.set(MessageDisplayType::FLIGHT_MODE, "STAB");
 			break;

-		case vehicle_status_s::NAVIGATION_STATE_UNUSED2:
-			display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED2");
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 			display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_TAKEOFF");
 			break;
@@ -540,6 +540,10 @@
                            "name": "stab",
                            "description": "Stabilized"
                        },
+                        "9": {
+                            "name": "position_slow",
+                            "description": "Position Slow"
+                        },
                        "10": {
                            "name": "auto_takeoff",
                            "description": "Takeoff"
@@ -528,10 +528,10 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const
 		value = -1.f * value;
 	}

-	uint16_t effective_output = value * (_max_value[i] - _min_value[i]) / 2 + (_max_value[i] + _min_value[i]) / 2;
+	const float output = math::interpolate(value, -1.f, 1.f,
+					       static_cast<float>(_min_value[i]), static_cast<float>(_max_value[i]));

-	// last line of defense against invalid inputs
-	return math::constrain(effective_output, _min_value[i], _max_value[i]);
+	return math::constrain(lroundf(output), 0L, static_cast<long>(UINT16_MAX));
 }

 void
@@ -206,6 +206,7 @@ public:

 protected:
 	void updateParams() override;
+	uint16_t output_limit_calc_single(int i, float value) const;

 private:

@@ -224,8 +225,6 @@ private:

 	void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates);

-	uint16_t output_limit_calc_single(int i, float value) const;
-
 	void output_limit_calc(const bool armed, const int num_channels, const float outputs[MAX_ACTUATORS]);

 	struct ParamHandles {
@@ -50,12 +50,11 @@
 #define PARAM_PREFIX "HIL_ACT"
 #endif

-static constexpr int max_num_outputs = 8;
-
-static constexpr int disarmed_value = 900;
-static constexpr int failsafe_value = 800;
-static constexpr int min_value = 1000;
-static constexpr int max_value = 2000;
+static constexpr int MAX_NUM_OUTPUTS = 8;
+static constexpr int DISARMED_VALUE = 900;
+static constexpr int FAILSAFE_VALUE = 800;
+static constexpr int MIN_VALUE = 1000;
+static constexpr int MAX_VALUE = 2000;

 class MixerModuleTest : public ::testing::Test
 {
@@ -101,9 +100,9 @@ public:
 		mixer_changed = true;
 	}

-	void configureFunctions(const std::array<int32_t, max_num_outputs> &functions)
+	void configureFunctions(const std::array<int32_t, MAX_NUM_OUTPUTS> &functions)
 	{
-		for (int i = 0; i < max_num_outputs; ++i) {
+		for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 			char buffer[17];

 			snprintf(buffer, sizeof(buffer), "%s_FUNC%u", PARAM_PREFIX, i + 1);
@@ -185,11 +184,11 @@ TEST_F(MixerModuleTest, basic)
 {
 	OutputModuleTest test_module;
 	test_module.configureFunctions({});
-	MixingOutput mixing_output{PARAM_PREFIX, max_num_outputs, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
-	mixing_output.setAllDisarmedValues(disarmed_value);
-	mixing_output.setAllFailsafeValues(failsafe_value);
-	mixing_output.setAllMinValues(min_value);
-	mixing_output.setAllMaxValues(max_value);
+	MixingOutput mixing_output{PARAM_PREFIX, MAX_NUM_OUTPUTS, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
+	mixing_output.setAllDisarmedValues(DISARMED_VALUE);
+	mixing_output.setAllFailsafeValues(FAILSAFE_VALUE);
+	mixing_output.setAllMinValues(MIN_VALUE);
+	mixing_output.setAllMaxValues(MAX_VALUE);
 	EXPECT_EQ(test_module.num_updates, 0);

 	// all functions disabled: not expected to get an update
@@ -204,10 +203,10 @@ TEST_F(MixerModuleTest, basic)
 	mixing_output.updateSubscriptions(false);
 	EXPECT_TRUE(test_module.mixer_changed);
 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

 	for (int i = 0; i < test_module.num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], disarmed_value);
+		EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 	}

 	test_module.reset();
@@ -222,10 +221,10 @@ TEST_F(MixerModuleTest, basic)

 	mixing_output.updateSubscriptions(false);
 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

 	for (int i = 0; i < test_module.num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], disarmed_value);
+		EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 	}

 	test_module.reset();
@@ -236,14 +235,14 @@ TEST_F(MixerModuleTest, basic)
 	test_module.sendMotors({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f});
 	mixing_output.updateSubscriptions(false);
 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

 	for (int i = 0; i < test_module.num_outputs; ++i) {
 		if (i == 3) {
-			EXPECT_EQ(test_module.outputs[i], max_value);
+			EXPECT_EQ(test_module.outputs[i], MAX_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -254,10 +253,10 @@ TEST_F(MixerModuleTest, basic)
 	test_module.sendMotors({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f});
 	mixing_output.updateSubscriptions(false);
 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

 	for (int i = 0; i < test_module.num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], disarmed_value);
+		EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 	}

 	test_module.reset();
@@ -274,11 +273,11 @@ TEST_F(MixerModuleTest, arming)
 		(int)OutputFunction::Motor1,
 		(int)OutputFunction::Motor5,
 		(int)OutputFunction::Servo3});
-	MixingOutput mixing_output{PARAM_PREFIX, max_num_outputs, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
-	mixing_output.setAllDisarmedValues(disarmed_value);
-	mixing_output.setAllFailsafeValues(failsafe_value);
-	mixing_output.setAllMinValues(min_value);
-	mixing_output.setAllMaxValues(max_value);
+	MixingOutput mixing_output{PARAM_PREFIX, MAX_NUM_OUTPUTS, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
+	mixing_output.setAllDisarmedValues(DISARMED_VALUE);
+	mixing_output.setAllFailsafeValues(FAILSAFE_VALUE);
+	mixing_output.setAllMinValues(MIN_VALUE);
+	mixing_output.setAllMaxValues(MAX_VALUE);

 	test_module.sendMotors({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f});
 	test_module.sendActuatorArmed(false);
@@ -286,10 +285,10 @@ TEST_F(MixerModuleTest, arming)
 	// ensure all disarmed
 	mixing_output.updateSubscriptions(false);
 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

-	for (int i = 0; i < max_num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], disarmed_value);
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
+		EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 	}

 	test_module.reset();
@@ -299,20 +298,20 @@ TEST_F(MixerModuleTest, arming)
 	test_module.sendActuatorArmed(true);

 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

-	for (int i = 0; i < max_num_outputs; ++i) {
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 		if (i == 1) {
-			EXPECT_EQ(test_module.outputs[i], (max_value - min_value) * 0.1f + min_value);
+			EXPECT_EQ(test_module.outputs[i], (MAX_VALUE - MIN_VALUE) * 0.1f + MIN_VALUE);

 		} else if (i == 2) {
-			EXPECT_EQ(test_module.outputs[i], (max_value - min_value) * 0.5f + min_value);
+			EXPECT_EQ(test_module.outputs[i], (MAX_VALUE - MIN_VALUE) * 0.5f + MIN_VALUE);

 		} else if (i == 3) {
-			EXPECT_EQ(test_module.outputs[i], max_value);
+			EXPECT_EQ(test_module.outputs[i], MAX_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -323,18 +322,18 @@ TEST_F(MixerModuleTest, arming)
 	mixing_output.updateSubscriptions(false);
 	mixing_output.update();

-	for (int i = 0; i < max_num_outputs; ++i) {
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 		if (i == 1) {
-			EXPECT_EQ(test_module.outputs[i], (max_value - min_value) * 0.24f + min_value);
+			EXPECT_EQ(test_module.outputs[i], (MAX_VALUE - MIN_VALUE) * 0.24f + MIN_VALUE);

 		} else if (i == 2) {
-			EXPECT_EQ(test_module.outputs[i], (max_value - min_value) * 0.9f + min_value);
+			EXPECT_EQ(test_module.outputs[i], (MAX_VALUE - MIN_VALUE) * 0.9f + MIN_VALUE);

 		} else if (i == 3) {
-			EXPECT_EQ(test_module.outputs[i], min_value);
+			EXPECT_EQ(test_module.outputs[i], MIN_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -345,8 +344,8 @@ TEST_F(MixerModuleTest, arming)
 	test_module.sendMotors({0.5f, 1.f, 0.1f, 0.2f, 1.f, 1.f, 1.f, 1.f});
 	mixing_output.update();

-	for (int i = 0; i < max_num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], failsafe_value);
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
+		EXPECT_EQ(test_module.outputs[i], FAILSAFE_VALUE);
 	}

 	test_module.reset();
@@ -356,12 +355,12 @@ TEST_F(MixerModuleTest, arming)
 	test_module.sendMotors({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f});
 	mixing_output.update();

-	for (int i = 0; i < max_num_outputs; ++i) {
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 		if (i >= 1 && i <= 3) {
-			EXPECT_EQ(test_module.outputs[i], max_value);
+			EXPECT_EQ(test_module.outputs[i], MAX_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -372,8 +371,8 @@ TEST_F(MixerModuleTest, arming)
 	test_module.sendMotors({0.5f, 1.f, 0.1f, 0.2f, 1.f, 1.f, 1.f, 1.f});
 	mixing_output.update();

-	for (int i = 0; i < max_num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], disarmed_value);
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
+		EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 	}

 	test_module.reset();
@@ -383,12 +382,12 @@ TEST_F(MixerModuleTest, arming)
 	test_module.sendMotors({0.f, 0.f, 0.f, 0.f, 0.f, 1.f, 1.f, 1.f});
 	mixing_output.update();

-	for (int i = 0; i < max_num_outputs; ++i) {
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 		if (i >= 1 && i <= 3) {
-			EXPECT_EQ(test_module.outputs[i], min_value);
+			EXPECT_EQ(test_module.outputs[i], MIN_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -400,18 +399,18 @@ TEST_F(MixerModuleTest, arming)
 	mixing_output.update();
 	EXPECT_EQ(mixing_output.reversibleOutputs(), 1u << 3);

-	for (int i = 0; i < max_num_outputs; ++i) {
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 		if (i == 1) {
-			EXPECT_EQ(test_module.outputs[i], min_value);
+			EXPECT_EQ(test_module.outputs[i], MIN_VALUE);

 		} else if (i == 2) {
-			EXPECT_EQ(test_module.outputs[i], min_value);
+			EXPECT_EQ(test_module.outputs[i], MIN_VALUE);

 		} else if (i == 3) {
-			EXPECT_EQ(test_module.outputs[i], (max_value - min_value) * 0.5f + min_value);
+			EXPECT_EQ(test_module.outputs[i], (MAX_VALUE - MIN_VALUE) * 0.5f + MIN_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -422,8 +421,8 @@ TEST_F(MixerModuleTest, arming)
 	test_module.sendMotors({0.f, 0.f, 0.f, 0.f, 0.f, 1.f, 1.f, 1.f}, 1u << 4);
 	mixing_output.update();

-	for (int i = 0; i < max_num_outputs; ++i) {
-		EXPECT_EQ(test_module.outputs[i], disarmed_value);
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
+		EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 	}

 	test_module.reset();
@@ -437,11 +436,11 @@ TEST_F(MixerModuleTest, prearm)
 	test_module.configureFunctions({
 		(int)OutputFunction::Motor1,
 		(int)OutputFunction::Servo1});
-	MixingOutput mixing_output{PARAM_PREFIX, max_num_outputs, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
-	mixing_output.setAllDisarmedValues(disarmed_value);
-	mixing_output.setAllFailsafeValues(failsafe_value);
-	mixing_output.setAllMinValues(min_value);
-	mixing_output.setAllMaxValues(max_value);
+	MixingOutput mixing_output{PARAM_PREFIX, MAX_NUM_OUTPUTS, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
+	mixing_output.setAllDisarmedValues(DISARMED_VALUE);
+	mixing_output.setAllFailsafeValues(FAILSAFE_VALUE);
+	mixing_output.setAllMinValues(MIN_VALUE);
+	mixing_output.setAllMaxValues(MAX_VALUE);

 	test_module.sendMotors({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f});
 	test_module.sendServos({1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, 1.f});
@@ -450,14 +449,14 @@ TEST_F(MixerModuleTest, prearm)
 	// ensure all disarmed, except the servo
 	mixing_output.updateSubscriptions(false);
 	EXPECT_EQ(test_module.num_updates, update(mixing_output));
-	EXPECT_EQ(test_module.num_outputs, max_num_outputs);
+	EXPECT_EQ(test_module.num_outputs, MAX_NUM_OUTPUTS);

-	for (int i = 0; i < max_num_outputs; ++i) {
+	for (int i = 0; i < MAX_NUM_OUTPUTS; ++i) {
 		if (i == 1) {
-			EXPECT_EQ(test_module.outputs[i], max_value);
+			EXPECT_EQ(test_module.outputs[i], MAX_VALUE);

 		} else {
-			EXPECT_EQ(test_module.outputs[i], disarmed_value);
+			EXPECT_EQ(test_module.outputs[i], DISARMED_VALUE);
 		}
 	}

@@ -465,3 +464,66 @@ TEST_F(MixerModuleTest, prearm)

 	EXPECT_FALSE(test_module.was_scheduled);
 }
+
+class TestMixingOutput : public MixingOutput
+{
+public:
+	TestMixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
+			 SchedulingPolicy scheduling_policy,
+			 bool support_esc_calibration, bool ramp_up = true)
+		: MixingOutput(param_prefix, max_num_outputs, interface, scheduling_policy, support_esc_calibration, ramp_up)
+	{};
+	uint16_t output_limit_calc_single(int i, float value) const { return MixingOutput::output_limit_calc_single(i, value); }
+};
+
+TEST_F(MixerModuleTest, OutputLimitCalcSingle)
+{
+	OutputModuleTest test_module;
+	test_module.configureFunctions({(int)OutputFunction::Motor1});
+	TestMixingOutput mixing_output{PARAM_PREFIX, MAX_NUM_OUTPUTS, test_module, MixingOutput::SchedulingPolicy::Disabled, false, false};
+
+	mixing_output.setAllMinValues(MIN_VALUE); // default range [1000,2000]
+	mixing_output.setAllMaxValues(MAX_VALUE);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.f), 1000); // In range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -.5f), 1250);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.f), 1500);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, .5f), 1750);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.f), 2000);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.1f), 1000); // Out of range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.1f), 2000);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1000.f), 1000); // Way ouf of range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1000.f), 2000);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.0005), 1500); // Rounding down
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.0015), 1501); // Rounding up
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.002), 1501); // Exact value
+
+	mixing_output.setAllMinValues(0); // lower range [0,20]
+	mixing_output.setAllMaxValues(20);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.f), 0); // In range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -.5f), 5);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.f), 10);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, .5f), 15);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.f), 20);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.1f), 0); // Out of range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.1f), 20);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1000.f), 0); // Way ouf of range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1000.f), 20);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.025), 10); // Rounding down
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.075), 11); // Rounding up
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.1), 11); // Exact value
+
+	mixing_output.setAllMinValues(20); // inverted range [20,0]
+	mixing_output.setAllMaxValues(0);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.f), 20); // In range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -.5f), 15);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.f), 10);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, .5f), 5);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.f), 0);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1.1f), 20); // Out of range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1.1f), 0);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, -1000.f), 20); // Way ouf of range
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 1000.f), 0);
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.025), 10); // Rounding down
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.075), 9); // Rounding up
+	EXPECT_EQ(mixing_output.output_limit_calc_single(0, 0.1), 9); // Exact value
+}
@@ -51,6 +51,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
@@ -62,7 +63,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);

-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "update valid nav states");
 }

 const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
@@ -72,7 +73,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"Mission",
 	"Hold",
 	"Return",
-	"6: unallocated",
+	"Position Slow",
 	"7: unallocated",
 	"8: unallocated",
 	"9: unallocated",
@@ -47,58 +47,58 @@ static constexpr int LON_DIM = 37;
 // Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.9095,
+// Date: 2023.9315,
 static constexpr const int16_t declination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ {  25953, 24208, 22462, 20717, 18972, 17226, 15481, 13736, 11990, 10245,  8500,  6754,  5009,  3264,  1518,  -227, -1972, -3718, -5463, -7208, -8954,-10699,-12444,-14190,-15935,-17680,-19426,-21171,-22916,-24662,-26407,-28152,-29898, 31189, 29444, 27698, 25953, },
-	/* LAT: -80 */ {  22516, 20388, 18452, 16680, 15042, 13504, 12042, 10631,  9256,  7907,  6576,  5258,  3949,  2644,  1336,    16, -1327, -2701, -4113, -5567, -7065, -8604,-10184,-11805,-13472,-15192,-16979,-18854,-20839,-22959,-25236,-27671,-30239, 29956, 27342, 24841, 22516, },
-	/* LAT: -70 */ {  14987, 13586, 12457, 11491, 10619,  9784,  8940,  8051,  7097,  6077,  5007,  3915,  2831,  1778,   758,  -252, -1290, -2400, -3606, -4912, -6298, -7733, -9185,-10629,-12057,-13475,-14906,-16396,-18026,-19951,-22491,-26317, 30596, 24094, 19618, 16857, 14987, }, // WARNING! black out zone
-	/* LAT: -60 */ {   8456,  8203,  7915,  7634,  7375,  7116,  6804,  6367,  5748,  4925,  3922,  2811,  1694,   672,  -201,  -953, -1681, -2509, -3523, -4732, -6074, -7451, -8776, -9987,-11050,-11947,-12660,-13154,-13325,-12877,-10764, -3395,  5040,  7737,  8485,  8599,  8456, }, // WARNING! black out zone
-	/* LAT: -50 */ {   5512,  5546,  5485,  5390,  5311,  5270,  5232,  5101,  4752,  4084,  3068,  1793,   455,  -714, -1569, -2118, -2507, -2950, -3652, -4689, -5952, -7240, -8384, -9281, -9871,-10099, -9892, -9123, -7604, -5234, -2324,   426,  2537,  3962,  4837,  5310,  5512, },
-	/* LAT: -40 */ {   3974,  4066,  4070,  4020,  3955,  3918,  3920,  3906,  3728,  3187,  2154,   706,  -855, -2156, -2999, -3431, -3598, -3650, -3833, -4437, -5448, -6538, -7420, -7947, -8037, -7640, -6733, -5342, -3635, -1941,  -488,   731,  1778,  2648,  3309,  3742,  3974, },
-	/* LAT: -30 */ {   3000,  3086,  3111,  3091,  3028,  2945,  2882,  2846,  2717,  2232,  1183,  -347, -1952, -3190, -3909, -4235, -4296, -4065, -3627, -3441, -3852, -4620, -5311, -5643, -5497, -4891, -3921, -2717, -1522,  -590,    80,   678,  1305,  1916,  2432,  2799,  3000, },
-	/* LAT: -20 */ {   2358,  2402,  2414,  2409,  2361,  2263,  2151,  2072,  1926,  1423,   352, -1149, -2623, -3666, -4175, -4270, -4047, -3469, -2600, -1825, -1589, -1972, -2624, -3079, -3098, -2725, -2083, -1271,  -509,   -32,   222,   521,   967,  1455,  1886,  2200,  2358, },
-	/* LAT: -10 */ {   1964,  1957,  1929,  1919,  1885,  1795,  1679,  1585,  1399,   842,  -232, -1617, -2883, -3691, -3931, -3680, -3090, -2301, -1457,  -714,  -269,  -325,  -807, -1309, -1510, -1410, -1093,  -598,  -111,   119,   158,   313,   690,  1140,  1544,  1840,  1964, },
-	/* LAT:   0 */ {   1750,  1715,  1654,  1638,  1621,  1546,  1434,  1314,  1059,   434,  -613, -1842, -2885, -3449, -3422, -2903, -2129, -1349,  -704,  -173,   232,   329,    37,  -380,  -628,  -680,  -581,  -323,   -40,    39,   -32,    52,   400,   855,  1286,  1615,  1750, },
-	/* LAT:  10 */ {   1611,  1617,  1570,  1581,  1601,  1546,  1416,  1223,   846,   127,  -895, -1968, -2781, -3100, -2876, -2257, -1478,  -772,  -264,   113,   435,   575,   403,    80,  -150,  -258,  -285,  -210,  -112,  -162,  -310,  -287,    21,   488,   979,  1395,  1611, },
-	/* LAT:  20 */ {   1419,  1567,  1624,  1712,  1795,  1769,  1607,  1299,   754,  -101, -1141, -2084, -2666, -2759, -2417, -1808, -1097,  -452,     2,   307,   564,   704,   606,   362,   165,    45,   -53,  -126,  -213,  -409,  -652,  -712,  -472,   -15,   534,  1058,  1419, },
-	/* LAT:  30 */ {   1106,  1472,  1731,  1953,  2111,  2119,  1927,  1500,   773,  -253, -1362, -2226, -2627, -2553, -2147, -1564,  -908,  -294,   163,   465,   693,   835,   809,   659,   511,   384,   218,    -4,  -295,  -675, -1046, -1205, -1040,  -607,   -27,   584,  1106, },
-	/* LAT:  40 */ {    735,  1320,  1815,  2208,  2458,  2502,  2285,  1751,   842,  -378, -1603, -2454, -2763, -2603, -2154, -1561,  -907,  -279,   226,   587,   854,  1047,  1135,  1122,  1046,   894,   615,   190,  -360,  -970, -1490, -1730, -1608, -1188,  -590,    79,   735, },
-	/* LAT:  50 */ {    429,  1173,  1855,  2417,  2788,  2898,  2672,  2021,   888,  -593, -1996, -2896, -3182, -2988, -2497, -1851, -1141,  -446,   167,   669,  1084,  1434,  1708,  1874,  1889,  1694,  1236,   509,  -397, -1302, -1973, -2251, -2121, -1681, -1052,  -328,   429, },
-	/* LAT:  60 */ {    209,  1057,  1864,  2565,  3078,  3300,  3097,  2301,   812, -1115, -2806, -3777, -4029, -3772, -3203, -2460, -1637,  -800,     5,   753,  1442,  2068,  2606,  2995,  3146,  2945,  2285,  1145,  -284, -1600, -2456, -2754, -2586, -2098, -1417,  -629,   209, },
-	/* LAT:  70 */ {    -69,   860,  1755,  2558,  3181,  3489,  3263,  2180,    31, -2600, -4531, -5373, -5402, -4936, -4179, -3254, -2237, -1176,  -106,   950,  1972,  2934,  3798,  4499,  4936,  4952,  4314,  2801,   575, -1536, -2820, -3255, -3097, -2575, -1839,  -984,   -69, }, // WARNING! black out zone
-	/* LAT:  80 */ {   -916,     2,   850,  1541,  1936,  1794,   725, -1608, -4564, -6660, -7485, -7429, -6856, -5977, -4914, -3734, -2483, -1188,   129,  1452,  2762,  4042,  5269,  6409,  7407,  8169,  8510,  8043,  6014,  2018, -1692, -3386, -3716, -3369, -2685, -1835,  -916, }, // WARNING! black out zone
-	/* LAT:  90 */ { -29317,-27572,-25826,-24081,-22336,-20590,-18845,-17100,-15354,-13609,-11864,-10118, -8373, -6628, -4882, -3137, -1392,   354,  2099,  3844,  5590,  7335,  9080, 10826, 12571, 14316, 16062, 17807, 19552, 21298, 23043, 24788, 26534, 28279, 30024,-31062,-29317, }, // WARNING! black out zone
+	/* LAT: -90 */ {  25952, 24207, 22462, 20716, 18971, 17226, 15480, 13735, 11990, 10244,  8499,  6754,  5008,  3263,  1518,  -228, -1973, -3718, -5464, -7209, -8954,-10700,-12445,-14190,-15936,-17681,-19426,-21172,-22917,-24662,-26408,-28153,-29898, 31188, 29443, 27698, 25952, },
+	/* LAT: -80 */ {  22515, 20388, 18451, 16680, 15041, 13504, 12041, 10631,  9256,  7907,  6575,  5257,  3949,  2644,  1336,    15, -1327, -2701, -4113, -5568, -7065, -8604,-10184,-11806,-13472,-15192,-16980,-18855,-20840,-22960,-25236,-27672,-30240, 29955, 27341, 24840, 22515, },
+	/* LAT: -70 */ {  14987, 13586, 12457, 11491, 10619,  9784,  8940,  8050,  7096,  6077,  5006,  3914,  2831,  1778,   758,  -252, -1290, -2400, -3606, -4912, -6299, -7734, -9185,-10630,-12058,-13476,-14907,-16397,-18027,-19952,-22493,-26319, 30594, 24093, 19618, 16857, 14987, }, // WARNING! black out zone
+	/* LAT: -60 */ {   8456,  8203,  7915,  7635,  7375,  7116,  6804,  6367,  5748,  4925,  3922,  2811,  1694,   672,  -201,  -953, -1681, -2509, -3523, -4733, -6074, -7452, -8776, -9988,-11051,-11947,-12661,-13154,-13326,-12878,-10764, -3393,  5042,  7739,  8486,  8600,  8456, }, // WARNING! black out zone
+	/* LAT: -50 */ {   5513,  5546,  5485,  5391,  5312,  5270,  5232,  5101,  4752,  4084,  3068,  1793,   455,  -714, -1569, -2118, -2506, -2950, -3652, -4689, -5953, -7241, -8384, -9282, -9872,-10100, -9893, -9123, -7604, -5234, -2323,   426,  2537,  3963,  4837,  5311,  5513, },
+	/* LAT: -40 */ {   3975,  4066,  4070,  4020,  3955,  3918,  3920,  3906,  3728,  3186,  2154,   705,  -856, -2156, -2999, -3430, -3598, -3650, -3832, -4437, -5448, -6539, -7421, -7947, -8037, -7640, -6733, -5341, -3634, -1941,  -488,   732,  1778,  2649,  3309,  3743,  3975, },
+	/* LAT: -30 */ {   3001,  3086,  3112,  3091,  3028,  2945,  2882,  2846,  2717,  2231,  1182,  -348, -1953, -3190, -3909, -4235, -4296, -4064, -3626, -3441, -3853, -4621, -5312, -5643, -5497, -4891, -3920, -2717, -1522,  -590,    80,   678,  1305,  1916,  2432,  2799,  3001, },
+	/* LAT: -20 */ {   2358,  2403,  2415,  2409,  2361,  2262,  2150,  2072,  1926,  1423,   351, -1150, -2624, -3666, -4175, -4270, -4047, -3468, -2599, -1824, -1589, -1972, -2624, -3079, -3098, -2724, -2082, -1271,  -509,   -32,   222,   521,   967,  1455,  1886,  2200,  2358, },
+	/* LAT: -10 */ {   1964,  1957,  1930,  1919,  1885,  1795,  1679,  1584,  1398,   842,  -233, -1618, -2883, -3691, -3931, -3679, -3089, -2300, -1456,  -713,  -268,  -325,  -807, -1309, -1510, -1410, -1093,  -598,  -111,   118,   157,   313,   690,  1140,  1544,  1840,  1964, },
+	/* LAT:   0 */ {   1751,  1716,  1654,  1638,  1621,  1546,  1433,  1314,  1058,   434,  -614, -1842, -2885, -3449, -3422, -2902, -2128, -1349,  -703,  -173,   233,   330,    37,  -380,  -628,  -679,  -581,  -323,   -40,    39,   -33,    51,   399,   855,  1286,  1616,  1751, },
+	/* LAT:  10 */ {   1611,  1618,  1570,  1581,  1601,  1546,  1416,  1223,   845,   127,  -896, -1968, -2781, -3100, -2875, -2256, -1477,  -771,  -263,   113,   435,   575,   403,    80,  -150,  -258,  -285,  -210,  -112,  -162,  -310,  -288,    21,   487,   979,  1395,  1611, },
+	/* LAT:  20 */ {   1419,  1567,  1624,  1712,  1795,  1769,  1607,  1299,   753,  -101, -1141, -2084, -2666, -2758, -2417, -1807, -1096,  -451,     2,   307,   564,   704,   606,   362,   165,    45,   -53,  -126,  -213,  -409,  -652,  -713,  -473,   -15,   534,  1058,  1419, },
+	/* LAT:  30 */ {   1106,  1471,  1730,  1953,  2111,  2119,  1926,  1500,   772,  -253, -1362, -2226, -2627, -2552, -2146, -1564,  -908,  -293,   164,   465,   694,   835,   809,   659,   511,   384,   218,    -4,  -296,  -675, -1047, -1205, -1040,  -607,   -27,   584,  1106, },
+	/* LAT:  40 */ {    734,  1319,  1814,  2208,  2458,  2501,  2285,  1751,   841,  -378, -1603, -2454, -2762, -2603, -2154, -1560,  -906,  -279,   227,   587,   854,  1048,  1135,  1122,  1046,   894,   615,   190,  -360,  -971, -1491, -1730, -1608, -1188,  -590,    79,   734, },
+	/* LAT:  50 */ {    428,  1172,  1855,  2416,  2787,  2897,  2671,  2021,   888,  -593, -1996, -2895, -3182, -2987, -2496, -1850, -1140,  -445,   168,   670,  1084,  1434,  1708,  1874,  1889,  1695,  1235,   509,  -398, -1302, -1974, -2252, -2121, -1681, -1052,  -329,   428, },
+	/* LAT:  60 */ {    208,  1056,  1863,  2564,  3077,  3299,  3096,  2301,   812, -1114, -2805, -3776, -4028, -3771, -3202, -2459, -1636,  -799,     6,   754,  1443,  2069,  2607,  2996,  3146,  2945,  2285,  1144,  -285, -1601, -2457, -2755, -2587, -2099, -1417,  -630,   208, },
+	/* LAT:  70 */ {    -70,   858,  1754,  2556,  3179,  3487,  3262,  2180,    33, -2597, -4529, -5370, -5400, -4934, -4177, -3252, -2235, -1175,  -105,   951,  1973,  2935,  3799,  4500,  4937,  4952,  4314,  2801,   574, -1537, -2821, -3256, -3098, -2576, -1840,  -985,   -70, }, // WARNING! black out zone
+	/* LAT:  80 */ {   -920,    -2,   846,  1538,  1932,  1791,   724, -1605, -4558, -6653, -7479, -7425, -6852, -5974, -4911, -3732, -2481, -1186,   131,  1453,  2764,  4044,  5271,  6410,  7409,  8171,  8512,  8044,  6013,  2014, -1698, -3391, -3721, -3373, -2689, -1839,  -920, }, // WARNING! black out zone
+	/* LAT:  90 */ { -29309,-27563,-25818,-24073,-22327,-20582,-18837,-17091,-15346,-13601,-11855,-10110, -8365, -6619, -4874, -3129, -1383,   362,  2107,  3853,  5598,  7343,  9089, 10834, 12579, 14325, 16070, 17815, 19561, 21306, 23051, 24797, 26542, 28287, 30033,-31054,-29309, }, // WARNING! black out zone
 };
-static constexpr float WMM_DECLINATION_MIN_RAD = -3.106; // latitude: 90, longitude: 170
+static constexpr float WMM_DECLINATION_MIN_RAD = -3.105; // latitude: 90, longitude: 170
 static constexpr float WMM_DECLINATION_MAX_RAD = 3.119; // latitude: -90, longitude: 150


 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.9095,
+// Date: 2023.9315,
 static constexpr const int16_t inclination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
 	/* LAT: -90 */ { -12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563,-12563, },
-	/* LAT: -80 */ { -13645,-13511,-13351,-13170,-12977,-12775,-12572,-12372,-12182,-12006,-11850,-11716,-11606,-11520,-11456,-11415,-11394,-11397,-11424,-11478,-11562,-11677,-11822,-11996,-12195,-12412,-12642,-12875,-13103,-13315,-13501,-13650,-13753,-13803,-13799,-13744,-13645, },
-	/* LAT: -70 */ { -14092,-13773,-13454,-13131,-12800,-12457,-12103,-11747,-11405,-11098,-10847,-10665,-10554,-10501,-10487,-10489,-10493,-10500,-10522,-10577,-10686,-10862,-11110,-11427,-11802,-12221,-12670,-13135,-13601,-14053,-14469,-14811,-14996,-14938,-14706,-14408,-14092, }, // WARNING! black out zone
-	/* LAT: -60 */ { -13510,-13156,-12817,-12484,-12141,-11769,-11354,-10900,-10434,-10007, -9680, -9509, -9510, -9648, -9852,-10039,-10158,-10194,-10176,-10159,-10207,-10370,-10664,-11078,-11580,-12139,-12726,-13323,-13912,-14473,-14970,-15258,-15076,-14688,-14280,-13885,-13510, }, // WARNING! black out zone
-	/* LAT: -50 */ { -12492,-12149,-11817,-11494,-11171,-10824,-10425, -9955, -9427, -8909, -8525, -8408, -8617, -9083, -9645,-10150,-10498,-10648,-10606,-10446,-10304,-10317,-10548,-10973,-11521,-12116,-12702,-13239,-13683,-13979,-14085,-14011,-13807,-13523,-13193,-12844,-12492, },
-	/* LAT: -40 */ { -11239,-10888,-10538,-10192, -9853, -9515, -9156, -8732, -8213, -7651, -7234, -7207, -7681, -8513, -9433,-10254,-10902,-11322,-11443,-11258,-10909,-10644,-10651,-10945,-11414,-11920,-12362,-12678,-12833,-12837,-12748,-12612,-12435,-12204,-11917,-11588,-11239, },
-	/* LAT: -30 */ {  -9603, -9219, -8835, -8444, -8052, -7678, -7324, -6937, -6425, -5817, -5376, -5482, -6275, -7489, -8738, -9824,-10727,-11422,-11800,-11766,-11379,-10871,-10549,-10564,-10823,-11145,-11396,-11502,-11435,-11259,-11088,-10959,-10818,-10613,-10332, -9984, -9603, },
-	/* LAT: -20 */ {  -7374, -6924, -6500, -6070, -5626, -5198, -4811, -4404, -3842, -3160, -2727, -3020, -4164, -5792, -7415, -8768, -9816,-10573,-10990,-11002,-10620, -9998, -9445, -9213, -9270, -9434, -9570, -9581, -9405, -9134, -8946, -8869, -8774, -8571, -8255, -7839, -7374, },
-	/* LAT: -10 */ {  -4419, -3870, -3407, -2966, -2508, -2060, -1651, -1206,  -588,   108,   450,   -13, -1379, -3317, -5284, -6867, -7935, -8545, -8791, -8710, -8282, -7584, -6922, -6587, -6555, -6648, -6762, -6780, -6589, -6296, -6150, -6175, -6150, -5941, -5557, -5024, -4419, },
-	/* LAT:   0 */ {   -912,  -273,   203,   610,  1029,  1445,  1830,  2261,  2833,  3399,  3581,  3054,  1724,  -208, -2246, -3866, -4841, -5244, -5285, -5101, -4643, -3913, -3208, -2846, -2788, -2855, -2977, -3047, -2916, -2684, -2641, -2805, -2887, -2710, -2285, -1648,  -912, },
-	/* LAT:  10 */ {   2555,  3196,  3640,  3985,  4341,  4707,  5054,  5429,  5868,  6226,  6246,  5734,  4626,  3043,  1356,     5,  -770,  -992,  -886,  -640,  -215,   435,  1068,  1399,  1463,  1425,  1327,  1237,  1278,  1377,  1289,  1003,   791,   854,  1208,  1817,  2555, },
-	/* LAT:  20 */ {   5412,  5950,  6336,  6635,  6949,  7291,  7629,  7968,  8291,  8481,  8381,  7903,  7052,  5947,  4825,  3932,  3424,  3324,  3486,  3737,  4076,  4549,  5010,  5262,  5322,  5311,  5266,  5208,  5193,  5169,  4986,  4639,  4327,  4231,  4404,  4833,  5412, },
-	/* LAT:  30 */ {   7567,  7945,  8265,  8550,  8859,  9205,  9559,  9894, 10163, 10268, 10115,  9684,  9049,  8336,  7680,  7177,  6898,  6868,  7019,  7235,  7487,  7791,  8081,  8254,  8313,  8332,  8337,  8327,  8301,  8214,  7985,  7624,  7261,  7037,  7021,  7219,  7567, },
-	/* LAT:  40 */ {   9266,  9487,  9744, 10030, 10356, 10715, 11080, 11414, 11661, 11738, 11582, 11219, 10745, 10273,  9881,  9603,  9461,  9464,  9581,  9747,  9926, 10113, 10286, 10411, 10490, 10554, 10612, 10646, 10626, 10511, 10264,  9909,  9538,  9251,  9109,  9122,  9266, },
-	/* LAT:  50 */ {  10802, 10922, 11123, 11392, 11713, 12065, 12417, 12730, 12949, 13004, 12863, 12565, 12202, 11859, 11587, 11404, 11316, 11317, 11387, 11494, 11610, 11728, 11846, 11962, 12082, 12206, 12322, 12394, 12381, 12252, 12002, 11672, 11331, 11046, 10857, 10776, 10802, },
-	/* LAT:  60 */ {  12320, 12391, 12539, 12754, 13022, 13320, 13621, 13885, 14059, 14086, 13951, 13704, 13417, 13148, 12930, 12776, 12688, 12659, 12678, 12727, 12797, 12884, 12992, 13126, 13286, 13463, 13628, 13735, 13737, 13614, 13387, 13107, 12829, 12593, 12423, 12331, 12320, },
-	/* LAT:  70 */ {  13757, 13797, 13889, 14028, 14205, 14406, 14612, 14792, 14897, 14881, 14748, 14550, 14334, 14132, 13960, 13826, 13733, 13680, 13662, 13676, 13720, 13793, 13898, 14036, 14204, 14392, 14576, 14715, 14758, 14682, 14520, 14321, 14127, 13962, 13842, 13772, 13757, }, // WARNING! black out zone
-	/* LAT:  80 */ {  14991, 15002, 15037, 15094, 15168, 15251, 15328, 15373, 15361, 15288, 15180, 15059, 14938, 14825, 14727, 14645, 14584, 14543, 14525, 14530, 14557, 14607, 14679, 14773, 14885, 15012, 15148, 15281, 15389, 15431, 15388, 15298, 15201, 15115, 15049, 15007, 14991, }, // WARNING! black out zone
-	/* LAT:  90 */ {  15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, }, // WARNING! black out zone
+	/* LAT: -80 */ { -13645,-13511,-13350,-13170,-12977,-12775,-12572,-12372,-12182,-12006,-11850,-11716,-11606,-11520,-11456,-11414,-11394,-11397,-11424,-11478,-11562,-11677,-11822,-11996,-12195,-12412,-12642,-12875,-13102,-13315,-13501,-13650,-13753,-13803,-13799,-13744,-13645, },
+	/* LAT: -70 */ { -14092,-13773,-13454,-13131,-12800,-12457,-12103,-11746,-11405,-11098,-10847,-10665,-10554,-10501,-10487,-10488,-10493,-10500,-10522,-10577,-10686,-10862,-11110,-11427,-11802,-12221,-12670,-13135,-13601,-14053,-14469,-14811,-14996,-14938,-14706,-14408,-14092, }, // WARNING! black out zone
+	/* LAT: -60 */ { -13510,-13156,-12817,-12484,-12141,-11769,-11354,-10900,-10434,-10007, -9680, -9509, -9510, -9649, -9852,-10040,-10158,-10194,-10176,-10159,-10207,-10370,-10664,-11078,-11581,-12139,-12726,-13323,-13912,-14473,-14971,-15259,-15076,-14688,-14280,-13885,-13510, }, // WARNING! black out zone
+	/* LAT: -50 */ { -12492,-12149,-11817,-11494,-11170,-10824,-10425, -9955, -9427, -8909, -8525, -8408, -8618, -9083, -9646,-10150,-10498,-10648,-10606,-10446,-10304,-10316,-10548,-10973,-11521,-12116,-12702,-13239,-13683,-13979,-14085,-14011,-13807,-13523,-13193,-12844,-12492, },
+	/* LAT: -40 */ { -11239,-10888,-10538,-10192, -9853, -9515, -9156, -8732, -8213, -7652, -7234, -7207, -7682, -8513, -9433,-10255,-10903,-11323,-11443,-11258,-10908,-10643,-10651,-10945,-11415,-11920,-12362,-12678,-12833,-12837,-12748,-12612,-12435,-12204,-11917,-11588,-11239, },
+	/* LAT: -30 */ {  -9603, -9219, -8835, -8443, -8051, -7678, -7324, -6937, -6425, -5817, -5376, -5482, -6276, -7490, -8739, -9825,-10728,-11423,-11800,-11766,-11378,-10870,-10549,-10564,-10823,-11145,-11396,-11502,-11435,-11259,-11088,-10959,-10818,-10613,-10332, -9985, -9603, },
+	/* LAT: -20 */ {  -7374, -6924, -6500, -6069, -5625, -5197, -4811, -4404, -3842, -3160, -2727, -3020, -4165, -5794, -7417, -8770, -9817,-10574,-10990,-11002,-10620, -9997, -9445, -9213, -9270, -9434, -9570, -9581, -9405, -9134, -8946, -8869, -8774, -8571, -8255, -7839, -7374, },
+	/* LAT: -10 */ {  -4419, -3870, -3407, -2966, -2508, -2059, -1651, -1206,  -588,   108,   450,   -14, -1381, -3318, -5286, -6868, -7936, -8546, -8791, -8710, -8282, -7584, -6922, -6587, -6554, -6648, -6761, -6780, -6589, -6295, -6150, -6175, -6150, -5941, -5557, -5024, -4419, },
+	/* LAT:   0 */ {   -912,  -273,   203,   610,  1030,  1446,  1830,  2261,  2833,  3399,  3581,  3053,  1722,  -210, -2248, -3868, -4842, -5244, -5285, -5101, -4643, -3912, -3207, -2845, -2788, -2855, -2976, -3047, -2916, -2684, -2641, -2805, -2888, -2711, -2286, -1648,  -912, },
+	/* LAT:  10 */ {   2555,  3196,  3640,  3985,  4342,  4708,  5054,  5429,  5868,  6226,  6246,  5733,  4625,  3041,  1354,     4,  -770,  -993,  -886,  -640,  -215,   436,  1069,  1400,  1463,  1425,  1328,  1237,  1278,  1377,  1290,  1003,   791,   854,  1207,  1817,  2555, },
+	/* LAT:  20 */ {   5412,  5950,  6336,  6635,  6949,  7291,  7629,  7968,  8290,  8481,  8381,  7903,  7051,  5946,  4824,  3932,  3424,  3324,  3486,  3737,  4076,  4549,  5010,  5263,  5322,  5311,  5267,  5208,  5193,  5170,  4986,  4639,  4327,  4231,  4404,  4832,  5412, },
+	/* LAT:  30 */ {   7567,  7945,  8266,  8550,  8859,  9205,  9559,  9894, 10163, 10268, 10114,  9684,  9048,  8335,  7679,  7177,  6897,  6868,  7019,  7236,  7487,  7791,  8081,  8254,  8314,  8332,  8338,  8328,  8302,  8214,  7985,  7624,  7260,  7037,  7021,  7219,  7567, },
+	/* LAT:  40 */ {   9266,  9487,  9744, 10030, 10356, 10715, 11080, 11414, 11661, 11737, 11582, 11218, 10744, 10273,  9881,  9603,  9461,  9464,  9581,  9747,  9926, 10113, 10286, 10411, 10490, 10554, 10612, 10646, 10626, 10511, 10264,  9909,  9538,  9251,  9109,  9122,  9266, },
+	/* LAT:  50 */ {  10802, 10922, 11123, 11392, 11713, 12065, 12417, 12730, 12948, 13004, 12863, 12565, 12202, 11859, 11587, 11404, 11315, 11317, 11387, 11494, 11610, 11729, 11846, 11962, 12082, 12207, 12322, 12394, 12382, 12252, 12002, 11672, 11331, 11046, 10857, 10776, 10802, },
+	/* LAT:  60 */ {  12320, 12391, 12539, 12754, 13022, 13320, 13620, 13884, 14059, 14086, 13951, 13704, 13417, 13148, 12929, 12776, 12688, 12659, 12678, 12728, 12798, 12885, 12992, 13126, 13286, 13464, 13628, 13736, 13738, 13614, 13387, 13107, 12829, 12593, 12423, 12331, 12320, },
+	/* LAT:  70 */ {  13757, 13797, 13889, 14028, 14205, 14406, 14612, 14792, 14897, 14881, 14748, 14550, 14334, 14132, 13960, 13826, 13733, 13680, 13662, 13676, 13720, 13793, 13898, 14036, 14204, 14392, 14576, 14715, 14758, 14683, 14520, 14321, 14127, 13962, 13842, 13772, 13757, }, // WARNING! black out zone
+	/* LAT:  80 */ {  14991, 15002, 15037, 15094, 15168, 15250, 15327, 15373, 15360, 15288, 15180, 15059, 14938, 14825, 14727, 14645, 14584, 14544, 14525, 14530, 14557, 14607, 14679, 14773, 14885, 15013, 15149, 15281, 15389, 15431, 15388, 15298, 15201, 15115, 15049, 15007, 14991, }, // WARNING! black out zone
+	/* LAT:  90 */ {  15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, }, // WARNING! black out zone
 };
 static constexpr float WMM_INCLINATION_MIN_RAD = -1.526; // latitude: -60, longitude: 130
 static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitude: 110
@@ -107,30 +107,32 @@ static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitu
 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.9095,
+// Date: 2023.9315,
 static constexpr const int16_t strength_table[19][37] {
 	//    LONGITUDE:  -180, -170, -160, -150, -140, -130, -120, -110, -100,  -90,  -80,  -70,  -60,  -50,  -40,  -30,  -20,  -10,    0,   10,   20,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130,  140,  150,  160,  170,  180,
 	/* LAT: -90 */ {  5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, 5443, },
-	/* LAT: -80 */ {  6050, 5986, 5906, 5813, 5710, 5598, 5479, 5358, 5235, 5115, 5001, 4896, 4803, 4724, 4661, 4616, 4592, 4591, 4613, 4660, 4731, 4825, 4941, 5073, 5217, 5367, 5517, 5661, 5792, 5905, 5997, 6066, 6110, 6128, 6124, 6097, 6050, },
-	/* LAT: -70 */ {  6295, 6160, 6008, 5842, 5663, 5470, 5265, 5050, 4831, 4616, 4414, 4232, 4075, 3945, 3841, 3765, 3719, 3707, 3736, 3814, 3943, 4125, 4354, 4623, 4919, 5227, 5531, 5815, 6064, 6268, 6418, 6513, 6553, 6544, 6494, 6408, 6295, },
-	/* LAT: -60 */ {  6180, 5987, 5783, 5573, 5353, 5118, 4861, 4583, 4291, 4002, 3736, 3510, 3331, 3198, 3099, 3026, 2975, 2955, 2982, 3074, 3245, 3499, 3828, 4216, 4640, 5074, 5496, 5879, 6203, 6451, 6613, 6690, 6691, 6626, 6511, 6358, 6180, },
-	/* LAT: -50 */ {  5839, 5607, 5374, 5142, 4908, 4663, 4391, 4088, 3759, 3428, 3127, 2889, 2729, 2639, 2591, 2556, 2521, 2494, 2499, 2573, 2750, 3045, 3445, 3919, 4427, 4933, 5407, 5825, 6163, 6403, 6540, 6579, 6535, 6423, 6260, 6060, 5839, },
-	/* LAT: -40 */ {  5390, 5142, 4896, 4655, 4420, 4182, 3927, 3642, 3328, 3003, 2705, 2484, 2369, 2345, 2364, 2383, 2386, 2372, 2358, 2388, 2523, 2805, 3229, 3747, 4294, 4817, 5282, 5668, 5959, 6146, 6233, 6234, 6163, 6031, 5849, 5630, 5390, },
-	/* LAT: -30 */ {  4876, 4634, 4393, 4158, 3931, 3712, 3492, 3261, 3005, 2729, 2472, 2292, 2224, 2251, 2317, 2388, 2451, 2499, 2518, 2529, 2601, 2807, 3177, 3670, 4203, 4698, 5113, 5426, 5627, 5726, 5752, 5724, 5644, 5511, 5331, 5113, 4876, },
-	/* LAT: -20 */ {  4320, 4105, 3895, 3690, 3493, 3310, 3141, 2977, 2799, 2600, 2409, 2277, 2240, 2286, 2376, 2487, 2614, 2740, 2824, 2857, 2883, 2985, 3233, 3623, 4075, 4501, 4846, 5077, 5179, 5186, 5156, 5109, 5027, 4899, 4732, 4534, 4320, },
-	/* LAT: -10 */ {  3789, 3627, 3473, 3326, 3190, 3070, 2966, 2873, 2774, 2658, 2536, 2438, 2396, 2424, 2513, 2643, 2798, 2955, 3075, 3134, 3148, 3178, 3307, 3564, 3893, 4215, 4477, 4636, 4669, 4616, 4548, 4485, 4397, 4273, 4123, 3958, 3789, },
-	/* LAT:   0 */ {  3412, 3318, 3233, 3159, 3104, 3065, 3038, 3017, 2991, 2942, 2863, 2769, 2691, 2664, 2710, 2814, 2947, 3081, 3194, 3267, 3297, 3320, 3398, 3560, 3771, 3985, 4164, 4268, 4271, 4203, 4115, 4023, 3912, 3780, 3646, 3521, 3412, },
-	/* LAT:  10 */ {  3282, 3250, 3229, 3224, 3248, 3295, 3349, 3400, 3433, 3422, 3353, 3239, 3114, 3023, 3000, 3044, 3126, 3225, 3325, 3409, 3472, 3535, 3625, 3746, 3883, 4023, 4142, 4211, 4211, 4148, 4038, 3894, 3732, 3573, 3437, 3339, 3282, },
-	/* LAT:  20 */ {  3399, 3400, 3425, 3478, 3569, 3689, 3816, 3931, 4011, 4022, 3948, 3807, 3642, 3505, 3433, 3423, 3460, 3534, 3632, 3729, 3821, 3920, 4032, 4144, 4255, 4368, 4471, 4536, 4545, 4485, 4346, 4143, 3915, 3705, 3541, 3439, 3399, },
-	/* LAT:  30 */ {  3722, 3727, 3779, 3877, 4018, 4188, 4362, 4517, 4624, 4650, 4577, 4423, 4236, 4075, 3973, 3929, 3935, 3988, 4076, 4175, 4274, 4381, 4498, 4616, 4736, 4864, 4986, 5072, 5097, 5037, 4879, 4638, 4363, 4108, 3908, 3779, 3722, },
-	/* LAT:  40 */ {  4222, 4217, 4279, 4400, 4565, 4751, 4934, 5090, 5194, 5220, 5152, 5005, 4821, 4650, 4524, 4451, 4427, 4452, 4514, 4595, 4682, 4780, 4896, 5029, 5181, 5344, 5496, 5605, 5642, 5585, 5428, 5189, 4915, 4656, 4445, 4298, 4222, },
-	/* LAT:  50 */ {  4832, 4821, 4873, 4980, 5124, 5283, 5432, 5554, 5628, 5638, 5576, 5451, 5290, 5129, 4993, 4898, 4845, 4835, 4861, 4913, 4983, 5075, 5195, 5345, 5521, 5706, 5871, 5988, 6030, 5983, 5851, 5655, 5432, 5217, 5037, 4906, 4832, },
-	/* LAT:  60 */ {  5393, 5376, 5401, 5460, 5544, 5638, 5726, 5793, 5828, 5821, 5768, 5676, 5557, 5430, 5312, 5217, 5152, 5120, 5119, 5147, 5204, 5292, 5410, 5558, 5725, 5893, 6039, 6141, 6183, 6159, 6076, 5951, 5806, 5665, 5542, 5449, 5393, },
-	/* LAT:  70 */ {  5726, 5703, 5698, 5708, 5729, 5754, 5779, 5794, 5795, 5778, 5740, 5685, 5616, 5541, 5468, 5405, 5358, 5331, 5326, 5347, 5392, 5462, 5554, 5663, 5780, 5894, 5992, 6064, 6102, 6105, 6076, 6023, 5956, 5886, 5820, 5766, 5726, },
-	/* LAT:  80 */ {  5790, 5772, 5756, 5743, 5733, 5723, 5713, 5702, 5688, 5670, 5649, 5625, 5599, 5572, 5548, 5528, 5515, 5511, 5516, 5532, 5557, 5593, 5636, 5684, 5733, 5781, 5823, 5857, 5881, 5894, 5896, 5889, 5874, 5855, 5833, 5811, 5790, },
+	/* LAT: -80 */ {  6050, 5985, 5906, 5813, 5710, 5598, 5479, 5357, 5235, 5115, 5001, 4896, 4803, 4723, 4661, 4616, 4592, 4591, 4613, 4659, 4731, 4825, 4940, 5073, 5217, 5367, 5517, 5660, 5791, 5905, 5997, 6066, 6110, 6128, 6123, 6097, 6050, },
+	/* LAT: -70 */ {  6294, 6160, 6008, 5842, 5663, 5470, 5264, 5049, 4831, 4616, 4414, 4232, 4075, 3945, 3841, 3765, 3718, 3707, 3736, 3813, 3943, 4124, 4354, 4623, 4919, 5227, 5531, 5815, 6064, 6268, 6418, 6513, 6553, 6544, 6493, 6408, 6294, },
+	/* LAT: -60 */ {  6180, 5986, 5783, 5573, 5353, 5118, 4861, 4583, 4291, 4002, 3736, 3509, 3331, 3197, 3099, 3025, 2975, 2955, 2982, 3074, 3245, 3499, 3828, 4216, 4640, 5074, 5496, 5879, 6203, 6451, 6613, 6690, 6691, 6626, 6511, 6358, 6180, },
+	/* LAT: -50 */ {  5839, 5607, 5373, 5141, 4908, 4662, 4391, 4088, 3759, 3428, 3127, 2889, 2729, 2639, 2591, 2556, 2521, 2493, 2499, 2573, 2750, 3045, 3445, 3919, 4427, 4933, 5407, 5825, 6163, 6403, 6540, 6579, 6535, 6423, 6260, 6060, 5839, },
+	/* LAT: -40 */ {  5389, 5142, 4896, 4655, 4420, 4182, 3927, 3642, 3328, 3003, 2705, 2484, 2369, 2345, 2364, 2383, 2386, 2372, 2358, 2388, 2523, 2805, 3229, 3747, 4295, 4818, 5282, 5668, 5959, 6146, 6233, 6234, 6163, 6031, 5849, 5629, 5389, },
+	/* LAT: -30 */ {  4876, 4634, 4393, 4158, 3931, 3711, 3492, 3261, 3005, 2729, 2472, 2291, 2224, 2251, 2317, 2388, 2451, 2499, 2518, 2529, 2600, 2807, 3177, 3670, 4203, 4698, 5113, 5426, 5627, 5726, 5752, 5724, 5644, 5511, 5331, 5113, 4876, },
+	/* LAT: -20 */ {  4320, 4105, 3895, 3690, 3493, 3310, 3141, 2977, 2799, 2600, 2409, 2277, 2240, 2286, 2376, 2487, 2614, 2740, 2824, 2857, 2883, 2985, 3233, 3623, 4076, 4501, 4846, 5077, 5179, 5186, 5156, 5109, 5027, 4899, 4732, 4534, 4320, },
+	/* LAT: -10 */ {  3789, 3627, 3473, 3326, 3190, 3070, 2966, 2873, 2774, 2658, 2536, 2438, 2396, 2424, 2513, 2643, 2799, 2955, 3075, 3134, 3148, 3178, 3307, 3564, 3893, 4215, 4477, 4636, 4669, 4616, 4548, 4485, 4397, 4273, 4123, 3958, 3789, },
+	/* LAT:   0 */ {  3412, 3318, 3233, 3159, 3103, 3065, 3038, 3017, 2991, 2942, 2862, 2769, 2691, 2664, 2710, 2814, 2947, 3081, 3194, 3267, 3297, 3320, 3398, 3560, 3771, 3985, 4164, 4268, 4271, 4203, 4115, 4023, 3912, 3780, 3646, 3521, 3412, },
+	/* LAT:  10 */ {  3282, 3250, 3229, 3224, 3248, 3295, 3349, 3400, 3433, 3422, 3353, 3239, 3114, 3023, 3000, 3044, 3126, 3225, 3325, 3409, 3472, 3535, 3625, 3746, 3883, 4023, 4142, 4211, 4212, 4149, 4038, 3894, 3732, 3573, 3437, 3339, 3282, },
+	/* LAT:  20 */ {  3399, 3400, 3425, 3478, 3568, 3688, 3816, 3931, 4010, 4022, 3948, 3807, 3641, 3505, 3433, 3423, 3460, 3535, 3632, 3729, 3821, 3920, 4032, 4145, 4255, 4368, 4471, 4536, 4545, 4485, 4346, 4143, 3915, 3705, 3541, 3439, 3399, },
+	/* LAT:  30 */ {  3722, 3726, 3779, 3876, 4018, 4188, 4362, 4517, 4624, 4650, 4577, 4422, 4236, 4074, 3972, 3929, 3935, 3988, 4076, 4175, 4274, 4381, 4498, 4616, 4736, 4865, 4986, 5072, 5097, 5037, 4879, 4638, 4363, 4108, 3908, 3779, 3722, },
+	/* LAT:  40 */ {  4222, 4217, 4279, 4400, 4565, 4751, 4933, 5089, 5194, 5220, 5152, 5005, 4821, 4649, 4524, 4451, 4427, 4452, 4514, 4595, 4682, 4780, 4896, 5029, 5181, 5344, 5496, 5605, 5642, 5585, 5428, 5189, 4915, 4656, 4445, 4298, 4222, },
+	/* LAT:  50 */ {  4832, 4821, 4873, 4980, 5124, 5282, 5432, 5554, 5628, 5638, 5576, 5451, 5290, 5129, 4993, 4898, 4845, 4835, 4861, 4913, 4983, 5075, 5195, 5345, 5521, 5706, 5872, 5988, 6030, 5983, 5851, 5655, 5432, 5217, 5037, 4906, 4832, },
+	/* LAT:  60 */ {  5393, 5376, 5400, 5460, 5544, 5638, 5725, 5793, 5828, 5821, 5768, 5676, 5557, 5430, 5312, 5218, 5152, 5120, 5119, 5147, 5205, 5292, 5411, 5558, 5725, 5893, 6039, 6141, 6183, 6159, 6076, 5951, 5807, 5665, 5542, 5449, 5393, },
+	/* LAT:  70 */ {  5726, 5703, 5698, 5708, 5728, 5754, 5778, 5794, 5795, 5778, 5740, 5685, 5616, 5541, 5468, 5405, 5358, 5331, 5327, 5347, 5392, 5462, 5554, 5663, 5780, 5894, 5992, 6064, 6102, 6105, 6076, 6023, 5957, 5886, 5820, 5766, 5726, },
+	/* LAT:  80 */ {  5790, 5772, 5756, 5743, 5733, 5723, 5713, 5702, 5688, 5670, 5649, 5625, 5599, 5572, 5548, 5529, 5515, 5511, 5516, 5532, 5558, 5593, 5636, 5684, 5733, 5781, 5823, 5858, 5881, 5894, 5896, 5889, 5874, 5855, 5833, 5811, 5790, },
 	/* LAT:  90 */ {  5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, },
 };
 static constexpr float WMM_STRENGTH_MIN_GS = 22.2; // latitude: -30, longitude: -60
 static constexpr float WMM_STRENGTH_MAX_GS = 66.9; // latitude: -60, longitude: 140
 static constexpr float WMM_STRENGTH_MEAN_GS = 46.4;
 static constexpr float WMM_STRENGTH_MEDIAN_GS = 48.8;
+
+
@@ -376,6 +376,10 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "posctl")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL);

+			} else if (!strcmp(argv[1], "position:slow")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL,
+						     PX4_CUSTOM_SUB_MODE_POSCTL_SLOW);
+
 			} else if (!strcmp(argv[1], "auto:mission")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_MISSION);
@@ -780,7 +784,16 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;

 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
-					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+					switch (custom_sub_mode) {
+					default:
+					case PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL:
+						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+						break;
+
+					case PX4_CUSTOM_SUB_MODE_POSCTL_SLOW:
+						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
+						break;
+					}

 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					if (custom_sub_mode > 0) {
@@ -2969,7 +2982,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("land");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
-	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("lockdown");
@@ -72,6 +72,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		break;

 	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW:
 		vehicle_control_mode.flag_control_manual_enabled = true;
 		vehicle_control_mode.flag_control_position_enabled = true;
 		vehicle_control_mode.flag_control_velocity_enabled = true;
@@ -58,6 +58,8 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)

 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;

+	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW: return navigation_mode_t::position_slow;
+
 	case vehicle_status_s::NAVIGATION_STATE_ACRO: return navigation_mode_t::acro;

 	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: return navigation_mode_t::offboard;
@@ -93,7 +95,7 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8;
 	}

-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update navigation mode map");

 	return navigation_mode_t::unknown;
 }
@@ -75,8 +75,16 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_manual_control);

+	// NAVIGATION_STATE_POSITION_SLOW
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_position_relaxed);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_manual_control);
+
 	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
+		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_global_position);
 	}

 	// NAVIGATION_STATE_AUTO_MISSION
@@ -27,6 +27,7 @@ parameters:
                0: Manual
                1: Altitude
                2: Position
+                9: Position Slow
                3: Mission
                4: Hold
                10: Takeoff
@@ -77,7 +77,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {

 enum PX4_CUSTOM_SUB_MODE_POSCTL {
 	PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
-	PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
+	PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT,
+	PX4_CUSTOM_SUB_MODE_POSCTL_SLOW
 };

 union px4_custom_mode {
@@ -115,6 +116,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 		break;

+	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_SLOW;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
@@ -43,6 +43,7 @@ list(APPEND flight_tasks_all
 	Descend
 	Failsafe
 	ManualAcceleration
+	ManualAccelerationSlow
 	ManualAltitude
 	ManualAltitudeSmoothVel
 	ManualPosition
@@ -196,6 +196,13 @@ void FlightModeManager::start_flight_task()
 		}
 	}

+	// position slow mode
+	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) {
+		found_some_task = true;
+		FlightTaskError error = switchTask(FlightTaskIndex::ManualAccelerationSlow);
+		task_failure = error != FlightTaskError::NoError;
+	}
+
 	// Manual position control
 	if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) || task_failure) {
 		found_some_task = true;
@@ -31,13 +31,6 @@
 *
 ****************************************************************************/

-/**
- * @file FlightTaskManualPosition.hpp
- *
- * Flight task for manual position controlled mode.
- *
- */
-
 #pragma once

 #include "FlightTaskManualAltitudeSmoothVel.hpp"
@@ -53,7 +46,7 @@ public:
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;

-private:
+protected:
 	void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023 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
@@ -31,21 +31,9 @@
 #
 ############################################################################

-add_subdirectory(broadcom)
-add_subdirectory(cm8jl65)
-add_subdirectory(leddar_one)
-add_subdirectory(ll40ls)
-add_subdirectory(ll40ls_pwm)
-add_subdirectory(mappydot)
-add_subdirectory(mb12xx)
-add_subdirectory(pga460)
-add_subdirectory(lightware_laser_i2c)
-add_subdirectory(lightware_laser_serial)
-add_subdirectory(srf02)
-add_subdirectory(teraranger)
-add_subdirectory(tfmini)
-add_subdirectory(ulanding_radar)
-add_subdirectory(vl53l0x)
-add_subdirectory(vl53l1x)
-add_subdirectory(gy_us42)
-add_subdirectory(tf02pro)
+px4_add_library(FlightTaskManualAccelerationSlow
+	FlightTaskManualAccelerationSlow.cpp
+)
+target_include_directories(FlightTaskManualAccelerationSlow PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(FlightTaskManualAccelerationSlow PUBLIC FlightTaskManualAcceleration FlightTaskUtility)
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskManualAccelerationSlow.cpp
+ */
+
+#include "FlightTaskManualAccelerationSlow.hpp"
+#include <px4_platform_common/events.h>
+
+using namespace time_literals;
+
+bool FlightTaskManualAccelerationSlow::update()
+{
+	// Used to apply a configured default slowdown if neither MAVLink nor remote knob commands limits
+	bool velocity_horizontal_limited = false;
+	bool velocity_vertical_limited = false;
+	bool yaw_rate_limited = false;
+
+	// Limits which can only slow down from the nominal configuration we initialize with here
+	// This is ensured by the executing classes
+	float velocity_horizontal = _param_mpc_vel_manual.get();
+	float velocity_up = _param_mpc_z_vel_max_up.get();
+	float velocity_down = _param_mpc_z_vel_max_dn.get();
+	float yaw_rate = math::radians(_param_mpc_man_y_max.get());
+
+	// MAVLink commanded limits
+	if (_velocity_limits_sub.update(&_velocity_limits)) {
+		_velocity_limits_received_before = true;
+	}
+
+	if (_velocity_limits_received_before) {
+		// message received once since mode was started
+		if (PX4_ISFINITE(_velocity_limits.horizontal_velocity)) {
+			velocity_horizontal = fmaxf(_velocity_limits.horizontal_velocity, _param_mc_slow_min_hvel.get());
+			velocity_horizontal_limited = true;
+		}
+
+		if (PX4_ISFINITE(_velocity_limits.vertical_velocity)) {
+			velocity_up = velocity_down = fmaxf(_velocity_limits.vertical_velocity, _param_mc_slow_min_vvel.get());
+			velocity_vertical_limited = true;
+		}
+
+		if (PX4_ISFINITE(_velocity_limits.yaw_rate)) {
+			yaw_rate = fmaxf(_velocity_limits.yaw_rate, math::radians(_param_mc_slow_min_yawr.get()));
+			yaw_rate_limited = true;
+		}
+	}
+
+	// Remote knob commanded limits
+	if (_param_mc_slow_map_hvel.get() != 0) {
+		const float min_horizontal_velocity_scale = _param_mc_slow_min_hvel.get() / fmaxf(velocity_horizontal, FLT_EPSILON);
+		const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_hvel.get());
+		const float aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_horizontal_velocity_scale, 1.f);
+		velocity_horizontal *= aux_based_scale;
+		velocity_horizontal_limited = true;
+	}
+
+	if (_param_mc_slow_map_vvel.get() != 0) {
+		const float min_up_speed_scale = _param_mc_slow_min_vvel.get() / fmaxf(velocity_up, FLT_EPSILON);
+		const float min_down_speed_scale = _param_mc_slow_min_vvel.get() / fmaxf(velocity_down, FLT_EPSILON);
+		const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_vvel.get());
+		const float up_aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_up_speed_scale, 1.f);
+		const float down_aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_down_speed_scale, 1.f);
+		velocity_up *= up_aux_based_scale;
+		velocity_down *= down_aux_based_scale;
+		velocity_vertical_limited = true;
+	}
+
+	if (_param_mc_slow_map_yawr.get() != 0) {
+		const float min_yaw_rate_scale = math::radians(_param_mc_slow_min_yawr.get()) / fmaxf(yaw_rate, FLT_EPSILON);
+		const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_yawr.get());
+		const float aux_based_scale =
+			math::interpolate(aux_input, -1.f, 1.f, min_yaw_rate_scale, 1.f);
+		yaw_rate *= aux_based_scale;
+		yaw_rate_limited = true;
+	}
+
+	// No input from remote and MAVLink -> use default slow mode limits
+	if (!velocity_horizontal_limited) {
+		velocity_horizontal = _param_mc_slow_def_hvel.get();
+	}
+
+	if (!velocity_vertical_limited) {
+		velocity_up = velocity_down = _param_mc_slow_def_vvel.get();
+	}
+
+	if (!yaw_rate_limited) {
+		yaw_rate = math::radians(_param_mc_slow_def_yawr.get());
+	}
+
+	// Interface to set resulting velocity limits
+	FlightTaskManualAcceleration::_stick_acceleration_xy.setVelocityConstraint(velocity_horizontal);
+	FlightTaskManualAltitude::_velocity_constraint_up = velocity_up;
+	FlightTaskManualAltitude::_velocity_constraint_down = velocity_down;
+	FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate);
+
+	return FlightTaskManualAcceleration::update();
+}
+
+float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value)
+{
+	const int sanitized_index = math::constrain(parameter_value - 1, 0, 5);
+	return _sticks.getAux()(sanitized_index);
+}
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskManualAccelerationSlow.hpp
+ *
+ * Flight task for manual position mode with knobs for slower velocity and acceleration.
+ *
+ */
+
+#pragma once
+
+#include "FlightTaskManualAcceleration.hpp"
+#include "StickAccelerationXY.hpp"
+
+#include <lib/weather_vane/WeatherVane.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/velocity_limits.h>
+#include <systemlib/mavlink_log.h>
+
+class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration
+{
+public:
+	FlightTaskManualAccelerationSlow() = default;
+	~FlightTaskManualAccelerationSlow() = default;
+	bool update() override;
+
+private:
+
+	/**
+	 * Get the input from a sanitized parameter aux index
+	 * @param parameter_value value of the parameter that specifies the AUX channel index to use
+	 * @return input from that AUX channel [-1,1]
+	 */
+	float getInputFromSanitizedAuxParameterIndex(int parameter_value);
+
+	bool _velocity_limits_received_before{false};
+
+	uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)};
+	velocity_limits_s _velocity_limits{};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration,
+					(ParamInt<px4::params::MC_SLOW_MAP_HVEL>) _param_mc_slow_map_hvel,
+					(ParamInt<px4::params::MC_SLOW_MAP_VVEL>) _param_mc_slow_map_vvel,
+					(ParamInt<px4::params::MC_SLOW_MAP_YAWR>) _param_mc_slow_map_yawr,
+					(ParamFloat<px4::params::MC_SLOW_MIN_HVEL>) _param_mc_slow_min_hvel,
+					(ParamFloat<px4::params::MC_SLOW_MIN_VVEL>) _param_mc_slow_min_vvel,
+					(ParamFloat<px4::params::MC_SLOW_MIN_YAWR>) _param_mc_slow_min_yawr,
+					(ParamFloat<px4::params::MC_SLOW_DEF_HVEL>) _param_mc_slow_def_hvel,
+					(ParamFloat<px4::params::MC_SLOW_DEF_VVEL>) _param_mc_slow_def_vvel,
+					(ParamFloat<px4::params::MC_SLOW_DEF_YAWR>) _param_mc_slow_def_yawr,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
+				       )
+};
@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Manual input mapped to scale horizontal velocity in position slow mode
+ *
+ * @value 0 No rescaling
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(MC_SLOW_MAP_HVEL, 0);
+
+/**
+ * Manual input mapped to scale vertical velocity in position slow mode
+ *
+ * @value 0 No rescaling
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(MC_SLOW_MAP_VVEL, 0);
+
+/**
+ * Manual input mapped to scale yaw rate in position slow mode
+ *
+ * @value 0 No rescaling
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(MC_SLOW_MAP_YAWR, 0);
+
+/**
+ * Horizontal velocity lower limit
+ *
+ * The lowest input maps and is clamped to this velocity.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(MC_SLOW_MIN_HVEL, .3f);
+
+/**
+ * Vertical velocity lower limit
+ *
+ * The lowest input maps and is clamped to this velocity.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(MC_SLOW_MIN_VVEL, .3f);
+
+/**
+ * Yaw rate lower limit
+ *
+ * The lowest input maps and is clamped to this rate.
+ *
+ * @unit deg/s
+ * @min 1
+ * @increment 0.1
+ * @decimal 0
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(MC_SLOW_MIN_YAWR, 3.f);
+
+/**
+ * Default horizontal velocity limit
+ *
+ * This value is used in slow mode if
+ * no aux channel is mapped and
+ * no limit is commanded through MAVLink.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(MC_SLOW_DEF_HVEL, 3.f);
+
+/**
+ * Default vertical velocity limit
+ *
+ * This value is used in slow mode if
+ * no aux channel is mapped and
+ * no limit is commanded through MAVLink.
+ *
+ * @unit m/s
+ * @min 0.1
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f);
+
+/**
+ * Default yaw rate limit
+ *
+ * This value is used in slow mode if
+ * no aux channel is mapped and
+ * no limit is commanded through MAVLink.
+ *
+ * @unit deg/s
+ * @min 1
+ * @increment 0.1
+ * @decimal 0
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f);
@@ -92,8 +92,9 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	// Use sticks input with deadzone and exponential curve for vertical velocity
-	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() :
-				_param_mpc_z_vel_max_up.get();
+	const float vel_max_up = fminf(_param_mpc_z_vel_max_up.get(), _velocity_constraint_up);
+	const float vel_max_down = fminf(_param_mpc_z_vel_max_dn.get(), _velocity_constraint_down);
+	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? vel_max_down : vel_max_up;
 	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }

@@ -75,6 +75,9 @@ protected:
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
 	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */

+	float _velocity_constraint_up{INFINITY};
+	float _velocity_constraint_down{INFINITY};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
@@ -73,10 +73,12 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
 		const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
-	// maximum commanded acceleration and velocity
-	Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
+	// maximum commanded velocity can be constrained dynamically
 	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
 	Vector2f velocity_scale(velocity_sc, velocity_sc);
+	// maximum commanded acceleration is scaled down with velocity
+	const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());
+	Vector2f acceleration_scale(acceleration_sc, acceleration_sc);

 	acceleration_scale *= 2.f; // because of drag the average acceleration is half

@@ -157,7 +159,13 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo

 	// increase drag with squareroot function when velocity is lower than 1m/s
 	const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
-	return drag_coefficient.emult(velocity_with_sqrt_boost);
+
+	// only apply the drag increase below 1m/s when actually braking such that speeds below 1m/s
+	// are exactly reached but do so by blending it with the filter to avoid any discontinuity when switching
+	const float brake_scale = math::interpolate(_brake_boost_filter.getState(), 1.f, 2.f, 0.f, 1.f);
+	const Vector2f mixed_velocity = brake_scale * velocity_with_sqrt_boost + (1.f - brake_scale) * vel_sp;
+
+	return drag_coefficient.emult(mixed_velocity);
 }

 void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
@@ -66,7 +66,8 @@ void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint
 	}

 	_yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get());
-	yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get()));
+	const float yawspeed_scale = math::min(math::radians(_param_mpc_man_y_max.get()), _yawspeed_constraint);
+	yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * yawspeed_scale);
 	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev);
 }

@@ -52,6 +52,7 @@ public:
 	void ekfResetHandler(float delta_yaw);
 	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime,
 				 float unaided_yaw = NAN);
+	void setYawspeedConstraint(float yawspeed) { _yawspeed_constraint = yawspeed; };

 private:
 	AlphaFilter<float> _yawspeed_filter;
@@ -78,6 +79,8 @@ private:
 	 */
 	float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const;

+	float _yawspeed_constraint{INFINITY};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection
 		(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau ///< time constant for yaw speed filtering
@@ -62,6 +62,13 @@ bool Sticks::checkAndUpdateStickInputs()
 		_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(),  _param_mpc_hold_dz.get());
 		_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(),    _param_mpc_hold_dz.get());

+		_aux_positions(0) = manual_control_setpoint.aux1;
+		_aux_positions(1) = manual_control_setpoint.aux2;
+		_aux_positions(2) = manual_control_setpoint.aux3;
+		_aux_positions(3) = manual_control_setpoint.aux4;
+		_aux_positions(4) = manual_control_setpoint.aux5;
+		_aux_positions(5) = manual_control_setpoint.aux6;
+
 		// valid stick inputs are required
 		_input_available = manual_control_setpoint.valid && _positions.isAllFinite();

@@ -74,6 +74,8 @@ public:
 	const matrix::Vector2f getPitchRoll() { return _positions.slice<2, 1>(0, 0); }
 	const matrix::Vector2f getPitchRollExpo() { return _positions_expo.slice<2, 1>(0, 0); }

+	const matrix::Vector<float, 6> &getAux() const { return _aux_positions; }
+
 	/**
 	 * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle
 	 * @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted
@@ -93,6 +95,8 @@ private:
 	matrix::Vector4f _positions; ///< unmodified manual stick inputs
 	matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function

+	matrix::Vector<float, 6> _aux_positions;
+
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};

@@ -38,11 +38,10 @@ px4_add_module(
 		FixedwingAttitudeControl.cpp
 		FixedwingAttitudeControl.hpp

-		ecl_controller.cpp
-		ecl_pitch_controller.cpp
-		ecl_roll_controller.cpp
-		ecl_wheel_controller.cpp
-		ecl_yaw_controller.cpp
+		fw_pitch_controller.cpp
+		fw_roll_controller.cpp
+		fw_wheel_controller.cpp
+		fw_yaw_controller.cpp
 	DEPENDS
 		px4_work_queue
 	)
@@ -322,31 +322,19 @@ void FixedwingAttitudeControl::Run()
 				}
 			}

-			/* Prepare data for attitude controllers */
-			ECL_ControlData control_input{};
-			control_input.roll = euler_angles.phi();
-			control_input.pitch = euler_angles.theta();
-			control_input.yaw = euler_angles.psi();
-			control_input.body_z_rate = angular_velocity.xyz[2];
-			control_input.roll_setpoint = _att_sp.roll_body;
-			control_input.pitch_setpoint = _att_sp.pitch_body;
-			control_input.yaw_setpoint = _att_sp.yaw_body;
-			control_input.euler_pitch_rate_setpoint = _pitch_ctrl.get_euler_rate_setpoint();
-			control_input.euler_yaw_rate_setpoint = _yaw_ctrl.get_euler_rate_setpoint();
-			control_input.airspeed_constrained = get_airspeed_constrained();
-			control_input.groundspeed = _groundspeed;
-			control_input.groundspeed_scaler = groundspeed_scale;
-
 			/* Run attitude controllers */

 			if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
-					_roll_ctrl.control_attitude(dt, control_input);
-					_pitch_ctrl.control_attitude(dt, control_input);
-					_yaw_ctrl.control_attitude(dt, control_input);
+					_roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+								euler_angles.theta());
+					_pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+								  euler_angles.theta());
+					_yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+							      euler_angles.theta(), get_airspeed_constrained());

 					if (wheel_control) {
-						_wheel_ctrl.control_attitude(dt, control_input);
+						_wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi());

 					} else {
 						_wheel_ctrl.reset_integrator();
@@ -404,8 +392,9 @@ void FixedwingAttitudeControl::Run()
 				// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
 				// position controller during auto modes _manual_control_setpoint.r gets passed
 				// whenever nudging is enabled, otherwise zero
-				wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input)
-					  + _att_sp.yaw_sp_move_rate : 0.f;
+				const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, euler_angles.psi(), _groundspeed,
+								      groundspeed_scale);
+				wheel_u = wheel_control ? wheel_controller_output +  _att_sp.yaw_sp_move_rate : 0.f;
 			}

 			_landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f;
@@ -34,10 +34,10 @@
 #pragma once

 #include <drivers/drv_hrt.h>
-#include "ecl_pitch_controller.h"
-#include "ecl_roll_controller.h"
-#include "ecl_wheel_controller.h"
-#include "ecl_yaw_controller.h"
+#include "fw_pitch_controller.h"
+#include "fw_roll_controller.h"
+#include "fw_wheel_controller.h"
+#include "fw_yaw_controller.h"
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
@@ -159,10 +159,10 @@ private:

 	)

-	ECL_RollController _roll_ctrl;
-	ECL_PitchController _pitch_ctrl;
-	ECL_YawController _yaw_ctrl;
-	ECL_WheelController _wheel_ctrl;
+	RollController _roll_ctrl;
+	PitchController _pitch_ctrl;
+	YawController _yaw_ctrl;
+	WheelController _wheel_ctrl;

 	void parameters_update();
 	void vehicle_manual_poll(const float yaw_body);
@@ -1,119 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ecl_controller.cpp
- * Definition of base class for other controllers
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
- */
-
-#include "ecl_controller.h"
-
-#include <stdio.h>
-#include <mathlib/mathlib.h>
-
-ECL_Controller::ECL_Controller() :
-	_last_run(0),
-	_tc(0.1f),
-	_k_p(0.0f),
-	_k_i(0.0f),
-	_k_ff(0.0f),
-	_integrator_max(0.0f),
-	_max_rate(0.0f),
-	_last_output(0.0f),
-	_integrator(0.0f),
-	_euler_rate_setpoint(0.0f),
-	_body_rate_setpoint(0.0f)
-{
-}
-
-void ECL_Controller::reset_integrator()
-{
-	_integrator = 0.0f;
-}
-
-void ECL_Controller::set_time_constant(float time_constant)
-{
-	if (time_constant > 0.1f && time_constant < 3.0f) {
-		_tc = time_constant;
-	}
-}
-
-void ECL_Controller::set_k_p(float k_p)
-{
-	_k_p = k_p;
-}
-
-void ECL_Controller::set_k_i(float k_i)
-{
-	_k_i = k_i;
-}
-
-void ECL_Controller::set_k_ff(float k_ff)
-{
-	_k_ff = k_ff;
-}
-
-void ECL_Controller::set_integrator_max(float max)
-{
-	_integrator_max = max;
-}
-
-void ECL_Controller::set_max_rate(float max_rate)
-{
-	_max_rate = max_rate;
-}
-
-float ECL_Controller::get_euler_rate_setpoint()
-{
-	return _euler_rate_setpoint;
-}
-
-float ECL_Controller::get_body_rate_setpoint()
-{
-	return _body_rate_setpoint;
-}
-
-float ECL_Controller::get_integrator()
-{
-	return _integrator;
-}
@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2022 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ecl_controller.h
- * Definition of base class for other controllers
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
- */
-
-#pragma once
-
-#include <drivers/drv_hrt.h>
-#include <px4_log.h>
-
-struct ECL_ControlData {
-	float roll;
-	float pitch;
-	float yaw;
-	float body_z_rate;
-	float roll_setpoint;
-	float pitch_setpoint;
-	float yaw_setpoint;
-	float euler_pitch_rate_setpoint;
-	float euler_yaw_rate_setpoint;
-	float airspeed_constrained;
-	float groundspeed;
-	float groundspeed_scaler;
-};
-
-class ECL_Controller
-{
-public:
-	ECL_Controller();
-	virtual ~ECL_Controller() = default;
-
-	/**
-	 * @brief Calculates both euler and body rate setpoints. Has different implementations for all body axes.
-	 *
-	 * @param dt Time step [s]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
-	 * @return Body rate setpoint [rad/s]
-	 */
-	virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
-
-	/* Setters */
-	void set_time_constant(float time_constant);
-	void set_k_p(float k_p);
-	void set_k_i(float k_i);
-	void set_k_ff(float k_ff);
-	void set_integrator_max(float max);
-	void set_max_rate(float max_rate);
-
-	/* Getters */
-	float get_euler_rate_setpoint();
-	float get_body_rate_setpoint();
-	float get_integrator();
-
-	void reset_integrator();
-
-protected:
-	uint64_t _last_run;
-	float _tc;
-	float _k_p;
-	float _k_i;
-	float _k_ff;
-	float _integrator_max;
-	float _max_rate;
-	float _last_output;
-	float _integrator;
-	float _euler_rate_setpoint;
-	float _body_rate_setpoint;
-};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -32,38 +32,34 @@
 ****************************************************************************/

 /**
- * @file ecl_pitch_controller.cpp
- * Implementation of a simple orthogonal pitch PID controller.
- *
- * Authors and acknowledgements in header.
+ * @file fw_pitch_controller.cpp
+ * Implementation of a simple pitch P controller.
 */

-#include "ecl_pitch_controller.h"
+#include "fw_pitch_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>

-float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float PitchController::control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
-	      PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) {
+	if (!(PX4_ISFINITE(pitch_setpoint) &&
+	      PX4_ISFINITE(euler_yaw_rate_setpoint) &&
+	      PX4_ISFINITE(pitch) &&
+	      PX4_ISFINITE(roll))) {

 		return _body_rate_setpoint;
 	}

-	/* Calculate the error */
-	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
-
-	/*  Apply P controller: rate setpoint from current error and time constant */
-	_euler_rate_setpoint =  pitch_error / _tc;
+	const float pitch_error = pitch_setpoint - pitch;
+	_euler_rate_setpoint = pitch_error / _tc;

 	/* Transform setpoint to body angular rates (jacobian) */
-	const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint +
-			cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
-	_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
+	const float pitch_body_rate_setpoint_raw = cosf(roll) * _euler_rate_setpoint +
+			cosf(pitch) * sinf(roll) * euler_yaw_rate_setpoint;
+
+	_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate_pos);

 	return _body_rate_setpoint;
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -32,56 +32,43 @@
 ****************************************************************************/

 /**
- * @file ecl_pitch_controller.h
- * Definition of a simple orthogonal pitch PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
+ * @file fw_pitch_controller.h
+ * Definition of a simple pitch P controller.
 */

-#ifndef ECL_PITCH_CONTROLLER_H
-#define ECL_PITCH_CONTROLLER_H
+#ifndef FW_PITCH_CONTROLLER_H
+#define FW_PITCH_CONTROLLER_H

-#include <mathlib/mathlib.h>
-
-#include "ecl_controller.h"
-
-class ECL_PitchController :
-	public ECL_Controller
+class PitchController
 {
 public:
-	ECL_PitchController() = default;
-	~ECL_PitchController() = default;
+	PitchController() = default;
+	~PitchController() = default;

 	/**
 	 * @brief Calculates both euler and body pitch rate setpoints.
 	 *
-	 * @param dt Time step [s]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
+	 * @param pitch_setpoint pitch setpoint [rad]
+	 * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
+	 * @param roll estimated roll [rad]
+	 * @param pitch estimated pitch [rad]
 	 * @return Pitch body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);

-	/* Additional Setters */
-	void set_max_rate_pos(float max_rate_pos)
-	{
-		_max_rate = max_rate_pos;
-	}
+	void set_time_constant(float time_constant) { _tc = time_constant; }
+	void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; }
+	void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; }

-	void set_max_rate_neg(float max_rate_neg)
-	{
-		_max_rate_neg = max_rate_neg;
-	}
+	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
+	float get_body_rate_setpoint() { return _body_rate_setpoint; }

-protected:
-	float _max_rate_neg{0.0f};
+private:
+	float _tc;
+	float _max_rate_pos;
+	float _max_rate_neg;
+	float _euler_rate_setpoint;
+	float _body_rate_setpoint;
 };

-#endif // ECL_PITCH_CONTROLLER_H
+#endif // FW_PITCH_CONTROLLER_H
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -32,37 +32,32 @@
 ****************************************************************************/

 /**
- * @file ecl_roll_controller.cpp
- * Implementation of a simple orthogonal roll PID controller.
- *
- * Authors and acknowledgements in header.
+ * @file fw_roll_controller.cpp
+ * Implementation of a simple roll P controller.
 */

-#include "ecl_roll_controller.h"
+#include "fw_roll_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>

-float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float RollController::control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
-	      PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.roll))) {
+	if (!(PX4_ISFINITE(roll_setpoint) &&
+	      PX4_ISFINITE(euler_yaw_rate_setpoint) &&
+	      PX4_ISFINITE(pitch) &&
+	      PX4_ISFINITE(roll))) {

 		return _body_rate_setpoint;
 	}

-	/* Calculate the error */
-	float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
-
-	/*  Apply P controller: rate setpoint from current error and time constant */
+	const float roll_error = roll_setpoint - roll;
 	_euler_rate_setpoint = roll_error / _tc;

 	/* Transform setpoint to body angular rates (jacobian) */
-	const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) *
-			ctl_data.euler_yaw_rate_setpoint;
+	const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(pitch) *
+			euler_yaw_rate_setpoint;
 	_body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);

 	return _body_rate_setpoint;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -32,41 +32,41 @@
 ****************************************************************************/

 /**
- * @file ecl_yaw_controller.h
- * Definition of a simple orthogonal coordinated turn yaw PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
+ * @file fw_roll_controller.h
+ * Definition of a simple roll P controller.
 */

-#ifndef ECL_YAW_CONTROLLER_H
-#define ECL_YAW_CONTROLLER_H
+#ifndef FW_ROLL_CONTROLLER_H
+#define FW_ROLL_CONTROLLER_H

-#include "ecl_controller.h"
-
-class ECL_YawController :
-	public ECL_Controller
+class RollController
 {
 public:
-	ECL_YawController() = default;
-	~ECL_YawController() = default;
+	RollController() = default;
+	~RollController() = default;

 	/**
-	 * @brief Calculates both euler and body yaw rate setpoints.
+	 * @brief Calculates both euler and body roll rate setpoints.
 	 *
-	 * @param dt Time step [s]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
-	 * @return Yaw body rate setpoint [rad/s]
+	 * @param roll_setpoint roll setpoint [rad]
+	 * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
+	 * @param roll estimated roll [rad]
+	 * @param pitch estimated pitch [rad]
+	 * @return Roll body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);

+	void set_time_constant(float time_constant) { _tc = time_constant; }
+	void set_max_rate(float max_rate) { _max_rate = max_rate; }
+
+	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
+	float get_body_rate_setpoint() { return _body_rate_setpoint; }
+
+private:
+	float _tc;
+	float _max_rate;
+	float _euler_rate_setpoint;
+	float _body_rate_setpoint;
 };

-#endif // ECL_YAW_CONTROLLER_H
+#endif // FW_ROLL_CONTROLLER_H
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -32,13 +32,11 @@
 ****************************************************************************/

 /**
- * @file ecl_wheel_controller.cpp
+ * @file fw_wheel_controller.cpp
 * Implementation of a simple PID wheel controller for heading tracking.
- *
- * Authors and acknowledgements in header.
 */

-#include "ecl_wheel_controller.h"
+#include "fw_wheel_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>
@@ -46,68 +44,56 @@

 using matrix::wrap_pi;

-float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
+float WheelController::control_bodyrate(float dt, float body_z_rate, float groundspeed, float groundspeed_scaler)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
-	      PX4_ISFINITE(ctl_data.groundspeed) &&
-	      PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
+	if (!(PX4_ISFINITE(body_z_rate) &&
+	      PX4_ISFINITE(groundspeed) &&
+	      PX4_ISFINITE(groundspeed_scaler))) {

-		return math::constrain(_last_output, -1.0f, 1.0f);
+		return math::constrain(_last_output, -1.f, 1.f);
 	}

-	/* input conditioning */
-	float min_speed = 1.0f;
+	const float rate_error = _body_rate_setpoint - body_z_rate;

-	/* Calculate body angular rate error */
-	const float rate_error = _body_rate_setpoint - ctl_data.body_z_rate; //body angular rate error
+	if (_k_i > 0.f && groundspeed > 1.f) { // only start integrating when above 1m/s

-	if (_k_i > 0.0f && ctl_data.groundspeed > min_speed) {
+		float id = rate_error * dt * groundspeed_scaler;

-		float id = rate_error * dt * ctl_data.groundspeed_scaler;
-
-		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
-		if (_last_output < -1.0f) {
+		if (_last_output < -1.f) {
 			/* only allow motion to center: increase value */
-			id = math::max(id, 0.0f);
+			id = math::max(id, 0.f);

-		} else if (_last_output > 1.0f) {
+		} else if (_last_output > 1.f) {
 			/* only allow motion to center: decrease value */
-			id = math::min(id, 0.0f);
+			id = math::min(id, 0.f);
 		}

-		/* add and constrain */
 		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
 	}

 	/* Apply PI rate controller and store non-limited output */
-	_last_output = _body_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
-		       ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator);
+	_last_output = _body_rate_setpoint * _k_ff * groundspeed_scaler +
+		       groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p + _integrator);

-	return math::constrain(_last_output, -1.0f, 1.0f);
+	return math::constrain(_last_output, -1.f, 1.f);
 }

-float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float WheelController::control_attitude(float yaw_setpoint, float yaw)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
-	      PX4_ISFINITE(ctl_data.yaw))) {
+	if (!(PX4_ISFINITE(yaw_setpoint) &&
+	      PX4_ISFINITE(yaw))) {

 		return _body_rate_setpoint;
 	}

-	/* Calculate the error */
-	float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
+	const float yaw_error = wrap_pi(yaw_setpoint - yaw);

-	/*  Apply P controller: rate setpoint from current error and time constant */
-	_euler_rate_setpoint =  yaw_error / _tc;
-	_body_rate_setpoint = _euler_rate_setpoint; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix
+	_body_rate_setpoint = yaw_error / _tc; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix

-	/* limit the rate */
 	if (_max_rate > 0.01f) {
-		if (_body_rate_setpoint > 0.0f) {
+		if (_body_rate_setpoint > 0.f) {
 			_body_rate_setpoint = (_body_rate_setpoint > _max_rate) ? _max_rate : _body_rate_setpoint;

 		} else {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -32,44 +32,48 @@
 ****************************************************************************/

 /**
- * @file ecl_wheel_controller.h
- * Definition of a simple orthogonal coordinated turn yaw PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- * @author Andreas Antener <andreas@uaventure.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
+ * @file fw_wheel_controller.h
+ * Definition of a simple wheel controller.
 */
-#ifndef ECL_HEADING_CONTROLLER_H
-#define ECL_HEADING_CONTROLLER_H
+#ifndef FW_WHEEL_CONTROLLER_H
+#define FW_WHEEL_CONTROLLER_H

-#include "ecl_controller.h"
-
-class ECL_WheelController :
-	public ECL_Controller
+class WheelController
 {
 public:
-	ECL_WheelController() = default;
-	~ECL_WheelController() = default;
+	WheelController() = default;
+	~WheelController() = default;

 	/**
 	 * @brief Calculates wheel body rate setpoint.
 	 *
-	 * @param dt Time step [s]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
+	 * @param yaw_setpoint yaw setpoint [rad]
+	 * @param yaw estimated yaw [rad]
 	 * @return Wheel body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_attitude(float yaw_setpoint, float yaw);

-	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data);
+	float control_bodyrate(float dt, float body_z_rate, float groundspeed, float groundspeed_scaler);

-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) { (void)ctl_data; return 0; }
+	void set_time_constant(float time_constant) { _tc = time_constant; }
+	void set_k_p(float k_p) { _k_p = k_p; }
+	void set_k_i(float k_i) { _k_i = k_i; }
+	void set_k_ff(float k_ff) { _k_ff = k_ff; }
+	void set_integrator_max(float max) { _integrator_max = max; }
+	void set_max_rate(float max_rate) { _max_rate = max_rate; }
+
+	void reset_integrator() { _integrator = 0.f; }
+
+private:
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_ff;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _body_rate_setpoint;
 };

-#endif // ECL_HEADING_CONTROLLER_H
+#endif // FW_WHEEL_CONTROLLER_H
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -32,24 +32,24 @@
 ****************************************************************************/

 /**
- * @file ecl_yaw_controller.cpp
- * Implementation of a simple orthogonal coordinated turn yaw PID controller.
- *
- * Authors and acknowledgements in header.
+ * @file fw_yaw_controller.cpp
+ * Implementation of a simple coordinated turn yaw controller.
 */

-#include "ecl_yaw_controller.h"
+#include "fw_yaw_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>

-float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float YawController::control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
+				 float airspeed)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.airspeed_constrained))) {
+	if (!(PX4_ISFINITE(roll_setpoint) &&
+	      PX4_ISFINITE(roll) &&
+	      PX4_ISFINITE(pitch) &&
+	      PX4_ISFINITE(euler_pitch_rate_setpoint) &&
+	      PX4_ISFINITE(airspeed))) {

 		return _body_rate_setpoint;
 	}
@@ -58,41 +58,40 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
 	bool inverted = false;

 	/* roll is used as feedforward term and inverted flight needs to be considered */
-	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
+	if (fabsf(roll) < math::radians(90.f)) {
 		/* not inverted, but numerically still potentially close to infinity */
-		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
+		constrained_roll = math::constrain(roll, math::radians(-80.f), math::radians(80.f));

 	} else {
 		inverted = true;

 		// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
 		//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
-		if (ctl_data.roll > 0.0f) {
+		if (roll > 0.f) {
 			/* right hemisphere */
-			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
+			constrained_roll = math::constrain(roll, math::radians(100.f), math::radians(180.f));

 		} else {
 			/* left hemisphere */
-			constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f));
+			constrained_roll = math::constrain(roll, math::radians(-180.f), math::radians(-100.f));
 		}
 	}

-	constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint));
+	constrained_roll = math::constrain(constrained_roll, -fabsf(roll_setpoint), fabsf(roll_setpoint));


 	if (!inverted) {
 		/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
-		_euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / ctl_data.airspeed_constrained;
+		_euler_rate_setpoint = tanf(constrained_roll) * cosf(pitch) * CONSTANTS_ONE_G / airspeed;

 		/* Transform setpoint to body angular rates (jacobian) */
-		const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
-				cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
+		const float yaw_body_rate_setpoint_raw = -sinf(roll) * euler_pitch_rate_setpoint +
+				cosf(roll) * cosf(pitch) * _euler_rate_setpoint;
 		_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
 	}

 	if (!PX4_ISFINITE(_body_rate_setpoint)) {
-		PX4_WARN("yaw rate sepoint not finite");
-		_body_rate_setpoint = 0.0f;
+		_body_rate_setpoint = 0.f;
 	}

 	return _body_rate_setpoint;
@@ -32,11 +32,8 @@
 ****************************************************************************/

 /**
- * @file ecl_roll_controller.h
- * Definition of a simple orthogonal roll PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @file fw_yaw_controller.h
+ * Definition of a simple coordinated turn controller.
 *
 * Acknowledgements:
 *
@@ -46,26 +43,37 @@
 *   Jonathan Challinger, 2012.
 */

-#ifndef ECL_ROLL_CONTROLLER_H
-#define ECL_ROLL_CONTROLLER_H
+#ifndef FW_YAW_CONTROLLER_H
+#define FW_YAW_CONTROLLER_H

-#include "ecl_controller.h"
-
-class ECL_RollController :
-	public ECL_Controller
+class YawController
 {
 public:
-	ECL_RollController() = default;
-	~ECL_RollController() = default;
+	YawController() = default;
+	~YawController() = default;

 	/**
-	 * @brief Calculates both euler and body roll rate setpoints.
+	 * @brief Calculates both euler and body yaw rate setpoints for coordinated turn based on current attitude and airspeed
 	 *
-	 * @param dt Time step [s]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
+	 * @param roll_setpoint roll setpoint [rad]
+	 * @param euler_pitch_rate_setpoint euler pitch rate setpoint [rad/s]
+	 * @param roll estimated roll [rad]
+	 * @param pitch estimated pitch [rad]
+	 * @param airspeed airspeed [m/s]
 	 * @return Roll body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
+			  float airspeed);
+
+	void set_max_rate(float max_rate) { _max_rate = max_rate; }
+
+	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
+	float get_body_rate_setpoint() { return _body_rate_setpoint; }
+
+private:
+	float _max_rate;
+	float _euler_rate_setpoint;
+	float _body_rate_setpoint;
 };

-#endif // ECL_ROLL_CONTROLLER_H
+#endif // FW_YAW_CONTROLLER_H
@@ -553,6 +553,7 @@ Module consuming manual_control_inputs publishing one manual_control_setpoint.

 int8_t ManualControl::navStateFromParam(int32_t param_value)
 {
+	// See src/modules/commander/module.yaml COM_FLTMODE${i}
 	switch(param_value) {
 		case 0: return vehicle_status_s::NAVIGATION_STATE_MANUAL;
 		case 1: return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
@@ -563,6 +564,7 @@ int8_t ManualControl::navStateFromParam(int32_t param_value)
 		case 6: return vehicle_status_s::NAVIGATION_STATE_ACRO;
 		case 7: return vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		case 8: return vehicle_status_s::NAVIGATION_STATE_STAB;
+		case 9: return vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
 		case 10: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
 		case 11: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 		case 12: return vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
@@ -323,6 +323,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_device_attitude_status(msg);
 		break;

+#if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
+
+	case MAVLINK_MSG_ID_SET_VELOCITY_LIMITS:
+		handle_message_set_velocity_limits(msg);
+		break;
+#endif
+
 	default:
 		break;
 	}
@@ -1211,6 +1218,21 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
 	handle_request_message_command(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN);
 }

+#if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
+void MavlinkReceiver::handle_message_set_velocity_limits(mavlink_message_t *msg)
+{
+	mavlink_set_velocity_limits_t mavlink_set_velocity_limits;
+	mavlink_msg_set_velocity_limits_decode(msg, &mavlink_set_velocity_limits);
+
+	velocity_limits_s velocity_limits{};
+	velocity_limits.horizontal_velocity = mavlink_set_velocity_limits.horizontal_speed_limit;
+	velocity_limits.vertical_velocity = mavlink_set_velocity_limits.vertical_speed_limit;
+	velocity_limits.yaw_rate = mavlink_set_velocity_limits.yaw_rate_limit;
+	velocity_limits.timestamp = hrt_absolute_time();
+	_velocity_limits_pub.publish(velocity_limits);
+}
+#endif // MAVLINK_MSG_ID_SET_VELOCITY_LIMITS
+
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
@@ -110,6 +110,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_bezier.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/velocity_limits.h>

 #if !defined(CONSTRAINED_FLASH)
 # include <uORB/topics/debug_array.h>
@@ -196,6 +197,9 @@ private:
 	void handle_message_trajectory_representation_bezier(mavlink_message_t *msg);
 	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
 	void handle_message_utm_global_position(mavlink_message_t *msg);
+#if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
+	void handle_message_set_velocity_limits(mavlink_message_t *msg);
+#endif
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
@@ -305,6 +309,7 @@ private:
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<onboard_computer_status_s>		_onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
+	uORB::Publication<velocity_limits_s>			_velocity_limits_pub{ORB_ID(velocity_limits)};
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__offboard_switch
+	MAIN offboard_switch
+	SRCS
+		offboard_switch.cpp
+		offboard_switch.hpp
+	DEPENDS
+		px4_work_queue
+	)
@@ -0,0 +1,12 @@
+menuconfig MODULES_OFFBOARD_SWITCH
+	bool "offboard_switch"
+	default n
+	---help---
+		Enable support for fw_att_control
+
+menuconfig USER_MODULES_OFFBOARD_SWITCH
+	bool "fw_rate_control running as userspace module"
+	default n
+	depends on BOARD_PROTECTED
+	---help---
+		Put offboard_switch in userspace memory
@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "offboard_switch.hpp"
+
+using namespace time_literals;
+using namespace matrix;
+
+OffboardSwitch::OffboardSwitch() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+OffboardSwitch::~OffboardSwitch()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+OffboardSwitch::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+int
+OffboardSwitch::parameters_update()
+{
+
+	return PX4_OK;
+}
+
+
+void OffboardSwitch::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// only run controller if angular velocity changed
+
+	///TODO: Timeout if setpoints fall out of sync
+	///TODO: Package offboard control mode messages to the correct level, depending on the setpoint
+	offboard_control_mode_s ocm{};
+	trajectory_setpoint_s trajectory_setpoint;
+
+	vehicle_status_s vehicle_status{};
+	_vehicle_status_sub.copy(&vehicle_status);
+
+	bool data_is_recent{false};
+
+	if (_offboard_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+		data_is_recent = data_is_recent || (hrt_absolute_time() < trajectory_setpoint.timestamp
+						    + static_cast<hrt_abstime>(_param_com_of_loss_t.get() * 1_s));
+
+		if (data_is_recent) {
+			if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1])
+			    && PX4_ISFINITE(trajectory_setpoint.position[2])) {
+				ocm.position = true;
+			}
+
+			if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1])
+			    && PX4_ISFINITE(trajectory_setpoint.velocity[2])) {
+				ocm.velocity = true;
+			}
+
+			if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
+			    && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
+				ocm.acceleration = true;
+			}
+
+			if ((ocm.position || ocm.velocity || ocm.acceleration)
+			    && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD)) {
+				_trajectory_setpoint_pub.publish(trajectory_setpoint);
+			}
+		}
+	}
+
+	vehicle_attitude_setpoint_s attitude_setpoint;
+
+	if (_offboard_vehicle_attitude_setpoint_sub.update(&attitude_setpoint)) {
+		data_is_recent = data_is_recent || (hrt_absolute_time() < attitude_setpoint.timestamp
+						    + static_cast<hrt_abstime>(_param_com_of_loss_t.get() * 1_s));
+		PX4_INFO("Data is recent: %f", double(data_is_recent));
+
+		if (data_is_recent) {
+			if (PX4_ISFINITE(attitude_setpoint.q_d[0]) && PX4_ISFINITE(attitude_setpoint.q_d[1])
+			    && PX4_ISFINITE(attitude_setpoint.q_d[2]) && PX4_ISFINITE(attitude_setpoint.q_d[3])) {
+				ocm.attitude = true;
+			}
+
+			if (ocm.attitude
+			    && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD)) {
+				_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
+			}
+		}
+	}
+
+	vehicle_rates_setpoint_s rates_setpoint;
+
+	if (_offboard_vehicle_rates_setpoint_sub.update(&rates_setpoint)) {
+		data_is_recent = data_is_recent || (hrt_absolute_time() < rates_setpoint.timestamp
+						    + static_cast<hrt_abstime>(_param_com_of_loss_t.get() * 1_s));
+
+		if (data_is_recent) {
+			if (PX4_ISFINITE(rates_setpoint.roll) && PX4_ISFINITE(rates_setpoint.pitch)
+			    && PX4_ISFINITE(rates_setpoint.yaw)) {
+				ocm.body_rate = true;
+			}
+
+			if (ocm.body_rate
+			    && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD)) {
+				_vehicle_rates_setpoint_pub.publish(rates_setpoint);
+			}
+		}
+	}
+
+	///TODO: Handle corner cases where ill-posed setpoints are mixed
+	bool valid_position_setpoint = ocm.position || ocm.velocity || ocm.acceleration;
+	bool valid_attitude_setpoint = ocm.attitude || ocm.body_rate;
+	bool valid_actuator_setpooint = ocm.thrust_and_torque || ocm.direct_actuator;
+
+	if (data_is_recent && (valid_position_setpoint || valid_attitude_setpoint || valid_actuator_setpooint)) {
+		// publish offboard_control_mode
+		ocm.timestamp = hrt_absolute_time();
+		_offboard_control_mode_pub.publish(ocm);
+	}
+
+	perf_end(_loop_perf);
+}
+
+int OffboardSwitch::task_spawn(int argc, char *argv[])
+{
+	OffboardSwitch *instance = new OffboardSwitch();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int OffboardSwitch::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int OffboardSwitch::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+offboard_switch filters and regulated offboard setpoints.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("offboard_switch", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int offboard_switch_main(int argc, char *argv[])
+{
+	return OffboardSwitch::main(argc, argv);
+}
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+#include <matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+
+using matrix::Eulerf;
+using matrix::Quatf;
+
+using uORB::SubscriptionData;
+
+using namespace time_literals;
+
+class OffboardSwitch final : public ModuleBase<OffboardSwitch>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	OffboardSwitch();
+	~OffboardSwitch() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
+	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _offboard_trajectory_setpoint_sub{ORB_ID(offboard_trajectory_setpoint)};
+	uORB::Subscription _offboard_vehicle_attitude_setpoint_sub{ORB_ID(offboard_attitude_setpoint)};
+	uORB::Subscription _offboard_vehicle_rates_setpoint_sub{ORB_ID(offboard_rates_setpoint)};
+
+	perf_counter_t _loop_perf;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t
+	)
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+};
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file offboard_switch_params.c
+ *
+ * Parameters defined by the offboard_switch
+ *
+ * @author Jaeyoung Lim <jalim@ethz.ch>
+ */
@@ -115,15 +115,24 @@ subscriptions:
  - topic: /fmu/in/trajectory_setpoint
    type: px4_msgs::msg::TrajectorySetpoint

+  - topic: /fmu/in/offboard_trajectory_setpoint
+    type: px4_msgs::msg::TrajectorySetpoint
+
  - topic: /fmu/in/vehicle_attitude_setpoint
    type: px4_msgs::msg::VehicleAttitudeSetpoint

+  - topic: /fmu/in/offboard_attitude_setpoint
+    type: px4_msgs::msg::VehicleAttitudeSetpoint
+
  - topic: /fmu/in/vehicle_mocap_odometry
    type: px4_msgs::msg::VehicleOdometry

  - topic: /fmu/in/vehicle_rates_setpoint
    type: px4_msgs::msg::VehicleRatesSetpoint

+  - topic: /fmu/in/offboard_rates_setpoint
+    type: px4_msgs::msg::VehicleRatesSetpoint
+
  - topic: /fmu/in/vehicle_visual_odometry
    type: px4_msgs::msg::VehicleOdometry

@@ -269,6 +269,10 @@ menu "Zenoh publishers/subscribers"
 		bool "gimbal_manager_status"
 		default n

+	config ZENOH_PUBSUB_GOTO_SETPOINT
+		bool "goto_setpoint"
+		default n
+
 	config ZENOH_PUBSUB_GPIO_CONFIG
 		bool "gpio_config"
 		default n
@@ -873,6 +877,7 @@ config ZENOH_PUBSUB_ALL_SELECTION
 	select ZENOH_PUBSUB_GIMBAL_MANAGER_SET_ATTITUDE
 	select ZENOH_PUBSUB_GIMBAL_MANAGER_SET_MANUAL_CONTROL
 	select ZENOH_PUBSUB_GIMBAL_MANAGER_STATUS
+	select ZENOH_PUBSUB_GOTO_SETPOINT
 	select ZENOH_PUBSUB_GPIO_CONFIG
 	select ZENOH_PUBSUB_GPIO_IN
 	select ZENOH_PUBSUB_GPIO_OUT
Author	SHA1	Message	Date
Jaeyoung Lim	cb77844c1d	Update src/modules/offboard_switch/offboard_switch.cpp Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-01-12 13:36:37 +01:00
Jaeyoung Lim	fe201393d8	Update src/modules/offboard_switch/CMakeLists.txt Co-authored-by: Daniel Agar <daniel@agar.ca>	2024-01-12 13:36:31 +01:00
Jaeyoung Lim	321cdde8bf	Update src/modules/offboard_switch/offboard_switch.hpp Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-01-12 13:36:25 +01:00
Jaeyoung Lim	09824b831d	Add offboard mode swtich This commit adds a offboard mode switch module to enable using dedicated topics for offboard mode	2023-12-15 05:52:48 +01:00
Matthias Grob	ebae9ae3d7	FlightTaskManualAccelerationSlow: MC_ prefix for parameter names As discussed in the maintainer call we should adhere to the parameter naming scheme that makes it clear what vehicle type the configuration is good for.	2023-12-08 21:17:59 +01:00
Matthias Grob	d03030e881	mavlink_receiver: ifdef guard for velocity limits Since this message is defined in development.xml and not yet common.xml and some targets use common.xml and the builds then failed.	2023-12-08 21:17:59 +01:00
Matthias Grob	ca6db94e39	Velocity limit: remove duplicate message and if(true)	2023-12-08 21:17:59 +01:00
Matthias Grob	da24811ce1	SickAccelerationXY: fix comment typo brak{e}ing	2023-12-08 21:17:59 +01:00
Marcin	4cf43a68a3	FlightTask: add subscription to VELOCITY_LIMITS msg	2023-12-08 21:17:59 +01:00
Matthias Grob	54ce9813c8	FlightModeManager: Add task for position slow mode	2023-12-08 21:17:59 +01:00
Matthias Grob	ef0926d64b	Commander: add position slow mode	2023-12-08 21:17:59 +01:00
Matthias Grob	dbbf585adb	StickYaw: yaw rate limit interface	2023-12-08 21:17:59 +01:00
Matthias Grob	84220407ea	FlightTaskManualAltitude: vertical velocity limit interface	2023-12-08 21:17:59 +01:00
Matthias Grob	bb617f6c4d	FlightTaskManualAcceleration: horizontal velocity limit interface	2023-12-08 21:17:59 +01:00
Matthias Grob	77c06a9f9e	Sticks: Provide auxiliary analog values from manual_control_setpoint	2023-12-08 21:17:59 +01:00
Matthias Grob	df41bc3d26	StickAccelerationXY: make sure speeds below 1m/s are exactly reached by only applying the sqrt linear drag when brakeing. It was originally not done this way to avoid discontinuities and the exact speed bewlo 1m/s didn't matter. With the position slow mode the exact slow speeds now matter. And the discontinuities are avoided by reusing the brake boost filter.	2023-12-08 21:17:59 +01:00
Daniel Agar	f703f07399	drivers/distance_sensor: update kconfig common sensors - mappydot is EOL - LL40LS_PWM is a fairly special case that's not common	2023-12-08 13:32:23 -05:00
Matthias Grob	e35380d6ae	mixer_module_tests: cover `output_limit_calc_single()`	2023-12-07 21:35:55 -05:00
Matthias Grob	0a78690356	mixer_module: correct output_limit_calc_single calculation	2023-12-07 21:35:55 -05:00
PX4 BuildBot	fb3123e33b	Update world_magnetic_model to latest Thu Dec 7 11:14:08 UTC 2023	2023-12-07 21:34:18 -05:00
David Sidrane	936c18733b	NuttX with NXP Backports	2023-12-07 21:13:40 -05:00
PX4 BuildBot	db1bb94ea4	Update submodule mavlink to latest Thu Dec 7 12:39:06 UTC 2023 - mavlink in PX4/Firmware (67cf6bd264055a5d13d5521c6c43bd5b42f374ca): https://github.com/mavlink/mavlink/commit/6cf005e996865d4af749f5f9f0fa95ea7721924e - mavlink current upstream: https://github.com/mavlink/mavlink/commit/5f85bd7d7d6155d2d349bd04ed67544610e8e65b - Changes: https://github.com/mavlink/mavlink/compare/6cf005e996865d4af749f5f9f0fa95ea7721924e...5f85bd7d7d6155d2d349bd04ed67544610e8e65b 5f85bd7d 2023-12-07 Hamish Willee - MAV_CMD_DO_SET_HOME - add home position roll/pitch (#1849) 55ba3887 2023-12-06 Hamish Willee - Fixed deprecation typo for MAV_CMD_SET_MESSAGE_INTERVAL (#2060)	2023-12-07 17:56:13 -05:00
PX4 BuildBot	3d1ce20c12	boards: update all NuttX defconfigs	2023-12-07 17:55:38 -05:00
Silvan Fuhrer	c61ac784b6	FW attitude controller: remove deprecated ecl_controller Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
Silvan Fuhrer	b1317daa9c	wheel controller remove from ecl Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
Silvan Fuhrer	00f5bba5e0	FW att controller: wheel controller: separate from ecl_controller Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
Silvan Fuhrer	448292c980	FW att controller: yaw controller: separate from ecl_controller Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
Silvan Fuhrer	d4206195c6	FW att controller: pitch controller: separate from ecl_controller Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
Silvan Fuhrer	7e467f7121	FW att controller: roll controller: seperate from ecl_controller Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
Silvan Fuhrer	48782723ab	FW att controller: ecl_controller: move setter/getter implementation to header Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-07 17:21:09 +01:00
David Sidrane	2af21ee0b6	NuttX with No TXDMA semaphore in serial backports	2023-12-06 11:46:47 -05:00