Plane: update for new AP_Navigation controller class

this switches ArduPlane over to use the L1 navigation controller, via
a generic nav_controller object pointer.

Note that the nav_controller controls all types of navigation now,
including level flight and heading hold. This provides a cleaner
abstraction than the old method of special case navigation handling

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

ArduPlane/ArduPlane.pde

@@ -57,6 +57,9 @@
 #include <APM_Control.h>
 #endif
 
+#include <AP_Navigation.h>
+#include <AP_L1_Control.h>
+
 // Pre-AP_HAL compatibility
 #include "compat.h"
 
@@ -216,6 +219,8 @@ AP_AHRS_HIL ahrs(&ins, g_gps);
 AP_AHRS_DCM ahrs(&ins, g_gps);
 #endif
 
+static AP_L1_Control L1_controller(&ahrs);
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
 #endif
@@ -230,6 +235,9 @@ static bool training_manual_pitch; // user has manual pitch control
 GCS_MAVLINK gcs0;
 GCS_MAVLINK gcs3;
 
+// selected navigation controller
+static AP_Navigation *nav_controller = &L1_controller;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Analog Inputs
 ////////////////////////////////////////////////////////////////////////////////
@@ -352,21 +360,10 @@ static bool have_position;
 // Constants
 const float radius_of_earth   = 6378100;        // meters
 
-// This is the currently calculated direction to fly.
-// deg * 100 : 0 to 360
-static int32_t nav_bearing_cd;
-
-// This is the direction to the next waypoint or loiter center
-// deg * 100 : 0 to 360
-static int32_t target_bearing_cd;
-
-//This is the direction from the last waypoint to the next waypoint
-// deg * 100 : 0 to 360
-static int32_t crosstrack_bearing_cd;
-
 // Direction held during phases of takeoff and landing
 // deg * 100 dir of plane,  A value of -1 indicates the course has not been set/is not in use
-static int32_t hold_course                   = -1;              // deg * 100 dir of plane
+// this is a 0..36000 value, or -1 for disabled
+static int32_t hold_course_cd                 = -1;              // deg * 100 dir of plane
 
 // There may be two active commands in Auto mode.
 // This indicates the active navigation command by index number
@@ -412,18 +409,9 @@ static uint8_t receiver_rssi;
 // The amount current ground speed is below min ground speed.  Centimeters per second
 static int32_t groundspeed_undershoot = 0;
 
-////////////////////////////////////////////////////////////////////////////////
-// Location Errors
-////////////////////////////////////////////////////////////////////////////////
-// Difference between current bearing and desired bearing.  Hundredths of a degree
-static int32_t bearing_error_cd;
-
 // Difference between current altitude and desired altitude.  Centimeters
 static int32_t altitude_error_cm;
 
-// Distance perpandicular to the course line that we are off trackline.  Meters
-static float crosstrack_error;
-
 ////////////////////////////////////////////////////////////////////////////////
 // Battery Sensors
 ////////////////////////////////////////////////////////////////////////////////
@@ -467,21 +455,10 @@ static bool throttle_suppressed;
 ////////////////////////////////////////////////////////////////////////////////
 // Loiter management
 ////////////////////////////////////////////////////////////////////////////////
-// Previous target bearing.  Used to calculate loiter rotations.  Hundredths of a degree
-static int32_t old_target_bearing_cd;
-
-// Total desired rotation in a loiter.  Used for Loiter Turns commands.  Degrees
-static int32_t loiter_total;
 
 // Direction for loiter. 1 for clockwise, -1 for counter-clockwise
 static int8_t loiter_direction = 1;
 
-// The amount in degrees we have turned since recording old_target_bearing
-static int16_t loiter_delta;
-
-// Total rotation in a loiter.  Used for Loiter Turns commands and to check for missed waypoints.  Degrees
-static int32_t loiter_sum;
-
 // The amount of time we have been in a Loiter.  Used for the Loiter Time command.  Milliseconds.
 static uint32_t loiter_time_ms;
 
@@ -507,6 +484,18 @@ uint32_t wp_distance;
 // Distance between previous and next waypoint.  Meters
 static uint32_t wp_totalDistance;
 
+/*
+  meta data to support counting the number of circles in a loiter
+ */
+static struct {
+    int32_t old_target_bearing_cd;
+    int32_t loiter_sum_cd;
+
+    // Total desired rotation in a loiter.  Used for Loiter Turns commands. 
+    int32_t loiter_total_cd;
+} loiter;
+
+
 // event control state
 enum event_type { 
     EVENT_TYPE_RELAY=0,
@@ -758,9 +747,6 @@ static void fast_loop()
 
     ahrs.update();
 
-    // uses the yaw from the DCM to give more accurate turns
-    calc_bearing_error();
-
     if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
         Log_Write_Attitude();
 
@@ -1030,7 +1016,7 @@ static void update_current_flight_mode(void)
 
         switch(nav_command_ID) {
         case MAV_CMD_NAV_TAKEOFF:
-            if (hold_course != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) {
+            if (hold_course_cd != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) {
                 calc_nav_roll();
             } else {
                 nav_roll_cd = 0;
@@ -1092,7 +1078,7 @@ static void update_current_flight_mode(void)
         default:
             // we are doing normal AUTO flight, the special cases
             // are for takeoff and landing
-            hold_course = -1;
+            hold_course_cd = -1;
             land_complete = false;
             calc_nav_roll();
             calc_nav_pitch();
@@ -1101,7 +1087,7 @@ static void update_current_flight_mode(void)
         }
     }else{
         // hold_course is only used in takeoff and landing
-        hold_course = -1;
+        hold_course_cd = -1;
 
         switch(control_mode) {
         case RTL:
@@ -1246,7 +1232,6 @@ static void update_navigation()
     case RTL:
     case GUIDED:
         update_loiter();
-        calc_bearing_error();
         break;
 
     case MANUAL:

ArduPlane/Attitude.pde

@@ -325,8 +325,9 @@ static void calc_throttle()
 // ----------------------------------------------------------------------
 static void calc_nav_yaw(float speed_scaler, float ch4_inf)
 {
-    if (hold_course != -1) {
+    if (hold_course_cd != -1) {
         // steering on or close to ground
+        int32_t bearing_error_cd = nav_controller->bearing_error_cd();
         g.channel_rudder.servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) + 
             g.kff_rudder_mix * g.channel_roll.servo_out;
         g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500);
@@ -364,35 +365,7 @@ static void calc_nav_pitch()
 
 static void calc_nav_roll()
 {
-#define NAV_ROLL_BY_RATE 0
+    nav_roll_cd = nav_controller->nav_roll_cd();
-#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
-    // to turn at 15 degrees per second.
-    float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01);
-
-    // Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
-    float speed;
-    if (!ahrs.airspeed_estimate(&speed)) {
-        speed = g.airspeed_cruise_cm*0.01;
-
-        // Floor the speed so that the user can't enter a bad value
-        if(speed < 6) {
-            speed = 6;
-        }
-    }
-
-    // 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);
-
-#else
-    // this is the old nav_roll calculation. We will use this for 2.50
-    // then remove for a future release
-    float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
-    nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
-    nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
-#endif
-
     nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
 }
 

ArduPlane/GCS_Mavlink.pde

@@ -271,17 +271,16 @@ static void NOINLINE send_location(mavlink_channel_t chan)
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 {
-    int16_t bearing = (hold_course==-1 ? nav_bearing_cd : hold_course) / 100;
     mavlink_msg_nav_controller_output_send(
         chan,
         nav_roll_cd * 0.01,
         nav_pitch_cd * 0.01,
-        bearing,
+        nav_controller->nav_bearing_cd() * 0.01f,
-        target_bearing_cd * 0.01,
+        nav_controller->target_bearing_cd() * 0.01f,
         wp_distance,
         altitude_error_cm * 0.01,
         airspeed_error_cm,
-        crosstrack_error);
+        nav_controller->crosstrack_error());
 }
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)

ArduPlane/Log.pde

@@ -489,8 +489,8 @@ static void Log_Write_Nav_Tuning()
         LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
         yaw                 : (uint16_t)ahrs.yaw_sensor,
         wp_distance         : wp_distance,
-        target_bearing_cd   : (uint16_t)target_bearing_cd,
+        target_bearing_cd   : (uint16_t)nav_controller->target_bearing_cd(),
-        nav_bearing_cd      : (uint16_t)nav_bearing_cd,
+        nav_bearing_cd      : (uint16_t)nav_controller->nav_bearing_cd(),
         altitude_error_cm   : (int16_t)altitude_error_cm,
         airspeed_cm         : (int16_t)airspeed.get_airspeed_cm()
     };

ArduPlane/Parameters.h

@@ -73,7 +73,7 @@ public:
         k_param_reset_mission_chan,
         k_param_land_flare_alt,
         k_param_land_flare_sec,
-        k_param_crosstrack_min_distance,
+        k_param_crosstrack_min_distance, // unused
         k_param_rudder_steer,
         k_param_throttle_nudge,
         k_param_alt_offset,
@@ -83,6 +83,7 @@ public:
         k_param_takeoff_heading_hold,
         k_param_hil_servos,
         k_param_vtail_output,
+        k_param_nav_controller,
 
         // 110: Telemetry control
         //
@@ -125,8 +126,8 @@ public:
         //
         // 150: Navigation parameters
         //
-        k_param_crosstrack_gain = 150,
+        k_param_crosstrack_gain = 150, // unused
-        k_param_crosstrack_entry_angle,
+        k_param_crosstrack_entry_angle, // unused
         k_param_roll_limit_cd,
         k_param_pitch_limit_max_cd,
         k_param_pitch_limit_min_cd,
@@ -134,7 +135,7 @@ public:
         k_param_RTL_altitude_cm,
         k_param_inverted_flight_ch,
         k_param_min_gndspeed_cm,
-        k_param_crosstrack_use_wind,
+        k_param_crosstrack_use_wind, // unused
 
 
         //
@@ -219,6 +220,7 @@ public:
         k_param_rollController,
         k_param_pitchController,
         k_param_yawController,
+        k_param_L1_controller,
 
         //
         // 240: PID Controllers
@@ -255,12 +257,8 @@ public:
     // speed used for speed scaling
     AP_Float scaling_speed;
 
-    // Crosstrack navigation
+    // navigation controller type. See AP_Navigation::ControllerType
-    //
+    AP_Int8  nav_controller;
-    AP_Float crosstrack_gain;
-    AP_Int16 crosstrack_entry_angle;
-    AP_Int8  crosstrack_use_wind;
-    AP_Int16 crosstrack_min_distance;
 
     // Estimation
     //

ArduPlane/Parameters.pde

@@ -143,38 +143,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(land_flare_sec,          "LAND_FLARE_SEC",  2.0),
 
-    // @Param: XTRK_GAIN_SC
+	// @Param: NAV_CONTROLLER
-    // @DisplayName: Crosstrack Gain
+	// @DisplayName: Navigation controller selection
-    // @Description: The scale between distance off the line and angle to meet the line (in Degrees * 100)
+	// @Description: Which navigation controller to enable
-    // @Range: 0 2000
+	// @Values: 0:Legacy,1:L1Controller
-    // @Increment: 1
+	// @User: Standard
-    // @User: Standard
+	GSCALAR(nav_controller,          "NAV_CONTROLLER",   AP_Navigation::CONTROLLER_L1),
-    GSCALAR(crosstrack_gain,        "XTRK_GAIN_SC",   XTRACK_GAIN_SCALED),
-
-    // @Param: XTRK_ANGLE_CD
-    // @DisplayName: Crosstrack Entry Angle
-    // @Description: Maximum angle used to correct for track following.
-    // @Units: centi-Degrees
-    // @Range: 0 9000
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD",  XTRACK_ENTRY_ANGLE_CENTIDEGREE),
-
-    // @Param: XTRK_USE_WIND
-    // @DisplayName: Crosstrack Wind correction
-    // @Description: If enabled, use wind estimation for navigation crosstrack when using a compass for yaw
-    // @Values: 0:Disabled,1:Enabled
-    // @User: Standard
-    GSCALAR(crosstrack_use_wind, "XTRK_USE_WIND",     1),
-
-    // @Param: XTRK_MIN_DIST
-    // @DisplayName: Crosstrack mininum distance
-    // @Description: Minimum distance in meters between waypoints to do crosstrack correction.
-    // @Units: Meters
-    // @Range: 0 32767
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST",  50),
 
     // @Param: ALT_MIX
     // @DisplayName: Gps to Baro Mix
@@ -208,7 +182,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @DisplayName: Waypoint Radius
     // @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit.
     // @Units: Meters
-    // @Range: 1 127
+    // @Range: 1 32767
     // @Increment: 1
     // @User: Standard
     GSCALAR(waypoint_radius,        "WP_RADIUS",      WP_RADIUS_DEFAULT),
@@ -736,6 +710,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
     GOBJECT(airspeed,                               "ARSPD_",   AP_Airspeed),
 
+    // @Group: NAVL1_
+    // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
+    GOBJECT(L1_controller,         "NAVL1_",   AP_L1_Control),
+
 #if MOUNT == ENABLED
     // @Group: MNT_
     // @Path: ../libraries/AP_Mount/AP_Mount.cpp

ArduPlane/commands.pde

@@ -193,23 +193,13 @@ static void set_next_WP(const struct Location *wp)
     }
 
     // zero out our loiter vals to watch for missed waypoints
