diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-01 14:02:24 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-01 14:02:24 +0400 |
commit | 8c1c7bca18e6d4c996852ef042a50ee34af6cc33 (patch) | |
tree | eca57acae5a0f9250c29471e026f13e6a810c420 /src/modules/mc_pos_control/mc_pos_control_main.cpp | |
parent | 40d03666fdc037b578400b8e04705ca0f5a1a092 (diff) | |
download | px4-firmware-8c1c7bca18e6d4c996852ef042a50ee34af6cc33.tar.gz px4-firmware-8c1c7bca18e6d4c996852ef042a50ee34af6cc33.tar.bz2 px4-firmware-8c1c7bca18e6d4c996852ef042a50ee34af6cc33.zip |
mc_pos_control: takeoff fixes, ignore position and yaw, takeoff vertically
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 28 |
1 files changed, 19 insertions, 9 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 c864db7ed..3c05cec07 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -609,29 +609,39 @@ MulticopterPositionControl::task_main() /* AUTO */ if (_mission_items.current_valid) { struct mission_item_s item = _mission_items.current; - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - // 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); + } - if (_mission_items.current.nav_cmd == NAV_CMD_TAKEOFF) { /* 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)); - _att_sp.yaw_body = _mission_items.current.yaw; + // TODO home altitude can be != ref_alt, check home_position topic + _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + _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) { + reset_sp_xy = false; _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); } if (reset_sp_z) { + reset_sp_z = false; _pos_sp(2) = _pos(2); } } |