Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Finished and tested in-air restore of arming state, as long as both boards reset at the same time armings state is now retained

Browse Source
This commit is contained in:
Lorenz Meier
2013-02-17 17:47:26 +01:00
parent f689f0abb0
commit 56bf9855a8
4 changed files with 38 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4io/px4io.cpp
+19 -8
View File
@@ -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, &reg, sizeof(reg));
/* get IO's last seen FMU state */
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, 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;
}
apps/px4io/protocol.h
+1
View File
@@ -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 */
apps/px4io/px4io.c
+8 -8
View File
@@ -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,
apps/px4io/registers.c
+10
View File
@@ -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;