From cb0383512439afa77379db7086fa1234e118e303 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 12 Feb 2024 15:12:59 +0100 Subject: [PATCH] RTL: use dest.yaw instead of a separate heading_sp --- src/modules/navigator/mission_block.cpp | 19 +++++------- src/modules/navigator/mission_block.h | 10 +++---- src/modules/navigator/rtl_direct.cpp | 39 +++++++++++-------------- 3 files changed, 29 insertions(+), 39 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 55f952caa6..b33cc6bbcd 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -928,16 +928,15 @@ MissionBlock::initialize() _mission_item.origin = ORIGIN_ONBOARD; } -void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, - float heading_sp) const +void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, + float loiter_radius) const { item.nav_cmd = NAV_CMD_LOITER_TO_ALT; item.lat = dest.lat; item.lon = dest.lon; item.altitude = dest.alt; item.altitude_is_relative = false; - - item.yaw = heading_sp; + item.yaw = dest.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; @@ -947,7 +946,7 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina } void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, - float loiter_radius, float heading_sp) const + float loiter_radius) const { const bool autocontinue = (loiter_time > -FLT_EPSILON); @@ -972,8 +971,7 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat item.loiter_radius = loiter_radius; } -void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, - float heading_sp) const +void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest) const { item.nav_cmd = NAV_CMD_WAYPOINT; item.lat = dest.lat; @@ -986,17 +984,16 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest item.time_inside = 0.f; item.origin = ORIGIN_ONBOARD; - item.yaw = heading_sp; + item.yaw = dest.yaw; } -void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, - const float heading_sp) const +void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest) const { item.nav_cmd = NAV_CMD_LAND; item.lat = dest.lat; item.lon = dest.lon; item.altitude = dest.alt; - item.yaw = heading_sp; + item.yaw = dest.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; item.autocontinue = true; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index deba348366..eea8a2cb3d 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -205,16 +205,14 @@ protected: */ void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); - void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, - float heading_sp) const; + void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius) const; void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, - float loiter_radius, float heading_sp) const; + float loiter_radius) const; - void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, - float heading_sp) const; + void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest) const; - void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, const float heading_sp) const; + void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest) const; void startPrecLand(uint16_t land_precision); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index fc17b190ab..3164626dd4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -186,9 +186,9 @@ void RtlDirect::set_rtl_item() .lat = _global_pos_sub.get().lat, .lon = _global_pos_sub.get().lon, .alt = _rtl_alt, + .yaw = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading, }; - const float heading_sp = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading; - setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), heading_sp); + setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius()); _rtl_state = RTLState::MOVE_TO_LOITER; break; @@ -199,18 +199,18 @@ void RtlDirect::set_rtl_item() .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = _rtl_alt, - .yaw = _destination.yaw, }; // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status // can be displayed on groundstation and the WP is accepted once within loiter radius if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, NAN); + dest.yaw = NAN; + setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m); } else { // already set final yaw if close to destination and WV is disabled - const float heading_sp = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN; - setMoveToPositionMissionItem(_mission_item, dest, heading_sp); + dest.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN; + setMoveToPositionMissionItem(_mission_item, dest); } _rtl_state = RTLState::LOITER_DOWN; @@ -223,12 +223,10 @@ void RtlDirect::set_rtl_item() .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = loiter_altitude, - .yaw = _destination.yaw, + .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if WV is disabled }; - // set final yaw if WV is disabled - const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; - setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, heading_sp); + setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m); pos_sp_triplet->next.valid = true; pos_sp_triplet->next.lat = _destination.lat; @@ -252,13 +250,11 @@ void RtlDirect::set_rtl_item() .lat = _land_approach.lat, .lon = _land_approach.lon, .alt = loiter_altitude, - .yaw = _destination.yaw, + .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if WV is disabled }; // set final yaw if WV is disabled - const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; - setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m, - heading_sp); + setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m); if (_param_rtl_land_delay.get() < -FLT_EPSILON) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); @@ -280,8 +276,9 @@ void RtlDirect::set_rtl_item() DestinationPosition dest{_destination}; dest.alt = loiter_altitude; + dest.yaw = NAN; - setMoveToPositionMissionItem(_mission_item, dest, NAN); + setMoveToPositionMissionItem(_mission_item, dest); // Prepare for transition _mission_item.vtol_back_transition = true; @@ -310,10 +307,9 @@ void RtlDirect::set_rtl_item() case RTLState::MOVE_TO_LAND_HOVER: { DestinationPosition dest{_destination}; dest.alt = loiter_altitude; + dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled - // set final yaw if WV is disabled - const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; - setMoveToPositionMissionItem(_mission_item, dest, heading_sp); + setMoveToPositionMissionItem(_mission_item, dest); _navigator->reset_position_setpoint(pos_sp_triplet->previous); _rtl_state = RTLState::LAND; @@ -322,10 +318,9 @@ void RtlDirect::set_rtl_item() } case RTLState::LAND: { - - // set final yaw if WV is disabled - const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; - setLandMissionItem(_mission_item, _destination, heading_sp); + DestinationPosition dest{_destination}; + dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled + setLandMissionItem(_mission_item, dest); _mission_item.land_precision = _param_rtl_pld_md.get();