From abfd00aeb9cab339077f7b92e881f68eb2e3acbe Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 2 Oct 2023 09:30:17 +0200 Subject: [PATCH] mission_base: if FW and takeoff do not enter climb before mission start For FW takeoffs we need to keep the course straight towards the first waypoint, and not climb in loiter patterns first. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission_base.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index ebdd026e74..5e9a77802e 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -1213,7 +1213,11 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index) const bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, reinterpret_cast(&mission), sizeof(mission), MAX_DATAMAN_LOAD_WAIT); - if (success) { + const bool is_fw_and_takeoff = mission.nav_cmd == NAV_CMD_TAKEOFF + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + // for FW when on a Takeoff item do not require climb before mission, as we need to keep course to takeoff item straight + if (success && !is_fw_and_takeoff) { const float altitude_amsl_next_position_item = MissionBlock::get_absolute_altitude_for_item(mission); const float error_below_setpoint = altitude_amsl_next_position_item - _navigator->get_global_position()->alt;