From a3c2851120f3572893bdf29ddc0ee24dac67cbe1 Mon Sep 17 00:00:00 2001 From: Brandon Jones Date: Mon, 4 Feb 2013 20:23:41 -0500 Subject: [PATCH] AP_L1_Control: Addition of library for geometry calculations required for L1 Control. 1) Explicit control of tracking loop period and damping which removes previous variation in period with speed and fixed damping ratio 2) Explicit control of track capture angle (now set to 45 degrees by default) 3) Removal of restriction on loiter radius being greater than L1 distance The circle(loiter) control is a L1 and PD hybrid utilising L1 for waypoint capture and PD control for circle tracking. Pair-Programmed-With: Paul Riseborough Pair-Programmed-With: Andrew Tridgell --- libraries/AP_L1_Control/AP_L1_Control.cpp | 342 ++++++++++++++++++++++ libraries/AP_L1_Control/AP_L1_Control.h | 110 +++++++ libraries/AP_L1_Control/keywords.txt | 1 + 3 files changed, 453 insertions(+) create mode 100644 libraries/AP_L1_Control/AP_L1_Control.cpp create mode 100644 libraries/AP_L1_Control/AP_L1_Control.h create mode 100644 libraries/AP_L1_Control/keywords.txt diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp new file mode 100644 index 0000000000..6dc3557f22 --- /dev/null +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -0,0 +1,342 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include "AP_L1_Control.h" + +// table of user settable parameters +const AP_Param::GroupInfo AP_L1_Control::var_info[] PROGMEM = { + // @Param: PERIOD + // @DisplayName: L1 control period + // @Description: Period in seconds of L1 tracking loop. This needs to be larger for less responsive airframes. The default of 30 is very conservative, and for most RC aircraft will lead to slow and lazy turns. For smaller more agile aircraft a value closer to 20 is appropriate. When tuning, change this value in small increments, as a value that is much too small (say 5 or 10 below the right value) can lead to very radical turns, and a risk of stalling. + // @Units: seconds + // @Range: 1-60 + // @Increment: 1 + AP_GROUPINFO("PERIOD", 0, AP_L1_Control, _L1_period, 30), + + // @Param: DAMPING + // @DisplayName: L1 control damping ratio + // @Description: Damping ratio for L1 control. Increase this if you are getting overshoot in path tracking. + // @Range: 0.6-1.0 + // @Increment: 0.05 + AP_GROUPINFO("DAMPING", 1, AP_L1_Control, _L1_damping, 0.75f), + + AP_GROUPEND +}; + +//Bank angle command based on angle between aircraft velocity vector and reference vector to path. +//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 enable period and damping of guidance loop to be set explicitly +//Modified to provide explicit control over capture angle + + +/* + return the bank angle needed to achieve tracking from the last + update_*() operation + */ +int32_t AP_L1_Control::nav_roll_cd(void) +{ + int32_t ret; + ret = degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81 + return ret; +} + +int32_t AP_L1_Control::nav_bearing_cd(void) +{ + return wrap_180_cd(RadiansToCentiDegrees(_nav_bearing)); +} + +int32_t AP_L1_Control::bearing_error_cd(void) +{ + return RadiansToCentiDegrees(_bearing_error); +} + +int32_t AP_L1_Control::target_bearing_cd(void) +{ + return _target_bearing_cd; +} + +float AP_L1_Control::turn_distance(float wp_radius) +{ + return max(wp_radius, _L1_dist); +} + +bool AP_L1_Control::reached_loiter_target(void) +{ + return _WPcircle; +} + +float AP_L1_Control::crosstrack_error(void) +{ + return _crosstrack_error; +} + +// update L1 control for waypoint navigation +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; + + // Get current position and velocity + _ahrs->get_position(&_current_loc); + + // update _target_bearing_cd + _target_bearing_cd = get_bearing_cd(&_current_loc, &next_WP); + + Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); + + //Calculate groundspeed + 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; + + //Convert current location and WP positions to 2D vectors in lat and long + Vector2f A_air((_current_loc.lat/1.0e7f), (_current_loc.lng/1.0e7f)); + Vector2f A_v((prev_WP.lat/1.0e7f), (prev_WP.lng/1.0e7f)); + Vector2f B_v((next_WP.lat/1.0e7f), (next_WP.lng/1.0e7f)); + + //Calculate the NE position of the aircraft and WP B relative to WP A + A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH; + Vector2f AB = _geo2planar(A_v, B_v)*RADIUS_OF_EARTH; + + //Calculate the unit vector from WP A to WP B + Vector2f AB_unit = (AB).normalized(); + + // calculate distance to target track, for reporting + _crosstrack_error = _cross2D(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 + //Otherwise do normal L1 guidance + 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 + 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 + float Nu2 = atan2f(xtrackVel,ltrackVel); + + //Calculate Nu1 angle (Angle to L1 reference point) + float xtrackErr = _cross2D(A_air, AB_unit); + float sine_Nu1 = xtrackErr/_L1_dist; + //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg + sine_Nu1 = constrain(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(Nu, -1.5708f, +1.5708f); + _latAccDem = (xtrackVel*dampingWeight*Kv + 2.0f*sinf(Nu))*VomegaA; + + // Waypoint capture status is always false during waypoint following + _WPcircle = false; + + _bearing_error = Nu; // bearing error angle (radians), +ve to left of track +} + +// 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)) + // 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; + + //Get current position and velocity + _ahrs->get_position(&_current_loc); + + // update _target_bearing_cd + _target_bearing_cd = get_bearing_cd(&_current_loc, ¢er_WP); + + Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); + + //Calculate groundspeed + 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; + + //Convert current location and WP positionsto 2D vectors in lat and long + Vector2f A_air((_current_loc.lat/1.0e7f), (_current_loc.lng/1.0e7f)); + Vector2f A_v((center_WP.lat/1.0e7f), (center_WP.lng/1.0e7f)); + + //Calculate the NE position of the aircraft relative to WP A + A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH; + + //Calculate the unit vector from WP A to aircraft + 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 ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP + float Nu = atan2f(xtrackVelCap,ltrackVelCap); + Nu = constrain(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)); + + //Calculate radial position and velocity errors + float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity + float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle + + // keep crosstrack error for reporting + _crosstrack_error = xtrackErrCirc; + + //Calculate PD control correction to circle waypoint + float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv); + + //Calculate centripetal acceleration to circle waypoint + float velTangent = _maxf((xtrackVelCap * float(loiter_direction)) , 0.0f); + float latAccDemCircCtr = velTangent * velTangent / _maxf((0.5f * radius) , (radius + xtrackErrCirc)); + + //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand + float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); + + //Perform switchover between 'capture' and 'circle' modes at the point where the commands cross over to achieve a seamless transfer + if ((latAccDemCap < latAccDemCirc && loiter_direction > 0) | (latAccDemCap > latAccDemCirc && loiter_direction < 0)) { + _latAccDem = latAccDemCap; + _WPcircle = true; + _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track + _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point + } else { + _latAccDem = latAccDemCirc; + _WPcircle = false; + _bearing_error = 0.0f; // bearing error (radians), +ve to left of track + _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point + } +} + + +// update L1 control for heading hold navigation +void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd) +{ + // Calculate normalised frequency for tracking loop + const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period + // Calculate additional damping gain + + int32_t Nu_cd; + float Nu; + + // copy to _target_bearing_cd and _nav_bearing + _target_bearing_cd = wrap_180_cd(navigation_heading_cd); + _nav_bearing = radians(navigation_heading_cd * 0.01f); + + Nu_cd = _target_bearing_cd - wrap_180_cd(_ahrs->yaw_sensor); + Nu_cd = wrap_180_cd(Nu_cd); + Nu = radians(Nu_cd * 0.01f); + + Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); + + //Calculate groundspeed + 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; + + // Waypoint capture status is always false during heading hold + _WPcircle = false; + + _crosstrack_error = 0; + + _bearing_error = Nu; // bearing error angle (radians), +ve to left of track + + // Limit Nu to +-pi + Nu = constrain(Nu, -1.5708f, +1.5708f); + _latAccDem = 2.0f*sinf(Nu)*VomegaA; +} + +// update L1 control for level flight on current heading +void AP_L1_Control::update_level_flight(void) +{ + // copy to _target_bearing_cd and _nav_bearing + _target_bearing_cd = _ahrs->yaw_sensor; + _nav_bearing = _ahrs->yaw; + _bearing_error = 0; + _crosstrack_error = 0; + + // Waypoint capture status is always false during heading hold + _WPcircle = false; + + _latAccDem = 0; +} + + +Vector2f AP_L1_Control::_geo2planar(const Vector2f &ref, const Vector2f &wp) +{ + Vector2f out; + + out.x=radians((wp.x-ref.x)); + out.y=radians((wp.y-ref.y)*cosf(radians(ref.x))); + + 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; +} + +Vector2f AP_L1_Control::_planar2geo(const Vector2f &ref, const Vector2f &wp) +{ + Vector2f out; + + out.x=degrees(wp.x)+ref.x; + out.y=degrees(wp.y*(1/cosf(radians(ref.x))))+ref.y; + + return out; +} + +float AP_L1_Control::_maxf(const float &num1, const float &num2) +{ + float result; + + if (num1 > num2) + result = num1; + else + result = num2; + + return result; +} + diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h new file mode 100644 index 0000000000..2fc8dcb5b3 --- /dev/null +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -0,0 +1,110 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_L1_Control.h +/// @brief L1 Control algorithm. This is a instance of an +/// AP_Navigation class + +/* + * Originally written by Brandon Jones 2013 + * + * Modified by Paul Riseborough 2013 to provide: + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + */ + +#ifndef AP_L1_CONTROL_H +#define AP_L1_CONTROL_H + +#include +#include +#include +#include + +class AP_L1_Control : public AP_Navigation { +public: + AP_L1_Control(AP_AHRS *ahrs) : + _ahrs(ahrs) + { + AP_Param::setup_object_defaults(this, var_info); + } + + /* see AP_Navigation.h for the definitions and units of these + * functions */ + int32_t nav_roll_cd(void); + + // return the desired track heading angle(centi-degrees) + int32_t nav_bearing_cd(void); + + // return the heading error angle (centi-degrees) +ve to left of track + int32_t bearing_error_cd(void); + + float crosstrack_error(void); + + int32_t target_bearing_cd(void); + float turn_distance(float wp_radius); + void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP); + void update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction); + void update_heading_hold(int32_t navigation_heading_cd); + void update_level_flight(void); + bool reached_loiter_target(void); + + // this supports the NAVl1_* user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +private: + // reference to the AHRS object + AP_AHRS *_ahrs; + + struct Location _current_loc; + + // lateral acceration in m/s required to fly to the + // L1 reference point (+ve to right) + float _latAccDem; + + // L1 tracking distance in meters which is dynamically updated + float _L1_dist; + + // Status which is true when the vehicle has started circling the WP + bool _WPcircle; + + // bearing angle (radians) to L1 point + float _nav_bearing; + + // bearing error angle (radians) +ve to left of track + float _bearing_error; + + // crosstrack error in meters + float _crosstrack_error; + + // target bearing in centi-degrees from last update + int32_t _target_bearing_cd; + + // L1 tracking loop period (sec) + AP_Float _L1_period; + // L1 tracking loop damping ratio + AP_Float _L1_damping; + // L1 control loop enable + AP_Int8 _enable; + + // Convert a 2D vector from latitude and longitude to planar + // coordinates based on a reference point + Vector2f _geo2planar(const Vector2f &ref, const Vector2f &wp); + + //Calculate the cross product of two 2D vectors + float _cross2D(const Vector2f &v1, const Vector2f &v2); + + //Calculate the dot product of two 2D vectors + float _dot2D(const Vector2f &v1, const Vector2f &v2); + + // Convert a 2D vector from planar coordinates to latitude and + // longitude based on a reference point + Vector2f _planar2geo(const Vector2f &ref, const Vector2f &wp); + + //Calculate the maximum of two floating point numbers + float _maxf(const float &num1, const float &num2); + +}; + + +#endif //AP_L1_CONTROL_H diff --git a/libraries/AP_L1_Control/keywords.txt b/libraries/AP_L1_Control/keywords.txt new file mode 100644 index 0000000000..e422c164ba --- /dev/null +++ b/libraries/AP_L1_Control/keywords.txt @@ -0,0 +1 @@ +AP_L1_Control KEYWORD1 \ No newline at end of file