From 4b8f93de5c854f7a2e6cf5ed4c88f82bc42aa9de Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 27 Apr 2022 16:58:33 +0200 Subject: [PATCH] FW: rework spoiler/flap control logic - remove separate flaperon controls input to mixer instead enable spoiler support - add slew rate limiting on both flap and spoiler controls - add spoiler configuration for Landing and Descend - add trimming from spoiler deflection - FW Attitude control: remove FW_FLAPS_SCL param --> The flap settings for takeoff and landing are now specified relative to full range. Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/1030_plane | 2 + .../mixers-sitl/plane_sitl.main.mix | 6 +- ROMFS/px4fmu_common/mixers/AAERTWF.main.mix | 6 +- ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 6 +- .../mixers/AAVVTWFF_vtail.main.mix | 8 +- ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix | 8 +- ROMFS/px4fmu_test/mixers/AAERTWF.main.mix | 6 +- ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix | 8 +- ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix | 10 +-- src/modules/fw_att_control/CMakeLists.txt | 1 + .../FixedwingAttitudeControl.cpp | 88 +++++++++++-------- .../FixedwingAttitudeControl.hpp | 30 +++++-- .../fw_att_control/fw_att_control_params.c | 79 +++++++++++------ .../FixedwingPositionControl.cpp | 14 +-- 14 files changed, 167 insertions(+), 105 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane index 873555d7b5..4484448a83 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane @@ -25,6 +25,8 @@ param set-default FW_P_LIM_MIN -15 param set-default FW_RR_FF 0.1 param set-default FW_RR_P 0.3 +param set-default FW_SPOILERS_LND 0.8 + param set-default FW_THR_MAX 0.6 param set-default FW_THR_MIN 0.05 param set-default FW_THR_CRUISE 0.25 diff --git a/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix index 5fd6115f46..836bd0786e 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix +++ b/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix @@ -21,14 +21,16 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 # mixer for the left aileron -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 -10000 -10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 # mixer for the right aileron -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 # mixer for the elevator M: 1 diff --git a/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix index 18d8ed802f..8bd3fd0fa1 100644 --- a/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix @@ -10,7 +10,7 @@ to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -20,11 +20,11 @@ endpoints to suit. M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 Elevator mixer ------------ diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix index 17a2c0bbd6..07070c7535 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -11,7 +11,7 @@ to output 4, the wheel to output 5 and the flaps to output 6 and 7. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -21,11 +21,11 @@ endpoints to suit. M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 V-tail mixers ------------- diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix index 6bd4568da8..4baa82a89d 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix @@ -9,20 +9,20 @@ output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4, the wheel to output 5 and the flaps to output 6 and 7. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). +(roll), 1 (pitch), 2 (yaw), 3 (thrust) 4 (flaps), 5 (spoiler). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up mechanically reversed. M: 2 S: 0 0 -10000 -10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 S: 0 0 -10000 -10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 V-tail mixers ------------- diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix index e4853d66b3..e59584b213 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix @@ -5,14 +5,14 @@ Aileron/v-tail/throttle VTOL mixer for PX4FMU This file defines mixers suitable for controlling a fixed wing aircraft with aileron, v-tail (rudder, elevator) and throttle using PX4FMU. -The configuration assumes the aileron servos are connected to PX4FMU +The configuration assumes the aileron servos are connected to PX4FMU AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -22,11 +22,11 @@ endpoints to suit. M: 2 S: 1 0 -10000 -10000 0 -10000 10000 -S: 1 6 10000 10000 0 -10000 10000 +S: 1 5 10000 10000 0 -10000 10000 M: 2 S: 1 0 -10000 -10000 0 -10000 10000 -S: 1 6 -10000 -10000 0 -10000 10000 +S: 1 5 -10000 -10000 0 -10000 10000 V-tail mixers diff --git a/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix b/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix index 2d6cf285f0..150e95d894 100644 --- a/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix +++ b/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix @@ -10,7 +10,7 @@ to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -21,12 +21,12 @@ endpoints to suit. M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 Elevator mixer ------------ diff --git a/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix index e6520862d7..531cccaf1a 100644 --- a/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix @@ -9,9 +9,9 @@ output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4, the wheel to output 5 and the flaps to output 6 and 7. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 5 (spoiler). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -22,12 +22,12 @@ endpoints to suit. M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 V-tail mixers ------------- diff --git a/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix index 86581585c2..a9026ebd65 100644 --- a/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix +++ b/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix @@ -3,14 +3,14 @@ Aileron/v-tail/throttle VTOL mixer for PX4FMU This file defines mixers suitable for controlling a fixed wing aircraft with aileron, v-tail (rudder, elevator) and throttle using PX4FMU. -The configuration assumes the aileron servos are connected to PX4FMU +The configuration assumes the aileron servos are connected to PX4FMU AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 5 (spoiler). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -21,12 +21,12 @@ endpoints to suit. M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 10000 10000 0 -10000 10000 -S: 1 6 10000 10000 0 -10000 10000 +S: 1 5 10000 10000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 10000 10000 0 -10000 10000 -S: 1 6 -10000 -10000 0 -10000 10000 +S: 1 5 -10000 -10000 0 -10000 10000 V-tail mixers diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index a8c79cf31c..2cfa1435ba 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -44,4 +44,5 @@ px4_add_module( ecl_yaw_controller.cpp DEPENDS px4_work_queue + SlewRate ) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index bfb7ba7f51..ca0c8e55d1 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -67,6 +67,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); _rate_ctrl_status_pub.advertise(); + _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate); + _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -382,11 +384,14 @@ void FixedwingAttitudeControl::Run() /* if we are in rotary wing mode, do nothing */ if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { + _spoiler_setpoint_with_slewrate.setForcedValue(0.f); + _flaps_setpoint_with_slewrate.setForcedValue(0.f); perf_end(_loop_perf); return; } - control_flaps(dt); + controlFlaps(dt); + controlSpoilers(dt); /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_rates_enabled) { @@ -498,8 +503,11 @@ void FixedwingAttitudeControl::Run() } /* add trim increment if flaps are deployed */ - trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get(); - trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get(); + trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get(); + trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get(); + + // add trim increment from spoilers (only pitch) + trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get(); /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { @@ -645,11 +653,11 @@ void FixedwingAttitudeControl::Run() _actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); - _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; - _actuators.control[5] = _manual_control_setpoint.aux1; - _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; + _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + _actuators.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future - _actuators.control[7] = _manual_control_setpoint.aux3; + _actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); @@ -697,18 +705,16 @@ void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tam _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); } -void FixedwingAttitudeControl::control_flaps(const float dt) +void FixedwingAttitudeControl::controlFlaps(const float dt) { /* default flaps to center */ float flap_control = 0.0f; /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled - && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { - flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get(); + if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flap_control = _manual_control_setpoint.flaps; - } else if (_vcontrol_mode.flag_control_auto_enabled - && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { + } else if (_vcontrol_mode.flag_control_auto_enabled) { switch (_att_sp.apply_flaps) { case vehicle_attitude_setpoint_s::FLAPS_OFF: @@ -716,50 +722,54 @@ void FixedwingAttitudeControl::control_flaps(const float dt) break; case vehicle_attitude_setpoint_s::FLAPS_LAND: - flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get(); + flap_control = _param_fw_flaps_lnd_scl.get(); break; case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF: - flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get(); + flap_control = _param_fw_flaps_to_scl.get(); break; } } // move the actual control value continuous with time, full flap travel in 1sec - if (fabsf(_flaps_applied - flap_control) > 0.01f) { - _flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt; + _flaps_setpoint_with_slewrate.update(flap_control, dt); +} - } else { - _flaps_applied = flap_control; - } +void FixedwingAttitudeControl::controlSpoilers(const float dt) +{ + float spoiler_control = 0.f; - /* default flaperon to center */ - float flaperon_control = 0.0f; + if (_vcontrol_mode.flag_control_manual_enabled) { + switch (_param_fw_spoilers_man.get()) { + case 0: + break; - /* map flaperons by default to manual if valid */ - if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled - && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { + case 1: + spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f; + break; - flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); + case 2: + spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f; + break; + } - } else if (_vcontrol_mode.flag_control_auto_enabled - && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { + } else if (_vcontrol_mode.flag_control_auto_enabled) { + switch (_att_sp.apply_spoilers) { + case vehicle_attitude_setpoint_s::SPOILERS_OFF: + spoiler_control = 0.f; + break; - if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) { - flaperon_control = _param_fw_flaperon_scl.get(); + case vehicle_attitude_setpoint_s::SPOILERS_LAND: + spoiler_control = _param_fw_spoilers_lnd.get(); + break; - } else { - flaperon_control = 0.0f; + case vehicle_attitude_setpoint_s::SPOILERS_DESCEND: + spoiler_control = _param_fw_spoilers_desc.get(); + break; } } - // move the actual control value continuous with time, full flap travel in 1sec - if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) { - _flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt; - - } else { - _flaperons_applied = flaperon_control; - } + _spoiler_setpoint_with_slewrate.update(spoiler_control, dt); } void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 722c10dbd8..fe89f96e65 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,9 @@ using uORB::SubscriptionData; using namespace time_literals; +static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s] +static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s] + class FixedwingAttitudeControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -139,9 +143,6 @@ private: hrt_abstime _last_run{0}; - float _flaps_applied{0.0f}; - float _flaperons_applied{0.0f}; - float _airspeed_scaling{1.0f}; bool _landed{true}; @@ -156,6 +157,9 @@ private: float _control_energy[4] {}; float _control_prev[3] {}; + SlewRate _spoiler_setpoint_with_slewrate; + SlewRate _flaps_setpoint_with_slewrate; + DEFINE_PARAMETERS( (ParamFloat) _param_fw_acro_x_max, (ParamFloat) _param_fw_acro_y_max, @@ -171,6 +175,7 @@ private: (ParamBool) _param_fw_bat_scale_en, (ParamFloat) _param_fw_dtrim_p_flps, + (ParamFloat) _param_fw_dtrim_p_spoil, (ParamFloat) _param_fw_dtrim_p_vmax, (ParamFloat) _param_fw_dtrim_p_vmin, (ParamFloat) _param_fw_dtrim_r_flps, @@ -179,10 +184,11 @@ private: (ParamFloat) _param_fw_dtrim_y_vmax, (ParamFloat) _param_fw_dtrim_y_vmin, - (ParamFloat) _param_fw_flaperon_scl, (ParamFloat) _param_fw_flaps_lnd_scl, - (ParamFloat) _param_fw_flaps_scl, (ParamFloat) _param_fw_flaps_to_scl, + (ParamFloat) _param_fw_spoilers_lnd, + (ParamFloat) _param_fw_spoilers_desc, + (ParamInt) _param_fw_spoilers_man, (ParamFloat) _param_fw_man_p_max, (ParamFloat) _param_fw_man_p_sc, @@ -230,7 +236,19 @@ private: ECL_YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; - void control_flaps(const float dt); + /** + * @brief Update flap control setting + * + * @param dt Current time delta [s] + */ + void controlFlaps(const float dt); + + /** + * @brief Update spoiler control setting + * + * @param dt Current time delta [s] + */ + void controlSpoilers(const float dt); void updateActuatorControlsStatus(float dt); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 27666e20f6..4afe0f1192 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -457,22 +457,11 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); -/** - * Scale factor for flaps - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); - /** * Flaps setting during take-off * - * Sets a fraction of full flaps (FW_FLAPS_SCL) during take-off + * Sets a fraction of full flaps during take-off. + * Also applies to flaperons if enabled in the mixer/allocation. * * @unit norm * @min 0.0 @@ -486,7 +475,8 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); /** * Flaps setting during landing * - * Sets a fraction of full flaps (FW_FLAPS_SCL) during landing + * Sets a fraction of full flaps during landing. + * Also applies to flaperons if enabled in the mixer/allocation. * * @unit norm * @min 0.0 @@ -497,18 +487,6 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); */ PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); -/** - * Scale factor for flaperons - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); - /** * Airspeed mode * @@ -735,3 +713,52 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f); * @increment 0.01 */ PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f); + +/** + * Pitch trim increment for spoiler configuration + * + * This increment is added to the pitch trim whenever spoilers are fully deployed. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f); + +/** + * Spoiler landing setting + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); + +/** + * Spoiler descend setting + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f); + +/** + * Spoiler input in manual flight + * + * Chose source for manual setting of spoilers in manual flight modes. + * + * @value 0 Disabled + * @value 1 Flaps channel + * @value 2 Aux1 + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 2607a5ca7b..61eccce01e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -842,9 +842,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c _l1_control.set_dt(dt); } - _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder - _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps - /* save time when airplane is in air */ if (!_was_in_air && !_landed) { _was_in_air = true; @@ -1356,7 +1353,8 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa // have to do this switch (which can cause significant altitude errors) close to the ground. _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); airspeed_sp = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); - _att_sp.apply_flaps = true; + _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; + _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND; } float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt); @@ -1684,9 +1682,9 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec prev_wp(1) = pos_sp_curr.lon; } - // apply full flaps for landings. this flag will also trigger the use of flaperons - // if they have been enabled using the corresponding parameter + // Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; + _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND; // Enable tighter altitude control for landings _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); @@ -2391,6 +2389,10 @@ FixedwingPositionControl::Run() set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid); + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps + _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF; + switch (_control_mode_current) { case FW_POSCTRL_MODE_AUTO: { control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,