static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value) { switch (page) { case PX4IO_PAGE_STATUS: switch (offset) { case PX4IO_P_STATUS_ALARMS: /* clear bits being written */ 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; } break; case PX4IO_PAGE_SETUP: switch (offset) { case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; r_setup_features = value; /* no implemented feature selection at this point */ break; case PX4IO_P_SETUP_ARMING: value &= PX4IO_P_SETUP_ARMING_VALID; /* * Update arming state - disarm if no longer OK. * This builds on the requirement that the FMU driver * asks about the FMU arming state on initialization, * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; } r_setup_arming = value; break; case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: if (value < 50) value = 50; if (value > 400) value = 400; pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_ALTRATE: if (value < 50) value = 50; if (value > 400) value = 400; pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; POWER_RELAY1(value & (1 << 0) ? 1 : 0); POWER_RELAY2(value & (1 << 1) ? 1 : 0); POWER_ACC1(value & (1 << 2) ? 1 : 0); POWER_ACC2(value & (1 << 3) ? 1 : 0); break; case PX4IO_P_SETUP_SET_DEBUG: r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; default: return -1; } break; case PX4IO_PAGE_RC_CONFIG: { /* do not allow a RC config change while fully armed */ if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { break; } unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; if (channel >= MAX_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; switch (index) { case PX4IO_P_RC_CONFIG_MIN: case PX4IO_P_RC_CONFIG_CENTER: case PX4IO_P_RC_CONFIG_MAX: case PX4IO_P_RC_CONFIG_DEADZONE: case PX4IO_P_RC_CONFIG_ASSIGNMENT: conf[index] = value; break; case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint8_t count = 0; /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { count++; } if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { count++; } if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { count++; } if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { count++; } /* assert deadzone is sane */ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } // The following check isn't needed as constraint checks in controls.c will catch this. //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { // count++; //} //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { // count++; //} if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { count++; } /* sanity checks pass, enable channel */ if (count) { isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; } else { conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ } break; /* case PX4IO_RC_PAGE_CONFIG */ } default: return -1; } return 0; }
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value) { switch (page) { case PX4IO_PAGE_STATUS: switch (offset) { case PX4IO_P_STATUS_ALARMS: /* clear bits being written */ 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; } break; case PX4IO_PAGE_SETUP: switch (offset) { case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ /* switch S.Bus output pin as needed */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); /* disable the conflicting options with SBUS 1 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with SBUS 2 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif /* disable the conflicting options with ADC RSSI */ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* apply changes */ r_setup_features = value; break; case PX4IO_P_SETUP_ARMING: value &= PX4IO_P_SETUP_ARMING_VALID; /* * Update arming state - disarm if no longer OK. * This builds on the requirement that the FMU driver * asks about the FMU arming state on initialization, * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { 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; case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: if (value < 50) { value = 50; } if (value > 400) { value = 400; } pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_ALTRATE: if (value < 50) { value = 50; } if (value > 400) { value = 400; } pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); break; #endif case PX4IO_P_SETUP_VBATT_SCALE: r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; break; case PX4IO_P_SETUP_SET_DEBUG: r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; case PX4IO_P_SETUP_REBOOT_BL: if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) { break; } // we schedule a reboot rather than rebooting // immediately to allow the IO board to ACK // the reboot command schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: 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; } else { return -1; } break; case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else { return -1; } break; case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: if (value > 650 && value < 2350) { r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; } break; default: return -1; } break; case PX4IO_PAGE_RC_CONFIG: { /** * do not allow a RC config change while safety is off */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; if (channel >= PX4IO_RC_INPUT_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; switch (index) { case PX4IO_P_RC_CONFIG_MIN: case PX4IO_P_RC_CONFIG_CENTER: case PX4IO_P_RC_CONFIG_MAX: case PX4IO_P_RC_CONFIG_DEADZONE: case PX4IO_P_RC_CONFIG_ASSIGNMENT: conf[index] = value; break; case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* clear any existing RC disabled flag */ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint8_t count = 0; bool disabled = false; /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { count++; } if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { count++; } if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { count++; } if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { count++; } /* assert deadzone is sane */ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; } 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++; } /* sanity checks pass, enable channel */ if (count) { isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; } else if (!disabled) { conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ } break; /* case PX4IO_RC_PAGE_CONFIG */ } case PX4IO_PAGE_TEST: switch (offset) { case PX4IO_P_TEST_LED: LED_AMBER(value & 1); break; } break; default: return -1; } return 0; }
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value) { switch (page) { case PX4IO_PAGE_STATUS: switch (offset) { case PX4IO_P_STATUS_ALARMS: /* clear bits being written */ 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; } break; case PX4IO_PAGE_SETUP: switch (offset) { case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; r_setup_features = value; /* no implemented feature selection at this point */ break; case PX4IO_P_SETUP_ARMING: value &= PX4IO_P_SETUP_ARMING_VALID; /* * Update arming state - disarm if no longer OK. * This builds on the requirement that the FMU driver * asks about the FMU arming state on initialization, * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ // XXX do not reset IO's safety state by FMU for now // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; // } if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } r_setup_arming = value; break; case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: if (value < 50) value = 50; if (value > 400) value = 400; pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_ALTRATE: if (value < 50) value = 50; if (value > 400) value = 400; pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); break; #endif case PX4IO_P_SETUP_VBATT_SCALE: r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; break; case PX4IO_P_SETUP_SET_DEBUG: r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; case PX4IO_P_SETUP_DSM: dsm_bind(value & 0x0f, (value >> 4) & 7); break; default: return -1; } break; case PX4IO_PAGE_RC_CONFIG: { /** * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; if (channel >= PX4IO_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; switch (index) { case PX4IO_P_RC_CONFIG_MIN: case PX4IO_P_RC_CONFIG_CENTER: case PX4IO_P_RC_CONFIG_MAX: case PX4IO_P_RC_CONFIG_DEADZONE: case PX4IO_P_RC_CONFIG_ASSIGNMENT: conf[index] = value; break; case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* clear any existing RC disabled flag */ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint8_t count = 0; /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { count++; } if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { count++; } if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { count++; } if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { count++; } /* assert deadzone is sane */ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { count++; } /* sanity checks pass, enable channel */ if (count) { isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; } else { conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ } break; /* case PX4IO_RC_PAGE_CONFIG */ } case PX4IO_PAGE_TEST: switch (offset) { case PX4IO_P_TEST_LED: LED_AMBER(value & 1); break; } break; default: return -1; } return 0; }