From 9654546b5a261731bc460553f8bb1c0112b74702 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 5 May 2013 14:14:21 +1000 Subject: [PATCH] AP_L1_Control: Cleaned up calculation of damping to reduce computations also removed _cross2D, using math library instead --- libraries/AP_L1_Control/AP_L1_Control.cpp | 75 +++++++++-------------- libraries/AP_L1_Control/AP_L1_Control.h | 3 - 2 files changed, 28 insertions(+), 50 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 01da83c9e2..f5442bd0ce 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -78,15 +78,14 @@ float AP_L1_Control::crosstrack_error(void) void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) { - // Calculate normalised frequency for tracking loop - const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period - // Calculate additional damping gain - const float Kv = omegaA * 2.8284f * (_L1_damping - 0.7071f); // omegaA * 2*sqrt(2) * (dampingRatio - 1/sqrt(2)) - float Nu; - float dampingWeight; - float xtrackVel; struct Location _current_loc; + float Nu; + float xtrackVel; + float ltrackVel; + // Calculate L1 gain required for specified damping + float K_L1 = 4.0f * _L1_damping * _L1_damping; + // Get current position and velocity _ahrs->get_position(&_current_loc); @@ -99,8 +98,9 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct float groundSpeed = _groundspeed_vector.length(); // Calculate time varying control parameters - _L1_dist = groundSpeed / omegaA; // L1 distance is adjusted to maintain a constant tracking loop frequency - float VomegaA = groundSpeed * omegaA; + // Calculate the L1 length required for specified period + // 0.3183099 = 1/1/pipi + _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; //Convert current location and WP positions to 2D vectors in lat and long Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f)); @@ -115,7 +115,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Vector2f AB_unit = (AB).normalized(); // calculate distance to target track, for reporting - _crosstrack_error = _cross2D(AB_unit, A_air); + _crosstrack_error = AB_unit % A_air; //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 @@ -123,44 +123,33 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct float WP_A_dist = A_air.length(); float alongTrackDist = A_air * AB_unit; if (WP_A_dist > _L1_dist && alongTrackDist/(WP_A_dist + 1.0f) < -0.7071f) { - //Calc Nu to fly To WP A + + //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft - xtrackVel = _cross2D(_groundspeed_vector , -A_air_unit); // Velocity across line - float ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line + xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line + ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); - dampingWeight = 1.0f; - _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) - xtrackVel = _cross2D(_groundspeed_vector,AB_unit); // Velocity cross track - float ltrackVel = _groundspeed_vector * AB_unit; // Velocity along track + xtrackVel = _groundspeed_vector % AB_unit; // Velocity cross track + ltrackVel = _groundspeed_vector * AB_unit; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); - //Calculate Nu1 angle (Angle to L1 reference point) - float xtrackErr = _cross2D(A_air, AB_unit); + float xtrackErr = A_air % AB_unit; float sine_Nu1 = xtrackErr/_maxf(_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.7854f, 0.7854f); float Nu1 = asinf(sine_Nu1); - - //Calculate Nu Nu = Nu1 + Nu2; - - //Calculate a weight to apply to the damping augmentation - // This is used to reduce damping away from track so as not to interfere - // with the track capture angle - dampingWeight = 1.0f - fabsf(sine_Nu1 * 1.4142f); - dampingWeight = dampingWeight*dampingWeight; - _nav_bearing = atan2f(AB_unit.y, AB_unit.x) + Nu1; // bearing (radians) from AC to L1 point } //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); - _latAccDem = (xtrackVel*dampingWeight*Kv + 2.0f*sinf(Nu))*VomegaA; + _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); // Waypoint capture status is always false during waypoint following _WPcircle = false; @@ -171,15 +160,15 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct // update L1 control for loitering void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction) { - // Calculate normalised frequency for tracking loop - const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period - // Calculate additional damping gain used with L1 control (used during waypoint capture) - const float Kv_L1 = omegaA * 2.8284f * (_L1_damping - 0.7071f); // omegaA * 2*sqrt(2) * (dampingRatio - 1/sqrt(2)) + struct Location _current_loc; + // Calculate guidance gains used by PD loop (used during circle tracking) float omega = (6.2832f / _L1_period); float Kx = omega * omega; float Kv = 2.0f * _L1_damping * omega; - struct Location _current_loc; + + // Calculate L1 gain required for specified damping (used during waypoint capture) + float K_L1 = 4.0f * _L1_damping * _L1_damping; //Get current position and velocity _ahrs->get_position(&_current_loc); @@ -193,8 +182,9 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius float groundSpeed = _groundspeed_vector.length(); // Calculate time varying control parameters - _L1_dist = groundSpeed / omegaA; // L1 distance is adjusted to maintain a constant tracking loop frequency - float VomegaA = groundSpeed * omegaA; + // Calculate the L1 length required for specified period + // 0.3183099 = 1/pi + _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; //Convert current location and WP positionsto 2D vectors in lat and long Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f)); @@ -207,13 +197,13 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius Vector2f A_air_unit = (A_air).normalized(); //Calculate Nu to capture center_WP - float xtrackVelCap = _cross2D(A_air_unit , _groundspeed_vector); // Velocity across line - perpendicular to radial inbound to WP + float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP float Nu = atan2f(xtrackVelCap,ltrackVelCap); Nu = constrain_float(Nu, -1.5708f, +1.5708f); //Limit Nu to +- Pi/2 //Calculate lat accln demand to capture center_WP (use L1 guidance law) - float latAccDemCap = VomegaA * (xtrackVelCap * Kv_L1 + 2.0f * sinf(Nu)); + float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); //Calculate radial position and velocity errors float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity @@ -320,15 +310,6 @@ Vector2f AP_L1_Control::_geo2planar(const Vector2f &ref, const Vector2f &wp) con return out; } -float AP_L1_Control::_cross2D(const Vector2f &v1, const Vector2f &v2) -{ - float out; - - out = v1.x * v2.y - v1.y * v2.x; - - return out; -} - float AP_L1_Control::_maxf(const float &num1, const float &num2) const { float result; diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index ea559598e3..4535bedb61 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -87,9 +87,6 @@ private: // coordinates based on a reference point Vector2f _geo2planar(const Vector2f &ref, const Vector2f &wp) const; - //Calculate the cross product of two 2D vectors - float _cross2D(const Vector2f &v1, const Vector2f &v2); - //Calculate the maximum of two floating point numbers float _maxf(const float &num1, const float &num2) const;