fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 11:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: takeoff fixes, ignore position and yaw, takeoff vertically

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-01 14:02:24 +04:00
parent 40d03666fd
commit 8c1c7bca18
1 changed files with 21 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+21 -11
View File
@@ -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));
// TODO home altitude can be != ref_alt, check home_position topic
_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
_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;
}
_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);
}
}