Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

do not do jumped takeoff if already in air

Browse Source
This commit is contained in:
Roman
2016-01-26 18:18:52 +01:00
committed by Andreas Antener
parent 3928924c43
commit ba169ce0b5
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -1
View File
@@ -1360,6 +1360,12 @@ MulticopterPositionControl::task_main()
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore
if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON ) {
_takeoff_jumped = true;
}
if (!_takeoff_jumped) {
// ramp thrust setpoint up
if (_vel(2) > -(_params.tko_speed / 2.0f)) {
@@ -1371,6 +1377,7 @@ MulticopterPositionControl::task_main()
// into an integral part and into a P part
thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);
_vel_sp_prev(2) = _vel(2);
_takeoff_jumped = true;
reset_int_z = false;
}
@@ -1378,7 +1385,6 @@ MulticopterPositionControl::task_main()
if (_takeoff_jumped) {
_vel_sp(2) = -_params.tko_speed;
_vel_sp_prev(2) = -_params.tko_speed;
}
} else {