FlightModeManager: fix takeoff ramp from zero

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

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

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -109,6 +109,7 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
 		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time,
 		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t,
-		(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc
+		(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
+		(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min
 	);
 };

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)) {