Navigator: Loiter: always establish new Loiter with center at current pos

When switching into Hold mode establish a Loiter around current position,
even if we were before already loitering (eg in Mission mode).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-06-29 14:22:24 +02:00
parent 7e0f0516a5
commit 8838ebd77d
1 changed files with 4 additions and 8 deletions

View File

@ -61,6 +61,7 @@ Loiter::on_activation()
reposition();
} else {
// this is executed when the flight mode is switched to Hold manually, not through a reposition
set_loiter_position();
}
@ -109,16 +110,11 @@ Loiter::set_loiter_position()
_mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
} else {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
}
setLoiterItemFromCurrentPosition(&_mission_item);
}
}