Rework flaps/spoilers logic

- remove deprecated actuator_controls[INDEX_FLAPS/SPOILERS/AIRBRAKES]
- use new topic normalized_unsigned_setpoint.msg (with instances flaps_setpoint
and spoilers_setpoint) to pass into control allocation
- remove flaps/spoiler related fields from attitude_setpoint topic
- CA: add possibility to map flaps/spoilers to any control surface
- move flaps/spoiler pitch trimming to CA (previously called DTRIM_FLAPS/SPOILER)
- move manual flaps/spoiler handling from rate to attitude controller

FW Position controller: change how negative switch readings are intepreted
for flaps/spoilers (considered negative as 0).

VTOL: Rework spoiler publishing in hover

- pushlish spoiler_setpoint.msg in the VTOL module if in hover
- also set spoilers to land configuration if in Descend mode

Allocation: add slew rate limit of 0.5 to flaps/spoilers configuration change

Instead of doing the flaps/spoilers slew rate limiting in the FW Position Controller
(which then only is applied in Auto flight), do it consistently over all flight
modes, so also for manual modes.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -182,6 +182,13 @@ public:
 	 */
 	virtual const char *name() const = 0;
 
+	/**
+	 * Callback from the control allocation, allowing to manipulate the setpoint.
+	 * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
+	 *
+	 * @param actuator_sp input & output setpoint
+	 */
+	virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}
 
 	/**
 	 * Callback from the control allocation, allowing to manipulate the setpoint.

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -52,8 +52,15 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
 		_param_handles[i].torque[2] = param_find(buffer);
 		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
 		_param_handles[i].trim = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_FLAP", i);
+		_param_handles[i].scale_flap = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_SPOIL", i);
+		_param_handles[i].scale_spoiler = param_find(buffer);
 	}
 
+	_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
+	_spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate);
+
 	_count_handle = param_find("CA_SV_CS_COUNT");
 	updateParams();
 }
@@ -81,6 +88,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 		}
 
 		param_get(_param_handles[i].trim, &_params[i].trim);
+		param_get(_param_handles[i].scale_flap, &_params[i].scale_flap);
+		param_get(_param_handles[i].scale_spoiler, &_params[i].scale_spoiler);
 
 		// TODO: enforce limits (note that tailsitter uses different limits)?
 		switch (_params[i].type) {
@@ -109,12 +118,12 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 		case Type::RightVTail:
 			break;
 
-		case Type::LeftFlaps:
-		case Type::RightFlaps:
+		case Type::LeftFlap:
+		case Type::RightFlap:
 			torque.setZero();
 			break;
 
-		case Type::Airbrakes:
+		case Type::Airbrake:
 			torque.setZero();
 			break;
 
@@ -134,6 +143,10 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 			torque.setZero();
 			break;
 
+		case Type::LeftSpoiler:
+		case Type::RightSpoiler:
+			torque.setZero();
+			break;
 		}
 	}
 }
@@ -151,32 +164,25 @@ bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configura
 	return true;
 }
 
-void ActuatorEffectivenessControlSurfaces::applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control,
-		float wheel_control, int first_actuator_idx,
-		ActuatorVector &actuator_sp) const
+void ActuatorEffectivenessControlSurfaces::applyFlaps(float flaps_control, int first_actuator_idx, float dt,
+		ActuatorVector &actuator_sp)
 {
+	_flaps_setpoint_with_slewrate.update(flaps_control, dt);
+
 	for (int i = 0; i < _count; ++i) {
-		switch (_params[i].type) {
-		// TODO: check sign
-		case ActuatorEffectivenessControlSurfaces::Type::LeftFlaps:
-			actuator_sp(i + first_actuator_idx) += flaps_control;
-			break;
-
-		case ActuatorEffectivenessControlSurfaces::Type::RightFlaps:
-			actuator_sp(i + first_actuator_idx) -= flaps_control;
-			break;
-
-		case ActuatorEffectivenessControlSurfaces::Type::Airbrakes:
-			actuator_sp(i + first_actuator_idx) += airbrakes_control;
-			break;
-
-		case ActuatorEffectivenessControlSurfaces::Type::SteeringWheel:
-			actuator_sp(i + first_actuator_idx) += wheel_control;
-			break;
-
-		default:
-			break;
-		}
+		// map [0, 1] to [-1, 1]
+		// TODO: this currently only works for dedicated flaps, not flaperons
+		actuator_sp(i + first_actuator_idx) += (_flaps_setpoint_with_slewrate.getState() * 2.f - 1.f) * _params[i].scale_flap;
+	}
+}
+
+void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, int first_actuator_idx, float dt,
+		ActuatorVector &actuator_sp)
+{
+	_spoilers_setpoint_with_slewrate.update(spoilers_control, dt);
+
+	for (int i = 0; i < _count; ++i) {
+		// TODO: this currently only works for spoilerons, not dedicated spoilers
+		actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler;
 	}
-
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -36,6 +36,10 @@
 #include "ActuatorEffectiveness.hpp"
 
 #include <px4_platform_common/module_params.h>
+#include <lib/slew_rate/SlewRate.hpp>
+
+static constexpr float kFlapSlewRate = 0.5f; // slew rate for normalized flaps setpoint [1/s]
+static constexpr float kSpoilersSlewRate = 0.5f; // slew rate for normalized spoilers setpoint [1/s]
 
 class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness
 {
@@ -53,14 +57,16 @@ public:
 		RightElevon = 6,
 		LeftVTail = 7,
 		RightVTail = 8,
-		LeftFlaps = 9,
-		RightFlaps = 10,
-		Airbrakes = 11,
+		LeftFlap = 9,
+		RightFlap = 10,
+		Airbrake = 11,
 		Custom = 12,
 		LeftATail = 13,
 		RightATail = 14,
 		SingleChannelAileron = 15,
 		SteeringWheel = 16,
+		LeftSpoiler = 17,
+		RightSpoiler = 18,
 	};
 
 	struct Params {
@@ -68,6 +74,8 @@ public:
 
 		matrix::Vector3f torque;
 		float trim;
+		float scale_flap;
+		float scale_spoiler;
 	};
 
 	ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
@@ -81,8 +89,8 @@ public:
 
 	const Params &config(int idx) const { return _params[idx]; }
 
-	void applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control, float wheel_control, int first_actuator_idx,
-				      ActuatorVector &actuator_sp) const;
+	void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
+	void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
 
 private:
 	void updateParams() override;
@@ -92,10 +100,15 @@ private:
 
 		param_t torque[3];
 		param_t trim;
+		param_t scale_flap;
+		param_t scale_spoiler;
 	};
 	ParamHandles _param_handles[MAX_COUNT];
 	param_t _count_handle;
 
 	Params _params[MAX_COUNT] {};
 	int _count{0};
+
+	SlewRate<float> _flaps_setpoint_with_slewrate;
+	SlewRate<float> _spoilers_setpoint_with_slewrate;
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -61,18 +61,20 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	return (rotors_added_successfully && surfaces_added_successfully);
 }
 
-void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
 {
 	// apply flaps
-	actuator_controls_s actuator_controls_0;
+	normalized_unsigned_setpoint_s flaps_setpoint;
 
-	if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
-		const float flaps_control = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS];
-		const float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES];
-		const float steering_wheel_control = actuator_controls_0.control[actuator_controls_s::INDEX_YAW];
-		_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
-				_first_control_surface_idx, actuator_sp);
+	if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+		_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+	}
+
+	// apply spoilers
+	normalized_unsigned_setpoint_s spoilers_setpoint;
+
+	if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+		_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
 	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -37,7 +37,7 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
 
 class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness
 {
@@ -49,15 +49,14 @@ public:
 
 	const char *name() const override { return "Fixed Wing"; }
 
-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
-	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -63,20 +63,22 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }
 
-void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
 {
-	// apply flaps
 	if (matrix_index == 1) {
-		actuator_controls_s actuator_controls_1;
+		// apply flaps
+		normalized_unsigned_setpoint_s flaps_setpoint;
 
-		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
-			const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
-			const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
-			_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
-					_first_control_surface_idx, actuator_sp);
+		if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+			_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+
+		// apply spoilers
+		normalized_unsigned_setpoint_s spoilers_setpoint;
+
+		if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
 		}
 	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -45,7 +45,8 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+
 
 class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness
 {
@@ -72,9 +73,7 @@ public:
 		normalize[1] = false;
 	}
 
-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
@@ -89,6 +88,7 @@ private:
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 
-	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -68,6 +68,27 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }
 
+void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
+{
+	if (matrix_index == 1) {
+		// apply flaps
+		normalized_unsigned_setpoint_s flaps_setpoint;
+
+		if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+			_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+
+		// apply spoilers
+		normalized_unsigned_setpoint_s spoilers_setpoint;
+
+		if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+	}
+
+}
+
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
 {
 	if (_flight_phase == flight_phase) {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -43,6 +43,8 @@
 #include "ActuatorEffectivenessRotors.hpp"
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+
 #include <uORB/Subscription.hpp>
 
 class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness
@@ -68,6 +70,8 @@ public:
 		normalize[1] = false;
 	}
 
+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }
@@ -80,4 +84,7 @@ protected:
 	uint32_t _stopped_motors{}; ///< currently stopped motors
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
+
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -91,23 +91,31 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
 
+void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt, int matrix_index,
+		ActuatorVector &actuator_sp)
+{
+	if (matrix_index == 1) {
+		// apply flaps
+		normalized_unsigned_setpoint_s flaps_setpoint;
+
+		if (_flaps_setpoint_sub.copy(&flaps_setpoint)) {
+			_control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+
+		// apply spoilers
+		normalized_unsigned_setpoint_s spoilers_setpoint;
+
+		if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) {
+			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
+		}
+	}
+
+}
+
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
-	// apply flaps
-	if (matrix_index == 1) {
-		actuator_controls_s actuator_controls_1;
-
-		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
-			const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
-			const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
-			_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
-					_first_control_surface_idx, actuator_sp);
-		}
-	}
-
 	// apply tilt
 	if (matrix_index == 0) {
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -46,8 +46,9 @@
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 #include "ActuatorEffectivenessTilts.hpp"
 
+#include <uORB/topics/normalized_unsigned_setpoint.h>
 #include <uORB/topics/tiltrotor_extra_controls.h>
-#include <uORB/topics/actuator_controls.h>
+
 #include <uORB/Subscription.hpp>
 
 class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness
@@ -75,6 +76,8 @@ public:
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
+	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
+
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
@@ -99,8 +102,8 @@ protected:
 
 	float _last_collective_tilt_control{NAN};
 
-	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
-	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
+	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 
 	struct YawTiltSaturationFlags {
 		bool tilt_yaw_pos;