Copter: allow ch6 tuning of EKF

ArduCopter/ArduCopter.pde

@@ -1531,9 +1531,19 @@ static void tuning(){
         g.sonar_gain.set(tuning_value);
         break;
 
-    case CH6_LOIT_SPEED:
-        // set max loiter speed to 0 ~ 1000 cm/s
-        wp_nav.set_loiter_velocity(g.rc_6.control_in);
+    case CH6_EKF_VERTICAL_POS:
+        // EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
+        ahrs.get_EKF()->_gpsVertPosNoise = tuning_value;
+        break;
+
+    case CH6_EKF_HORIZONTAL_POS:
+        // EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
+        ahrs.get_EKF()->_gpsHorizPosNoise = tuning_value;
+        break;
+
+    case CH6_EKF_ACCEL_NOISE:
+        // EKF's accel noise (lower means trust accels more, gps & baro less)
+        ahrs.get_EKF()->_accNoise = tuning_value;
         break;
     }
 }

ArduCopter/defines.h

@@ -145,7 +145,9 @@
 #define CH6_DECLINATION                 38  // compass declination in radians
 #define CH6_CIRCLE_RATE                 39  // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
 #define CH6_SONAR_GAIN                  41  // sonar gain
-#define CH6_LOIT_SPEED                  42  // maximum speed during loiter (0 to 10m/s)
+#define CH6_EKF_VERTICAL_POS            42  // EKF's baro vs accel (higher rely on accels more, baro impact is reduced).  Range should be 0.2 ~ 4.0?  2.0 is default
+#define CH6_EKF_HORIZONTAL_POS          43  // EKF's gps vs accel (higher rely on accels more, gps impact is reduced).  Range should be 1.0 ~ 3.0?  1.5 is default
+#define CH6_EKF_ACCEL_NOISE             44  // EKF's accel noise (lower means trust accels more, gps & baro less).  Range should be 0.02 ~ 0.5  0.5 is default (but very robust at that level)
 
 // Acro Trainer types
 #define ACRO_TRAINER_DISABLED   0