dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: fix smooth takeoff ramp 

- start from a velocity setpoint pushing into the ground
to ramp from idle thrust up.
- start with a bit higher velocity setpoint threshold to make
sure the vehicle has a chance to really get off the ground.
- calculate ramp slope from initialization setpoint to the desired one
instead from zero to the desired. this ramps up quicker when you demand
a very small upwards velocity like the AutoLineSmoothVel and
ManualPositionSmoothVel tasks do at the beginning.
- don't stay in the takeoff ramp depending on the land detector, this
is unnecessary.
This commit is contained in:
Matthias Grob 2019-05-11 19:28:21 +01:00 committed by Lorenz Meier
parent bc77302fc9
commit 5e23883376
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -593,7 +593,7 @@ MulticopterPositionControl::run()
_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
}
// start takeoff after delay to allow motors to reach idle speed
// takeoff delay for motors to reach idle speed
_spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed);
if (_spoolup_time_hysteresis.get_state()) {
@ -980,6 +980,8 @@ MulticopterPositionControl::start_flight_task()
void
MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground)
{
const float takeoff_ramp_initialization = -0.7f;
// check if takeoff is triggered
if (_vehicle_land_detected.landed && !_in_takeoff_ramp) {
// vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered
@ -990,7 +992,7 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz
}
// upwards velocity setpoint larger than a minimal speed
_velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
_velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f;
// upwards jerk setpoint
const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
// position setpoint above the minimum altitude
@ -999,7 +1001,7 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz
_velocity_triggered_takeoff |= jerk_triggered_takeoff;
if (_velocity_triggered_takeoff || position_triggered_takeoff) {
// takeoff was triggered, start velocity ramp
_takeoff_ramp_velocity = 0.f;
_takeoff_ramp_velocity = takeoff_ramp_initialization;
_in_takeoff_ramp = true;
_takeoff_reference_z = _states.position(2);
}
@ -1016,22 +1018,22 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz
// ramp up vrtical velocity in TKO_RAMP_T seconds
if (_param_mpc_tko_ramp_t.get() > _dt) {
_takeoff_ramp_velocity += takeoff_desired_velocity * _dt / _param_mpc_tko_ramp_t.get();
_takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get();
} else {
// no ramp, directly go to desired takeoff speed
_takeoff_ramp_velocity = takeoff_desired_velocity;
}
_takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, _param_mpc_tko_speed.get());
// make sure we cannot overshoot the desired setpoint with the ramp
_takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity);
// smooth takeoff is achieved once desired altitude/velocity setpoint is reached
if (!_velocity_triggered_takeoff) {
_in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());
} else {
// make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
_in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp) || _vehicle_land_detected.landed;
_in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp);
}
}
}