Closing all opened file descriptors, fixed param save issue, tests clean

8 files changed, 57 insertions, 48 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index ed6707f04..cfa7d9e8a 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			break;
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
-		if (orient < 0)
+		if (orient < 0) {
+			close(sensor_combined_sub);
 			return ERROR;
+		}
 
 		if (data_collected[orient]) {
 			mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index e414e5f70..248eb4a66 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			close(diff_pres_sub);
 			return ERROR;
 		}
 	}
@@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			close(diff_pres_sub);
 			return ERROR;
 		}
 
@@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
 			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			close(diff_pres_sub);
 			return ERROR;
 		}
 
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "airspeed calibration done");
-
+		close(diff_pres_sub);
 		return OK;
 
 	} else {
 		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		close(diff_pres_sub);
 		return ERROR;
 	}
 }
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 33566d4e5..82a0349c9 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
 
-	const int calibration_count = 5000;
+	const unsigned calibration_count = 5000;
 
 	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s raw;
 
-	int calibration_counter = 0;
+	unsigned calibration_counter = 0;
 	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
 
 	/* set offsets to zero */
@@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd)
 			gyro_offset[1] += raw.gyro_rad_s[1];
 			gyro_offset[2] += raw.gyro_rad_s[2];
 			calibration_counter++;
+			if (calibration_counter % (calibration_count / 20) == 0)
+				mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
 
 		} else {
 			poll_errcount++;
@@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd)
 
 		if (poll_errcount > 1000) {
 			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
+			close(sub_sensor_combined);
 			return ERROR;
 		}
 	}
@@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warnx("WARNING: auto-save of params to storage failed");
 			mavlink_log_critical(mavlink_fd, "gyro store failed");
+			close(sub_sensor_combined);
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
 		tune_neutral();
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+		close(sub_sensor_combined);
 		return ERROR;
 	}
 
 
 	/*** --- SCALING --- ***/
 
-	mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
-	mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
+	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
 	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
 
 	unsigned rotations_count = 30;
@@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd)
 		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
 		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
 			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
-			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
-			mavlink_log_info(mavlink_fd, "gyro calibration done");
+			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
+			close(sub_sensor_combined);
 			return OK;
 		}
 
@@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd)
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
 		/* third beep by cal end routine */
+		close(sub_sensor_combined);
 		return OK;
 	} else {
 		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		close(sub_sensor_combined);
 		return ERROR;
 	}
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index e38616027..b0d4266be 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd)
 			axis_index++;
 
 			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
 			tune_neutral();
 
 			axis_deadline += calibration_interval / 3;
@@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd)
 			break;
 		}
 
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
 		int poll_ret = poll(fds, 1, 1000);
 
 		if (poll_ret > 0) {
@@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd)
 			y[calibration_counter] = mag.y;
 			z[calibration_counter] = mag.z;
 
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
 			calibration_counter++;
+			if (calibration_counter % (calibration_maxcount / 20) == 0)
+				mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
+
 
 		} else {
 			poll_errcount++;
@@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (poll_errcount > 1000) {
 			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
+			close(sub_mag);
+			free(x);
+			free(y);
+			free(z);
 			return ERROR;
 		}
 
@@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd)
 	float sphere_z;
 	float sphere_radius;
 
+	mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
 	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+	mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
 
 	free(x);
 	free(y);
@@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
 			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			close(sub_mag);
 			return ERROR;
 		}
 
@@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd)
 		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
 		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
 
+		close(sub_mag);
 		return OK;
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+		close(sub_mag);
 		return ERROR;
 	}
 }
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index fe87a3323..9fc1d6470 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -40,6 +40,7 @@
 #include "commander_helper.h"
 
 #include <poll.h>
+#include <unistd.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <mavlink/mavlink_log.h>
@@ -80,9 +81,11 @@ int do_rc_calibration(int mavlink_fd)
 
 	if (save_ret != 0) {
 		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		close(sub_man);
 		return ERROR;
 	}
 
 	mavlink_log_info(mavlink_fd, "trim calibration done");
+	close(sub_man);
 	return OK;
 }
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 69a9bdf9b..24b52d1a9 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -505,8 +505,15 @@ param_get_default_file(void)
 int
 param_save_default(void)
 {
+	int result;
+
 	/* delete the file in case it exists */
-	unlink(param_get_default_file());
+	struct stat buffer;
+	if (stat(param_get_default_file(), &buffer) == 0) {
+		result = unlink(param_get_default_file());
+		if (result != OK)
+			warnx("unlinking file %s failed.", param_get_default_file());
+	}
 
 	/* create the file */
 	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
@@ -516,7 +523,7 @@ param_save_default(void)
 		return -1;
 	}
 
-	int result = param_export(fd, false);
+	result = param_export(fd, false);
 	close(fd);
 
 	if (result != 0) {
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index f3a2aee9e..60d6473b8 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -66,7 +66,7 @@ int rc_calibration_check(void) {
 		// 	count++;
 		// }
 
-
+	int channel_fail_count = 0;
 
 	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
 		/* should the channel be enabled? */
@@ -142,7 +142,12 @@ int rc_calibration_check(void) {
 		/* sanity checks pass, enable channel */
 		if (count) {
 			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 		}
+
+		channel_fail_count += count;
 	}
+
+	return channel_fail_count;
 }
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index a323261cc..f5bd01ce8 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -142,6 +142,10 @@ int preflight_check_main(int argc, char *argv[])
 
 	bool rc_ok = (OK == rc_calibration_check());
 
+	/* warn */
+	if (!rc_ok)
+		warnx("rc calibration test failed");
+
 	/* require RC ok to keep system_ok */
 	system_ok &= rc_ok;
 
@@ -156,6 +160,9 @@ system_eval:
 	} else {
 		fflush(stdout);
 
+		warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
+		fflush(stderr);
+
 		int buzzer = open("/dev/tone_alarm", O_WRONLY);
 		int leds = open(LED_DEVICE_PATH, 0);