Multicopter Takeoff: add downwards ramp for landing

msg/TakeoffStatus.msg

@@ -8,6 +8,7 @@ uint8 TAKEOFF_STATE_SPOOLUP           = 2
 uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 3
 uint8 TAKEOFF_STATE_RAMPUP            = 4
 uint8 TAKEOFF_STATE_FLIGHT            = 5
+uint8 TAKEOFF_STATE_RAMPDOWN          = 6
 
 uint8 takeoff_state
 

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -492,30 +492,32 @@ void MulticopterPositionControl::Run()
 
 			bool skip_takeoff = _param_com_throw_en.get();
 			// handle smooth takeoff
-			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed,
-						    _vehicle_constraints.want_takeoff,
-						    _vehicle_constraints.speed_up, skip_takeoff, vehicle_local_position.timestamp_sample);
+			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.ground_contact, _vehicle_land_detected.landed,
+						    _vehicle_constraints.want_takeoff, skip_takeoff, vehicle_local_position.timestamp_sample);
 
-			const bool not_taken_off             = (_takeoff.getTakeoffState() < TakeoffState::rampup);
-			const bool flying                    = (_takeoff.getTakeoffState() >= TakeoffState::flight);
-			const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);
-
-			if (!flying) {
+			if (!_takeoff.getInFlight() && !_takeoff.getRampdown()) {
 				_control.setHoverThrust(_param_mpc_thr_hover.get());
 			}
 
 			// make sure takeoff ramp is not amended by acceleration feed-forward
-			if (_takeoff.getTakeoffState() == TakeoffState::rampup && PX4_ISFINITE(_setpoint.velocity[2])) {
+			if (_takeoff.getRamping() && PX4_ISFINITE(_setpoint.velocity[2])) {
 				_setpoint.acceleration[2] = NAN;
 			}
 
-			if (not_taken_off || flying_but_ground_contact) {
-				// we are not flying yet and need to avoid any corrections
-				_setpoint = PositionControl::empty_trajectory_setpoint;
-				_setpoint.timestamp = vehicle_local_position.timestamp_sample;
-				Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
+			if (!_takeoff.getInFlight()) {
+				_control.resetIntegralXY(); // during ramp down we need the downwards integrator
+			}
 
-				// prevent any integrator windup
+			if (!_takeoff.getRampup() && !_takeoff.getInFlight()) {
+				// we are not flying and need to avoid any horizontal corrections
+				_setpoint.velocity[0] = _setpoint.velocity[1] = _setpoint.position[0] = _setpoint.position[1] = NAN;
+				_setpoint.acceleration[0] = _setpoint.acceleration[1] = 0.f;
+			}
+
+			if (!_takeoff.getRamping() && !_takeoff.getInFlight()) {
+				// we need to cut the thrust outside of ramping and flying
+				_setpoint.velocity[2] = _setpoint.position[2] = NAN;
+				_setpoint.acceleration[2] = 100.f; // High downwards acceleration to make sure there's no thrust
 				_control.resetIntegral();
 			}
 
@@ -530,7 +532,7 @@ void MulticopterPositionControl::Run()
 						 _param_mpc_z_vel_max_dn.get();
 
 			// Allow ramping from zero thrust on takeoff
-			const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
+			const float minimum_thrust = _takeoff.getInFlight() ? _param_mpc_thr_min.get() : 0.f;
 			_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
 
 			float max_speed_xy = _param_mpc_xy_vel_max.get();
@@ -613,8 +615,8 @@ void MulticopterPositionControl::Run()
 
 		} else {
 			// an update is necessary here because otherwise the takeoff state doesn't get skipped with non-altitude-controlled modes
-			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true,
-						    vehicle_local_position.timestamp_sample);
+			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.ground_contact, _vehicle_land_detected.landed, false,
+						    true, vehicle_local_position.timestamp_sample);
 			_control.resetIntegral();
 		}
 

src/modules/mc_pos_control/Takeoff/Takeoff.cpp

@@ -45,8 +45,8 @@ void TakeoffHandling::generateInitialRampValue(float velocity_p_gain)
 	_takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain;
 }
 
