diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 86783bed8c151..36c3871dc48ea 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -242,7 +242,7 @@ class Plane : public AP_Vehicle { #endif AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS}; - AP_L1_Control L1_controller{ahrs, &TECS_controller}; + AP_L1_Control L1_controller{ahrs, TECS_controller, aparm}; // Attitude to servo controllers AP_RollController rollController{aparm}; diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index 4727cc585015a..6613793ff75df 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -129,9 +129,10 @@ bool ModeAutoLand::_enter() // Try and minimize loiter radius by using the smaller of the waypoint loiter radius or 1/3 of the final WP distance const float loiter_radius = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius)); - // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases. - // Strictly this gets the loiter radius at the current altitude, really we want the loiter radius at final_wp_alt. - const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius); + // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases + const float corrected_loiter_radius = + plane.nav_controller->calc_corrected_loiter_radius(loiter_radius, NAN, + final_wp_alt); cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT; cmd_loiter.p1 = loiter_radius; diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index b37f3bdcb0311..4eebc7865bbbe 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -42,8 +42,9 @@ bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location // Return true if current heading is aligned to vector to targetLoc. // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed. - // Corrected radius for altitude - const float loiter_radius = plane.nav_controller->loiter_radius(fabsf(plane.loiter.radius)); + const float loiter_radius = + plane.nav_controller->calc_corrected_loiter_radius( + fabsf(plane.loiter.radius)); if (!is_positive(loiter_radius)) { // Zero is invalid, protect against divide by zero for destination inside loiter radius case return true; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index b47160836aff8..2be42dc5f248b 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -321,7 +321,8 @@ void Plane::update_loiter_update_nav(uint16_t radius) if ((loiter.start_time_ms == 0 && (control_mode == &mode_auto || control_mode == &mode_guided) && auto_state.crosstrack && - current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) || + current_loc.get_distance(next_WP_loc) > + 3 * nav_controller->calc_corrected_loiter_radius(radius)) || quadplane_qrtl_switch) { /* if never reached loiter point and using crosstrack and somewhat far away from loiter point diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index a05ac4f33c907..06a7774f01690 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1603,7 +1603,6 @@ def test_fence_breach_circle_at(self, loc, disable_on_breach=False): expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius self.set_parameters({ "RTL_RADIUS": want_radius, - "NAVL1_LIM_BANK": 60, "FENCE_ACTION": 1, # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 }) @@ -4026,7 +4025,6 @@ def FenceNoFenceReturnPoint(self): "FENCE_ACTION": 6, "FENCE_TYPE": 4, "RTL_RADIUS": want_radius, - "NAVL1_LIM_BANK": 60, }) home_loc = self.mav.location() locs = [ @@ -4101,7 +4099,6 @@ def FenceNoFenceReturnPointInclusion(self): "FENCE_TYPE": 2, "FENCE_RADIUS": 300, "RTL_RADIUS": want_radius, - "NAVL1_LIM_BANK": 60, }) self.clear_fence() @@ -5381,7 +5378,6 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): self.change_mode('AUTO') self.wait_ready_to_arm() self.arm_vehicle() - self.set_parameter("NAVL1_LIM_BANK", 50) self.wait_current_waypoint(2) diff --git a/Tools/autotest/default_params/glider.parm b/Tools/autotest/default_params/glider.parm index a64d9686edb83..cc471de14b93a 100644 --- a/Tools/autotest/default_params/glider.parm +++ b/Tools/autotest/default_params/glider.parm @@ -46,7 +46,6 @@ TECS_PTCH_FF_V0 25 TECS_HDEM_TCONST 2 NAVL1_PERIOD 20 -NAVL1_LIM_BANK 30 SCHED_LOOP_RATE 200 diff --git a/Tools/autotest/default_params/plane-soaring.parm b/Tools/autotest/default_params/plane-soaring.parm index a7f1aa65d47e3..cb3a552e00563 100644 --- a/Tools/autotest/default_params/plane-soaring.parm +++ b/Tools/autotest/default_params/plane-soaring.parm @@ -30,7 +30,6 @@ TECS_SPDWEIGHT 2.000000 AIRSPEED_CRUISE 9.00 -NAVL1_LIM_BANK 60.000000 NAVL1_PERIOD 10.000 PTCH_RATE_FF 0.385000 PTCH_RATE_P 0.040000 diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 6fdbb02880e62..57cd09cadb038 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -111,7 +111,7 @@ class AP_Baro float get_temperature_from_altitude(float alt) const; float get_altitude_from_pressure(float pressure) const; - // EAS2TAS for SITL + // EAS2TAS at the given altitude AMSL static float get_EAS2TAS_for_alt_amsl(float alt_amsl); #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_atmosphere.cpp b/libraries/AP_Baro/AP_Baro_atmosphere.cpp index ed26927cb9f89..53a0de12b21b3 100644 --- a/libraries/AP_Baro/AP_Baro_atmosphere.cpp +++ b/libraries/AP_Baro/AP_Baro_atmosphere.cpp @@ -243,18 +243,38 @@ float AP_Baro::get_EAS2TAS_extended(float altitude) const return sqrtf(SSL_AIR_DENSITY / density); } +#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED + /* Given the geometric altitude (m) return scale factor that converts from equivalent to true airspeed - used by SITL only */ float AP_Baro::get_EAS2TAS_for_alt_amsl(float alt_amsl) { +#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED const float density = get_air_density_for_alt_amsl(alt_amsl); - return sqrtf(SSL_AIR_DENSITY / MAX(0.00001,density)); -} +#else + // estimate temperature at the given altitude using the standard lapse rate + float temp_at_alt_K = SSL_AIR_TEMPERATURE - ISA_LAPSE_RATE * alt_amsl; -#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED + // return if temperature is invalid + if (temp_at_alt_K <= 0.0f) { + return 1.0f; + } + + // compute the pressure at the target altitude using the standard barometric + // formula for a gradient layer + float pressure_at_alt = + SSL_AIR_PRESSURE * + powf(temp_at_alt_K / SSL_AIR_TEMPERATURE, + GRAVITY_MSS / (ISA_GAS_CONSTANT * ISA_LAPSE_RATE)); + + // compute air density using ideal gas law + float density = pressure_at_alt / (ISA_GAS_CONSTANT * temp_at_alt_K); +#endif + + return sqrtf(SSL_AIR_DENSITY / MAX(0.00001, density)); +} /* return geometric altitude difference in meters between current pressure and a diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 4a5c83eb5d004..2da1a2597ef79 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -30,14 +30,6 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = { // @User: Advanced AP_GROUPINFO("XTRACK_I", 2, AP_L1_Control, _L1_xtrack_i_gain, 0.02), - // @Param: LIM_BANK - // @DisplayName: Loiter Radius Bank Angle Limit - // @Description: The sealevel bank angle limit for a continous loiter. (Used to calculate airframe loading limits at higher altitudes). Setting to 0, will instead just scale the loiter radius directly - // @Units: deg - // @Range: 0 89 - // @User: Advanced - AP_GROUPINFO("LIM_BANK", 3, AP_L1_Control, _loiter_bank_limit, 0.0f), - AP_GROUPEND }; @@ -45,7 +37,7 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = { // S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," // Proceedings of the AIAA Guidance, Navigation and Control // Conference, Aug 2004. AIAA-2004-4900. -// Modified to use PD control for circle tracking to enable loiter radius less than L1 length +// Modified to use PID control for circle tracking to enable loiter radius less than L1 length // Modified to enable period and damping of guidance loop to be set explicitly // Modified to provide explicit control over capture angle @@ -123,7 +115,6 @@ int32_t AP_L1_Control::target_bearing_cd(void) const */ float AP_L1_Control::turn_distance(float wp_radius) const { - wp_radius *= sq(_ahrs.get_EAS2TAS()); return MIN(wp_radius, _L1_dist); } @@ -145,36 +136,41 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const return distance_90 * turn_angle / 90.0f; } -float AP_L1_Control::loiter_radius(const float radius) const +float AP_L1_Control::_calc_min_turn_radius(float indicated_airspeed, + float altitude_amsl) const { - // prevent an insane loiter bank limit - float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f); - float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS; - float nominal_velocity_sea_level = 0.0f; - if(_tecs != nullptr) { - nominal_velocity_sea_level = _tecs->get_target_airspeed(); - } + float reference_ias = isnan(indicated_airspeed) + ? _tecs.get_target_airspeed() + : indicated_airspeed; + float reference_alt_amsl = isnan(altitude_amsl) + ? _baro.get_altitude_AMSL() + : altitude_amsl; - float eas2tas_sq = sq(_ahrs.get_EAS2TAS()); + float tas_at_alt = + reference_ias * AP_Baro::get_EAS2TAS_for_alt_amsl(reference_alt_amsl); - if (is_zero(sanitized_bank_limit) || is_zero(nominal_velocity_sea_level) || - is_zero(lateral_accel_sea_level)) { - // Missing a sane input for calculating the limit, or the user has - // requested a straight scaling with altitude. This will always vary - // with the current altitude, but will at least protect the airframe - return radius * eas2tas_sq; - } else { - float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level; - if (sea_level_radius > radius) { - // If we've told the plane that its sea level radius is unachievable fallback to - // straight altitude scaling - return radius * eas2tas_sq; - } else { - // select the requested radius, or the required altitude scale, whichever is safer - return MAX(sea_level_radius * eas2tas_sq, radius); - } + return sq(tas_at_alt) / (GRAVITY_MSS * tanf(_aparm.roll_limit)); +} + +bool AP_L1_Control::is_loiter_achievable(float radius, float indicated_airspeed, + float altitude_amsl) const +{ + if (radius < 0.0f) { + return false; } + + float min_radius = _calc_min_turn_radius(indicated_airspeed, altitude_amsl); + + return radius >= min_radius; +} + +float AP_L1_Control::calc_corrected_loiter_radius(float original_radius, + float indicated_airspeed, + float altitude_amsl) const +{ + return MAX(original_radius, + _calc_min_turn_radius(indicated_airspeed, altitude_amsl)); } bool AP_L1_Control::reached_loiter_target(void) @@ -202,27 +198,55 @@ void AP_L1_Control::_prevent_indecision(float &Nu) } } +void AP_L1_Control::_update_xtrack_integral(float error, float max_abs_error, float clamp) +{ + uint32_t now_us = AP_HAL::micros(); + float dt = (now_us - _last_update_xtrack_i_us) * 1.0e-6f; + if (dt > 1) { + // Controller hasn't been called for an extended period of + // time. Reset it. + _L1_xtrack_i = 0.0f; + } + if (dt > 0.1) { + dt = 0.1; + } + _last_update_xtrack_i_us = now_us; + + // Reset integral error component if gain parameter has changed. This allows + // for much easier tuning by having it re-converge each time it changes. + if (!is_positive(_L1_xtrack_i_gain) || + !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) { + _L1_xtrack_i = 0; + _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; + + return; + } + + // Return if error is too large + if (fabsf(error) >= max_abs_error) { + return; + } + + // Compute integral error component to converge to a crosstrack of zero, and + // clamp it + _L1_xtrack_i += error * _L1_xtrack_i_gain * dt; + _L1_xtrack_i = constrain_float(_L1_xtrack_i, -clamp, clamp); +} + // update L1 control for waypoint navigation void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &next_WP, float dist_min) { + // Update nav. mode + if (_current_nav_mode != NavMode::WAYPOINT) { + _L1_xtrack_i = 0.0f; // Reset integral component on nav. mode change + _current_nav_mode = NavMode::WAYPOINT; + } Location _current_loc; float Nu; float xtrackVel; float ltrackVel; - uint32_t now = AP_HAL::micros(); - float dt = (now - _last_update_waypoint_us) * 1.0e-6f; - if (dt > 1) { - // controller hasn't been called for an extended period of - // time. Reinitialise it. - _L1_xtrack_i = 0.0f; - } - if (dt > 0.1) { - dt = 0.1; - } - _last_update_waypoint_us = now; - // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; @@ -310,18 +334,10 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); float Nu1 = asinf(sine_Nu1); - // compute integral error component to converge to a crosstrack of zero when traveling - // straight but reset it when disabled or if it changes. That allows for much easier - // tuning by having it re-converge each time it changes. - if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) { - _L1_xtrack_i = 0; - _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; - } else if (fabsf(Nu1) < radians(5)) { - _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; - - // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at - _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f); - } + // Update the integral term up to 5ยบ of error + // An AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good + // worst-case to clip at + _update_xtrack_integral(Nu1, radians(5), 0.1f); // to converge to zero we must push Nu1 harder Nu1 += _L1_xtrack_i; @@ -349,15 +365,18 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex // update L1 control for loitering void AP_L1_Control::update_loiter(const Location ¢er_WP, float radius, int8_t loiter_direction) { + // Update nav. mode + if (_current_nav_mode != NavMode::LOITER) { + _L1_xtrack_i = 0.0f; // Reset integral component on nav. mode change + _current_nav_mode = NavMode::LOITER; + } + const float radius_unscaled = radius; Location _current_loc; - // scale loiter radius with square of EAS2TAS to allow us to stay - // stable at high altitude - radius = loiter_radius(fabsf(radius)); - // Calculate guidance gains used by PD loop (used during circle tracking) + // Calculate guidance gains used by PID loop (used during circle tracking) float omega = (6.2832f / _L1_period); float Kx = omega * omega; float Kv = 2.0f * _L1_damping * omega; @@ -425,22 +444,27 @@ void AP_L1_Control::update_loiter(const Location ¢er_WP, float radius, int8_ // keep crosstrack error for reporting _crosstrack_error = xtrackErrCirc; - // Calculate PD control correction to circle waypoint_ahrs.roll - float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv); + // Correct errors up to half the radius. Clamping to a value of 4 + // produces good results, allowing for precise 50 m radius loiters at a + // speed of 22 m/s + _update_xtrack_integral(_crosstrack_error, radius / 2, 4.0f); + + // Calculate PID control correction to circle waypoint_ahrs.roll + float latAccDemCircPID = xtrackErrCirc * Kx + xtrackVelCirc * Kv + _L1_xtrack_i; // Calculate tangential velocity float velTangent = xtrackVelCap * float(loiter_direction); - // Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way + // Prevent PID demand from turning the wrong way by limiting the command when flying the wrong way if (ltrackVelCap < 0.0f && velTangent < 0.0f) { - latAccDemCircPD = MAX(latAccDemCircPD, 0.0f); + latAccDemCircPID = MAX(latAccDemCircPID, 0.0f); } // Calculate centripetal acceleration demand float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc)); - // Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand - float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); + // Sum PID control and centripetal acceleration to calculate lateral manoeuvre demand + float latAccDemCirc = loiter_direction * (latAccDemCircPID + latAccDemCircCtr); // Perform switchover between 'capture' and 'circle' modes at the // point where the commands cross over to achieve a seamless transfer @@ -489,6 +513,11 @@ void AP_L1_Control::update_loiter(const Location ¢er_WP, float radius, int8_ // update L1 control for heading hold navigation void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd) { + // Update nav. mode + if (_current_nav_mode != NavMode::HEADING_HOLD) { + _current_nav_mode = NavMode::HEADING_HOLD; + } + // Calculate normalised frequency for tracking loop const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period // Calculate additional damping gain @@ -531,6 +560,11 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd) // update L1 control for level flight on current heading void AP_L1_Control::update_level_flight(void) { + // Update nav. mode + if (_current_nav_mode != NavMode::LEVEL_FLIGHT) { + _current_nav_mode = NavMode::LEVEL_FLIGHT; + } + // copy to _target_bearing_cd and _nav_bearing _target_bearing_cd = _ahrs.yaw_sensor; _nav_bearing = _ahrs.get_yaw(); diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index 20eb6e577713c..1b1fc49042286 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -17,13 +17,17 @@ #include <AP_Param/AP_Param.h> #include <AP_Navigation/AP_Navigation.h> #include <AP_TECS/AP_TECS.h> +#include <AP_Baro/AP_Baro.h> +#include <AP_Vehicle/AP_FixedWing.h> #include <AP_Common/Location.h> class AP_L1_Control : public AP_Navigation { public: - AP_L1_Control(AP_AHRS &ahrs, const AP_TECS *tecs) + AP_L1_Control(const AP_AHRS &ahrs, const AP_TECS &tecs, const AP_FixedWing &aparm) : _ahrs(ahrs) , _tecs(tecs) + , _baro(*AP_Baro::get_singleton()) + , _aparm(aparm) { AP_Param::setup_object_defaults(this, var_info); } @@ -48,8 +52,12 @@ class AP_L1_Control : public AP_Navigation { int32_t target_bearing_cd(void) const override; float turn_distance(float wp_radius) const override; float turn_distance(float wp_radius, float turn_angle) const override; - float loiter_radius (const float loiter_radius) const override; void update_waypoint(const class Location &prev_WP, const class Location &next_WP, float dist_min = 0.0f) override; + bool is_loiter_achievable(float radius, float indicated_airspeed, + float altitude_amsl) const override; + float calc_corrected_loiter_radius(float original_radius, + float indicated_airspeed, + float altitude_amsl) const override; void update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) override; void update_heading_hold(int32_t navigation_heading_cd) override; void update_level_flight(void) override; @@ -76,10 +84,25 @@ class AP_L1_Control : public AP_Navigation { private: // reference to the AHRS object - AP_AHRS &_ahrs; + const AP_AHRS &_ahrs; - // pointer to the SpdHgtControl object - const AP_TECS *_tecs; + // reference to the TECS object + const AP_TECS &_tecs; + + // reference to the barometer object + const AP_Baro &_baro; + + // reference to the fixed wing parameters object + const AP_FixedWing &_aparm; + + enum class NavMode { + NONE, + WAYPOINT, + LOITER, + HEADING_HOLD, + LEVEL_FLIGHT + }; + NavMode _current_nav_mode = NavMode::NONE; // lateral acceration in m/s required to fly to the // L1 reference point (+ve to right) @@ -116,13 +139,16 @@ class AP_L1_Control : public AP_Navigation { // integral feedback to correct crosstrack error. Used to ensure xtrack converges to zero. // For tuning purposes it's helpful to clear the integrator when it changes so a _prev is used + void _update_xtrack_integral(float error, float max_abs_error, float clamp); + uint32_t _last_update_xtrack_i_us; float _L1_xtrack_i = 0; AP_Float _L1_xtrack_i_gain; float _L1_xtrack_i_gain_prev = 0; - uint32_t _last_update_waypoint_us; bool _data_is_stale = true; - AP_Float _loiter_bank_limit; + // calculate minimum achievable turn radius based on indicated airspeed and + // altitude AMSL (assuming steady, coordinated, level turn) + float _calc_min_turn_radius(float ias = NAN, float altitude_amsl = NAN) const; // remember reached_loiter_target decision struct { diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 96b395a2e9ff8..55041d3bdac48 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -505,7 +505,8 @@ bool AP_Landing_Deepstall::terminate(void) { void AP_Landing_Deepstall::build_approach_path(bool use_current_heading) { - float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius); + float loiter_radius = landing.nav_controller->calc_corrected_loiter_radius( + landing.aparm.loiter_radius); Vector3f wind = landing.ahrs.wind_estimate(); // TODO: Support a user defined approach heading diff --git a/libraries/AP_Navigation/AP_Navigation.h b/libraries/AP_Navigation/AP_Navigation.h index 26d25ad19a8c8..5fde0db211d19 100644 --- a/libraries/AP_Navigation/AP_Navigation.h +++ b/libraries/AP_Navigation/AP_Navigation.h @@ -56,9 +56,16 @@ class AP_Navigation { // mission when approaching a waypoint virtual float turn_distance(float wp_radius, float turn_angle) const = 0; - // return the target loiter radius for the current location that - // will not cause excessive airframe loading - virtual float loiter_radius(const float radius) const = 0; + // return true if the loiter radius is achievable at the current indicated + // airspeed and altitude AMSL, or at the given ones if specified + virtual bool is_loiter_achievable(float radius, + float indicated_airspeed = NAN, float altitude_amsl = NAN) const = 0; + + // return the provided loiter radius if its achievable, and if not, the + // smallest radius that can be achieved at the current indicated airspeed + // and altitude AMSL, or at the given ones if specified + virtual float calc_corrected_loiter_radius(float original_radius, + float indicated_airspeed = NAN, float altitude_amsl = NAN) const = 0; // update the internal state of the navigation controller, given // the previous and next waypoints. This is the step function for diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 5bf3c82c55360..408c9d9dc7b75 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -334,7 +334,6 @@ MNT_RETRACT_Z,0 MNT_TYPE,0 NAV_CONTROLLER,1 NAVL1_DAMPING,1 -NAVL1_LIM_BANK,60 NAVL1_PERIOD,20 NAVL1_XTRACK_I,0.02 NTF_BUZZ_ENABLE,1 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 5eb9fc9cde642..44197a52087e4 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -337,7 +337,6 @@ MNT_RETRACT_Z,0 MNT_TYPE,0 NAV_CONTROLLER,1 NAVL1_DAMPING,1 -NAVL1_LIM_BANK,60 NAVL1_PERIOD,20 NAVL1_XTRACK_I,0.02 NTF_BUZZ_ENABLE,1