Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane: Fix loiter navigation accuracy #29165

Open
wants to merge 13 commits into
base: master
Choose a base branch
from
+183 −100
Open

Plane: Fix loiter navigation accuracy #29165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
acfa496
AP_Baro: Add simple model to EAS2TAS for alt calc.
rubenp02 Jan 30, 2025
872f7a5
AP_L1_Control: Remove explicit loiter radius comp.
rubenp02 Jan 30, 2025
ff7ccdd
AP_L1_Control: Make dependencies const references
rubenp02 Jan 30, 2025
ddd2b3a
Plane: Update L1 instantiation with new arguments
rubenp02 Jan 30, 2025
c14e10c
AP_L1_Control: Rework effective turn radius calc.
rubenp02 Jan 27, 2025
628b0fd
AP_Navigation: Modify loiter radius interface
rubenp02 Jan 27, 2025
31b9ae1
AP_Landing: Use reworked loiter radius comp.
rubenp02 Jan 27, 2025
6163dac
Plane: Use reworked loiter radius compensation
rubenp02 Jan 27, 2025
61ce1fe
autotest: Remove NAVL1_LIM_BANK parameter
rubenp02 Jan 30, 2025
fbefddb
Tools: Remove NAVL1_LIM_BANK parameter
rubenp02 Jan 30, 2025
fde7fb1
SITL: Remove NAVL1_LIM_BANK parameter
rubenp02 Jan 30, 2025
0e28469
AP_L1_Control: Keep track of current nav. mode
rubenp02 Jan 27, 2025
29ce287
AP_L1_Control: Add integral term to loiter nav.
rubenp02 Jan 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
7 changes: 4 additions & 3 deletions ArduPlane/mode_autoland.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,9 +143,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, abs(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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not a fan of passing nans.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Me neither! But overloading isn't a good solution here because the idea is to be able to pass the altitude, the airspeed, both or neither. That would be 4 overloads.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But your not actually using all those combinations. I think its fine to even not pass altitude to start with, that is a existing issue that could be fixed in a separate PR.

Copy link
Contributor Author

@rubenp02 rubenp02 Feb 10, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

They aren't being used at this time, and neither is the is_loiter_achievable method, but since this is being reworked anyway I tried to make it as rigorous and flexible as possible. I think that's a good thing: when writing this code I got the impression that the current way this is handled is a bit sloppy, and more parts of the code would benefit from altitude or speed-specific compensation.

If the NaNs are an antipattern in ArduPilot (maybe -1.0f would be preferable?) I can get this down to 2 overloads, one using the current alt. and speed and the other using the specified ones. But any less than this and I don't see too much of a point in reworking the loiter compensation calcs.

Maybe this can be discussed in the dev call? Could I reach out to you by Discord to get some of my other PRs tagged for discussion as well, like this page says?

final_wp_alt);

cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT;
cmd_loiter.p1 = loiter_radius;
Expand Down
5 changes: 3 additions & 2 deletions ArduPlane/mode_loiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
3 changes: 2 additions & 1 deletion ArduPlane/navigation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 0 additions & 4 deletions Tools/autotest/arduplane.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1602,7 +1602,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
})

Expand Down Expand Up @@ -4025,7 +4024,6 @@ def FenceNoFenceReturnPoint(self):
"FENCE_ACTION": 6,
"FENCE_TYPE": 4,
"RTL_RADIUS": want_radius,
"NAVL1_LIM_BANK": 60,
})
home_loc = self.mav.location()
locs = [
Expand Down Expand Up @@ -4100,7 +4098,6 @@ def FenceNoFenceReturnPointInclusion(self):
"FENCE_TYPE": 2,
"FENCE_RADIUS": 300,
"RTL_RADIUS": want_radius,
"NAVL1_LIM_BANK": 60,
})

self.clear_fence()
Expand Down Expand Up @@ -5387,7 +5384,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)

Expand Down
1 change: 0 additions & 1 deletion Tools/autotest/default_params/glider.parm
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,6 @@ TECS_PTCH_FF_V0 25
TECS_HDEM_TCONST 2

NAVL1_PERIOD 20
NAVL1_LIM_BANK 30

SCHED_LOOP_RATE 200

Expand Down
1 change: 0 additions & 1 deletion Tools/autotest/default_params/plane-soaring.parm
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Baro/AP_Baro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
28 changes: 24 additions & 4 deletions libraries/AP_Baro/AP_Baro_atmosphere.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading
Loading