From 6111843e7aca01c6a1e749e6c2b4159cea5a5a00 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 26 Nov 2025 05:46:48 +0100 Subject: [PATCH] Multicopter Takeoff: add downwards ramp for landing --- .../MulticopterPositionControl.cpp | 25 ++++++++++------ .../mc_pos_control/Takeoff/Takeoff.cpp | 16 ++++++++-- .../mc_pos_control/Takeoff/Takeoff.hpp | 5 ++-- .../mc_pos_control/Takeoff/TakeoffTest.cpp | 30 +++++++++++++++---- 4 files changed, 57 insertions(+), 19 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d2f54f6e42..9964a6a7d0 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -493,24 +493,31 @@ void MulticopterPositionControl::Run() bool skip_takeoff = _param_com_throw_en.get(); // handle smooth takeoff _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); + _vehicle_constraints.want_takeoff, skip_takeoff, vehicle_local_position.timestamp_sample); 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 (!_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 + 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(); } @@ -609,7 +616,7 @@ 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.ground_contact, _vehicle_land_detected.landed, false, - 10.f, true, vehicle_local_position.timestamp_sample); + 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 7e14a9399e..a017de35f5 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -46,7 +46,7 @@ void TakeoffHandling::generateInitialRampValue(float velocity_p_gain) } 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) + const bool skip_takeoff, const hrt_abstime &now_us) { _spoolup_time_hysteresis.set_state_and_update(armed, now_us); @@ -91,6 +91,7 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool ground_con case TakeoffState::flight: if (ground_contact) { _takeoff_state = TakeoffState::rampdown; + _takeoff_ramp_progress = 1.f; } break; @@ -138,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; } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index e7859bcfd1..20f2cf2341 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -76,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 ground_contact, 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. @@ -93,6 +93,7 @@ public: 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; diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp index d718ef687b..fb40ca175c 100644 --- a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp +++ b/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); }