diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 14:52:49 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 14:52:49 +0400 |
commit | fe43a900c744c788a8a0231ec9f0748098fdcb14 (patch) | |
tree | ed28d155fe27a4d6589c8814febd59bf6552ae39 /src | |
parent | 5bedd8c714685b73eff67da1586c90c874d990ab (diff) | |
download | px4-firmware-fe43a900c744c788a8a0231ec9f0748098fdcb14.tar.gz px4-firmware-fe43a900c744c788a8a0231ec9f0748098fdcb14.tar.bz2 px4-firmware-fe43a900c744c788a8a0231ec9f0748098fdcb14.zip |
mc_pos_control: takeoff hack removed
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 32 |
1 files changed, 7 insertions, 25 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3ee98bd41..a50a9e50e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -458,7 +458,6 @@ MulticopterPositionControl::task_main() bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; - bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; @@ -511,7 +510,6 @@ MulticopterPositionControl::task_main() /* reset setpoints and integrals on arming */ reset_sp_z = true; reset_sp_xy = true; - reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -607,8 +605,6 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); } - reset_takeoff_sp = true; - } else { /* AUTO */ if (_mission_items.current_valid) { @@ -617,30 +613,16 @@ MulticopterPositionControl::task_main() // TODO home altitude can be != ref_alt, check home_position topic _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); - if (item.nav_cmd == NAV_CMD_TAKEOFF) { - /* use current position setpoint or current position */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } - - /* add gap for takeoff setpoint */ - _pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2); - reset_sp_z = true; - - } else { - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - - if (isfinite(_mission_items.current.yaw)) { - _att_sp.yaw_body = _mission_items.current.yaw; - } + map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + if (isfinite(_mission_items.current.yaw)) { + _att_sp.yaw_body = _mission_items.current.yaw; } + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; + } else { /* no waypoint, loiter, reset position setpoint if needed */ if (reset_sp_xy) { |