Browse Source

AntennaTracker: don't stall EKF during baro cal

master
Andrew Tridgell 9 years ago
parent
commit
8c3bbdaf73
  1. 2
      AntennaTracker/GCS_Mavlink.cpp
  2. 2
      AntennaTracker/Tracker.h
  3. 8
      AntennaTracker/sensors.cpp
  4. 2
      AntennaTracker/system.cpp

2
AntennaTracker/GCS_Mavlink.cpp

@ -592,7 +592,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -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;
}

2
AntennaTracker/Tracker.h

@ -214,7 +214,7 @@ private: @@ -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);

8
AntennaTracker/sensors.cpp

@ -2,10 +2,14 @@ @@ -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");
}

2
AntennaTracker/system.cpp

@ -77,7 +77,7 @@ void Tracker::init_tracker() @@ -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…
Cancel
Save