-    loiter_delta            = 0;
+    loiter_angle_reset();
-    loiter_sum                      = 0;
-    loiter_total            = 0;
 
     // this is handy for the groundstation
     wp_totalDistance        = get_distance(&current_loc, &next_WP);
     wp_distance             = wp_totalDistance;
-    target_bearing_cd       = get_bearing_cd(&current_loc, &next_WP);
-    nav_bearing_cd          = target_bearing_cd;
 
-    // to check if we have missed the WP
+    loiter_angle_reset();
-    // ----------------------------
-    old_target_bearing_cd   = target_bearing_cd;
-
-    // set a new crosstrack bearing
-    // ----------------------------
-    reset_crosstrack();
 }
 
 static void set_guided_WP(void)
@@ -230,15 +220,8 @@ static void set_guided_WP(void)
     // this is handy for the groundstation
     wp_totalDistance        = get_distance(&current_loc, &next_WP);
     wp_distance             = wp_totalDistance;
-    target_bearing_cd       = get_bearing_cd(&current_loc, &next_WP);
 
-    // to check if we have missed the WP
+    loiter_angle_reset();
-    // ----------------------------
-    old_target_bearing_cd = target_bearing_cd;
-
-    // set a new crosstrack bearing
-    // ----------------------------
-    reset_crosstrack();
 }
 
 // run this at setup on the ground

