diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-05 13:24:21 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-05 13:24:21 +0200 |
commit | aa785b0d2b339a8fcc730e11b63264b1ff8d146d (patch) | |
tree | 31a90c976af06b5ced0e7f0f463a1de39cede658 | |
parent | 09db74da0a88908aa761940a3de8383c0c64d1e4 (diff) | |
download | px4-firmware-aa785b0d2b339a8fcc730e11b63264b1ff8d146d.tar.gz px4-firmware-aa785b0d2b339a8fcc730e11b63264b1ff8d146d.tar.bz2 px4-firmware-aa785b0d2b339a8fcc730e11b63264b1ff8d146d.zip |
Hotfix: Better error reporting, fixed sched param setup
-rw-r--r-- | src/modules/commander/commander.cpp | 25 | ||||
-rw-r--r-- | src/modules/commander/rc_calibration.cpp | 14 | ||||
-rw-r--r-- | 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 8e8226f09..333fe30ae 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, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); @@ -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 9fc1d6470..90ede499a 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 24b52d1a9..c69de52b7 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; |