Copter: faster baro calibration when arming
using update_calibration() instead of the full calibrate() cuts 1.5seconds of the arming time
This commit is contained in:
parent
30a210cfa6
commit
8a29d63d89
@ -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) {
|
||||
|
@ -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
|
||||
|
@ -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"));
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
|
@ -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…
Reference in New Issue
Block a user