diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index dbf3868c6c..624dfbbd43 100644 --- a/ArduCopter/ArduCopter.pde +++ b/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; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 97a7e42230..955872aff8 100644 --- a/ArduCopter/defines.h +++ b/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