From 8329208b848e7c1d1473c8cbddca139a2c3952cd Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 24 Oct 2020 13:15:00 +0200 Subject: [PATCH] FlightModeManager: fix takeoff ramp from zero --- msg/vehicle_constraints.msg | 1 + .../flight_mode_manager/FlightModeManager.cpp | 13 +++++++------ .../flight_mode_manager/FlightModeManager.hpp | 3 ++- .../mc_pos_control/MulticopterPositionControl.cpp | 1 + 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index da8c39aa24..0982e09165 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -11,3 +11,4 @@ float32 tilt # in radians [0, PI] float32 min_distance_to_ground # in meters float32 max_distance_to_ground # in meters bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) +float32 minimum_thrust # tell controller what the minimum collective output thrust should be diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 8a18c5f7e1..0067744046 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -464,13 +464,14 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, _trajectory_setpoint_pub.publish(setpoint); - // if (flying) { TODO - // _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); + // Allow ramping from zero thrust on takeoff + if (flying) { + constraints.minimum_thrust = _param_mpc_thr_min.get(); - // } else { - // // allow zero thrust when taking off and landing - // _control.setThrustLimits(0.f, _param_mpc_thr_max.get()); - // } + } else { + // allow zero thrust when taking off and landing + constraints.minimum_thrust = 0.f; + } // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 71f71b8a21..0352196d16 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -109,6 +109,7 @@ private: (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_spoolup_time, (ParamFloat) _param_mpc_tko_ramp_t, - (ParamFloat) _param_mpc_z_vel_p_acc + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_thr_min ); }; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 860cfeca72..95b97f551a 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -248,6 +248,7 @@ void MulticopterPositionControl::Run() vehicle_constraints_s constraints; _vehicle_constraints_sub.update(&constraints); _control.setConstraints(constraints); + _control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get()); // Run position control if (!_control.update(dt)) {