fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 11:50:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Multicopter Takeoff: add downwards ramp for landing

Browse Source
This commit is contained in:
Matthias Grob
2025-11-26 05:46:48 +01:00
parent a5fcbc2722
commit 6111843e7a
4 changed files with 57 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+16 -9
View File
@@ -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();
}
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
+14 -2
View File
@@ -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;
}
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
+3 -2
View File
@@ -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;
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
+24 -6
View File
@@ -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);
}