From 56bf9855a8f8140a8a5edeeb08f4246249b27085 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Feb 2013 17:47:26 +0100 Subject: Finished and tested in-air restore of arming state, as long as both boards reset at the same time armings state is now retained --- apps/drivers/px4io/px4io.cpp | 27 +++++++++++++++++++-------- apps/px4io/protocol.h | 1 + apps/px4io/px4io.c | 16 ++++++++-------- apps/px4io/registers.c | 10 ++++++++++ 4 files changed, 38 insertions(+), 16 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 069028c14..b2e0c6ee4 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -279,6 +279,8 @@ PX4IO::PX4IO() : _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), + _status(0), + _alarms(0), _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), @@ -360,8 +362,8 @@ PX4IO::init() uint16_t reg; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®, sizeof(reg)); + /* get IO's last seen FMU state */ + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); if (ret != OK) return ret; @@ -392,8 +394,8 @@ PX4IO::init() /* wait 10 ms */ usleep(10000); - /* abort after 10s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -432,8 +434,8 @@ PX4IO::init() /* wait 10 ms */ usleep(10000); - /* abort after 10s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } @@ -770,19 +772,28 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) { + if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; _status &= PX4IO_P_STATUS_FLAGS_ARMED; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + /* set the sync flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); + /* set new status */ + _status = status; + } else { ret = 0; /* set new status */ _status = status; } + return ret; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 4f1b067bd..a957a9e79 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -102,6 +102,7 @@ #define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 96f49492f..c102bf479 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -84,7 +84,7 @@ static volatile uint8_t msg_next_out, msg_next_in; * output. */ #define NUM_MSG 2 -static char msg[NUM_MSG][40]; +static char msg[NUM_MSG][50]; /* add a debug message to be printed on the console @@ -183,23 +183,23 @@ int user_start(int argc, char *argv[]) /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - /* run the mixer at 100Hz (for now...) */ + /* run the mixer at ~300Hz (for now...) */ /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ - uint8_t counter=0; + uint16_t counter=0; for (;;) { /* - if we are not scheduled for 100ms then reset the I2C bus + if we are not scheduled for 10ms then reset the I2C bus */ - hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL); + hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL); - poll(NULL, 0, 10); + poll(NULL, 0, 3); perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); show_debug_messages(); - if (counter++ == 200) { + if (counter++ == 800) { counter = 0; - isr_debug(1, "tick dbg=%u stat=0x%x arm=0x%x feat=0x%x rst=%u", + isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index be3bebada..40bf72482 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -270,6 +270,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_alarms &= ~value; break; + case PX4IO_P_STATUS_FLAGS: + /* + * Allow FMU override of arming state (to allow in-air restores), + * but only if the arming state is not in sync on the IO side. + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + r_status_flags = value; + } + break; + default: /* just ignore writes to other registers in this page */ break; -- cgit v1.2.3