Copter: disable centrifugal correction when disarmed

This commit is contained in:
Randy Mackay 2013-05-06 13:32:11 +09:00
parent ceda7d26b0
commit 324200b52c
2 changed files with 4 additions and 4 deletions

View File

@ -966,8 +966,8 @@ static void load_parameters(void)
if (!ahrs.gps_gain.load()) {
ahrs.gps_gain.set_and_save(1.0);
}
// set gps_gain to zero, it will be set back to 1 as part of the arming process
ahrs.gps_gain.set(0);
// disable centrifugal force correction, it will be enabled as part of the arming process
ahrs.set_correct_centrifugal(false);
// setup different AHRS gains for ArduCopter than the default
// but allow users to override in their config

View File

@ -145,7 +145,7 @@ static void init_arm_motors()
#endif
// enable gps velocity based centrefugal force compensation
ahrs.gps_gain.load();
ahrs.set_correct_centrifugal(true);
// finally actually arm the motors
motors.armed(true);
@ -214,7 +214,7 @@ static void init_disarm_motors()
#endif
// disable gps velocity based centrefugal force compensation
ahrs.gps_gain.set(0);
ahrs.set_correct_centrifugal(false);
}
/*****************************************