forked from Archive/PX4-Autopilot
navigator: when landed only exit idle if mission/takoff
This commit is contained in:
parent
a55f97503e
commit
9e383f2cd3
|
@ -1605,6 +1605,7 @@ void MulticopterPositionControl::control_auto(float dt)
|
|||
|
||||
} else {
|
||||
/* no waypoint, do nothing, setpoint was already reset */
|
||||
/* we are in idle */
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue