diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 1e373a2233..843d76262b 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -369,6 +369,16 @@ static void load_parameters(void) ahrs.gps_gain.set_and_save(0.0); } + // setup different AHRS gains for ArduCopter than the default + // but allow users to override in their config + if (!ahrs._kp.load()) { + ahrs._kp.set_and_save(0.1); + } + if (!ahrs._kp_yaw.load()) { + ahrs._kp_yaw.set_and_save(0.1); + } + + if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 690d860a63..f9a31b5a19 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -148,9 +148,6 @@ static void init_arm_motors() startup_ground(); } - ahrs._kp.set(0.1); - ahrs._kp_yaw.set(0.1); - #if HIL_MODE != HIL_MODE_ATTITUDE // read Baro pressure at ground - // this resets Baro for more accuracy