mirror of https://github.com/ArduPilot/ardupilot
AntennaTracker: don't stall EKF during baro cal
This commit is contained in:
parent
f61ba903dd
commit
8c3bbdaf73
|
@ -592,7 +592,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
}
|
||||
}
|
||||
if (is_equal(packet.param3,1.0f)) {
|
||||
tracker.init_barometer();
|
||||
tracker.init_barometer(false);
|
||||
// zero the altitude difference on next baro update
|
||||
tracker.nav_status.need_altitude_calibration = true;
|
||||
}
|
||||
|
|
|
@ -214,7 +214,7 @@ private:
|
|||
void update_scan(void);
|
||||
bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm);
|
||||
void read_radio();
|
||||
void init_barometer(void);
|
||||
void init_barometer(bool full_calibration);
|
||||
void update_barometer(void);
|
||||
void update_ahrs();
|
||||
void update_compass(void);
|
||||
|
|
|
@ -2,10 +2,14 @@
|
|||
|
||||
#include "Tracker.h"
|
||||
|
||||
void Tracker::init_barometer(void)
|
||||
void Tracker::init_barometer(bool full_calibration)
|
||||
{
|
||||
gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
|
||||
barometer.calibrate();
|
||||
if (full_calibration) {
|
||||
barometer.calibrate();
|
||||
} else {
|
||||
barometer.update_calibration();
|
||||
}
|
||||
gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
|
||||
}
|
||||
|
||||
|
|
|
@ -77,7 +77,7 @@ void Tracker::init_tracker()
|
|||
ins.init(scheduler.get_loop_rate_hz());
|
||||
ahrs.reset();
|
||||
|
||||
init_barometer();
|
||||
init_barometer(true);
|
||||
|
||||
// set serial ports non-blocking
|
||||
serial_manager.set_blocking_writes_all(false);
|
||||
|
|
Loading…
Reference in New Issue