airspeed calibration: Update and resolve compile errors

1 files changed, 33 insertions, 42 deletions

diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index a1a20abc9..421e53ae0 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -51,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <systemlib/airspeed.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -103,58 +104,48 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
 	}
 
-	for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) {
+	unsigned calibration_counter = 0;
 
-		unsigned calibration_counter = 0;
-
-		if (i == 0) {
-			mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
-			usleep(500 * 1000);
-		}
-
-		float diff_pres_offset = 0.0f;
-
-		while (calibration_counter < calibration_count) {
-
-			/* wait blocking for new data */
-			struct pollfd fds[1];
-			fds[0].fd = diff_pres_sub;
-			fds[0].events = POLLIN;
-
-			int poll_ret = poll(fds, 1, 1000);
-
-			if (poll_ret) {
-				orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+	usleep(500 * 1000);
 
-				if (i > 0) {
+	while (calibration_counter < calibration_count) {
 
-					if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) {
-						mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot");
-						usleep(5000 * 1000);
-						continue;
-					}
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
 
-					/* do not log negative values in the second go */
-					if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) {
-						mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart");
-						close(diff_pres_sub);
-						return ERROR;
-					}
-				}
+		int poll_ret = poll(fds, 1, 1000);
 
-				diff_pres_offset += diff_pres.differential_pressure_raw_pa;
-				calibration_counter++;
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
 
-				if (calibration_counter % (calibration_count / 20) == 0) {
-					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2);
-				}
+			if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) {
+				mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot");
+				usleep(5000 * 1000);
+				continue;
+			}
 
-			} else if (poll_ret == 0) {
-				/* any poll failure for 1s is a reason to abort */
-				mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			/* do not log negative values in the second go */
+			if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) {
+				mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart");
 				close(diff_pres_sub);
 				return ERROR;
 			}
+
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
+			calibration_counter++;
+
+			if (calibration_counter % (calibration_count / 20) == 0) {
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2);
+			}
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			close(diff_pres_sub);
+			return ERROR;
 		}
 	}