mc_pos_control: switch back to velocity ramp

But fix the two crucial problems:
- When to begin the ramp?
There's a calculation now for the velocity ramp initial value
such that the resulting thrust is zero at the beginning.
- When to end the ramp?
The ramp is applied to the upwards velocity constraint and it
just ramps from the initial value to the velocity constraint
which is applied during flight. Slower/going down is always possible.

src/modules/mc_pos_control/Takeoff/Takeoff.cpp

@@ -36,10 +36,16 @@
  */
 
 #include "Takeoff.hpp"
-#include <float.h>
+#include <mathlib/mathlib.h>
+
+void Takeoff::generateInitialValue(const float hover_thrust, float velocity_p_gain)
+{
+	velocity_p_gain = math::max(velocity_p_gain, 0.01f);
+	_takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain;
+}
 
 void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
-				 const float takeoff_desired_thrust, const bool skip_takeoff)
+				 const float takeoff_desired_vz, const bool skip_takeoff)
 {
 	_spoolup_time_hysteresis.set_state_and_update(armed);
 
@@ -63,14 +69,14 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool
 	case TakeoffState::ready_for_takeoff:
 		if (want_takeoff) {
 			_takeoff_state = TakeoffState::rampup;
-			_takeoff_ramp_thrust = 0.0f;
+			_takeoff_ramp_vz = _takeoff_ramp_vz_init;
 
 		} else {
 			break;
 		}
 
 	case TakeoffState::rampup:
-		if (_takeoff_ramp_thrust <= takeoff_desired_thrust) {
+		if (_takeoff_ramp_vz >= takeoff_desired_vz) {
 			_takeoff_state = TakeoffState::flight;
 
 		} else {
@@ -98,24 +104,24 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool
 	}
 }
 
-float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt)
+float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz)
 {
 	if (_takeoff_state < TakeoffState::rampup) {
-		return 0.f;
+		return _takeoff_ramp_vz_init;
 	}
 
 	if (_takeoff_state == TakeoffState::rampup) {
-		if (_takeoff_ramp_time > FLT_EPSILON) {
-			_takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time;
+		if (_takeoff_ramp_time > dt) {
+			_takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time;
 
 		} else {
-			_takeoff_ramp_thrust = takeoff_desired_thrust;
+			_takeoff_ramp_vz = takeoff_desired_vz;
 		}
 
-		if (_takeoff_ramp_thrust > takeoff_desired_thrust) {
-			return _takeoff_ramp_thrust;
+		if (_takeoff_ramp_vz < takeoff_desired_vz) {
+			return _takeoff_ramp_vz;
 		}
 	}
 
-	return takeoff_desired_thrust;
+	return takeoff_desired_vz;
 }

src/modules/mc_pos_control/Takeoff/Takeoff.hpp

@@ -55,29 +55,31 @@ enum class TakeoffState {
 class Takeoff
 {
 public:
-
 	Takeoff() = default;
 	~Takeoff() = default;
 
+	// initialize parameters
+	void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
+	void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
+	void generateInitialValue(const float hover_thrust, const float velocity_p_gain);
+
 	/**
 	 * Update the state for the takeoff.
 	 * @param setpoint a vehicle_local_position_setpoint_s structure
 	 * @return true if setpoint has updated correctly
 	 */
 	void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
-				const float takeoff_desired_thrust, const bool skip_takeoff);
-	float updateThrustRamp(const float dt, const float takeoff_desired_thrust);
+				const float takeoff_desired_vz, const bool skip_takeoff);
+	float updateRamp(const float dt, const float takeoff_desired_vz);
 
-	void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
-	void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
 	TakeoffState getTakeoffState() { return _takeoff_state; }
 
-	// TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time
-	systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
-
 private:
 	TakeoffState _takeoff_state = TakeoffState::disarmed;
 
 	float _takeoff_ramp_time = 0.f;
-	float _takeoff_ramp_thrust = 0.f;
+	float _takeoff_ramp_vz_init = 0.f;
+	float _takeoff_ramp_vz = 0.f;
+
+	systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
 };

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -151,7 +151,9 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 		(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
 		(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
-		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */
+		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
+		(ParamFloat<px4::params::MPC_THR_HOVER>)_param_mpc_thr_hover,
+		(ParamFloat<px4::params::MPC_Z_VEL_P>)_param_mpc_z_vel_p
 	);
 
 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -358,6 +360,7 @@ MulticopterPositionControl::parameters_update(bool force)
 		// set trigger time for takeoff delay
 		_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
 		_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
+		_takeoff.generateInitialValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get());
 
 		if (_wv_controller != nullptr) {
 			_wv_controller->update_parameters();
@@ -582,7 +585,7 @@ MulticopterPositionControl::run()
 		}
 
 		// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
-		_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, -1.f, !_control_mode.flag_control_climb_rate_enabled);
+		_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled);
 
 		// takeoff delay for motors to reach idle speed
 		if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) {
@@ -634,6 +637,30 @@ MulticopterPositionControl::run()
 			// check if all local states are valid and map accordingly
 			set_vehicle_states(setpoint.vz);
 
+			// handle smooth takeoff
+			_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled);
+			constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);
+
+			if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
+				// we are not flying yet and need to avoid any corrections
+				reset_setpoint_to_nan(setpoint);
+				setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
+				// set yaw-sp to current yaw
+				// TODO: we need a clean way to disable yaw control
+				setpoint.yaw = _states.yaw;
+				setpoint.yawspeed = 0.f;
+				// prevent any integrator windup
+				_control.resetIntegralXY();
+				_control.resetIntegralZ();
+				// reactivate the task which will reset the setpoint to current state
+				_flight_tasks.reActivate();
+			}
+
+			if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
+				constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
+			}
+
+
 			// limit altitude only if local position is valid
 			if (PX4_ISFINITE(_states.position(2))) {
 				limit_altitude(setpoint);
@@ -672,28 +699,6 @@ MulticopterPositionControl::run()
 			local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
 			_control.getThrustSetpoint().copyTo(local_pos_sp.thrust);
 
-			// handle smooth takeoff
-			_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled);
-			local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt);
-
-			if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
-				// we are not flying yet and need to avoid any corrections
-				// set the horizontal thrust to zero
-				local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.0f;
-				// set yaw-sp to current yaw to avoid any corrections
-				// TODO: we need a clean way to disable yaw control
-				local_pos_sp.yaw = _states.yaw;
-				local_pos_sp.yawspeed = 0.f;
-				// prevent any integrator windup
-				_control.resetIntegralXY();
-				_control.resetIntegralZ();
-			}
-
-			if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
-				// reactivate the task which will reset the setpoint to current state
-				_flight_tasks.reActivate();
-			}
-
 			// Publish local position setpoint
 			// This message will be used by other modules (such as Landdetector) to determine
 			// vehicle intention.