ArduPlane/commands_logic.pde

@@ -281,7 +281,7 @@ static void do_loiter_unlimited()
 static void do_loiter_turns()
 {
     set_next_WP(&next_nav_command);
-    loiter_total = next_nav_command.p1 * 360;
+    loiter.loiter_total_cd = next_nav_command.p1 * 36000UL;
     loiter_set_direction_wp(&next_nav_command);
 }
 
@@ -299,24 +299,24 @@ static void do_loiter_time()
 static bool verify_takeoff()
 {
     if (ahrs.yaw_initialised()) {
-        if (hold_course == -1 && g.takeoff_heading_hold != 0) {
+        if (hold_course_cd == -1 && g.takeoff_heading_hold != 0) {
             // save our current course to take off
-            hold_course = ahrs.yaw_sensor;
+            hold_course_cd = ahrs.yaw_sensor;
-            gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course);
+            gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course_cd);
         }
     }
 
-    if (hold_course != -1) {
+    if (hold_course_cd != -1) {
-        // recalc bearing error with hold_course;
+        // call navigation controller for heading hold
-        nav_bearing_cd = hold_course;
+        nav_controller->update_heading_hold(hold_course_cd);
-        // recalc bearing error
+    } else {
-        calc_bearing_error();
+        nav_controller->update_level_flight();        
     }
 
     if (adjusted_altitude_cm() > takeoff_altitude)  {
-        hold_course = -1;
+        hold_course_cd = -1;
         takeoff_complete = true;
-        next_WP = current_loc;
+        next_WP = prev_WP = current_loc;
         return true;
     } else {
         return false;
@@ -337,18 +337,18 @@ static bool verify_land()
 
         land_complete = true;
 
-        if (hold_course == -1) {
+        if (hold_course_cd == -1) {
             // we have just reached the threshold of to flare for landing.
             // We now don't want to do any radical
             // turns, as rolling could put the wings into the runway.
-            // To prevent further turns we set hold_course to the
+            // To prevent further turns we set hold_course_cd to the
             // current heading. Previously we set this to
             // crosstrack_bearing, but the xtrack bearing can easily
             // be quite large at this point, and that could induce a
             // sudden large roll correction which is very nasty at
             // this point in the landing.
-            hold_course = ahrs.yaw_sensor;
+            hold_course_cd = ahrs.yaw_sensor;
-            gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course);
+            gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course_cd);
         }
 
         if (g_gps->ground_speed*0.01 < 3.0) {
@@ -362,30 +362,30 @@ static bool verify_land()
         }
     }
 
