VTOL: add flap and spoiler support

- including slew rate limiting
- adds option to set spoiler setting during the Land phase (hover)

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

src/modules/vtol_att_control/CMakeLists.txt

@@ -39,5 +39,7 @@ px4_add_module(
 		vtol_type.cpp
 		tailsitter.cpp
 		standard.cpp
+	DEPENDS
+		SlewRate
 	)
 

src/modules/vtol_att_control/standard.cpp

@@ -288,6 +288,10 @@ void Standard::update_transition_state()
 			}
 		}
 
+		// set spoiler and flaps to 0
+		_flaps_setpoint_with_slewrate.update(0.f, _dt);
+		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
+
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
 
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
@@ -370,8 +374,9 @@ void Standard::fill_actuator_outputs()
 		fw_out[actuator_controls_s::INDEX_PITCH]        = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH];
 		fw_out[actuator_controls_s::INDEX_YAW]          = 0;
 		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;
-		fw_out[actuator_controls_s::INDEX_FLAPS]        = 0;
-		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = 0;
+		fw_out[actuator_controls_s::INDEX_FLAPS]        = _flaps_setpoint_with_slewrate.getState();
+		fw_out[actuator_controls_s::INDEX_SPOILERS]    	= _spoiler_setpoint_with_slewrate.getState();
+		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = 0.f;
 
 		break;
 
@@ -391,7 +396,8 @@ void Standard::fill_actuator_outputs()
 		fw_out[actuator_controls_s::INDEX_PITCH]        = fw_in[actuator_controls_s::INDEX_PITCH];
 		fw_out[actuator_controls_s::INDEX_YAW]          = fw_in[actuator_controls_s::INDEX_YAW];
 		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;
-		fw_out[actuator_controls_s::INDEX_FLAPS]        = fw_in[actuator_controls_s::INDEX_FLAPS];
+		fw_out[actuator_controls_s::INDEX_FLAPS]        = _flaps_setpoint_with_slewrate.getState();
+		fw_out[actuator_controls_s::INDEX_SPOILERS]    	= _spoiler_setpoint_with_slewrate.getState();
 		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = _reverse_output;
 
 		break;
@@ -409,7 +415,8 @@ void Standard::fill_actuator_outputs()
 		fw_out[actuator_controls_s::INDEX_PITCH]        = fw_in[actuator_controls_s::INDEX_PITCH];
 		fw_out[actuator_controls_s::INDEX_YAW]          = fw_in[actuator_controls_s::INDEX_YAW];
 		fw_out[actuator_controls_s::INDEX_THROTTLE]     = fw_in[actuator_controls_s::INDEX_THROTTLE];
-		fw_out[actuator_controls_s::INDEX_FLAPS]        = fw_in[actuator_controls_s::INDEX_FLAPS];
+		fw_out[actuator_controls_s::INDEX_FLAPS]        = _flaps_setpoint_with_slewrate.getState();
+		fw_out[actuator_controls_s::INDEX_SPOILERS]    	= _spoiler_setpoint_with_slewrate.getState();
 		fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = 0;
 		break;
 	}

src/modules/vtol_att_control/tiltrotor.cpp

@@ -370,6 +370,10 @@ void Tiltrotor::update_transition_state()
 		// add minimum throttle for front transition
 		_thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN);
 
+		// set spoiler and flaps to 0
+		_flaps_setpoint_with_slewrate.update(0.f, _dt);
+		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
+
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = math::constrain(_params_tiltrotor.tilt_transition +
@@ -392,6 +396,10 @@ void Tiltrotor::update_transition_state()
 		// if this is not then then there is race condition where the fw rate controller still publishes a zero sample throttle after transition
 		_v_att_sp->thrust_body[0] = _thrust_transition;
 
+		// set spoiler and flaps to 0
+		_flaps_setpoint_with_slewrate.update(0.f, _dt);
+		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
+
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 
 		// set idle speed for rotary wing mode
@@ -534,9 +542,9 @@ void Tiltrotor::fill_actuator_outputs()
 	fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
 
 	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
-		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
-		fw_out[actuator_controls_s::INDEX_YAW]   = 0;
+		fw_out[actuator_controls_s::INDEX_ROLL]  	= 0;
+		fw_out[actuator_controls_s::INDEX_PITCH] 	= 0;
+		fw_out[actuator_controls_s::INDEX_YAW]   	= 0;
 
 	} else {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = fw_in[actuator_controls_s::INDEX_ROLL];
@@ -547,6 +555,10 @@ void Tiltrotor::fill_actuator_outputs()
 		_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
 	}
 
