Rover: use L1 controller for navigation

this uses the AP_L1_Control library for rover navigation

APMrover2/APMrover2.pde

@@ -90,6 +90,8 @@
 #include <SITL.h>
 #include <AP_Scheduler.h>       // main loop scheduler
 #include <stdarg.h>
+#include <AP_Navigation.h>
+#include <AP_L1_Control.h>
 
 #include <AP_HAL_AVR.h>
 #include <AP_HAL_AVR_SITL.h>
@@ -242,6 +244,11 @@ AP_InertialSensor_Oilpan ins( &adc );
 
 AP_AHRS_DCM ahrs(&ins, g_gps);
 
+static AP_L1_Control L1_controller(ahrs);
+
+// selected navigation controller
+static AP_Navigation *nav_controller = &L1_controller;
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
 #endif
@@ -372,17 +379,6 @@ const	float radius_of_earth 	= 6378100;	// meters
 // true if we have a position estimate from AHRS
 static bool have_position;
 
-// This is the currently calculated direction to fly.  
-// deg * 100 : 0 to 360
-static int32_t nav_bearing;
-// This is the direction to the next waypoint
-// deg * 100 : 0 to 360
-static int32_t target_bearing;	
-//This is the direction from the last waypoint to the next waypoint 
-// deg * 100 : 0 to 360
-static int32_t crosstrack_bearing;
-// A gain scaler to account for ground speed/headwind/tailwind
-static float	nav_gain_scaler 		= 1.0f;		
 static bool rtl_complete = false;
 
 // There may be two active commands in Auto mode.  
@@ -427,15 +423,6 @@ static bool auto_triggered;
 static float 	ground_speed = 0;
 static int16_t throttle_last = 0, throttle = 500;
 
-////////////////////////////////////////////////////////////////////////////////
-// Location Errors
-////////////////////////////////////////////////////////////////////////////////
-// Difference between current bearing and desired bearing.  in centi-degrees
-static int32_t bearing_error_cd;
-
-// Distance perpandicular to the course line that we are off trackline.  Meters 
-static float	crosstrack_error;
-
 ////////////////////////////////////////////////////////////////////////////////
 // CH7 control
 ////////////////////////////////////////////////////////////////////////////////
@@ -462,8 +449,8 @@ static float	current_total1;
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation control variables
 ////////////////////////////////////////////////////////////////////////////////
-// The instantaneous desired steering angle.  Hundredths of a degree
-static int32_t nav_steer_cd;
+// The instantaneous desired lateral acceleration in m/s/s
+static float lateral_acceleration;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Waypoint distances
@@ -663,9 +650,6 @@ static void fast_loop()
 
     read_sonars();
 
-	// 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();
 
@@ -865,23 +849,22 @@ static void update_current_mode(void)
     case AUTO:
     case RTL:
     case GUIDED:
+        calc_lateral_acceleration();
         calc_nav_steer();
         calc_throttle(g.speed_cruise);
         break;
 
-    case STEERING:
+    case STEERING: {
         /*
-          in steering mode we control the bearing error, which gives
-          the same type of steering control as auto mode. The throttle
-          controls the target speed, in proportion to the throttle
+          in steering mode we control lateral acceleration directly
          */
-        bearing_error_cd = channel_steer->pwm_to_angle();
+        lateral_acceleration = g.turn_max_g * GRAVITY_MSS * (channel_steer->pwm_to_angle()/4500.0f);
         calc_nav_steer();
 
-        /* we need to reset the I term or it will build up */
-        g.pidNavSteer.reset_I();
+        // and throttle gives speed in proportion to cruise speed
         calc_throttle(channel_throttle->pwm_to_angle() * 0.01 * g.speed_cruise);
         break;
+    }
 
     case LEARNING:
     case MANUAL:
@@ -923,8 +906,8 @@ static void update_navigation()
     case RTL:
     case GUIDED:
         // no loitering around the wp with the rover, goes direct to the wp position
+        calc_lateral_acceleration();
         calc_nav_steer();
-        calc_bearing_error();
         if (verify_RTL()) {  
             channel_throttle->servo_out = g.throttle_min.get();
             set_mode(HOLD);

APMrover2/GCS_Mavlink.pde

@@ -237,17 +237,16 @@ static void NOINLINE send_location(mavlink_channel_t chan)
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 {
-    int16_t bearing = nav_bearing / 100;
     mavlink_msg_nav_controller_output_send(
         chan,
-        nav_steer_cd / 1.0e2,
+        lateral_acceleration,
         0,
-        bearing,
-        target_bearing / 100,
+        nav_controller->nav_bearing_cd() * 0.01f,
+        nav_controller->target_bearing_cd() * 0.01f,
         wp_distance,
         0,
         groundspeed_error,
-        crosstrack_error);
+        nav_controller->crosstrack_error());
 }
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)

