From ab257ebcced2af6ddb528a9d48355dc2cac7d10a Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Mon, 28 Apr 2014 00:52:19 -0400 Subject: Proper data manager restart handling Introduce SYS_RESTART_TYPE parameter having one of 3 values: boot restart, inflight restart, or unknown restart, and defaulting to unknown restart. px4io.cpp sets this parameter according to the type of restart detected. dataman.c retrieves this parameter and clears data entries according to their persistence level. Does nothing if unknown restart. --- src/drivers/px4io/px4io.cpp | 17 +++++++++++++++++ src/modules/dataman/dataman.c | 33 +++++++++++++++++++++++++-------- src/modules/dataman/dataman.h | 5 +++-- src/modules/systemlib/system_params.c | 27 +++++++++++++++++++-------- 4 files changed, 64 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e318e206a..ba13d0ff7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -91,6 +91,8 @@ #include "uploader.h" +#include "modules/dataman/dataman.h" + extern device::Device *PX4IO_i2c_interface() weak_function; extern device::Device *PX4IO_serial_interface() weak_function; @@ -568,9 +570,15 @@ int PX4IO::init() { int ret; + param_t sys_restart_param; + int sys_restart_val = DM_INIT_REASON_VOLATILE; ASSERT(_task == -1); + sys_restart_param = param_find("SYS_RESTART_TYPE"); + /* Indicate restart type is unknown */ + param_set(sys_restart_param, &sys_restart_val); + /* do regular cdev init */ ret = CDev::init(); @@ -720,6 +728,11 @@ PX4IO::init() /* keep waiting for state change for 2 s */ } while (!safety.armed); + /* Indicate restart type is in-flight */ + sys_restart_val = DM_INIT_REASON_IN_FLIGHT; + param_set(sys_restart_param, &sys_restart_val); + + /* regular boot, no in-air restart, init IO */ } else { @@ -745,6 +758,10 @@ PX4IO::init() } } + /* Indicate restart type is power on */ + sys_restart_val = DM_INIT_REASON_POWER_ON; + param_set(sys_restart_param, &sys_restart_val); + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index c132b0040..7505ba358 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -416,26 +416,26 @@ static int _restart(dm_reset_reason reason) { unsigned char buffer[2]; - int offset, result = 0; + int offset = 0, result = 0; /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ /* Loop through all of the data segments and delete those that are not persistent */ - offset = 0; - while (1) { size_t len; /* Get data segment at current offset */ if (lseek(g_task_fd, offset, SEEK_SET) != offset) { - result = -1; + /* must be at eof */ break; } len = read(g_task_fd, buffer, sizeof(buffer)); - if (len == 0) + if (len != sizeof(buffer)) { + /* must be at eof */ break; + } /* check if segment contains data */ if (buffer[0]) { @@ -443,12 +443,12 @@ _restart(dm_reset_reason reason) /* Whether data gets deleted depends on reset type and data segment's persistence setting */ if (reason == DM_INIT_REASON_POWER_ON) { - if (buffer[1] != DM_PERSIST_POWER_ON_RESET) { + if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { clear_entry = 1; } } else { - if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) { + if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { clear_entry = 1; } } @@ -628,6 +628,23 @@ task_main(int argc, char *argv[]) fsync(g_task_fd); + /* see if we need to erase any items based on restart type */ + int sys_restart_val; + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { + if (sys_restart_val == DM_INIT_REASON_POWER_ON) { + warnx("Power on restart"); + _restart(DM_INIT_REASON_POWER_ON); + } + else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + warnx("In flight restart"); + _restart(DM_INIT_REASON_IN_FLIGHT); + } + else + warnx("Unknown restart"); + } + else + warnx("Unknown restart"); + /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ /* worker thread is shutting down but still processing requests */ @@ -724,7 +741,7 @@ start(void) return -1; } - /* wait for the thread to actuall initialize */ + /* wait for the thread to actually initialize */ sem_wait(&g_init_sema); sem_destroy(&g_init_sema); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 33c9fcd15..4382baeb5 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -75,7 +75,8 @@ extern "C" { /* The reason for the last reset */ typedef enum { DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ - DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ } dm_reset_reason; /* Maximum size in bytes of a single item instance */ @@ -100,7 +101,7 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); - /* Retrieve from the data manager store */ + /* Erase all items of this type */ __EXPORT int dm_clear( dm_item_t item /* The item type to clear */ diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index ec2bc3a9a..702e435ac 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -62,12 +62,23 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); /** - * Set usage of IO board - * - * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. - * - * @min 0 - * @max 1 - * @group System - */ +* Set usage of IO board +* +* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. +* +* @min 0 +* @max 1 +* @group System +*/ PARAM_DEFINE_INT32(SYS_USE_IO, 1); + +/** +* Set restart type +* +* Set by px4io to indicate type of restart +* +* @min 0 +* @max 2 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); -- cgit v1.2.3 From b0b2f714f19a48d3823c2ddf4d82662150a58a7e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Apr 2014 15:23:56 +0200 Subject: add options do disable or fake gps output in rcS --- ROMFS/px4fmu_common/init.d/rcS | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea5cf8deb..c12d876cb 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -120,6 +120,8 @@ then set EXIT_ON_END no set MAV_TYPE none set LOAD_DEFAULT_APPS yes + set GPS yes + set GPS_FAKE no # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -437,9 +439,20 @@ then echo "[init] Start logging" sh /etc/init.d/rc.logging fi - - gps start + if [ $GPS == yes ] + then + echo "[init] Start GPS" + if [ $GPS_FAKE == yes ] + then + echo "[init] Faking GPS" + gps start -f + else + gps start + fi + fi + + # # Start up ARDrone Motor interface # -- cgit v1.2.3 From 6a2ecfa162fe49bf5c5cef0100035c3ca270dc2f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Apr 2014 15:57:14 +0200 Subject: remove whitespace --- ROMFS/px4fmu_common/init.d/rcS | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c12d876cb..492afcb4c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -452,7 +452,6 @@ then fi fi - # # Start up ARDrone Motor interface # -- cgit v1.2.3 From 0c58588a87697a371249b7eb5ca9cdac7169c6e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Apr 2014 19:51:05 +0200 Subject: mc_att_control: yaw feed-forward in manual control modes fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 6b0c44fb3..36d95bf06 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -496,7 +496,8 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt); + yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; } -- cgit v1.2.3