dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator: when landed only exit idle if mission/takoff

This commit is contained in:
Dennis Mannhart 2017-05-03 17:13:33 +02:00
parent a55f97503e
commit 9e383f2cd3
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1605,6 +1605,7 @@ void MulticopterPositionControl::control_auto(float dt)
} else {
/* no waypoint, do nothing, setpoint was already reset */
/* we are in idle */
}
}

13
src/modules/navigator/navigator_main.cpp
View File

@ -200,6 +200,7 @@ void
Navigator::vehicle_land_detected_update()
{
orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
}
void
@ -633,6 +634,18 @@ Navigator::task_main()
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
/* if we landed and have not received takoff setpoint then stay in idle */
if (_land_detected.landed &&
!((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
|| (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
}
/* if nothing is running, set position setpoint triplet invalid once */
if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
_pos_sp_triplet_published_invalid_once = true;