From aa785b0d2b339a8fcc730e11b63264b1ff8d146d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 5 Sep 2013 13:24:21 +0200
Subject: [PATCH] Hotfix: Better error reporting, fixed sched param setup

---
 src/modules/commander/commander.cpp      | 25 ++++++++++++++++++++----
 src/modules/commander/rc_calibration.cpp | 14 +++++++------
 src/modules/systemlib/param/param.c      |  4 ++--
 3 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8e8226f091..333fe30ae2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -50,6 +50,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <systemlib/err.h>
 #include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
@@ -599,6 +600,7 @@ int commander_thread_main(int argc, char *argv[])
 	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
 
 	struct sched_param param;
+	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
@@ -1655,13 +1657,13 @@ void *commander_low_prio_loop(void *arg)
 
 				if (((int)(cmd.param1)) == 1) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					usleep(1000000);
+					usleep(100000);
 					/* reboot */
 					systemreset(false);
 
 				} else if (((int)(cmd.param1)) == 3) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					usleep(1000000);
+					usleep(100000);
 					/* reboot to bootloader */
 					systemreset(true);
 
@@ -1704,6 +1706,7 @@ void *commander_low_prio_loop(void *arg)
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					calib_ret = do_rc_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
@@ -1729,22 +1732,36 @@ void *commander_low_prio_loop(void *arg)
 		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
 				if (((int)(cmd.param1)) == 0) {
-					if (0 == param_load_default()) {
+					int ret = param_load_default();
+					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						/* convenience as many parts of NuttX use negative errno */
+						if (ret < 0)
+							ret = -ret;
+
+						if (ret < 1000)
+							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 
 				} else if (((int)(cmd.param1)) == 1) {
-					if (0 == param_save_default()) {
+					int ret = param_save_default();
+					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						/* convenience as many parts of NuttX use negative errno */
+						if (ret < 0)
+							ret = -ret;
+
+						if (ret < 1000)
+							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 				}
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 9fc1d64700..90ede499a1 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -57,14 +57,16 @@ int do_rc_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "trim calibration starting");
 
-	/* XXX fix this */
-	// if (current_status.rc_signal) {
-	// 	mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-	// 	return;
-	// }
-
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp;
+	bool changed;
+	orb_check(sub_man, &changed);
+
+	if (!changed) {
+		mavlink_log_critical(mavlink_fd, "no manual control, aborting");
+		return ERROR;
+	}
+
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
 	/* set parameters */
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 24b52d1a92..c69de52b70 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -520,7 +520,7 @@ param_save_default(void)
 
 	if (fd < 0) {
 		warn("opening '%s' for writing failed", param_get_default_file());
-		return -1;
+		return fd;
 	}
 
 	result = param_export(fd, false);
@@ -529,7 +529,7 @@ param_save_default(void)
 	if (result != 0) {
 		warn("error exporting parameters to '%s'", param_get_default_file());
 		unlink(param_get_default_file());
-		return -2;
+		return result;
 	}
 
 	return 0;