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(); } } }
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(); } } }
void controls_tick() { /* * Gather R/C control inputs from supported sources. * * Note that if you're silly enough to connect more than * one control input source, they're going to fight each * other. Don't do that. */ /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; #ifdef ADC_RSSI if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { unsigned counts = adc_measure(ADC_RSSI); if (counts != 0xffff) { /* use 1:1 scaling on 3.3V ADC input */ unsigned mV = counts * 3300 / 4096; /* scale to 0..253 */ rssi = mV / 13; } } #endif perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); if (dsm_updated) { r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; else r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; rssi = 255; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; rssi = 100; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; rssi = 0; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } } perf_end(c_gather_sbus); /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_ppm); /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; /* store RSSI */ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; /* * In some cases we may have received a frame, but input has still * been lost. */ bool rc_input_lost = false; /* * If we received a new frame from any of the RC sources, process it. */ if (dsm_updated || sbus_updated || ppm_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); /* do not command anything in failsafe, kick in the RC loss counter */ if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* update RC-received timestamp */ system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ for (unsigned i = 0; i < r_raw_rc_count; i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint16_t raw = r_raw_rc_values[i]; int16_t scaled; /* * 1) Constrain to min/max values, as later processing depends on bounds. */ if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) raw = conf[PX4IO_P_RC_CONFIG_MIN]; if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) raw = conf[PX4IO_P_RC_CONFIG_MAX]; /* * 2) Scale around the mid point differently for lower and upper range. * * This is necessary as they don't share the same endpoints and slope. * * First normalize to 0..1 range with correct sign (below or above center), * then scale to 20000 range (if center is an actual center, -10000..10000, * if parameters only support half range, scale to 10000 range, e.g. if * center == min 0..10000, if center == max -10000..0). * * As the min and max bounds were enforced in step 1), division by zero * cannot occur, as for the case of center == min or center == max the if * statement is mutually exclusive with the arithmetic NaN case. * * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ scaled = 0; } /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); } } } /* set un-assigned controls to zero */ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; /* if we have enough channels (5) to control the vehicle, the mapping is ok */ if (assigned_channels > 4) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } } /* * Export the valid channel bitmap */ r_rc_valid = assigned_channels; } /* * If we haven't seen any new control data in 200ms, assume we * have lost input. */ if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); } /* * Handle losing RC input */ /* this kicks in if the receiver is gone or the system went to failsafe */ if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } /* this kicks in if the receiver is completely gone */ if (rc_input_lost) { /* Set channel count to zero */ r_raw_rc_count = 0; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we * must have R/C input. * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { bool override = false; /* * Check mapped channel 5 (can be any remote channel, * depends on RC_MAP_OVER parameter); * If the value is 'high' then the pilot has * requested override. * */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ if (dsm_updated || sbus_updated || ppm_updated) mixer_tick(); } else {
void controls_tick() { /* * Gather R/C control inputs from supported sources. * * Note that if you're silly enough to connect more than * one control input source, they're going to fight each * other. Don't do that. */ perf_begin(c_gather_dsm); bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); if (dsm_updated) r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; perf_end(c_gather_dsm); perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); if (sbus_updated) r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; perf_end(c_gather_sbus); /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); if (ppm_updated) r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); /* * In some cases we may have received a frame, but input has still * been lost. */ bool rc_input_lost = false; /* * If we received a new frame from any of the RC sources, process it. */ if (dsm_updated || sbus_updated || ppm_updated) { /* update RC-received timestamp */ system_state.rc_channels_timestamp = hrt_absolute_time(); /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ for (unsigned i = 0; i < r_raw_rc_count; i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint16_t raw = r_raw_rc_values[i]; int16_t scaled; /* * 1) Constrain to min/max values, as later processing depends on bounds. */ if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) raw = conf[PX4IO_P_RC_CONFIG_MIN]; if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) raw = conf[PX4IO_P_RC_CONFIG_MAX]; /* * 2) Scale around the mid point differently for lower and upper range. * * This is necessary as they don't share the same endpoints and slope. * * First normalize to 0..1 range with correct sign (below or above center), * then scale to 20000 range (if center is an actual center, -10000..10000, * if parameters only support half range, scale to 10000 range, e.g. if * center == min 0..10000, if center == max -10000..0). * * As the min and max bounds were enforced in step 1), division by zero * cannot occur, as for the case of center == min or center == max the if * statement is mutually exclusive with the arithmetic NaN case. * * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ scaled = 0; } /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; ASSERT(mapped < MAX_CONTROL_CHANNELS); /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); } } /* set un-assigned controls to zero */ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } /* * If we got an update with zero channels, treat it as * a loss of input. * * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ if (assigned_channels == 0) { rc_input_lost = true; } else { /* set RC OK flag and clear RC lost alarm */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Export the valid channel bitmap */ r_rc_valid = assigned_channels; } /* * If we haven't seen any new control data in 200ms, assume we * have lost input. */ if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); } /* * Handle losing RC input */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; /* Mark the arrays as empty */ r_raw_rc_count = 0; r_rc_valid = 0; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we * must have R/C input. * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { bool override = false; /* * Check mapped channel 5 (can be any remote channel, * depends on RC_MAP_OVER parameter); * If the value is 'high' then the pilot has * requested override. * */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) override = true; if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ if (dsm_updated || sbus_updated || ppm_updated) mixer_tick(); } else {
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 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { switch (page) { /* handle bulk controls input */ case PX4IO_PAGE_CONTROLS: /* copy channel data */ while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ r_page_controls[offset] = *values; offset++; num_values--; values++; } system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; /* handle raw PWM input */ case PX4IO_PAGE_DIRECT_PWM: { /* copy channel data */ uint16_t widest_pulse = 0; while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) { /* XXX range-check value? */ if (*values != PWM_IGNORE_THIS_CHANNEL) { r_page_direct_pwm[offset] = *values; if (*values > widest_pulse) { widest_pulse = *values; } } offset++; num_values--; values++; } if (widest_pulse > 2300) { // don't allow extreme pulses to cause issues with oneshot delays widest_pulse = 2300; } system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; if (r_setup_features & PX4IO_P_SETUP_FEATURES_ONESHOT) { /* in oneshot we run the mixer as soon as we get new data from the FMU */ mixer_tick(); /* we now need to trigger the output pulses, ensuring that the new pulses don't interrupt the previous pulses. We add a 50 usec safety margin to ensure the ESC has registered the end of the previous pulses. */ static hrt_abstime oneshot_delay_till; hrt_abstime now = hrt_absolute_time(); if (now < oneshot_delay_till) { up_udelay(oneshot_delay_till - now); } up_pwm_servo_trigger(r_setup_pwm_rates); oneshot_delay_till = hrt_absolute_time() + widest_pulse + 50; } break; } /* handle setup for servo failsafe values */ case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) { if (*values == 0) { /* ignore 0 */ } else if (*values < PWM_LOWEST_MIN) { r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; } else { r_page_servo_failsafe[offset] = *values; } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; offset++; num_values--; values++; } break; case PX4IO_PAGE_CONTROL_MIN_PWM: /* copy channel data */ while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) { if (*values == 0) { /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; } else if (*values < PWM_LOWEST_MIN) { r_page_servo_control_min[offset] = PWM_LOWEST_MIN; } else { r_page_servo_control_min[offset] = *values; } offset++; num_values--; values++; } break; case PX4IO_PAGE_CONTROL_MAX_PWM: /* copy channel data */ while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) { if (*values == 0) { /* ignore 0 */ } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { r_page_servo_control_max[offset] = *values; } offset++; num_values--; values++; } break; case PX4IO_PAGE_DISARMED_PWM: { /* flag for all outputs */ bool all_disarmed_off = true; /* copy channel data */ while ((offset < PX4IO_ACTUATOR_COUNT) && (num_values > 0)) { if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; } else if (*values < PWM_LOWEST_MIN) { r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; } else { r_page_servo_disarmed[offset] = *values; all_disarmed_off = false; } offset++; num_values--; values++; } if (all_disarmed_off) { /* disable PWM output if disarmed */ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); } else { /* enable PWM output always */ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; } } break; /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: /* 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 */ if ((offset + num_values) > 255) { num_values = 255 - offset; } /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) { return -1; } offset++; values++; } break; } return 0; }
static void comms_handle_command(const void *buffer, size_t length) { const struct px4io_command *cmd = (struct px4io_command *)buffer; if (length != sizeof(*cmd)) return; irqstate_t flags = irqsave(); /* fetch new PWM output values */ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) system_state.fmu_channel_data[i] = cmd->output_control[i]; /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ if (system_state.arm_ok && !cmd->arm_ok) system_state.armed = false; system_state.arm_ok = cmd->arm_ok; system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; system_state.manual_override_ok = cmd->manual_override_ok; system_state.mixer_fmu_available = true; system_state.fmu_data_received_time = hrt_absolute_time(); /* set PWM update rate if changed (after limiting) */ uint16_t new_servo_rate = cmd->servo_rate; /* reject faster than 500 Hz updates */ if (new_servo_rate > 500) { new_servo_rate = 500; } /* reject slower than 50 Hz updates */ if (new_servo_rate < 50) { new_servo_rate = 50; } if (system_state.servo_rate != new_servo_rate) { up_pwm_servo_set_rate(new_servo_rate); system_state.servo_rate = new_servo_rate; } /* * update servo values immediately. * the updates are done in addition also * in the mainloop, since this function will only * update with a connected FMU. */ mixer_tick(); /* handle relay state changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { if (system_state.relays[i] != cmd->relay_state[i]) { switch (i) { case 0: POWER_ACC1(cmd->relay_state[i]); break; case 1: POWER_ACC2(cmd->relay_state[i]); break; case 2: POWER_RELAY1(cmd->relay_state[i]); break; case 3: POWER_RELAY2(cmd->relay_state[i]); break; } } system_state.relays[i] = cmd->relay_state[i]; } irqrestore(flags); }
void controls_main(void) { struct pollfd fds[2]; /* DSM input */ fds[0].fd = dsm_init("/dev/ttyS0"); fds[0].events = POLLIN; /* S.bus input */ fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; for (;;) { /* run this loop at ~100Hz */ poll(fds, 2, 10); /* * Gather R/C control inputs from supported sources. * * Note that if you're silly enough to connect more than * one control input source, they're going to fight each * other. Don't do that. */ bool locked = false; if (fds[0].revents & POLLIN) locked |= dsm_input(); if (fds[1].revents & POLLIN) locked |= sbus_input(); /* * If we don't have lock from one of the serial receivers, * look for PPM. It shares an input with S.bus, so there's * a possibility it will mis-parse an S.bus frame. * * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * disable the PPM decoder completely if we have an alternate * receiver lock. */ if (!locked) ppm_input(); /* * If we haven't seen any new control data in 200ms, assume we * have lost input and tell FMU. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; /* trigger an immediate report to the FMU */ system_state.fmu_report_due = true; } /* XXX do bypass mode, etc. here */ /* do PWM output updates */ mixer_tick(); } }