From 006dfbb14f658b23c0a760229bac05e861db48dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jul 2015 10:43:12 +0200 Subject: [PATCH] Commander: Speed up airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 837a3f1e83..32077d5f3b 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -72,12 +72,12 @@ int do_airspeed_calibration(int mavlink_fd) { int result = OK; unsigned calibration_counter = 0; - const unsigned maxcount = 3000; + const unsigned maxcount = 2400; /* give directions */ mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - const unsigned calibration_count = 2000; + const unsigned calibration_count = (maxcount * 2) / 3; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -201,10 +201,10 @@ int do_airspeed_calibration(int mavlink_fd) /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { - goto error_return; - } - + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub;