Plane: L1 Control for Straight and Curved Path Following

ArduPlane/ArduPlane.pde

@@ -57,6 +57,10 @@
 #include <APM_Control.h>
 #endif
 
+#if L1_CONTROL == ENABLED
+#include <AP_L1_Control.h>
+#endif
+
 // Pre-AP_HAL compatibility
 #include "compat.h"
 
@@ -376,6 +380,14 @@ static uint8_t non_nav_command_index;
 static uint8_t nav_command_ID          = NO_COMMAND;
 static uint8_t non_nav_command_ID      = NO_COMMAND;
 
+#if L1_CONTROL
+////////////////////////////////////////////////////////////////////////////////
+// L1 Control
+////////////////////////////////////////////////////////////////////////////////
+static int32_t nu_cd;
+struct Location L1_ref;
+static uint8_t L1=L1_REFERENCE_LENGTH;
+#endif
 ////////////////////////////////////////////////////////////////////////////////
 // Airspeed
 ////////////////////////////////////////////////////////////////////////////////

ArduPlane/Attitude.pde

@@ -338,6 +338,11 @@ static void calc_nav_roll()
     // Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
     nav_roll = ToDeg(atanf(speed*turn_rate/GRAVITY_MSS)*100);
 
+#elif L1_CONTROL 
+    //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.
+    nav_roll_cd=degrees( (2*sq(g_gps->ground_speed*0.01) / L1) * sin( radians(nu_cd*0.01))  * 10.1972); //10.1972 = (1/9.81)*100
 #else
     // this is the old nav_roll calculation. We will use this for 2.50
     // then remove for a future release

ArduPlane/config.h

@@ -838,7 +838,18 @@
 # define APM_CONTROL DISABLED
 #endif
 
+//L1 Control library by Brandon Jones
+#ifndef L1_CONTROL
+ # define L1_CONTROL DISABLED
+#endif
+
+#if L1_CONTROL
+ #ifndef L1_REFERENCE_LENGTH
+  #error Need to #define L1_REFERENCE_LENGTH [length].
+ #endif
+#endif
+
 #ifndef SERIAL_BUFSIZE
-# define SERIAL_BUFSIZE 256
+ # define SERIAL_BUFSIZE 256
 #endif
 

ArduPlane/navigation.pde