APMrover2/Log.pde

@@ -305,7 +305,6 @@ struct PACKED log_Nav_Tuning {
     float    wp_distance;
     uint16_t target_bearing_cd;
     uint16_t nav_bearing_cd;
-    int16_t  nav_gain_scalar;
     int8_t   throttle;
 };
 
@@ -316,9 +315,8 @@ static void Log_Write_Nav_Tuning()
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
         yaw                 : (uint16_t)ahrs.yaw_sensor,
         wp_distance         : wp_distance,
-        target_bearing_cd   : (uint16_t)target_bearing,
-        nav_bearing_cd      : (uint16_t)nav_bearing,
-        nav_gain_scalar     : (int16_t)(nav_gain_scaler*1000),
+        target_bearing_cd   : (uint16_t)nav_controller->target_bearing_cd(),
+        nav_bearing_cd      : (uint16_t)nav_controller->nav_bearing_cd(),
         throttle            : (int8_t)(100 * channel_throttle->norm_output())
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
@@ -364,7 +362,7 @@ static void Log_Write_Mode()
 
 struct PACKED log_Sonar {
     LOG_PACKET_HEADER;
-    int16_t nav_steer;
+    float    lateral_accel;
     uint16_t sonar1_distance;
     uint16_t sonar2_distance;
     uint16_t detected_count;
@@ -383,7 +381,7 @@ static void Log_Write_Sonar()
     }
     struct log_Sonar pkt = {
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
-        nav_steer       : (int16_t)nav_steer_cd,
+        lateral_accel   : lateral_acceleration,
         sonar1_distance : (uint16_t)sonar.distance_cm(),
         sonar2_distance : (uint16_t)sonar2.distance_cm(),
         detected_count  : obstacle.detected_count,
@@ -466,9 +464,9 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_CTUN_MSG, sizeof(log_Control_Tuning),     
       "CTUN", "hcchf",      "Steer,Roll,Pitch,ThrOut,AccY" },
     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
