diff options
Diffstat (limited to 'src/modules/px4iofirmware/registers.c')
| -rw-r--r-- | src/modules/px4iofirmware/registers.c | 50 |
1 files changed, 36 insertions, 14 deletions
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 43161aa70..f0c2cfd26 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -190,7 +190,9 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -285,7 +287,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ - r_page_servos[offset] = *values; + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_servos[offset] = *values; + } offset++; num_values--; @@ -403,11 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - return mixer_handle_text(values, num_values * sizeof(*values)); - } - break; + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return mixer_handle_text(values, num_values * sizeof(*values)); default: /* avoid offset wrap */ @@ -516,6 +520,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + r_setup_arming = value; break; @@ -566,8 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } @@ -587,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_ON: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; @@ -607,10 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /** - * do not allow a RC config change while outputs armed + * do not allow a RC config change while safety is off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } @@ -670,7 +691,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { count++; } |