Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: Cleaned up calculation of damping to reduce computations

Browse Source 
also removed _cross2D, using math library instead
This commit is contained in:
Andrew Tridgell 2013-05-05 14:14:21 +10:00
parent 372ebb602c
commit 9654546b5a
2 changed files with 28 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -78,14 +78,13 @@ 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
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 &center_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 &center_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 &center_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;

3
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -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;