diff --git a/msg/TakeoffStatus.msg b/msg/TakeoffStatus.msg index 4cc49d5094..43637f8493 100644 --- a/msg/TakeoffStatus.msg +++ b/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 diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0501dadcc8..d2f54f6e42 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/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(); } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 2e4fbc1f40..7e14a9399e 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/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; } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 1de486e325..e7859bcfd1 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/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;