Plane: Using the L1 reference distance parameter at runtime to setup …

…the L1 navigation controller.

This way the L1 controller can be switched on and modified in flight, setting the paramter L1_dist to zero disables the controller and uses the old PID for navigation. This commit is part of issue ArduPilot#101

ArduPlane/ArduPlane.pde

@@ -386,7 +386,6 @@ static uint8_t non_nav_command_ID      = NO_COMMAND;
 ////////////////////////////////////////////////////////////////////////////////
 static int32_t nu_cd;
 struct Location L1_ref;
-static uint8_t L1=L1_REFERENCE_LENGTH;
 #endif
 ////////////////////////////////////////////////////////////////////////////////
 // Airspeed

ArduPlane/Attitude.pde

@@ -318,6 +318,18 @@ static void calc_nav_pitch()
 static void calc_nav_roll()
 {
 #define NAV_ROLL_BY_RATE 0
+
+#if L1_CONTROL == ENABLED
+	if(g.L1_dist > 0){
+		//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) / g.L1_dist) * sin( radians(nu_cd*0.01))  * 10.1972); //10.1972 = (1/9.81)*100
+		nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
+		return;
+	}	// If L1 is disable via parameter use the other controllers
+#endif
+
 #if NAV_ROLL_BY_RATE
     // Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
     // desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
@@ -338,11 +350,6 @@ 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

@@ -843,12 +843,6 @@
  # 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
 #endif

ArduPlane/navigation.pde

@@ -148,8 +148,11 @@ 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();
+    if(g.L1_dist>0){
+	calc_L1_circ(g.L1_dist, g.loiter_radius, next_WP, current_loc, L1_ref, loiter_direction);
+	calc_nu_cd();
+        return;
+    }
 #else
     float power;
 
@@ -209,8 +212,11 @@ static void wind_correct_bearing(int32_t &nav_bearing_cd)
 static void update_crosstrack(void)
 {
 #if L1_CONTROL
-    calc_L1_line(L1, prev_WP, next_WP, current_loc, L1_ref);
+  if(g.L1_dist>0){
+    calc_L1_line(g.L1_dist, prev_WP, next_WP, current_loc, L1_ref);
     calc_nu_cd();
+    return;
+  }
 #else
     wind_correct_bearing(nav_bearing_cd);