Skip to content

AP_L1_Control: Add integral term to loiter navigation #30438

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

Closed
rubenp02 wants to merge 3 commits into from
Closed

AP_L1_Control: Add integral term to loiter navigation #30438

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
04c2608
AP_L1_Control: Keep track of current nav. mode
rubenp02 Jan 27, 2025
df24e77
AP_L1_Control: Add integral term to loiter nav.
rubenp02 Jan 27, 2025
7e4a0c4
autotest: Remove the really bad fugde factor
rubenp02 Jul 2, 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
12 changes: 4 additions & 8 deletions Tools/autotest/arduplane.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1594,12 +1594,9 @@ def FenceStatic(self):

def test_fence_breach_circle_at(self, loc, disable_on_breach=False):
self.load_fence("CMAC-fence.txt")
want_radius = 100
# when ArduPlane is fixed, remove this fudge factor
REALLY_BAD_FUDGE_FACTOR = 1.16
expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius
rtl_radius = 100
self.set_parameters({
"RTL_RADIUS": want_radius,
"RTL_RADIUS": rtl_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 @@ -1629,7 +1626,7 @@ def test_fence_breach_circle_at(self, loc, disable_on_breach=False):
self.assert_fence_sys_status(True, True, False)
break

self.wait_circling_point_with_radius(loc, expected_radius)
self.wait_circling_point_with_radius(loc, rtl_radius)
self.do_fence_disable()
self.disarm_vehicle(force=True)
self.reboot_sitl()
Expand Down Expand Up @@ -2320,8 +2317,7 @@ def ADSBResumeActionResumeLoiter(self):
self.wait_heading(290)
self.wait_heading(300)
dest = self.position_target_loc()
REALLY_BAD_FUDGE_FACTOR = 1.25 # FIXME
expected_radius = REALLY_BAD_FUDGE_FACTOR * self.get_parameter('WP_LOITER_RAD')
expected_radius = self.get_parameter('WP_LOITER_RAD')
self.wait_circling_point_with_radius(dest, expected_radius)

self.start_subtest("Testing mission resume")
Expand Down
103 changes: 71 additions & 32 deletions libraries/AP_L1_Control/AP_L1_Control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,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

Expand Down Expand Up @@ -202,27 +202,53 @@ 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;
}
dt = MIN(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;

Expand Down Expand Up @@ -310,18 +336,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);
Copy link
Member

@IamPete1 IamPete1 Jul 17, 2025

Choose a reason for hiding this comment

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

I find it a odd that here were using Nu1 in radians and in the loiter case were using crosstrack error in meters.

Copy link
Contributor Author

@rubenp02 rubenp02 Jul 21, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

Yeah I agree but I figured changing that could overcomplicate the patch, as standardizing it to meters seems like the best solution but also introduces an unnecessary functional change to the waypoint navigation. What solution do you propose? I'll happily add it


// to converge to zero we must push Nu1 harder
Nu1 += _L1_xtrack_i;
Expand Down Expand Up @@ -349,6 +367,12 @@ 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 &center_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;
Expand All @@ -357,7 +381,7 @@ void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_
// 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;
Expand Down Expand Up @@ -425,22 +449,27 @@ void AP_L1_Control::update_loiter(const Location &center_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
Expand Down Expand Up @@ -489,6 +518,11 @@ void AP_L1_Control::update_loiter(const Location &center_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
Expand Down Expand Up @@ -531,6 +565,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_rad();
Expand Down
12 changes: 11 additions & 1 deletion libraries/AP_L1_Control/AP_L1_Control.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,15 @@ class AP_L1_Control : public AP_Navigation {
// pointer to the SpdHgtControl object
const AP_TECS *_tecs;

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)
float _latAccDem;
Expand Down Expand Up @@ -116,10 +125,11 @@ 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;
Expand Down
Loading