diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e0006ebddf..55f952caa6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -929,7 +929,7 @@ MissionBlock::initialize() } void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, - HeadingMode heading_mode) const + float heading_sp) const { item.nav_cmd = NAV_CMD_LOITER_TO_ALT; item.lat = dest.lat; @@ -937,7 +937,7 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina item.altitude = dest.alt; item.altitude_is_relative = false; - item. yaw = setYawFromHeadingMode(dest, heading_mode); + item.yaw = heading_sp; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; @@ -947,7 +947,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, HeadingMode heading_mode) const + float loiter_radius, float heading_sp) const { const bool autocontinue = (loiter_time > -FLT_EPSILON); @@ -963,7 +963,7 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat item.altitude = dest.alt; item.altitude_is_relative = false; - item. yaw = setYawFromHeadingMode(dest, heading_mode); + item.yaw = NAN; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = math::max(loiter_time, 0.0f); @@ -973,7 +973,7 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat } void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, - HeadingMode heading_mode) const + float heading_sp) const { item.nav_cmd = NAV_CMD_WAYPOINT; item.lat = dest.lat; @@ -986,46 +986,23 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest item.time_inside = 0.f; item.origin = ORIGIN_ONBOARD; - item. yaw = setYawFromHeadingMode(dest, heading_mode); + item.yaw = heading_sp; } void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, - HeadingMode heading_mode) const + const float heading_sp) const { item.nav_cmd = NAV_CMD_LAND; item.lat = dest.lat; item.lon = dest.lon; item.altitude = dest.alt; - - if (heading_mode == HeadingMode::CURRENT_HEADING) { - item.yaw = _navigator->get_local_position()->heading; - - } else { - item.yaw = dest.yaw; - } - + item.yaw = heading_sp; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; item.autocontinue = true; item.origin = ORIGIN_ONBOARD; } -float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const -{ - float desired_yaw(_navigator->get_local_position()->heading); - - if (heading_mode == HeadingMode::NAVIGATION_HEADING) { - desired_yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, dest.lat, dest.lon); - - } else if (heading_mode == HeadingMode::DESTINATION_HEADING) { - desired_yaw = dest.yaw; - - } - - return desired_yaw; -} - void MissionBlock::startPrecLand(uint16_t land_precision) { if (_mission_item.land_precision == 1) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 619ea818df..deba348366 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -206,17 +206,15 @@ 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, - HeadingMode heading_mode) const; + float heading_sp) const; void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, - float loiter_radius, HeadingMode heading_mode) const; + float loiter_radius, float heading_sp) const; void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, - HeadingMode heading_mode) const; + float heading_sp) const; - void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const; - - float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const; + void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, const float heading_sp) const; void startPrecLand(uint16_t land_precision); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index d65054ea6d..fc17b190ab 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -178,11 +178,7 @@ void RtlDirect::set_rtl_item() _global_pos_sub.get().lat, _global_pos_sub.get().lon); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); - HeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); - - if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) { - rtl_heading_mode = HeadingMode::DESTINATION_HEADING; - } + const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get(); switch (_rtl_state) { case RTLState::CLIMBING: { @@ -191,8 +187,8 @@ void RtlDirect::set_rtl_item() .lon = _global_pos_sub.get().lon, .alt = _rtl_alt, }; - - setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_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); _rtl_state = RTLState::MOVE_TO_LOITER; break; @@ -209,10 +205,12 @@ void RtlDirect::set_rtl_item() // 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, rtl_heading_mode); + setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, NAN); } else { - setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + // 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); } _rtl_state = RTLState::LOITER_DOWN; @@ -228,7 +226,9 @@ void RtlDirect::set_rtl_item() .yaw = _destination.yaw, }; - setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, rtl_heading_mode); + // 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); pos_sp_triplet->next.valid = true; pos_sp_triplet->next.lat = _destination.lat; @@ -255,8 +255,10 @@ void RtlDirect::set_rtl_item() .yaw = _destination.yaw, }; + // 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, - rtl_heading_mode); + heading_sp); if (_param_rtl_land_delay.get() < -FLT_EPSILON) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); @@ -279,7 +281,7 @@ void RtlDirect::set_rtl_item() DestinationPosition dest{_destination}; dest.alt = loiter_altitude; - setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + setMoveToPositionMissionItem(_mission_item, dest, NAN); // Prepare for transition _mission_item.vtol_back_transition = true; @@ -309,7 +311,9 @@ void RtlDirect::set_rtl_item() DestinationPosition dest{_destination}; dest.alt = loiter_altitude; - setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + // set final yaw if WV is disabled + const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; + setMoveToPositionMissionItem(_mission_item, dest, heading_sp); _navigator->reset_position_setpoint(pos_sp_triplet->previous); _rtl_state = RTLState::LAND; @@ -319,7 +323,9 @@ void RtlDirect::set_rtl_item() case RTLState::LAND: { - setLandMissionItem(_mission_item, _destination, rtl_heading_mode); + // set final yaw if WV is disabled + const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; + setLandMissionItem(_mission_item, _destination, heading_sp); _mission_item.land_precision = _param_rtl_pld_md.get(); diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 3a5597d4ad..04883dcdc6 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -190,9 +190,11 @@ private: (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_pld_md, (ParamFloat) _param_rtl_loiter_rad, - (ParamInt) _param_rtl_hdg_md, (ParamFloat) _param_rtl_time_factor, - (ParamInt) _param_rtl_time_margin + (ParamInt) _param_rtl_time_margin, + + // external params + (ParamBool) _param_wv_en ) param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7b4b573bc3..6fd734cc8e 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -160,18 +160,6 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0); */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 80.0f); -/** - * RTL heading mode - * - * Defines the heading behavior during RTL - * - * @value 0 Towards next waypoint. - * @value 1 Heading matches destination. - * @value 2 Use current heading. - * @group Return Mode - */ -PARAM_DEFINE_INT32(RTL_HDG_MD, 0); - /** * RTL time estimate safety margin factor *