-      "NTUN", "HfHHhb",     "Yaw,WpDist,TargBrg,NavBrg,NavGain,Thr" },
+      "NTUN", "HfHHb",     "Yaw,WpDist,TargBrg,NavBrg,Thr" },
     { LOG_SONAR_MSG, sizeof(log_Sonar),             
-      "SONR", "hHHHbHCb",   "NavStr,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
+      "SONR", "fHHHbHCb",   "LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
     { LOG_CURRENT_MSG, sizeof(log_Current),             
       "CURR", "hhhHf",      "Thr,Volt,Curr,Vcc,CurrTot" },
     { LOG_MODE_MSG, sizeof(log_Mode),             

APMrover2/Parameters.h

@@ -74,6 +74,8 @@ public:
         k_param_ch7_option,
         k_param_auto_trigger_pin,
         k_param_auto_kickstart,
+        k_param_turn_circle,
+        k_param_turn_max_g,
 
         //
         // 160: Radio settings
@@ -156,6 +158,7 @@ public:
         k_param_ins,
         k_param_compass,
         k_param_rcmap,
+        k_param_L1_controller,
 
         // 254,255: reserved
         };
@@ -194,14 +197,14 @@ public:
 
     // navigation parameters
     //
-    AP_Float    crosstrack_gain;
-    AP_Int16    crosstrack_entry_angle;
     AP_Float    speed_cruise;
     AP_Int8     speed_turn_gain;
     AP_Float    speed_turn_dist;    
     AP_Int8	    ch7_option;
     AP_Int8     auto_trigger_pin;
     AP_Float    auto_kickstart;
+    AP_Float    turn_circle;
+    AP_Float    turn_max_g;
 
     // RC channels
     RC_Channel      rc_1;
@@ -265,7 +268,6 @@ public:
 
     // PID controllers
     //
-    PID         pidNavSteer;
     PID         pidServoSteer;
     PID         pidSpeedThrottle;
 
@@ -292,7 +294,6 @@ public:
 
         // PID controller    initial P        initial I        initial D        initial imax
         //-----------------------------------------------------------------------------------
-        pidNavSteer         (0.7,             0.1,             0.2,             2000),
         pidServoSteer       (0.5,             0.1,             0.2,             2000),
         pidSpeedThrottle    (0.7,             0.2,             0.2,             4000)
         {}

APMrover2/Parameters.pde

@@ -123,23 +123,6 @@ const AP_Param::Info var_info[] PROGMEM = {
 	// @User: Standard
 	GSCALAR(pack_capacity,          "BATT_CAPACITY",    HIGH_DISCHARGE),
 
-    // @Param: XTRK_GAIN_SC
-    // @DisplayName: Crosstrack Gain
-    // @Description: This controls how hard the Rover tries to follow the lines between waypoints, as opposed to driving directly to the next waypoint. The value is the scale between distance off the line and angle to meet the line (in Degrees * 100)
-    // @Range: 0 2000
-    // @Increment: 1
-    // @User: Standard
-	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: AUTO_TRIGGER_PIN
 	// @DisplayName: Auto mode trigger pin
 	// @Description: pin number to use to enable the throttle in auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will enable the motor to run. If the switch is released while in AUTO then the motor will stop again. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver.
@@ -431,7 +414,24 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
 	GSCALAR(waypoint_radius,        "WP_RADIUS",        2.0f),
 
-	GGROUP(pidNavSteer,             "HDNG2STEER_",  PID),
+    // @Param: TURN_CIRCLE
+    // @DisplayName: Turning circle
+    // @Description: The diameter of the turning circle in meters of the rover at low speed when wheels are turned to the maximum turn angle
+    // @Units: meters
+    // @Range: 0.5 50
+    // @Increment: 0.1
+    // @User: Standard
+	GSCALAR(turn_circle,            "TURN_CIRCLE",      1.5f),
+
+    // @Param: TURN_MAX_G
+    // @DisplayName: Turning maximum G force
+    // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
+    // @Units: gravities
+    // @Range: 0.2 10
+    // @Increment: 0.1
+    // @User: Standard
+	GSCALAR(turn_max_g,             "TURN_MAX_G",      2.0f),
+
 	GGROUP(pidServoSteer,           "STEER2SRV_",   PID),
 	GGROUP(pidSpeedThrottle,        "SPEED2THR_", PID),
 
@@ -456,6 +456,10 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GOBJECT(gcs0,					"SR0_",     GCS_MAVLINK),
 	GOBJECT(gcs3,					"SR3_",     GCS_MAVLINK),
 
+    // @Group: NAVL1_
+    // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
+    GOBJECT(L1_controller,         "NAVL1_",   AP_L1_Control),
+
     // @Group: SONAR_
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
     GOBJECT(sonar,                  "SONAR_", AP_RangeFinder_analog),

APMrover2/Steering.pde

@@ -87,7 +87,7 @@ static void calc_throttle(float target_speed)
       reduce target speed in proportion to turning rate, up to the
       SPEED_TURN_GAIN percentage.
     */
-    float steer_rate = fabsf((nav_steer_cd/nav_gain_scaler) / (float)SERVO_MAX);
+    float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
     steer_rate = constrain_float(steer_rate, 0.0, 1.0);
     float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
     
@@ -117,27 +117,55 @@ static void calc_throttle(float target_speed)
  * Calculate desired turn angles (in medium freq loop)
  *****************************************/
 
-static void calc_nav_steer()
+static void calc_lateral_acceleration()
 {
-	// Adjust gain based on ground speed
-    if (ground_speed < 0.01) {
-        nav_gain_scaler = 1.4f;
-    } else {
-        nav_gain_scaler = g.speed_cruise / ground_speed;
+    switch (control_mode) {
+    case AUTO:
+        nav_controller->update_waypoint(prev_WP, next_WP);
+        break;
+
+    case RTL:
+    case GUIDED:
+    case STEERING:
+        nav_controller->update_waypoint(current_loc, next_WP);
+        break;
+    default:
+        return;
     }
-	nav_gain_scaler = constrain_float(nav_gain_scaler, 0.2f, 1.4f);
 
-	// Calculate the required turn of the wheels rover
-	// ----------------------------------------
+	// Calculate the required turn of the wheels
 
     // negative error = left turn
 	// positive error = right turn
-	nav_steer_cd = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler);
+    lateral_acceleration = nav_controller->lateral_acceleration();
+}
+
+/*
+  calculate steering angle given lateral_acceleration
+ */
+static void calc_nav_steer()
+{
+    float speed = g_gps->ground_speed_cm * 0.01f;
+    float steer;
+
+    if (speed < 1.0f) {
+        // gps speed isn't very accurate at low speed
+        speed = 1.0f;
+    }
+
+    // add in obstacle avoidance
+    lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+
+    // constrain to max G force
+    lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
 
-    // avoid obstacles, if any
-    nav_steer_cd += obstacle.turn_angle*100;
+    // this is a linear approximation of the inverse steering
+    // equation for a rover. It returns steering as a proportion
+    // from -1 to 1
+    steer = 0.5 * lateral_acceleration * g.turn_circle / (speed*speed);
+    steer = constrain_float(steer, -1, 1);
 
-    channel_steer->servo_out = nav_steer_cd;
+    channel_steer->servo_out = steer * 4500;
 }
 
 /*****************************************

APMrover2/commands.pde

@@ -123,12 +123,6 @@ static void set_next_WP(const struct Location *wp)
 	// this is handy for the groundstation
 	wp_totalDistance 	= get_distance(current_loc, next_WP);
 	wp_distance 		= wp_totalDistance;
-	target_bearing 		= get_bearing_cd(current_loc, next_WP);
-	nav_bearing 		= target_bearing;
-
-	// set a new crosstrack bearing
-	// ----------------------------
-	reset_crosstrack();
 }
 
 static void set_guided_WP(void)
@@ -144,11 +138,6 @@ 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 		= get_bearing_cd(current_loc, next_WP);
-
-	// set a new crosstrack bearing
-	// ----------------------------
-	reset_crosstrack();
 }
 
 // run this at setup on the ground
@@ -185,7 +174,6 @@ void init_home()
 
 static void restart_nav()
 {  
-    g.pidNavSteer.reset_I();
     g.pidSpeedThrottle.reset_I();
     prev_WP = current_loc;
     nav_command_ID = NO_COMMAND;

APMrover2/commands_logic.pde

@@ -6,10 +6,6 @@
 static void
 handle_process_nav_cmd()
 {
-	// reset navigation integrators
-	// -------------------------
-    g.pidNavSteer.reset_I();
-
     gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
 
 	switch(next_nav_command.id){
@@ -211,8 +207,6 @@ static bool verify_takeoff()
 
 static bool verify_nav_wp()
 {
-    update_crosstrack();
-
     if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
         gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
                           (unsigned)nav_command_index,

APMrover2/control_modes.pde

@@ -24,9 +24,7 @@ static void read_control_switch()
 		oldSwitchPosition = switchPosition;
 		prev_WP = current_loc;
 
-		// reset navigation and speed integrators
-		// -------------------------
-        g.pidNavSteer.reset_I();
+		// reset speed integrator
         g.pidSpeedThrottle.reset_I();
 	}
 

APMrover2/navigation.pde

@@ -24,46 +24,12 @@ static void navigate()
 		return;
 	}
 
-	// target_bearing is where we should be heading
-	// --------------------------------------------
-	target_bearing 	= get_bearing_cd(current_loc, next_WP);
-
-	// nav_bearing will includes xtrac correction
-	// ------------------------------------------
-	nav_bearing = target_bearing;
-
 	// control mode specific updates to nav_bearing
 	// --------------------------------------------
 	update_navigation();
 }
 
 
-static void calc_bearing_error()
-{    
-    static butter10hz1_6 butter;
-
-	bearing_error_cd = wrap_180_cd(nav_bearing - ahrs.yaw_sensor);
-    bearing_error_cd = butter.filter(bearing_error_cd);
-}
-
-static void update_crosstrack(void)
-{
-	// Crosstrack Error
-	// ----------------
-
-    // If we are too far off or too close we don't do track following
-	if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500 && wp_distance >= 3.0f) {	 
-		crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance;	 // Meters we are off track line
-		nav_bearing += constrain_float(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
-		nav_bearing = wrap_360_cd(nav_bearing);
-	}
-}
-
-static void reset_crosstrack()
-{
-	crosstrack_bearing 	= get_bearing_cd(prev_WP, next_WP);	// Used for track following
-}
-
 void reached_waypoint()
 {       
 

APMrover2/setup.pde

@@ -109,7 +109,6 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	report_radio();
 	report_batt_monitor();
 	report_gains();
-	report_xtrack();
 	report_throttle();
 	report_modes();
 	report_compass();
@@ -510,28 +509,12 @@ static void report_gains()
 	cliSerial->printf_P(PSTR("servo steer:\n"));
 	print_PID(&g.pidServoSteer);
 
-	cliSerial->printf_P(PSTR("nav steer:\n"));
-	print_PID(&g.pidNavSteer);
-
 	cliSerial->printf_P(PSTR("speed throttle:\n"));
 	print_PID(&g.pidSpeedThrottle);
 
 	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);