Rover: added STEERING_LEARN parameter

when enabled (set to 1) it will learn TURN_CIRCLE based on the
demanded and actual lateral acceleration

APMrover2/APMrover2.pde

@@ -541,6 +541,10 @@ 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
 ////////////////////////////////////////////////////////////////////////////////
@@ -864,6 +868,9 @@ 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,
-        0,
+        g.turn_circle.get(),
         groundspeed_error,
         nav_controller->crosstrack_error());
 }

APMrover2/Parameters.h

@@ -55,6 +55,7 @@ public:
         // 130: Sensor parameters
         //
         k_param_compass_enabled = 130,
+        k_param_steering_learn,
 
         // 140: battery controls
         k_param_battery_monitoring = 140,
@@ -205,6 +206,7 @@ public:
     AP_Float    auto_kickstart;
     AP_Float    turn_circle;
     AP_Float    turn_max_g;
+    AP_Int8     steering_learn;
 
     // RC channels
     RC_Channel      rc_1;

APMrover2/Parameters.pde

@@ -423,6 +423,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @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

APMrover2/Steering.pde

@@ -268,3 +268,40 @@ static void demo_servos(uint8_t i) {
         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 
+     */
+    float demanded = lateral_acceleration;
+    float actual = ins.get_accel().y;
+    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();
+    }
+}