Rover: switched to new steering controller

this uses a steering rate controller, based upon the planes roll
controller

APMrover2/APMrover2.pde

@@ -91,6 +91,7 @@
 #include <AP_Scheduler.h>       // main loop scheduler
 #include <stdarg.h>
 #include <AP_Navigation.h>
+#include <APM_Control.h>
 #include <AP_L1_Control.h>
 
 #include <AP_HAL_AVR.h>
@@ -249,6 +250,9 @@ static AP_L1_Control L1_controller(ahrs);
 // selected navigation controller
 static AP_Navigation *nav_controller = &L1_controller;
 
+// steering controller
+static AP_SteerController steerController(ahrs);
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
 #endif
@@ -541,10 +545,6 @@ static uint8_t 		delta_ms_fast_loop;
 // Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
-static struct {
-    float last_saved_value;
-} learning;
-
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
 ////////////////////////////////////////////////////////////////////////////////
@@ -868,9 +868,6 @@ static void update_current_mode(void)
         // and throttle gives speed in proportion to cruise speed
         throttle_nudge = 0;
         calc_throttle(channel_throttle->pwm_to_angle() * 0.01 * g.speed_cruise);
-        if (g.steering_learn) {
-            steering_learning();
-        }
         break;
     }
 

APMrover2/GCS_Mavlink.pde

@@ -244,7 +244,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
         nav_controller->nav_bearing_cd() * 0.01f,
         nav_controller->target_bearing_cd() * 0.01f,
         wp_distance,
-        g.turn_circle.get(),
+        0,
         groundspeed_error,
         nav_controller->crosstrack_error());
 }

APMrover2/Parameters.h

@@ -55,7 +55,7 @@ public:
         // 130: Sensor parameters
         //
         k_param_compass_enabled = 130,
-        k_param_steering_learn,
+        k_param_steering_learn, // unused
 
         // 140: battery controls
         k_param_battery_monitoring = 140,
@@ -75,7 +75,7 @@ public:
         k_param_ch7_option,
         k_param_auto_trigger_pin,
         k_param_auto_kickstart,
-        k_param_turn_circle,
+        k_param_turn_circle, // unused
         k_param_turn_max_g,
 
         //
@@ -144,7 +144,7 @@ public:
         //
         // 240: PID Controllers
         k_param_pidNavSteer = 230,
-        k_param_pidServoSteer,
+        k_param_pidServoSteer, // unused
         k_param_pidSpeedThrottle,
 
         // high RC channels
@@ -160,6 +160,7 @@ public:
         k_param_compass,
         k_param_rcmap,
         k_param_L1_controller,
+        k_param_steerController,
 
         // 254,255: reserved
         };
@@ -204,9 +205,7 @@ public:
     AP_Int8	    ch7_option;
     AP_Int8     auto_trigger_pin;
     AP_Float    auto_kickstart;
-    AP_Float    turn_circle;
     AP_Float    turn_max_g;
-    AP_Int8     steering_learn;
 
     // RC channels
     RC_Channel      rc_1;
@@ -270,7 +269,6 @@ public:
 
     // PID controllers
     //
-    PID         pidServoSteer;
     PID         pidSpeedThrottle;
 
     Parameters() :
@@ -296,7 +294,6 @@ public:
 
         // PID controller    initial P        initial I        initial D        initial imax
         //-----------------------------------------------------------------------------------
-        pidServoSteer       (0.5,             0.1,             0.2,             2000),
         pidSpeedThrottle    (0.7,             0.2,             0.2,             4000)
         {}
 };

APMrover2/Parameters.pde

@@ -414,22 +414,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
 	GSCALAR(waypoint_radius,        "WP_RADIUS",        2.0f),
 
-    // @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: STEERING_LEARN
-    // @DisplayName: Steering learn enable
-    // @Description: When this option is enabled in STEERING mode the APM will try to learn the right tuning parameters for steering mode automatically while you are driving
-    // @User: Standard
-    // @Values: 0:Disabled,1:Enabled
-	GSCALAR(steering_learn,         "STEERING_LEARN",   0),
-
     // @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
@@ -439,7 +423,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
 	GSCALAR(turn_max_g,             "TURN_MAX_G",      2.0f),
 
-	GGROUP(pidServoSteer,           "STEER2SRV_",   PID),
+    // @Group: STEER2SRV_
+    // @Path: ../libraries/APM_Control/AP_SteerController.cpp
+	GOBJECT(steerController,        "STEER2SRV_",   AP_SteerController),
+
 	GGROUP(pidSpeedThrottle,        "SPEED2THR_", PID),
 
 	// variables not in the g class which contain EEPROM saved variables

APMrover2/Steering.pde

@@ -146,7 +146,6 @@ static void calc_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
@@ -159,13 +158,7 @@ static void calc_nav_steer()
     // constrain to max G force
     lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
 
-    // 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 = steer * 4500;
+    channel_steer->servo_out = steerController.get_steering_out(lateral_acceleration);
 }
 
 /*****************************************
@@ -269,45 +262,3 @@ static void demo_servos(uint8_t i) {
     }
 }
 
-
-/*
-  learning of TURN_CIRCLE in STEERING mode
- */
-static void steering_learning(void)
-{
-    /*
-      only do learning when we are moving at least at 2m/s, and do not
-      have saturated steering
-     */
-    if (abs(channel_steer->servo_out) >= 4490 ||
-        abs(channel_steer->servo_out) < 100 ||
-        g_gps->status() < GPS::GPS_OK_FIX_3D ||
-        g_gps->ground_speed_cm < 100) {
-        return;
-    }
-    /*
-      the idea is to slowly adjust the turning circle to bring the
-      actual and desired turn rates into line      
-     */
-    float demanded = lateral_acceleration;
-    /*
-      for Y accel use the gyro times the velocity, as that is less
-      noise sensitive, and is a more direct measure of steering rate,
-      which is what we are really trying to control
-     */
-    float actual = ins.get_gyro().z * 0.01f * g_gps->ground_speed_cm;
-    if (fabsf(actual) < 0.1f) {
-        // too little acceleration to really measure accurately
-        return;
-    }
-    float ratio = demanded/actual;
-    if (ratio > 1.0f) {
-        g.turn_circle.set(g.turn_circle * 1.0005f);
-    } else {
-        g.turn_circle.set(g.turn_circle * 0.9995f);
-    }
-    if (fabs(learning.last_saved_value - g.turn_circle) > 0.05f) {
-        learning.last_saved_value = g.turn_circle;
-        g.turn_circle.save();
-    }
-}

APMrover2/setup.pde

@@ -506,9 +506,6 @@ static void report_gains()
 	cliSerial->printf_P(PSTR("Gains\n"));
 	print_divider();
 
-	cliSerial->printf_P(PSTR("servo steer:\n"));
-	print_PID(&g.pidServoSteer);
-
 	cliSerial->printf_P(PSTR("speed throttle:\n"));
 	print_PID(&g.pidSpeedThrottle);