From 695efb8df3c9d575d8a63b733f1b7ea9b37a7045 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 21 Sep 2015 21:02:54 -0700 Subject: [PATCH] AP_L1_Control: added integrator to help xtrack error converge to zero new param: NAVL1_XTRACK_I // @Description: Crosstrack error integrator gain. This gain is applied to the crosstrack error to ensure it converges to zero. Set to zero to disable. Smaller values converge slower, higher values will cause crosstrack error oscillation. fixes https://github.com/diydrones/ardupilot/issues/2650 when param is changed the integrator is set to zero. This makes for easier tuning by seeing it converge to zero on each change. --- libraries/AP_L1_Control/AP_L1_Control.cpp | 31 ++++++++++++++++++++--- libraries/AP_L1_Control/AP_L1_Control.h | 7 +++++ 2 files changed, 35 insertions(+), 3 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index c7c1874a7e..c6a3eb9aa6 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -22,6 +22,13 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] PROGMEM = { // @Increment: 0.05 AP_GROUPINFO("DAMPING", 1, AP_L1_Control, _L1_damping, 0.75f), + // @Param: XTRACK_I + // @DisplayName: L1 control crosstrack integrator gain + // @Description: Crosstrack error integrator gain. This gain is applied to the crosstrack error to ensure it converges to zero. Set to zero to disable. Smaller values converge slower, higher values will cause crosstrack error oscillation. + // @Range: 0 to 0.1 + // @Increment: 0.01 + AP_GROUPINFO("XTRACK_I", 2, AP_L1_Control, _L1_xtrack_i_gain, 0.02), + AP_GROUPEND }; @@ -179,7 +186,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Vector2f A_air = location_diff(prev_WP, _current_loc); // calculate distance to target track, for reporting - _crosstrack_error = AB % A_air; + _crosstrack_error = A_air % AB; //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point @@ -202,11 +209,29 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) - float xtrackErr = A_air % AB; - float sine_Nu1 = xtrackErr/max(_L1_dist, 0.1f); + float sine_Nu1 = _crosstrack_error/max(_L1_dist, 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg 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, _L1_xtrack_i_gain_prev)) { + _L1_xtrack_i = 0; + _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; + } else if (fabsf(Nu1) < radians(5)) { + + const float dt = 0.1f; // 10Hz + _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); + } + + // to converge to zero we must push Nu1 harder + Nu1 += _L1_xtrack_i; + Nu = Nu1 + Nu2; _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index 4f1836c078..cd90e9dde7 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -100,6 +100,13 @@ private: // prevent indecision in waypoint tracking void _prevent_indecision(float &Nu); + + // 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 + float _L1_xtrack_i = 0; + AP_Float _L1_xtrack_i_gain; + float _L1_xtrack_i_gain_prev = 0; + };