Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

mc_pos_control - add takeoff_ramp_time zero division guard

Browse Source
This commit is contained in:
bresch
2019-02-08 16:10:33 +01:00
parent 70ddb8049a
commit 106f0769fd
2 changed files with 15 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+11 -6
View File
@@ -1045,7 +1045,7 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl
0.2f; 0.2f;
// takeoff was initiated through velocity setpoint // takeoff was initiated through velocity setpoint
_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < math::min(-_tko_speed.get(), -0.6f); _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) {
// There is a position setpoint above current position or velocity setpoint larger than // There is a position setpoint above current position or velocity setpoint larger than
@@ -1076,19 +1076,24 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float
} }
// Ramp up takeoff speed. // Ramp up takeoff speed.
if (_takeoff_ramp_time.get() > _dt) {
_takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get();
_takeoff_speed = math::min(_takeoff_speed, desired_tko_speed);
} else {
// No ramp, directly go to desired takeoff speed
_takeoff_speed = desired_tko_speed;
}
_takeoff_speed = math::min(_takeoff_speed, _tko_speed.get());
// Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached.
if (!_smooth_velocity_takeoff) { if (!_smooth_velocity_takeoff) {
_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get()); _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get());
} else { } else {
_in_smooth_takeoff = _takeoff_speed < -vz_sp; // Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
_in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed;
} }
} else {
_in_smooth_takeoff = false;
} }
} }
src/modules/mc_pos_control/mc_pos_control_params.c
+2 -1
View File
@@ -650,8 +650,9 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
* *
* Increasing this value will make automatic and manual takeoff slower. * Increasing this value will make automatic and manual takeoff slower.
* If it's too slow the drone might scratch the ground and tip over. * If it's too slow the drone might scratch the ground and tip over.
* A time constant of 0 disables the ramp
* *
* @min 0.1 * @min 0
* @max 1 * @max 1
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */