Multicopter Takeoff: add rampdown state 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,15 +492,10 @@ 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, _vehicle_constraints.speed_up, 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());
 			}
 
@@ -509,8 +504,8 @@ void MulticopterPositionControl::Run()
 				_setpoint.acceleration[2] = NAN;
 			}
 
-			if (not_taken_off || flying_but_ground_contact) {
-				// we are not flying yet and need to avoid any corrections
+			if (!_takeoff.getRampup() && !_takeoff.getInFlight()) {
+				// we are not flying 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
@@ -530,7 +525,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 +608,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,
+						    10.f, true, vehicle_local_position.timestamp_sample);
 			_control.resetIntegral();
 		}
 

src/modules/mc_pos_control/Takeoff/Takeoff.cpp

@@ -45,7 +45,7 @@ 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,
+void TakeoffHandling::updateTakeoffState(const bool armed, const bool ground_contact, const bool landed, const bool want_takeoff,
 		const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
 {
 	_spoolup_time_hysteresis.set_state_and_update(armed, now_us);
@@ -89,6 +89,18 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co
 
 	// FALLTHROUGH
 	case TakeoffState::flight:
+		if (ground_contact) {
+			_takeoff_state = TakeoffState::rampdown;
+		}
+
+		break;
+
+	// FALLTHROUGH
+	case TakeoffState::rampdown:
+		if (!ground_contact) {
+			_takeoff_state = TakeoffState::flight;
+		}
+
 		if (landed) {
 			_takeoff_state = TakeoffState::ready_for_takeoff;
 		}

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,7 +76,7 @@ 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,
+	void updateTakeoffState(const bool armed, const bool ground_contact, const bool landed, const bool want_takeoff,
 				const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
 
 	/**
@@ -88,7 +89,10 @@ 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; }
 
 private:
 	TakeoffState _takeoff_state = TakeoffState::disarmed;