fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Hotfix: Better error reporting, fixed sched param setup

Browse Source
This commit is contained in:
Lorenz Meier 2013-09-05 13:24:21 +02:00
parent 09db74da0a
commit aa785b0d2b
3 changed files with 31 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/commander/commander.cpp
View File

@ -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);
}
}

14
src/modules/commander/rc_calibration.cpp
View File

@ -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 */

4
src/modules/systemlib/param/param.c
View File

@ -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;