-void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
-		const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
+void TakeoffHandling::updateTakeoffState(const bool armed, const bool ground_contact, const bool landed, const bool want_takeoff,
+		const bool skip_takeoff, const hrt_abstime &now_us)
 {
 	_spoolup_time_hysteresis.set_state_and_update(armed, now_us);
 
@@ -89,6 +89,19 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co
 
 	// FALLTHROUGH
 	case TakeoffState::flight:
+		if (ground_contact) {
+			_takeoff_state = TakeoffState::rampdown;
+			_takeoff_ramp_progress = 1.f;
+		}
+
+		break;
+
+	// FALLTHROUGH
+	case TakeoffState::rampdown:
+		if (!ground_contact) {
+			_takeoff_state = TakeoffState::flight;
+		}
+
 		if (landed) {
 			_takeoff_state = TakeoffState::ready_for_takeoff;
 		}
@@ -126,9 +139,20 @@ float TakeoffHandling::updateRamp(const float dt, const float takeoff_desired_vz
 		}
 
 		if (_takeoff_ramp_progress < 1.f) {
-			upwards_velocity_limit = _takeoff_ramp_vz_init + _takeoff_ramp_progress * (takeoff_desired_vz - _takeoff_ramp_vz_init);
+			upwards_velocity_limit = math::interpolate(_takeoff_ramp_progress, 0.f, 1.f, _takeoff_ramp_vz_init, takeoff_desired_vz);
 		}
 	}
 
+	if (_takeoff_state == TakeoffState::rampdown) {
+		if (_takeoff_ramp_time > dt) {
+			_takeoff_ramp_progress -= dt / _takeoff_ramp_time;
+
+		} else {
+			_takeoff_ramp_progress = 0.f;
+		}
+
+		upwards_velocity_limit = math::interpolate(_takeoff_ramp_progress, 0.f, 1.f, _takeoff_ramp_vz_init, takeoff_desired_vz);
+	}
+
 	return upwards_velocity_limit;
 }

src/modules/mc_pos_control/Takeoff/Takeoff.hpp

@@ -50,7 +50,8 @@ enum class TakeoffState {
 	spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP,
 	ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF,
 	rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP,
-	flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT
+	flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT,
+	rampdown = takeoff_status_s::TAKEOFF_STATE_RAMPDOWN
 };
 
 class TakeoffHandling
@@ -75,8 +76,8 @@ public:
 	 * Update the state for the takeoff.
 	 * Has to be called also when not flying altitude controlled to skip the takeoff and not do it in flight when switching mode.
 	 */
-	void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
-				const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
+	void updateTakeoffState(const bool armed, const bool ground_contact, const bool landed, const bool want_takeoff, const bool skip_takeoff,
+				const hrt_abstime &now_us);
 
 	/**
 	 * Update and return the velocity constraint ramp value during takeoff.
@@ -88,7 +89,11 @@ public:
 	 */
 	float updateRamp(const float dt, const float takeoff_desired_vz);
 
-	TakeoffState getTakeoffState() { return _takeoff_state; }
+	TakeoffState getTakeoffState() const { return _takeoff_state; }
+	bool getRampup() const { return _takeoff_state == TakeoffState::rampup; }
+	bool getInFlight() const { return _takeoff_state == TakeoffState::flight; }
+	bool getRampdown() const { return _takeoff_state == TakeoffState::rampdown; }
+	bool getRamping() const { return getRampup() || getRampdown(); }
 
 private:
 	TakeoffState _takeoff_state = TakeoffState::disarmed;

src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp

@@ -50,23 +50,24 @@ TEST(TakeoffTest, RegularTakeoffRamp)
 	takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
 
 	// disarmed, landed, don't want takeoff
-	takeoff.updateTakeoffState(false, true, false, 1.f, false, 0);
+	takeoff.updateTakeoffState(false, true, true, false, false, 0);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
 
 	// armed, not landed anymore, don't want takeoff
-	takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms);
+	takeoff.updateTakeoffState(true, false, false, false, false, 500_ms);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup);
 
 	// armed, not landed, don't want takeoff yet, spoolup time passed
-	takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s);
+	takeoff.updateTakeoffState(true, false, false, false, false, 2_s);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff);
 
 	// armed, not landed, want takeoff
-	takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s);
+	takeoff.updateTakeoffState(true, false, false, true, false, 3_s);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);
 
 	// armed, not landed, want takeoff, ramping up
-	takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s);
+	takeoff.updateTakeoffState(true, false, false, true, false, 4_s);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(0.f, 1.5f), -.5f);
 	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f);
 	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f);
 	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f);
@@ -74,6 +75,23 @@ TEST(TakeoffTest, RegularTakeoffRamp)
 	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
 
 	// armed, not landed, want takeoff, rampup time passed
-	takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms);
+	takeoff.updateTakeoffState(true, false, false, true, false, 6500_ms);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight);
+
+	// armed, ground contact, not landed, don't want takeoff
+	takeoff.updateTakeoffState(true, true, false, false, false, 10_s);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampdown);
+
+	// armed, ground contact, not landed, don't want takeoff, ramping down
+	takeoff.updateTakeoffState(true, true, false, false, false, 11_s);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(0.f, 1.5f), 1.5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), -.5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), -.5f);
+
+	// armed, ground contact, landed, don't want takeoff
+	takeoff.updateTakeoffState(true, true, true, false, false, 18_s);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff);
 }