APM: allow for separate calibration of airspeed/barometer

this is useful just before takeoff, to account for temperature changes
This commit is contained in:
Andrew Tridgell 2012-08-15 19:49:09 +10:00
parent ad73c229ab
commit c960db7af5
3 changed files with 7 additions and 3 deletions

View File

@ -1038,9 +1038,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (packet.param1 == 1 ||
packet.param2 == 1 ||
packet.param3 == 1) {
packet.param2 == 1) {
startup_IMU_ground(true);
} else if (packet.param3 == 1) {
init_barometer();
if (airspeed.enabled()) {
zero_airspeed();
}
}
if (packet.param4 == 1) {
trim_radio();

View File

@ -34,6 +34,7 @@ static void read_airspeed(void)
static void zero_airspeed(void)
{
airspeed.calibrate(mavlink_delay);
gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated"));
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE

View File

@ -450,7 +450,6 @@ static void startup_IMU_ground(bool force_accel_level)
// initialize airspeed sensor
// --------------------------
zero_airspeed();
gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated"));
} else {
gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed"));
}