+	fw_out[actuator_controls_s::INDEX_FLAPS]        = _flaps_setpoint_with_slewrate.getState();
+	fw_out[actuator_controls_s::INDEX_SPOILERS]     = _spoiler_setpoint_with_slewrate.getState();
+	fw_out[actuator_controls_s::INDEX_AIRBRAKES]    = 0;
+
 	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -109,6 +109,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 
 	_params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
 
+	_params_handles.vt_spoiler_mc_ld = param_find("VT_SPOILER_MC_LD");
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -372,6 +373,8 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
 	param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
 
+	param_get(_params_handles.vt_spoiler_mc_ld, &_params.vt_spoiler_mc_ld);
+
 	// update the parameters of the instances of base VtolType
 	if (_vtol_type != nullptr) {
 		_vtol_type->parameters_update();
@@ -401,6 +404,7 @@ VtolAttitudeControl::Run()
 
 #endif // !ENABLE_LOCKSTEP_SCHEDULER
 
+	const float dt = math::min((now - _last_run_timestamp) / 1e6f, kMaxVTOLAttitudeControlTimeStep);
 	_last_run_timestamp = now;
 
 	if (!_initialized) {
@@ -415,6 +419,8 @@ VtolAttitudeControl::Run()
 		}
 	}
 
+	_vtol_type->setDt(dt);
+
 	perf_begin(_loop_perf);
 
 	const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in);

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -92,6 +92,8 @@ using namespace time_literals;
 
 extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
 
+static constexpr float kMaxVTOLAttitudeControlTimeStep = 0.1f; // max time step [s]
+
 class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
 {
 public:
@@ -249,6 +251,7 @@ private:
 		param_t mpc_land_alt1;
 		param_t mpc_land_alt2;
 		param_t sys_ctrl_alloc;
+		param_t vt_spoiler_mc_ld;
 	} _params_handles{};
 
 	hrt_abstime _last_run_timestamp{0};

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -384,3 +384,15 @@ PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_LND_PTCH_MIN, -5.0f);
+
+/**
+ * Spoiler setting while landing (hover)
+ *
+ * @unit norm
+ * @min -1
+ * @max 1
+ * @decimal 1
+ * @increment 0.05
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_SPOILER_MC_LD, 0.f);

src/modules/vtol_att_control/vtol_type.cpp

@@ -136,6 +136,9 @@ bool VtolType::init()
 		}
 	}
 
+	_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol);
+	_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol);
+
 	return true;
 
 }
@@ -157,6 +160,16 @@ void VtolType::update_mc_state()
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
 	_mc_throttle_weight = 1.0f;
+
+	float spoiler_setpoint_hover = 0.f;
+
+	if (_attc->get_pos_sp_triplet()->current.valid
+	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		spoiler_setpoint_hover = _params->vt_spoiler_mc_ld;
+	}
+
+	_spoiler_setpoint_with_slewrate.update(spoiler_setpoint_hover, _dt);
+	_flaps_setpoint_with_slewrate.update(0.f, _dt);
 }
 
 void VtolType::update_fw_state()
@@ -205,6 +218,9 @@ void VtolType::update_fw_state()
 	}
 
 	check_quadchute_condition();
+
+	_spoiler_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_SPOILERS], _dt);
+	_flaps_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_FLAPS], _dt);
 }
 
 void VtolType::update_transition_state()
@@ -215,8 +231,6 @@ void VtolType::update_transition_state()
 	_last_loop_ts = t_now;
 	_throttle_blend_start_ts = t_now;
 
-
-
 	check_quadchute_condition();
 }
 

src/modules/vtol_att_control/vtol_type.h

@@ -46,6 +46,10 @@
 #include <lib/mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
+#include <lib/slew_rate/SlewRate.hpp>
+
+static constexpr float kFlapSlewRateVtol = 1.f; // minimum time from none to full flap deflection [s]
+static constexpr float kSpoilerSlewRateVtol = 1.f; // minimum time from none to full spoiler deflection [s]
 
 struct Params {
 	int32_t ctrl_alloc;
@@ -81,6 +85,7 @@ struct Params {
 	int32_t vt_forward_thrust_enable_mode;
 	float mpc_land_alt1;
 	float mpc_land_alt2;
+	float vt_spoiler_mc_ld;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
@@ -207,6 +212,14 @@ public:
 
 	virtual void parameters_update() = 0;
 
+	/**
+	 * @brief Set current time delta
+	 *
+	 * @param dt Current time delta [s]
+	 */
+	void setDt(float dt) {_dt = dt; }
+
+protected:
 	VtolAttitudeControl *_attc;
 	mode _vtol_mode;
 
@@ -292,6 +305,11 @@ public:
 
 	float update_and_get_backtransition_pitch_sp();
 
+	SlewRate<float> _spoiler_setpoint_with_slewrate;
+	SlewRate<float> _flaps_setpoint_with_slewrate;
+
+	float _dt{0.0025f}; // time step [s]
+
 private: