diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index d6ce8cc2c2..052968ed7e 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -406,7 +406,7 @@ MissionBase::isLanding() // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); + _navigator->get_default_loiter_rad(); on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); } @@ -883,7 +883,7 @@ MissionBase::do_abort_landing() _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.altitude_is_relative = false; _mission_item.altitude = alt_sp; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_radius = _navigator->get_default_loiter_rad(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index d43c4bee36..168debf23a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -170,7 +170,7 @@ MissionBlock::is_mission_item_reached_or_completed() // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); + _navigator->get_default_loiter_rad(); dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl, _navigator->get_global_position()->lat, @@ -626,7 +626,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; sp->loiter_radius = (fabsf(item.loiter_radius) > FLT_EPSILON) ? fabsf(item.loiter_radius) : - _navigator->get_loiter_radius(); + _navigator->get_default_loiter_rad(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; if (item.acceptance_radius > FLT_EPSILON && PX4_ISFINITE(item.acceptance_radius)) { @@ -730,7 +730,7 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item) } item->altitude = loiter_altitude_amsl; - item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_radius = _navigator->get_default_loiter_rad(); item->yaw = NAN; } @@ -742,7 +742,7 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s _navigator->preproject_stop_point(item->lat, item->lon); item->altitude = _navigator->get_global_position()->alt; - item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_radius = _navigator->get_default_loiter_rad(); item->yaw = NAN; } @@ -773,7 +773,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) item->altitude_is_relative = false; item->acceptance_radius = _navigator->get_acceptance_radius(); - item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_radius = _navigator->get_default_loiter_rad(); item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } @@ -808,7 +808,7 @@ MissionBlock::set_land_item(struct mission_item_s *item) item->altitude = 0; item->altitude_is_relative = false; - item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_radius = _navigator->get_default_loiter_rad(); item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->autocontinue = true; @@ -824,7 +824,7 @@ MissionBlock::set_idle_item(struct mission_item_s *item) item->altitude_is_relative = false; item->altitude = _navigator->get_home_position()->alt; item->yaw = NAN; - item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_radius = _navigator->get_default_loiter_rad(); item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->autocontinue = true; @@ -896,7 +896,7 @@ MissionBlock::initialize() _mission_item.lat = (double)NAN; _mission_item.lon = (double)NAN; _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_radius = _navigator->get_default_loiter_rad(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 30b9cd769a..e9d04c53dd 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -179,7 +179,7 @@ public: Geofence &get_geofence() { return _geofence; } - float get_loiter_radius() { return _param_nav_loiter_rad.get(); } + float get_default_loiter_rad() { return _param_nav_loiter_rad.get(); } /** * Returns the default acceptance radius defined by the parameter diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 31544aeca0..f28ee849ca 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -368,7 +368,7 @@ void Navigator::run() } else { - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = get_default_loiter_rad(); } if (PX4_ISFINITE(curr->current.loiter_minor_radius) && fabsf(curr->current.loiter_minor_radius) > FLT_EPSILON) { @@ -471,7 +471,7 @@ void Navigator::run() rep->current.loiter_radius = curr->current.loiter_radius; } else { - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = get_default_loiter_rad(); } rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise; @@ -509,7 +509,7 @@ void Navigator::run() if (geofence_allows_position(position_setpoint)) { position_setpoint_triplet_s *rep = get_reposition_triplet(); rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = get_default_loiter_rad(); rep->current.loiter_direction_counter_clockwise = false; rep->current.loiter_orientation = 0.0f; rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_ORBIT; @@ -557,8 +557,8 @@ void Navigator::run() if (geofence_allows_position(position_setpoint)) { position_setpoint_triplet_s *rep = get_reposition_triplet(); rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - rep->current.loiter_minor_radius = fabsf(get_loiter_radius()); - rep->current.loiter_direction_counter_clockwise = get_loiter_radius() < 0; + rep->current.loiter_minor_radius = fabsf(get_default_loiter_rad()); + rep->current.loiter_direction_counter_clockwise = get_default_loiter_rad() < 0; rep->current.loiter_orientation = 0.0f; rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_FIGUREEIGHT; rep->current.cruising_speed = get_cruising_speed(); @@ -604,7 +604,7 @@ void Navigator::run() rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = get_default_loiter_rad(); rep->current.loiter_direction_counter_clockwise = false; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.cruising_speed = -1.f; // reset to default @@ -941,7 +941,7 @@ void Navigator::geofence_breach_check() vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); } else { - test_point_distance = 2.0f * get_loiter_radius(); + test_point_distance = 2.0f * get_default_loiter_rad(); vertical_test_point_distance = 5.0f; if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { @@ -1056,7 +1056,7 @@ void Navigator::geofence_breach_check() rep->current.lon = loiter_longitude; rep->current.alt = loiter_altitude_amsl; rep->current.valid = true; - rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_radius = get_default_loiter_rad(); rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; rep->current.cruising_throttle = get_cruising_throttle(); rep->current.acceptance_radius = get_acceptance_radius(); @@ -1173,7 +1173,7 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp) sp.lat = static_cast(NAN); sp.lon = static_cast(NAN); sp.yaw = NAN; - sp.loiter_radius = get_loiter_radius(); + sp.loiter_radius = get_default_loiter_rad(); sp.acceptance_radius = get_default_acceptance_radius(); sp.cruising_speed = get_cruising_speed(); sp.cruising_throttle = get_cruising_throttle(); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 1d73483465..03d39e9d54 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -244,7 +244,7 @@ void RtlDirect::set_rtl_item() .alt = _rtl_alt, .yaw = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading, }; - setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_loiter_radius()); + setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_default_loiter_rad()); break; } diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 6b7f2f649a..7e8b3d4aa4 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -142,7 +142,7 @@ void RtlDirectMissionLand::setActiveMissionItems() _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_radius = _navigator->get_default_loiter_rad(); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL Mission land: climb to %d m\t", (int)ceilf(_rtl_alt)); diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 90e7f86b26..618108ca4b 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -77,7 +77,7 @@ Takeoff::on_active() // as the loiter is established. therefore, set a small loiter time so that the mission item will be reached quickly, // however it will just continue loitering as there is no next mission item _mission_item.time_inside = 1.f; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_radius = _navigator->get_default_loiter_rad(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.altitude = _loiter_altitude_msl; @@ -114,7 +114,7 @@ Takeoff::on_active() const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); + _navigator->get_default_loiter_rad(); lateral_acceptance_reached = distance_to_loiter < _navigator->get_acceptance_radius() + mission_item_loiter_radius_abs; } diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 503a0279f9..686886a911 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -116,7 +116,7 @@ VtolTakeoff::on_active() // as the loiter is established. therefore, set a small loiter time so that the mission item will be reached quickly, // however it will just continue loitering as there is no next mission item _mission_item.time_inside = 1.f; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_radius = _navigator->get_default_loiter_rad(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.altitude = _takeoff_alt_msl + _param_loiter_alt.get();