Browse Source

Copter: faster baro calibration when arming

using update_calibration() instead of the full calibrate() cuts
1.5seconds of the arming time
master
Randy Mackay 11 years ago
parent
commit
8a29d63d89
  1. 2
      ArduCopter/GCS_Mavlink.pde
  2. 6
      ArduCopter/motors.pde
  3. 8
      ArduCopter/sensors.pde
  4. 2
      ArduCopter/system.pde
  5. 2
      ArduCopter/test.pde

2
ArduCopter/GCS_Mavlink.pde

@ -1177,7 +1177,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1177,7 +1177,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
if (packet.param3 == 1) {
#if HIL_MODE != HIL_MODE_ATTITUDE
init_barometer();
init_barometer(false); // fast barometer calibratoin
#endif
}
if (packet.param4 == 1) {

6
ArduCopter/motors.pde

@ -169,10 +169,8 @@ static void init_arm_motors() @@ -169,10 +169,8 @@ static void init_arm_motors()
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
// fast baro calibration to reset ground pressure
init_barometer(false);
#endif
// go back to normal AHRS gains

8
ArduCopter/sensors.pde

@ -14,10 +14,14 @@ static void init_sonar(void) @@ -14,10 +14,14 @@ static void init_sonar(void)
}
#endif
static void init_barometer(void)
static void init_barometer(bool full_calibration)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
barometer.calibrate();
if (full_calibration) {
barometer.calibrate();
}else{
barometer.update_calibration();
}
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
}

2
ArduCopter/system.pde

@ -263,7 +263,7 @@ static void init_ardupilot() @@ -263,7 +263,7 @@ static void init_ardupilot()
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
init_barometer(true);
#endif
// initialise sonar

2
ArduCopter/test.pde

@ -64,7 +64,7 @@ test_baro(uint8_t argc, const Menu::arg *argv) @@ -64,7 +64,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
{
int32_t alt;
print_hit_enter();
init_barometer();
init_barometer(true);
while(1) {
delay(100);

Loading…
Cancel
Save