-    if (hold_course != -1) {
+    if (hold_course_cd != -1) {
         // recalc bearing error with hold_course;
-        nav_bearing_cd = hold_course;
+        nav_controller->update_heading_hold(hold_course_cd);
-        // recalc bearing error
-        calc_bearing_error();
     } else {
-        update_crosstrack();
+        nav_controller->update_waypoint(prev_WP, next_WP);
     }
     return false;
 }
 
 static bool verify_nav_wp()
 {
-    hold_course = -1;
+    hold_course_cd = -1;
-    update_crosstrack();
+
-    if (wp_distance <= (uint32_t)max(g.waypoint_radius,0)) {
+    nav_controller->update_waypoint(prev_WP, next_WP);
+    
+    if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) {
         gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
                           (unsigned)nav_command_index,
                           (unsigned)get_distance(&current_loc, &next_WP));
         return true;
-    }
+	}
 
     // have we circled around the waypoint?
-    if (loiter_sum > 300) {
+    if (loiter.loiter_sum_cd > 30000) {
         gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
         return true;
     }
@@ -404,14 +404,12 @@ static bool verify_nav_wp()
 static bool verify_loiter_unlim()
 {
     update_loiter();
-    calc_bearing_error();
     return false;
 }
 
 static bool verify_loiter_time()
 {
     update_loiter();
-    calc_bearing_error();
     if ((millis() - loiter_time_ms) > loiter_time_max_ms) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
         return true;
@@ -422,9 +420,8 @@ static bool verify_loiter_time()
 static bool verify_loiter_turns()
 {
     update_loiter();
-    calc_bearing_error();
+    if (loiter.loiter_sum_cd > loiter.loiter_total_cd) {
-    if(loiter_sum > loiter_total) {
+        loiter.loiter_total_cd = 0;
-        loiter_total = 0;
         gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
         // clear the command queue;
         return true;
@@ -434,12 +431,14 @@ static bool verify_loiter_turns()
 
 static bool verify_RTL()
 {
-    if (wp_distance <= (uint32_t)max(g.waypoint_radius,0)) {
+    update_loiter();
-        gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+	if (wp_distance <= (uint32_t)max(g.waypoint_radius,0) || 
-        return true;
+        nav_controller->reached_loiter_target()) {
-    }else{
+			gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+			return true;
+    } else {
         return false;
-    }
+	}
 }
 
 /********************************************************************************/

ArduPlane/config.h

@@ -834,6 +834,6 @@
 #endif
 
 #ifndef SERIAL_BUFSIZE
-# define SERIAL_BUFSIZE 256
+ # define SERIAL_BUFSIZE 256
 #endif
 

ArduPlane/navigation.pde

@@ -1,17 +1,59 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+
+// set the nav_controller pointer to the right controller
+static void set_nav_controller(void)
+{
+    switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
+    case AP_Navigation::CONTROLLER_L1:
+        nav_controller = &L1_controller;
+        break;
+    }
+}
+
+/*
+  reset the total loiter angle
+ */
+static void loiter_angle_reset(void)
+{
+    loiter.loiter_sum_cd = 0;
+    loiter.loiter_total_cd = 0;
+}
+
+/*
+  update the total angle we have covered in a loiter. Used to support
+  commands to do N circles of loiter
+ */
+static void loiter_angle_update(void)
+{
+    int32_t target_bearing_cd = nav_controller->target_bearing_cd();
+    int32_t loiter_delta_cd;
+    if (loiter.loiter_sum_cd == 0) {
+        loiter_delta_cd = 0;
+    } else {
+        loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
+    }
+    loiter.old_target_bearing_cd = target_bearing_cd;
+    loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
+
+    loiter.loiter_sum_cd += loiter_delta_cd;
+}
+
 //****************************************************************
 // Function that will calculate the desired direction to fly and distance
 //****************************************************************
 static void navigate()
 {
+    // allow change of nav controller mid-flight
+    set_nav_controller();
+
     // do not navigate with corrupt data
     // ---------------------------------
     if (!have_position) {
         return;
     }
 
-    if(next_WP.lat == 0) {
+    if (next_WP.lat == 0) {
         return;
     }
 
@@ -24,24 +66,8 @@ static void navigate()
         return;
     }
 
-    // target_bearing is where we should be heading
+    // update total loiter angle
-    // --------------------------------------------
+    loiter_angle_update();
-    target_bearing_cd       = get_bearing_cd(&current_loc, &next_WP);
-
-    // nav_bearing will includes xtrac correction
-    // ------------------------------------------
-    nav_bearing_cd = target_bearing_cd;
-
-    // check if we have missed the WP
-    loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100;
-
-    // reset the old value
-    old_target_bearing_cd = target_bearing_cd;
-
-    // wrap values
-    if (loiter_delta > 180) loiter_delta -= 360;
-    if (loiter_delta < -180) loiter_delta += 360;
-    loiter_sum += abs(loiter_delta);
 
     // control mode specific updates to nav_bearing
     // --------------------------------------------
@@ -49,16 +75,6 @@ static void navigate()
 }
 
 
-#if 0
-// Disabled for now
-void calc_distance_error()
-{
-    distance_estimate       += (float)g_gps->ground_speed * .0002f * cosf(radians(bearing_error_cd * .01f));
-    distance_estimate       -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
-    wp_distance             = max(distance_estimate,10);
-}
-#endif
-
 static void calc_airspeed_errors()
 {
     float aspeed_cm = airspeed.get_airspeed_cm();
@@ -105,12 +121,6 @@ static void calc_gndspeed_undershoot()
     }
 }
 
-static void calc_bearing_error()
-{
-    bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
-    bearing_error_cd = wrap_180_cd(bearing_error_cd);
-}
-
 static void calc_altitude_error()
 {
     if (control_mode == AUTO && offset_altitude_cm != 0) {
@@ -132,63 +142,6 @@ static void calc_altitude_error()
 
 static void update_loiter()
 {
-    float power;
+    nav_controller->update_loiter(next_WP, g.loiter_radius, loiter_direction);
-
-    if(wp_distance <= (uint32_t)max(g.loiter_radius,0)) {
-        power = float(wp_distance) / float(g.loiter_radius);
-        power = constrain(power, 0.5, 1);
-        nav_bearing_cd += 9000.0 * (2.0 + power) * loiter_direction;
-    } else if(wp_distance < (uint32_t)max((g.loiter_radius + LOITER_RANGE),0)) {
-        power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
-        power = constrain(power, 0.5, 1);                               //power = constrain(power, 0, 1);
-        nav_bearing_cd -= power * 9000 * loiter_direction;
-    } else{
-        update_crosstrack();
-        loiter_time_ms = millis();                              // keep start time for loiter updating till we get within LOITER_RANGE of orbit
-
-    }
-/*
- *       if (wp_distance < g.loiter_radius){
- *               nav_bearing += 9000;
- *       }else{
- *               nav_bearing -= 100 * M_PI / 180 * asinf(g.loiter_radius / wp_distance);
- *       }
- *
- *       update_crosstrack();
- */
-    nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
-}
-
-static void update_crosstrack(void)
-{
-    // 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_adjusted = nav_vector - wind2d;
-            nav_bearing_cd = degrees(atan2f(nav_adjusted.y, nav_adjusted.x)) * 100;
-        }
-    }
-
-    // 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) &&
-        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;               
-        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);
-    }
-
-}
-
-static void reset_crosstrack()
-{
-    crosstrack_bearing_cd   = get_bearing_cd(&prev_WP, &next_WP);       // Used for track following
 }
 

ArduPlane/setup.pde

@@ -110,7 +110,6 @@ setup_show(uint8_t argc, const Menu::arg *argv)
     report_radio();
     report_batt_monitor();
     report_gains();
-    report_xtrack();
     report_throttle();
     report_flight_modes();
     report_ins();
@@ -544,19 +543,6 @@ static void report_gains()
     print_blanks(2);
 }
 
-static void report_xtrack()
-{
-    //print_blanks(2);
-    cliSerial->printf_P(PSTR("Crosstrack\n"));
-    print_divider();
-    // radio
-    cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
-                         "XTRACK angle: %d\n"),
-                    (float)g.crosstrack_gain,
-                    (int)g.crosstrack_entry_angle);
-    print_blanks(2);
-}
-
 static void report_throttle()
 {
     //print_blanks(2);

ArduPlane/system.pde

@@ -259,6 +259,9 @@ static void init_ardupilot()
             Log_Write_Startup(TYPE_GROUNDSTART_MSG);
     }
 
+    // choose the nav controller
+    set_nav_controller();
+
     set_mode(MANUAL);
 
     // set the correct flight mode
@@ -352,10 +355,12 @@ static void set_mode(enum FlightMode mode)
         break;
 
     case AUTO:
+        prev_WP = current_loc;
         update_auto();
         break;
 
     case RTL:
+        prev_WP = current_loc;
         do_RTL();
         break;
 
@@ -368,6 +373,7 @@ static void set_mode(enum FlightMode mode)
         break;
 
     default:
+        prev_WP = current_loc;
         do_RTL();
         break;
     }