int mixer_handle_text_create_mixer() { /* only run on update */ if (!mixer_update_pending) { return 0; } /* do not allow a mixer change while safety off and FMU armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } /* abort if we're in the mixer - it will be tried again in the next iteration */ if (in_mixer) { return 1; } /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; mixer_group.load_from_buf(&mixer_text[0], resid); /* if anything was parsed */ if (resid != mixer_text_length) { isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) { memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); /* enforce null termination */ mixer_text[resid] = '\0'; } mixer_text_length = resid; } mixer_update_pending = false; return 0; }
int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* calculate our fw CRC so FMU can decide if we need to update */ calculate_fw_crc(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); #ifdef GPIO_LED4 LED_RING(false); #endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO POWER_SERVO(true); #endif /* turn off S.Bus out (if supported) */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(false); #endif /* start the safety switch handler */ safety_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); /* initialise the control inputs */ controls_init(); /* set up the ADC */ adc_init(); /* start the FMU interface */ interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); /* * P O L I C E L I G H T S * * Not enough memory, lock down. * * We might need to allocate mixers later, and this will * ensure that a developer doing a change will notice * that he just burned the remaining RAM with static * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. * */ if (minfo.mxordblk < 600) { lowsyslog("ERR: not enough MEM"); bool phase = false; while (true) { if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } up_udelay(250000); phase = !phase; } } /* Start the failsafe led init */ failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); } ring_blink(); check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); /* post debug state at ~1Hz - this is via an auxiliary serial port * DEFAULTS TO OFF! */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } }
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. */ 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; r_setup_pwm_rates = value; /* XXX re-configure timers */ break; case PX4IO_P_SETUP_PWM_LOWRATE: if (value < 50) value = 50; if (value > 400) value = 400; r_setup_pwm_lowrate = value; /* XXX re-configure timers */ break; case PX4IO_P_SETUP_PWM_HIGHRATE: if (value < 50) value = 50; if (value > 400) value = 400; r_setup_pwm_highrate = value; /* XXX re-configure timers */ 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: debug_level = value; isr_debug(0, "set debug %u\n", (unsigned)debug_level); break; default: return -1; } break; case PX4IO_PAGE_RC_CONFIG: { 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; /* 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) { /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) break; if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) break; if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) break; if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) break; /* assert deadzone is sane */ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) break; if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) break; if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) break; if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) break; /* sanity checks pass, enable channel */ 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; 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; }
void mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } px4io_mixdata *msg = (px4io_mixdata *)buffer; isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); /* FIRST mark the mixer as invalid */ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; return; } /* append mixer text and nul-terminate */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; isr_debug(2, "buflen %u", mixer_text_length); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; mixer_group.load_from_buf(&mixer_text[0], resid); /* if anything was parsed */ if (resid != mixer_text_length) { /* only set mixer ok if no residual is left over */ if (resid == 0) { r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; } else { /* not yet reached the end of the mixer, set as not ok */ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; } isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); mixer_text_length = resid; /* update failsafe values */ mixer_set_failsafe(); } break; } }
void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } /* default to failsafe mixing */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; } } /* * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; } } else if (source != MIX_NONE) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; uint16_t ramp_promille; /* update esc init state, but only if we are truely armed and not just PWM enabled */ if (mixer_servos_armed && should_arm) { switch (esc_state) { /* after arming, some ESCs need an initalization period, count the time from here */ case ESC_OFF: esc_init_time = hrt_absolute_time(); esc_state = ESC_INIT; break; /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ case ESC_INIT: if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { esc_state = ESC_RAMP; } break; /* then ramp until the min speed is reached */ case ESC_RAMP: if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { esc_state = ESC_ON; } break; case ESC_ON: default: break; } } else { esc_state = ESC_OFF; } /* do the calculations during the ramp for all at once */ if(esc_state == ESC_RAMP) { ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; } /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); switch (esc_state) { case ESC_INIT: r_page_servos[i] = (outputs[i] * 600 + 1500); break; case ESC_RAMP: r_page_servos[i] = (outputs[i] * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); break; case ESC_ON: r_page_servos[i] = (outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); break; case ESC_OFF: default: break; } } for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } /* * Decide whether the servos should be armed right now. * * We must be armed, and we must have a PWM source; either raw from * FMU or from the mixer. * * XXX correct behaviour for failsafe may require an additional case * here. */ should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and FMU is armed */ && ( ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); } if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the idle servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_idle[i]); } }
void mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while fully armed */ if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { return; } px4io_mixdata *msg = (px4io_mixdata *)buffer; isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); /* FIRST mark the mixer as invalid */ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); /* check for overflow - this is really fatal */ /* XXX could add just what will fit & try to parse, then repeat... */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; return; } /* append mixer text and nul-terminate */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; isr_debug(2, "buflen %u", mixer_text_length); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; mixer_group.load_from_buf(&mixer_text[0], resid); /* if anything was parsed */ if (resid != mixer_text_length) { /* ideally, this should test resid == 0 ? */ r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); mixer_text_length = resid; } break; } }
void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ if ((system_state.fmu_data_received_time == 0) || hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; /* this flag is never cleared once OK */ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; } /* default to failsafe mixing - it will be forced below if flag is set */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && /* do not enter manual override if we asked for termination failsafe and FMU is lost */ !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; } } /* * Decide whether the servos should be armed right now. * * We must be armed, and we must have a PWM source; either raw from * FMU or from the mixer. * */ should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and FMU is armed */ && ( ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ if ( /* if we have requested flight termination style failsafe (noreturn) */ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && /* and we should be armed, so we intended to provide outputs */ should_arm && /* and FMU is initialized */ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } /* * Check if we should force failsafe - and do it if we have to */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { source = MIX_FAILSAFE; } /* * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ /* poor mans mutex */ in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); in_mixer = false; /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } } /* set arming */ bool needs_to_arm = (should_arm || should_always_enable_pwm); /* check any conditions that prevent arming */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { needs_to_arm = false; } if (!should_arm && !should_always_enable_pwm) { needs_to_arm = false; } if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); } if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); } /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); } } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } }
void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } /* default to failsafe mixing */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* don't actually mix anything - we already have raw PWM values or not a valid mixer. */ source = MIX_NONE; } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; } } /* * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; } } else if (source != MIX_NONE) { float outputs[IO_SERVO_COUNT]; unsigned mixed; /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to servo output */ r_page_servos[i] = (outputs[i] * 600.0f) + 1500; } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } /* * Decide whether the servos should be armed right now. * * We must be armed, and we must have a PWM source; either raw from * FMU or from the mixer. * * XXX correct behaviour for failsafe may require an additional case * here. */ bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && /* FMU is available or FMU is not available but override is an option */ ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) ); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; } if (mixer_servos_armed) { /* update the servo outputs. */ for (unsigned i = 0; i < IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); } }
int user_start(int argc, char *argv[]) { /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) /* run C++ ctors before we go any further */ up_cxxinitialize(); # if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE) # error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE. # endif #else # error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined. #endif /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* calculate our fw CRC so FMU can decide if we need to update */ calculate_fw_crc(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ syslog(LOG_INFO, "\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); #ifdef GPIO_LED4 LED_RING(false); #endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO POWER_SERVO(true); #endif /* turn off S.Bus out (if supported) */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(false); #endif /* start the safety switch handler */ safety_init(); /* initialise the control inputs */ controls_init(); /* set up the ADC */ adc_init(); /* start the FMU interface */ interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); /* * P O L I C E L I G H T S * * Not enough memory, lock down. * * We might need to allocate mixers later, and this will * ensure that a developer doing a change will notice * that he just burned the remaining RAM with static * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. * */ if (minfo.mxordblk < 600) { syslog(LOG_ERR, "ERR: not enough MEM"); bool phase = false; while (true) { if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } up_udelay(250000); phase = !phase; } } /* Start the failsafe led init */ failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; uint64_t last_loop_time = 0; for (;;) { dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f; last_loop_time = hrt_absolute_time(); if (dt < 0.0001f) { dt = 0.0001f; } else if (dt > 0.02f) { dt = 0.02f; } /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); /* some boards such as Pixhawk 2.1 made the unfortunate choice to combine the blue led channel with the IMU heater. We need a software hack to fix the hardware hack by allowing to disable the LED / heater. */ if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) { /* blink blue LED at 4Hz in normal operation. When in override blink 4x faster so the user can clearly see that override is happening. This helps when pre-flight testing the override system */ uint32_t heartbeat_period_us = 250 * 1000UL; if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { heartbeat_period_us /= 4; } if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); } } else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) { /* switch resistive heater off */ LED_BLUE(false); } else { /* switch resistive heater hard on */ LED_BLUE(true); } update_mem_usage(); ring_blink(); check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); /* post debug state at ~1Hz - this is via an auxiliary serial port * DEFAULTS TO OFF! */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } }
int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off and FMU armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } /* disable mixing, will be enabled once load is complete */ PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) { return 0; } unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); return 0; } /* check if the last item has been processed - bail out if not */ if (mixer_update_pending) { return 1; } /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; isr_debug(2, "buflen %u", mixer_text_length); /* flag the buffer as ready */ mixer_update_pending = true; break; } return 0; }
void register_interrupt_handler(uint8_t interrupt_number, isr_t handler) { isr_debug("Register: [%d, %p]\n", interrupt_number, (uint32_t) handler); interrupt_handlers[interrupt_number] = handler; }
void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } /* default to failsafe mixing */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; } } /* * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } /* * Decide whether the servos should be armed right now. * * We must be armed, and we must have a PWM source; either raw from * FMU or from the mixer. * * XXX correct behaviour for failsafe may require an additional case * here. */ should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and FMU is armed */ && ( ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ /* poor mans mutex */ in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } } if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); } if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); } }
void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ if ((system_state.fmu_data_received_time == 0) || hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; /* this flag is never cleared once OK */ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; } /* default to failsafe mixing - it will be forced below if flag is set */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ /* Do not mix if we have raw PWM and FMU is ok. */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) { /* a channel based override has been * triggered, with FMU active */ source = MIX_OVERRIDE_FMU_OK; } else { /* don't actually mix anything - copy values from r_page_direct_pwm */ source = MIX_NONE; memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT); } } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; } else { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; } } } /* * Decide whether the servos should be armed right now. * * We must be armed, and we must have a PWM source; either raw from * FMU or from the mixer. * */ should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and FMU is armed */ && ( ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); should_arm_nothrottle = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ if ( /* if we have requested flight termination style failsafe (noreturn) */ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && /* and we should be armed, so we intended to provide outputs */ should_arm && /* and FMU is initialized */ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } /* * Check if we should force failsafe - and do it if we have to */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { source = MIX_FAILSAFE; } /* * Set failsafe status flag depending on mixing source */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } /* * Set simple mixer trim values * (there should be a "dirty" flag to indicate that r_page_servo_control_trim has changed) */ mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT); /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { // maximum value the ouputs of the multirotor mixer are allowed to change in this cycle // factor 2 is needed because actuator ouputs are in the range [-1,1] float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( r_setup_slew_max); mixer_group.set_max_delta_out_once(delta_out_max); } /* mix */ /* update parameter for mc thrust model if it updated */ if (update_mc_thrust_param) { mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); update_mc_thrust_param = false; } /* mix */ mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; outputs[i] = 0.0f; } /* store normalized outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } } /* set arming */ bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm); /* lockdown means to send a valid pulse which disables the outputs */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { needs_to_arm = true; } if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); } if (mixer_servos_armed && (should_arm || should_arm_nothrottle) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); } /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } } else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); /* copy values into reporting register */ r_page_servos[i] = r_page_servo_disarmed[i]; } /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } } }
int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ up_cxxinitialize(); /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ #ifdef CONFIG_ARCH_DMA hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); /* turn on servo power */ POWER_SERVO(true); /* start the safety switch handler */ safety_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); /* initialise the control inputs */ controls_init(); #ifdef CONFIG_STM32_I2C1 /* start the i2c handler */ i2c_init(); #endif /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* add a performance counter for controls */ perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); bool phase = false; if (phase) { LED_AMBER(true); LED_BLUE(false); } else { LED_AMBER(false); LED_BLUE(true); } phase = !phase; usleep(300000); } #endif /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; for (;;) { /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); controls_tick(); perf_end(controls_perf); /* check for debug activity */ show_debug_messages(); /* post debug state at ~1Hz */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } }
int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off and FMU armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } /* disable mixing, will be enabled once load is complete */ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) { return 0; } unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; return 0; } /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; isr_debug(2, "buflen %u", mixer_text_length); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; mixer_group.load_from_buf(&mixer_text[0], resid); /* if anything was parsed */ if (resid != mixer_text_length) { isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) { memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); /* enforce null termination */ mixer_text[resid] = '\0'; } mixer_text_length = resid; } break; } 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; }