From 4979fb419398f7554f7dd47bc0eb554d159d256f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 19 Apr 2020 13:01:57 +0200 Subject: [PATCH] FlightTasks: do not adjust tilt limit of the position control Adjusting the tilt limit can lead to diverging position control and should only be used by setting a sanity limit for the controller and not to adjust during the descent phase of a Land or RTL. Otherwise it leads to flyaways in important failsafe modes when there's stronger disturbance e.g. wind. --- .../flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp | 6 +----- src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp | 2 +- src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp | 3 +-- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 2 -- .../tasks/ManualPosition/FlightTaskManualPosition.cpp | 2 -- .../mc_pos_control/PositionControl/PositionControl.hpp | 2 +- 6 files changed, 4 insertions(+), 13 deletions(-) diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 003e41491683..7ae06d5108a1 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -127,14 +127,10 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints() void FlightTaskAutoMapper::_prepareLandSetpoints() { - float land_speed = _getLandSpeed(); - // Keep xy-position and go down with landspeed + float land_speed = _getLandSpeed(); _position_setpoint = Vector3f(_target(0), _target(1), NAN); _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed)); - - // set constraints - _constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); _gear.landing_gear = landing_gear_s::GEAR_DOWN; } diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index 1bf30a874795..95bf42402fdc 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -181,7 +181,7 @@ void FlightTask::_setDefaultConstraints() _constraints.speed_xy = _param_mpc_xy_vel_max.get(); _constraints.speed_up = _param_mpc_z_vel_max_up.get(); _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); - _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get()); + _constraints.tilt = NAN; _constraints.min_distance_to_ground = NAN; _constraints.max_distance_to_ground = NAN; _constraints.want_takeoff = false; diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp index ce3e0cbb55a8..c3db9c8acaab 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp @@ -260,7 +260,6 @@ class FlightTask : public ModuleParams DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamFloat) _param_mpc_xy_vel_max, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_z_vel_max_up, - (ParamFloat) _param_mpc_tiltmax_air + (ParamFloat) _param_mpc_z_vel_max_up ) }; diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 4b7858a7d3a2..48ccc4dde653 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -60,8 +60,6 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s _velocity_setpoint(2) = 0.f; _setDefaultConstraints(); - _constraints.tilt = math::radians(_param_mpc_man_tilt_max.get()); - _updateConstraintsFromEstimator(); _max_speed_up = _constraints.speed_up; diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 8f6944ced95e..a771e8aa5826 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -61,8 +61,6 @@ bool FlightTaskManualPosition::activate(vehicle_local_position_setpoint_s last_s // all requirements from altitude-mode still have to hold bool ret = FlightTaskManualAltitude::activate(last_setpoint); - _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get()); - // set task specific constraint if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) { _constraints.speed_xy = _param_mpc_vel_manual.get(); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 2cfab32ee4f0..a581ec3529d1 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -109,7 +109,7 @@ class PositionControl /** * Set the maximum tilt angle in radians the output attitude is allowed to have - * @param tilt angle from level orientation in radians + * @param tilt angle in radians from level orientation */ void setTiltLimit(const float tilt) { _lim_tilt = tilt; }