@@ -146,6 +146,11 @@ static int32_t wrap_180_cd(int32_t error)
 
 static void update_loiter()
 {
+
+#if L1_CONTROL
+    calc_L1_circ(L1, g.loiter_radius, next_WP, current_loc, L1_ref, loiter_direction);
+    calc_nu_cd();
+#else
     float power;
 
     if(wp_distance <= (uint32_t)max(g.loiter_radius,0)) {
@@ -171,34 +176,55 @@ static void update_loiter()
  *       update_crosstrack();
  */
     nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
+#endif
 }
 
-static void update_crosstrack(void)
+#if L1_CONTROL
+static void calc_nu_cd()
+{
+    nav_bearing_cd = get_bearing_cd(&current_loc, &L1_ref);
+    wind_correct_bearing(nav_bearing_cd);
+    nu_cd = nav_bearing_cd - ahrs.yaw_sensor;
+    nu_cd = constrain_int32(wrap_180_cd(nu_cd),-9000, 9000);
+}
+#endif
+
+static void wind_correct_bearing(int32_t &nav_bearing_cd)
 {
     // if we are using a compass for navigation, then adjust the
     // heading to account for wind
+
     if (g.crosstrack_use_wind && compass.use_for_yaw()) {
         Vector3f wind = ahrs.wind_estimate();
         Vector2f wind2d = Vector2f(wind.x, wind.y);
         float speed;
         if (ahrs.airspeed_estimate(&speed)) {
-            Vector2f nav_vector = Vector2f(cosf(radians(nav_bearing_cd*0.01)), sinf(radians(nav_bearing_cd*0.01))) * speed;
+            Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
             Vector2f nav_adjusted = nav_vector - wind2d;
-            nav_bearing_cd = degrees(atan2f(nav_adjusted.y, nav_adjusted.x)) * 100;
+            nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
         }
     }
+}
+
+static void update_crosstrack(void)
+{
+#if L1_CONTROL
+    calc_L1_line(L1, prev_WP, next_WP, current_loc, L1_ref);
+    calc_nu_cd();
+#else
+    wind_correct_bearing(nav_bearing_cd);
 
     // Crosstrack Error
     // ----------------
     // If we are too far off or too close we don't do track following
-    if (wp_totalDistance >= (uint32_t)max(g.crosstrack_min_distance,0) &&
+    if (wp_totalDistance >= (uint32_t)max(g.crosstrack_min_distance,0) && 
         abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
         // Meters we are off track line
-        crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;               
+        crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;
         nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
         nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
     }
-
+#endif
 }
 
 static void reset_crosstrack()

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -0,0 +1,102 @@
+#include "AP_L1_Control.h"
+#define RADIUS_OF_EARTH 6378100
+
+Vector2f geo2planar(const Vector2f &ref, const Vector2f &wp)
+{
+    Vector2f out;
+
+    out.x=radians((wp.x-ref.x));
+    out.y=radians((wp.y-ref.y)*cos(radians(ref.x)));
+
+    return out;
+}
+
+Vector2f planar2geo(const Vector2f &ref, const Vector2f &wp)
+{
+    Vector2f out;
+
+    out.x=degrees(wp.x)+ref.x;
+    out.y=degrees(wp.y*(1/cos(radians(ref.x))))+ref.y;
+
+    return out;
+}
+
+void calc_L1_circ(  uint8_t                 L1,
+                    const uint8_t           turn_radius,
+                    const struct Location & turn_center_loc,
+                    const struct Location & current_loc,
+                    struct Location &       L1_ref,
+                    const int8_t            dir)
+{
+    int8_t side;
+    float beta, gamma, wp_distance;
+    Vector2f q1, q2(0,1);
+    Vector2f air((current_loc.lat/1.0e7), (current_loc.lng/1.0e7));
+    Vector2f X(turn_center_loc.lat/1.0e7, turn_center_loc.lng/1.0e7);
+
+    air=geo2planar(X, air)*RADIUS_OF_EARTH;
+    wp_distance=sqrt(sq(air.x)+sq(air.y)); //distance from vehicle to circle center.
+
+    q1=(-air).normalized(); //unit vector from vehicle to circle center.
+
+    //Calculate which side of circle vehicle is located.
+    if (q1.x > 0) {
+        side=1;
+    }else{
+        side=-1;
+    }
+
+    if (wp_distance > (turn_radius+L1) || wp_distance < (turn_radius-L1)) {
+        //Vehicle is far away from circle or very far within the circle
+        gamma=acos(q1* -q2);
+        L1_ref=turn_center_loc;
+        location_offset(&L1_ref, turn_radius*sin(-side*gamma), turn_radius*cos(-side*gamma));
+    } else {
+        gamma=acos(q1*q2);
+        L1_ref=current_loc;
+        //beta is angle between vector from vehicle to circle center, and vehcile to L1_ref
+        beta=-dir* -acos (( sq(wp_distance) + sq(L1) - sq(turn_radius) ) / (2*wp_distance*L1));
+        location_offset(&L1_ref, L1*sin(beta+side*gamma), L1*cos(beta+side*gamma));
+    }
+}
+
+void calc_L1_line(  uint8_t                 L1,
+                    const struct Location & A,
+                    const struct Location & B,
+                    const struct Location & current_loc,
+                    struct Location &       L1_ref)
+{
+    float psi_seg, tmp_L1, p_dist, lambda, d1;
+    Vector2f q1, q2, B_p;
+
+    Vector2f air((current_loc.lat/1.0e7), (current_loc.lng/1.0e7));
+    Vector2f A_v((A.lat/1.0e7), (A.lng/1.0e7));
+    Vector2f B_v((B.lat/1.0e7), (B.lng/1.0e7));
+
+    //Calculate angle from A to B
+    //Note: this could be pulled out of the function for efficiency.
+    B_p=geo2planar(A_v, B_v)*RADIUS_OF_EARTH;
+    q2=(B_p).normalized();
+    psi_seg=atan2(q2.x,q2.y);       //Angle from A to B
+
+    air=geo2planar(A_v, air)*RADIUS_OF_EARTH;
+    d1=sqrt(sq(air.x)+sq(air.y));   //distance from A to aircraft
+    q1=(air).normalized();          //Unit vector from A to aircraft
+
+    lambda=acos(q1*q2);             //Angle between A and aircraft
+
+    if (fabs(lambda) >= 1.5708) {   //Aircraft is somewhere behind A
+        L1=max(L1, d1);
+    } else {
+        tmp_L1=d1*sin(lambda);
+        if(fabs(tmp_L1) >= L1) {    //Aircraft is farther than L1 from segment AB
+            L1=tmp_L1*1.05;
+        }
+    }
+
+    //p_dist is distance from A to L1_ref, as measured along segment AB
+    //see:law of cosines
+    p_dist=d1*cos(lambda) + sqrt( sq(L1)-sq(d1)*sq(sin(lambda)) );
+    L1_ref=A;
+    location_offset(&L1_ref, p_dist*sin(psi_seg), p_dist*cos(psi_seg));
+}

libraries/AP_L1_Control/AP_L1_Control.h

@@ -0,0 +1,55 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file    AP_L1_Control.h
+/// @brief   L1 Control algorithm utilities
+
+/*
+ *  L1_Control algorithm utilities
+ *  Functions to generate a L1 reference point.
+ *  Brandon Jones 2013
+ *
+ *  calc_L1_circ and calc_L1_line based on:
+ *  Ducard, G.; Kulling, K.C.; Geering, H.P.; , "A simple and adaptive on-line path planning system
+ *  for a UAV," Control & Automation, 2007. MED '07. Mediterranean Conference on , vol., no., pp.1-6,
+ *  27-29 June 2007 */
+
+#ifndef AP_L1_CONTROL_H
+#define AP_L1_CONTROL_H
+
+#include <AP_Math.h>
+
+
+//Convert a 2D vector from latitude and longitude to planar coordinates based on a reference point
+Vector2f        geo2planar(const Vector2f &ref, const Vector2f &wp);
+
+//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 a reference point for L1 control based on a circle.
+ *  L1:  Reference length, smaller is equivalent to higher gain [meters]
+ *  turn_radius: Radius of the circle [meters]
+ *  turn_center_loc: Center of the circle
+ *  current_loc: Current location of vehicle
+ *  L1_ref: Generated Reference Point  
+ *  dir: direction of turn, 1 for clockwise, -1 for counter-clockwise */
+void        calc_L1_circ(  uint8_t                  L1,
+                           const uint8_t            turn_radius,
+                           const struct Location &  turn_center_loc,
+                           const struct Location &  current_loc,
+                           struct Location &        L1_ref,
+                           const int8_t             dir);
+
+/*Calculate a reference point for L1 control based on a line.
+ *  L1: Reference length, smaller is equivalent to higher gain [meters]
+ *  A: First point in line
+ *  B: Second point in line
+ *  current_loc: Current location of vehicle
+ *  L1_ref: Generated Reference Point */
+void        calc_L1_line(  uint8_t                  L1,
+                           const struct Location &  A,
+                           const struct Location &  B,
+                           const struct Location &  current_loc,
+                           struct Location &        L1_ref);
+
+
+#endif //AP_L1_CONTROL_H

libraries/AP_L1_Control/keywords.txt

@@ -0,0 +1 @@
+AP_L1_Control          KEYWORD1