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

Auto-save parameters on change

Browse Source
This commit is contained in:
Lorenz Meier
2015-03-28 13:07:01 -07:00
parent 78741c87e5
commit f8fd67d3e1
1 changed files with 38 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+38 -10
View File
@@ -161,9 +161,10 @@ static int autostart_id;
/* flags */ /* flags */
static bool commander_initialized = false; static bool commander_initialized = false;
static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */ static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */ static int daemon_task; /**< Handle of daemon task / thread */
static bool _param_autosave = false;
static unsigned int leds_counter; static unsigned int leds_counter;
/* To remember when last notification was sent */ /* To remember when last notification was sent */
@@ -1070,8 +1071,6 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to parameters changed topic */ /* Subscribe to parameters changed topic */
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
struct parameter_update_s param_changed;
memset(&param_changed, 0, sizeof(param_changed));
/* Subscribe to battery topic */ /* Subscribe to battery topic */
int battery_sub = orb_subscribe(ORB_ID(battery_status)); int battery_sub = orb_subscribe(ORB_ID(battery_status));
@@ -1158,9 +1157,16 @@ int commander_thread_main(int argc, char *argv[])
/* update parameters */ /* update parameters */
orb_check(param_changed_sub, &updated); orb_check(param_changed_sub, &updated);
if (updated) {
/* trigger an autosave */
_param_autosave = true;
}
if (updated || param_init_forced) { if (updated || param_init_forced) {
param_init_forced = false; param_init_forced = false;
/* parameters changed */ /* parameters changed */
struct parameter_update_s param_changed;
orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed); orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
/* update parameters */ /* update parameters */
@@ -2532,6 +2538,9 @@ void *commander_low_prio_loop(void *arg)
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd)); memset(&cmd, 0, sizeof(cmd));
/* timeout for param autosave */
hrt_abstime _param_autosave_timeout = 0;
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[1]; struct pollfd fds[1];
@@ -2673,11 +2682,11 @@ void *commander_low_prio_loop(void *arg)
int ret = param_load_default(); int ret = param_load_default();
if (ret == OK) { if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); mavlink_log_info(mavlink_fd, "settings loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else { } else {
mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); mavlink_log_critical(mavlink_fd, "settings load ERROR");
/* convenience as many parts of NuttX use negative errno */ /* convenience as many parts of NuttX use negative errno */
if (ret < 0) { if (ret < 0) {
@@ -2685,7 +2694,7 @@ void *commander_low_prio_loop(void *arg)
} }
if (ret < 1000) { if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
} }
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
@@ -2695,11 +2704,11 @@ void *commander_low_prio_loop(void *arg)
int ret = param_save_default(); int ret = param_save_default();
if (ret == OK) { if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); mavlink_log_info(mavlink_fd, "settings saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else { } else {
mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); mavlink_log_critical(mavlink_fd, "settings save error");
/* convenience as many parts of NuttX use negative errno */ /* convenience as many parts of NuttX use negative errno */
if (ret < 0) { if (ret < 0) {
@@ -2707,7 +2716,7 @@ void *commander_low_prio_loop(void *arg)
} }
if (ret < 1000) { if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
} }
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
@@ -2726,6 +2735,25 @@ void *commander_low_prio_loop(void *arg)
break; break;
} }
/* trigger a param autosave if required */
if (_param_autosave) {
if (_param_autosave_timeout > 0 && hrt_elapsed_time(&_param_autosave_timeout) > 200000ULL) {
int ret = param_save_default();
if (ret == OK) {
mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
} else {
mavlink_and_console_log_critical(mavlink_fd, "settings save error");
}
_param_autosave = false;
_param_autosave_timeout = 0;
} else {
_param_autosave_timeout = hrt_absolute_time();
}
}
/* send any requested ACKs */ /* send any requested ACKs */
if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {