diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-12 22:50:56 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-12 22:50:56 +0200 |
commit | 34abf5c40cda538f204d313cc49715b5a938d168 (patch) | |
tree | bd37ca0822edc4512e0e950ca01efb73e4e12f5e /src/modules/commander | |
parent | 4824a4e8ac93d992e1401f768291cca65ba46acc (diff) | |
download | px4-firmware-34abf5c40cda538f204d313cc49715b5a938d168.tar.gz px4-firmware-34abf5c40cda538f204d313cc49715b5a938d168.tar.bz2 px4-firmware-34abf5c40cda538f204d313cc49715b5a938d168.zip |
airspeed cal: Fix up logic
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/airspeed_calibration.cpp | 25 |
1 files changed, 11 insertions, 14 deletions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01db34be2..47c9c7af5 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -165,11 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - calibration_counter = 0; - diff_pres_offset = 0.0f; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 50) { + while (calibration_counter < 10) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,19 +178,15 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - - if (fabsf(curr_avg) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); - usleep(5000 * 1000); + if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + usleep(3000 * 1000); continue; } - /* do not log negative values in the second go */ - if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); @@ -209,7 +202,11 @@ int do_airspeed_calibration(int mavlink_fd) /* save */ (void)param_save_default(); + close(diff_pres_sub); return ERROR; + } else { + close(diff_pres_sub); + return OK; } } else if (poll_ret == 0) { |