void CameraTrigger::control(bool on) { // always execute, even if already on // to reset timings if necessary if (on) { // schedule trigger on and off calls hrt_call_every(&_engagecall, 0, (_interval * 1000), (hrt_callout)&CameraTrigger::engage, this); // schedule trigger on and off calls hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), (hrt_callout)&CameraTrigger::disengage, this); } else { // cancel all calls hrt_cancel(&_engagecall); hrt_cancel(&_disengagecall); // ensure that the pin is off hrt_call_after(&_disengagecall, 0, (hrt_callout)&CameraTrigger::disengage, this); } _trigger_enabled = on; }
void safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); }
void LSM303D::start() { /* make sure we are stopped first */ stop(); /* reset the report ring */ _accel_reports->flush(); _mag_reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); }
__EXPORT int board_app_initialize(uintptr_t arg) { int result = OK; px4_platform_init(); /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); return result; }
void MPU9250::start() { /* make sure we are stopped first */ stop(); /* discard any stale data in the buffers */ _accel_reports->flush(); _gyro_reports->flush(); _mag->_mag_reports->flush(); if (_use_hrt) { /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval - MPU9250_TIMER_REDUCTION, (hrt_callout)&MPU9250::measure_trampoline, this);; } else { #ifdef USE_I2C /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1); #endif } }
__EXPORT int nsh_archinitialize(void) { /* configure the high-resolution time/callout interface */ hrt_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); return OK; }
__EXPORT int nsh_archinitialize(void) { /* the interruption subsystem is not initialized when stm32_boardinitialize() is called */ stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback); /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_SENSORS_EN); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ dma_alloc_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); led_off(LED_BLUE); /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); up_ledon(LED_AMBER); return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); return OK; }
__EXPORT int nsh_archinitialize(void) { int result; /* configure the high-resolution time/callout interface */ hrt_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); board_pwr_init(1); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); led_off(LED_BLUE); #if defined(FLASH_BASED_PARAMS) static sector_descriptor_t sector_map[] = { {1, 16 * 1024, 0x08004000}, {2, 16 * 1024, 0x08008000}, {0, 0, 0}, }; /* Initalizee the flashfs layer to use heap allocated memory */ result = parameter_flashfs_init(sector_map, NULL, 0); if (result != OK) { message("[boot] FAILED to init params in FLASH %d\n", result); up_ledon(LED_AMBER); return -ENODEV; } #endif return OK; }
void CameraTrigger::enable_keep_alive(bool on) { if (on) { // schedule keep-alive up and down calls hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), (hrt_callout)&CameraTrigger::keep_alive_up, this); hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), (hrt_callout)&CameraTrigger::keep_alive_down, this); } else { // cancel all calls hrt_cancel(&_keepalivecall_up); hrt_cancel(&_keepalivecall_down); } }
void CameraTrigger::update_intervalometer() { // the actual intervalometer runs in interrupt context, so we only need to call // control_intervalometer once on enabling/disabling trigger to schedule the calls. if (_trigger_enabled && !_trigger_paused) { // schedule trigger on and off calls hrt_call_every(&_engagecall, 0, (_interval * 1000), (hrt_callout)&CameraTrigger::engage, this); // schedule trigger on and off calls hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), (hrt_callout)&CameraTrigger::disengage, this); } }
__EXPORT int nsh_archinitialize(void) { int result; /* configure the high-resolution time/callout interface */ hrt_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); led_off(LED_BLUE); /* Get the SPI port for the microSD slot */ spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO); if (!spi) { message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); up_ledon(LED_AMBER); return -ENODEV; } /* Now bind the SPI interface to the MMCSD driver */ result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi); if (result != OK) { message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n"); up_ledon(LED_AMBER); return -ENODEV; } return OK; }
int ADC::open_first(struct file *filp) { /* get fresh data */ _tick(); /* and schedule regular updates */ hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); return 0; }
void L3GD20::start() { /* make sure we are stopped first */ stop(); /* reset the report ring */ _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); }
void BMA180::start() { /* make sure we are stopped first */ stop(); /* reset the report ring */ _oldest_report = _next_report = 0; /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); }
void FXAS21002C::start() { /* make sure we are stopped first */ stop(); /* reset the report ring */ _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_gyro_call, 1000, _call_interval - FXAS21002C_TIMER_REDUCTION, (hrt_callout)&FXAS21002C::measure_trampoline, this); }
int mixer_init(const char *mq_name) { /* open the control input queue; this should always exist */ input_queue = mq_open(mq_name, O_RDONLY | O_NONBLOCK); ASSERTCODE((input_queue >= 0), A_INPUTQ_OPEN_FAIL); /* open the servo driver */ mixer_servo_fd = open("/dev/pwm_servo", O_WRONLY); ASSERTCODE((mixer_servo_fd >= 0), A_SERVO_OPEN_FAIL); /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); return 0; }
void BMI055_accel::start() { /* make sure we are stopped first */ stop(); /* discard any stale data in the buffers */ _accel_reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval - BMI055_TIMER_REDUCTION, (hrt_callout)&BMI055_accel::measure_trampoline, this); reset(); }
/* * initialise the driver. This doesn't actually start the timer (that * is done on open). We don't start the timer to allow for this driver * to be started in init scripts when the user may be using the input * pin as PWM output */ int PWMIN::init() { /* we just register the device in /dev, and only actually * activate the timer when requested to when the device is opened */ CDev::init(); _reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s)); if (_reports == nullptr) { return -ENOMEM; } /* Schedule freeze check to invoke periodically */ hrt_call_every(&_freeze_test_call, 0, TIMEOUT_POLL, reinterpret_cast<hrt_callout>(&PWMIN::_freeze_test), this); return OK; }
__EXPORT int board_app_initialize(uintptr_t arg) { int result = OK; #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 /* configure the high-resolution time/callout interface */ hrt_init(); /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); return result; }
__EXPORT int board_app_initialize(uintptr_t arg) { #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 /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ if (board_dma_alloc_init() < 0) { message("DMA alloc FAILED"); } /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); #if defined(CONFIG_STM32_BBSRAM) /* NB. the use of the console requires the hrt running * to poll the DMA */ /* Using Battery Backed Up SRAM */ int filesizes[CONFIG_STM32_BBSRAM_FILES + 1] = BSRAM_FILE_SIZES; stm32_bbsraminitialize(BBSRAM_PATH, filesizes); #if defined(CONFIG_STM32_SAVE_CRASHDUMP) /* Panic Logging in Battery Backed Up Files */ /* * In an ideal world, if a fault happens in flight the * system save it to BBSRAM will then reboot. Upon * rebooting, the system will log the fault to disk, recover * the flight state and continue to fly. But if there is * a fault on the bench or in the air that prohibit the recovery * or committing the log to disk, the things are too broken to * fly. So the question is: * * Did we have a hard fault and not make it far enough * through the boot sequence to commit the fault data to * the SD card? */ /* Do we have an uncommitted hard fault in BBSRAM? * - this will be reset after a successful commit to SD */ int hadCrash = hardfault_check_status("boot"); if (hadCrash == OK) { message("[boot] There is a hard fault logged. Hold down the SPACE BAR," \ " while booting to halt the system!\n"); /* Yes. So add one to the boot count - this will be reset after a successful * commit to SD */ int reboots = hardfault_increment_reboot("boot", false); /* Also end the misery for a user that holds for a key down on the console */ int bytesWaiting; ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting)); if (reboots > 2 || bytesWaiting != 0) { /* Since we can not commit the fault dump to disk. Display it * to the console. */ hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n", reboots, (bytesWaiting == 0 ? "" : " Due to Key Press\n")); /* For those of you with a debugger set a break point on up_assert and * then set dbgContinue = 1 and go. */ /* Clear any key press that got us here */ static volatile bool dbgContinue = false; int c = '>'; while (!dbgContinue) { switch (c) { case EOF: case '\n': case '\r': case ' ': continue; default: putchar(c); putchar('\n'); switch (c) { case 'D': case 'd': hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); break; case 'C': case 'c': hardfault_rearm("boot"); hardfault_increment_reboot("boot", true); break; case 'B': case 'b': dbgContinue = true; break; default: break; } // Inner Switch message("\nEnter B - Continue booting\n" \ "Enter C - Clear the fault log\n" \ "Enter D - Dump fault log\n\n?>"); fflush(stdout); if (!dbgContinue) { c = getchar(); } break; } // outer switch } // for } // inner if } // outer if #endif // CONFIG_STM32_SAVE_CRASHDUMP #endif // CONFIG_STM32_BBSRAM /* initial LED state */ drv_led_start(); led_off(LED_RED); led_off(LED_GREEN); led_off(LED_BLUE); /* Configure SPI-based devices */ spi1 = stm32_spibus_initialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); board_autoled_on(LED_RED); return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); /* Get the SPI port for the FRAM */ spi2 = stm32_spibus_initialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); board_autoled_on(LED_RED); return -ENODEV; } /* Default SPI2 to 12MHz and de-assert the known chip selects. * MS5611 has max SPI clock speed of 20MHz */ // XXX start with 10.4 MHz and go up to 20 once validated SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); SPI_SELECT(spi2, PX4_SPIDEV_BARO, false); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); #endif return OK; }
__EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); up_ledon(LED_AMBER); return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\n"); /* Get the SPI port for the FRAM */ spi2 = up_spiinitialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); up_ledon(LED_AMBER); return -ENODEV; } /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 375000000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); message("[boot] Successfully initialized SPI port 2\n"); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); message("[boot] Initialized SDIO\n"); #endif return OK; }
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(); } } }
__EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); #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 /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ dma_alloc_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); board_led_on(LED_AMBER); return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); syslog(LOG_INFO, "[boot] Initialized SPI port 1 (SENSORS)\n"); /* Get the SPI port for the FRAM */ spi2 = up_spiinitialize(2); if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); board_led_on(LED_AMBER); return -ENODEV; } /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) * and de-assert the known chip selects. */ // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); syslog(LOG_INFO, "[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); spi4 = up_spiinitialize(4); /* Default SPI4 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi4, 10000000); SPI_SETBITS(spi4, 8); SPI_SETMODE(spi4, SPIDEV_MODE3); SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); syslog(LOG_INFO, "[boot] Initialized SPI port 4\n"); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); syslog(LOG_INFO, "[boot] Initialized SDIO\n"); #endif return OK; }
void safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); }
__EXPORT int nsh_archinitialize(void) { int result; message("\n"); /* configure always-on ADC pins */ stm32_configgpio(GPIO_ADC1_IN1); stm32_configgpio(GPIO_ADC1_IN2); stm32_configgpio(GPIO_ADC1_IN3); stm32_configgpio(GPIO_ADC1_IN10); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ dma_alloc_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); /* initial BUZZER state */ drv_buzzer_start(); buzzer_off(BUZZER_EXT); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); led_off(LED_BLUE); led_off(LED_GREEN); led_off(LED_EXT1); led_off(LED_EXT2); /* Configure SPI-based devices */ message("[boot] Initializing SPI port 1\n"); spi1 = up_spiinitialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\r\n"); led_on(LED_AMBER); return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, SPIDEV_WIRELESS, false); SPI_SELECT(spi1, SPIDEV_MS5611, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\r\n"); message("[boot] Initializing SPI port 2\n"); spi2 = up_spiinitialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\r\n"); led_on(LED_AMBER); return -ENODEV; } /* Default SPI2 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 10000000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_MPU6000, false); message("[boot] Successfully initialized SPI port 2\n"); /* Get the SPI port for the microSD slot */ message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); led_on(LED_AMBER); return -ENODEV; } /* Default SPI3 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi3, 10000000); SPI_SETBITS(spi3, 8); SPI_SETMODE(spi3, SPIDEV_MODE3); SPI_SELECT(spi3, SPIDEV_MMCSD, false); SPI_SELECT(spi3, SPIDEV_FLASH, false); message("[boot] Successfully initialized SPI port 3\n"); /* Now bind the SPI interface to the MMCSD driver */ result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); if (result != OK) { message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); led_on(LED_AMBER); return -ENODEV; } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); return OK; }
__EXPORT int board_app_initialize(uintptr_t arg) { int result; #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 /* configure the high-resolution time/callout interface */ hrt_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); #if defined(CONFIG_STM32_BBSRAM) /* NB. the use of the console requires the hrt running * to poll the DMA */ /* Using Battery Backed Up SRAM */ int filesizes[CONFIG_STM32_BBSRAM_FILES + 1] = BSRAM_FILE_SIZES; stm32_bbsraminitialize(BBSRAM_PATH, filesizes); #if defined(CONFIG_STM32_SAVE_CRASHDUMP) /* Panic Logging in Battery Backed Up Files */ /* * In an ideal world, if a fault happens in flight the * system save it to BBSRAM will then reboot. Upon * rebooting, the system will log the fault to disk, recover * the flight state and continue to fly. But if there is * a fault on the bench or in the air that prohibit the recovery * or committing the log to disk, the things are too broken to * fly. So the question is: * * Did we have a hard fault and not make it far enough * through the boot sequence to commit the fault data to * the SD card? */ /* Do we have an uncommitted hard fault in BBSRAM? * - this will be reset after a successful commit to SD */ int hadCrash = hardfault_check_status("boot"); if (hadCrash == OK) { message("[boot] There is a hard fault logged. Hold down the SPACE BAR," \ " while booting to halt the system!\n"); /* Yes. So add one to the boot count - this will be reset after a successful * commit to SD */ int reboots = hardfault_increment_reboot("boot", false); /* Also end the misery for a user that holds for a key down on the console */ int bytesWaiting; ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting)); if (reboots > 2 || bytesWaiting != 0) { /* Since we can not commit the fault dump to disk. Display it * to the console. */ hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n", reboots, (bytesWaiting == 0 ? "" : " Due to Key Press\n")); /* For those of you with a debugger set a break point on up_assert and * then set dbgContinue = 1 and go. */ /* Clear any key press that got us here */ static volatile bool dbgContinue = false; int c = '>'; while (!dbgContinue) { switch (c) { case EOF: case '\n': case '\r': case ' ': continue; default: putchar(c); putchar('\n'); switch (c) { case 'D': case 'd': hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); break; case 'C': case 'c': hardfault_rearm("boot"); hardfault_increment_reboot("boot", true); break; case 'B': case 'b': dbgContinue = true; break; default: break; } // Inner Switch message("\nEnter B - Continue booting\n" \ "Enter C - Clear the fault log\n" \ "Enter D - Dump fault log\n\n?>"); fflush(stdout); if (!dbgContinue) { c = getchar(); } break; } // outer switch } // for } // inner if } // outer if #endif // CONFIG_STM32_SAVE_CRASHDUMP #endif // CONFIG_STM32_BBSRAM /* initial LED state */ drv_led_start(); led_off(LED_RED); led_off(LED_GREEN); led_off(LED_BLUE); led_off(LED_TX); led_off(LED_RX); result = board_i2c_initialize(); if (result != OK) { led_on(LED_RED); return -ENODEV; } return OK; }
int nsh_archinitialize(void) { int result; /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ /* configure the high-resolution time/callout interface */ #ifdef CONFIG_HRT_TIMER hrt_init(); #endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ #ifdef SERIAL_HAVE_DMA { static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); } #endif message("\r\n"); up_ledoff(LED_BLUE); up_ledoff(LED_AMBER); up_ledon(LED_BLUE); /* Configure user-space led driver */ px4fmu_led_init(); /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\r\n"); up_ledon(LED_AMBER); return -ENODEV; } // Setup 10 MHz clock (maximum rate the BMA180 can sustain) SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\r\n"); /* initialize SPI peripherals redundantly */ int gyro_attempts = 0; int gyro_fail = 0; while (gyro_attempts < 5) { gyro_fail = l3gd20_attach(spi1, PX4_SPIDEV_GYRO); gyro_attempts++; if (gyro_fail == 0) break; up_udelay(1000); } if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n"); int acc_attempts = 0; int acc_fail = 0; while (acc_attempts < 5) { acc_fail = bma180_attach(spi1, PX4_SPIDEV_ACCEL); acc_attempts++; if (acc_fail == 0) break; up_udelay(1000); } if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n"); int mpu_attempts = 0; int mpu_fail = 0; while (mpu_attempts < 1) { mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU); mpu_attempts++; if (mpu_fail == 0) break; up_udelay(200); } if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n"); /* initialize I2C2 bus */ i2c2 = up_i2cinitialize(2); if (!i2c2) { message("[boot] FAILED to initialize I2C bus 2\r\n"); up_ledon(LED_AMBER); return -ENODEV; } /* set I2C2 speed */ I2C_SETFREQUENCY(i2c2, 400000); i2c3 = up_i2cinitialize(3); if (!i2c3) { message("[boot] FAILED to initialize I2C bus 3\r\n"); up_ledon(LED_AMBER); return -ENODEV; } /* set I2C3 speed */ I2C_SETFREQUENCY(i2c3, 400000); int mag_attempts = 0; int mag_fail = 0; while (mag_attempts < 5) { mag_fail = hmc5883l_attach(i2c2); mag_attempts++; if (mag_fail == 0) break; up_udelay(1000); } if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n"); int baro_attempts = 0; int baro_fail = 0; while (baro_attempts < 5) { baro_fail = ms5611_attach(i2c2); baro_attempts++; if (baro_fail == 0) break; up_udelay(1000); } if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n"); /* try to attach, don't fail if device is not responding */ (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); int eeprom_attempts = 0; int eeprom_fail; while (eeprom_attempts < 5) { /* try to attach, fail if device does not respond */ eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS, FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES, FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES, FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1); eeprom_attempts++; if (eeprom_fail == OK) break; up_udelay(1000); } if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n"); /* Report back sensor status */ if (acc_fail || gyro_fail || mag_fail || baro_fail || eeprom_fail) { up_ledon(LED_AMBER); } #if defined(CONFIG_STM32_SPI3) /* Get the SPI port */ message("[boot] Initializing SPI port 3\r\n"); spi3 = up_spiinitialize(3); if (!spi3) { message("[boot] FAILED to initialize SPI port 3\r\n"); up_ledon(LED_AMBER); return -ENODEV; } message("[boot] Successfully initialized SPI port 3\r\n"); /* Now bind the SPI interface to the MMCSD driver */ result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); if (result != OK) { message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n"); up_ledon(LED_AMBER); return -ENODEV; } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n"); #endif /* SPI3 */ /* initialize I2C1 bus */ i2c1 = up_i2cinitialize(1); if (!i2c1) { message("[boot] FAILED to initialize I2C bus 1\r\n"); up_ledon(LED_AMBER); return -ENODEV; } /* set I2C1 speed */ I2C_SETFREQUENCY(i2c1, 400000); /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ /* Get board information if available */ /* Initialize the user GPIOs */ px4fmu_gpio_init(); #ifdef CONFIG_ADC int adc_state = adc_devinit(); if (adc_state != OK) { /* Try again */ adc_state = adc_devinit(); if (adc_state != OK) { /* Give up */ message("[boot] FAILED adc_devinit: %d\r\n", adc_state); return -ENODEV; } } #endif /* configure the tone generator */ #ifdef CONFIG_TONE_ALARM tone_alarm_init(); #endif return OK; }
int sensors_main(int argc, char *argv[]) { /* inform about start */ printf("[sensors] Initializing..\n"); fflush(stdout); int ret = OK; /* start sensor reading */ if (sensors_init() != OK) { fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n"); /* Clean up */ close(fd_gyro); close(fd_accelerometer); close(fd_magnetometer); close(fd_barometer); close(fd_adc); fprintf(stderr, "[sensors] rebooting system.\n"); fflush(stderr); fflush(stdout); usleep(100000); /* Sensors are critical, immediately reboot system on failure */ reboot(); /* Not ever reaching here */ } else { /* flush stdout from init routine */ fflush(stdout); } bool gyro_healthy = false; bool acc_healthy = false; bool magn_healthy = false; bool baro_healthy = false; bool adc_healthy = false; bool hil_enabled = false; /**< HIL is disabled by default */ bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */ int magcounter = 0; int barocounter = 0; int adccounter = 0; unsigned int mag_fail_count = 0; unsigned int mag_success_count = 0; unsigned int baro_fail_count = 0; unsigned int baro_success_count = 0; unsigned int gyro_fail_count = 0; unsigned int gyro_success_count = 0; unsigned int acc_fail_count = 0; unsigned int acc_success_count = 0; unsigned int adc_fail_count = 0; unsigned int adc_success_count = 0; ssize_t ret_gyro; ssize_t ret_accelerometer; ssize_t ret_magnetometer; ssize_t ret_barometer; ssize_t ret_adc; int nsamples_adc; int16_t buf_gyro[3]; int16_t buf_accelerometer[3]; int16_t buf_magnetometer[7]; float buf_barometer[3]; int16_t mag_offset[3] = {0, 0, 0}; int16_t acc_offset[3] = {0, 0, 0}; int16_t gyro_offset[3] = {0, 0, 0}; bool mag_calibration_enabled = false; #pragma pack(push,1) struct adc_msg4_s { uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ }; #pragma pack(pop) struct adc_msg4_s buf_adc; size_t adc_readsize = 1 * sizeof(struct adc_msg4_s); float battery_voltage_conversion; battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION]; if (-1 == (int)battery_voltage_conversion) { /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f; } #ifdef CONFIG_HRT_PPM int ppmcounter = 0; #endif /* initialize to 100 to execute immediately */ int paramcounter = 100; int excessive_readout_time_counter = 0; int read_loop_counter = 0; /* Empty sensor buffers, avoid junk values */ /* Read first two values of each sensor into void */ (void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); (void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); (void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer)); struct sensor_combined_s raw = { .timestamp = hrt_absolute_time(), .gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]}, .gyro_raw_counter = 0, .gyro_rad_s = {0, 0, 0}, .accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]}, .accelerometer_raw_counter = 0, .accelerometer_m_s2 = {0, 0, 0}, .magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]}, .magnetometer_raw_counter = 0, .baro_pres_mbar = 0, .baro_alt_meter = 0, .baro_temp_celcius = 0, .battery_voltage_v = BAT_VOL_INITIAL, .adc_voltage_v = {0, 0 , 0}, .baro_raw_counter = 0, .battery_voltage_counter = 0, .battery_voltage_valid = false, }; /* condition to wait for */ pthread_mutex_init(&sensors_read_ready_mutex, NULL); pthread_cond_init(&sensors_read_ready, NULL); /* advertise the topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); publishing = true; /* advertise the rc topic */ struct rc_channels_s rc; memset(&rc, 0, sizeof(rc)); int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); /* subscribe to system status */ struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC)); struct hrt_call sensors_hrt_call; /* Enable high resolution timer callback to unblock main thread, run after 2 ms */ hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL); while (1) { pthread_mutex_lock(&sensors_read_ready_mutex); struct timespec time_to_wait = {0, 0}; /* Wait 2 seconds until timeout */ time_to_wait.tv_nsec = 0; time_to_wait.tv_sec = time(NULL) + 2; if (pthread_cond_timedwait(&sensors_read_ready, &sensors_read_ready_mutex, &time_to_wait) == OK) { pthread_mutex_unlock(&sensors_read_ready_mutex); bool gyro_updated = false; bool acc_updated = false; bool magn_updated = false; bool baro_updated = false; bool adc_updated = false; /* store the time closest to all measurements */ uint64_t current_time = hrt_absolute_time(); raw.timestamp = current_time; if (paramcounter == 100) { // XXX paramcounter is not a good name, rename / restructure // XXX make counter ticks dependent on update rate of sensor main loop /* Check HIL state */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { hil_enabled = true; publishing = false; int ret = close(sensor_pub); printf("[sensors] Closing sensor pub: %i \n", ret); /* switching from HIL to non-HIL mode */ } else if (!publishing && !hil_enabled) { /* advertise the topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); hil_enabled = false; publishing = true; } /* Update RC scalings and function mappings */ rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]); rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM]; rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]); rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM]; rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]); rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM]; rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]); rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM]; rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]); rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM]; rc.function[0] = global_data_parameter_storage->pm.param_values[PARAM_THROTTLE_CHAN] - 1; rc.function[1] = global_data_parameter_storage->pm.param_values[PARAM_ROLL_CHAN] - 1; rc.function[2] = global_data_parameter_storage->pm.param_values[PARAM_PITCH_CHAN] - 1; rc.function[3] = global_data_parameter_storage->pm.param_values[PARAM_YAW_CHAN] - 1; rc.function[4] = global_data_parameter_storage->pm.param_values[PARAM_OVERRIDE_CHAN] - 1; gyro_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET]; gyro_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET]; gyro_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET]; mag_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET]; mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET]; mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET]; paramcounter = 0; } paramcounter++; /* try reading gyro */ uint64_t start_gyro = hrt_absolute_time(); ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); int gyrotime = hrt_absolute_time() - start_gyro; if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); /* GYROSCOPE */ if (ret_gyro != sizeof(buf_gyro)) { gyro_fail_count++; if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { // global_data_send_subsystem_info(&gyro_present_enabled); gyro_healthy = false; gyro_success_count = 0; } } else { gyro_success_count++; if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { // global_data_send_subsystem_info(&gyro_present_enabled_healthy); gyro_healthy = true; gyro_fail_count = 0; } gyro_updated = true; } gyrotime = hrt_absolute_time() - start_gyro; if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); /* try reading acc */ uint64_t start_acc = hrt_absolute_time(); ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); /* ACCELEROMETER */ if (ret_accelerometer != sizeof(buf_accelerometer)) { acc_fail_count++; if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) { fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { // global_data_send_subsystem_info(&acc_present_enabled); gyro_healthy = false; acc_success_count = 0; } } else { acc_success_count++; if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { // global_data_send_subsystem_info(&acc_present_enabled_healthy); acc_healthy = true; acc_fail_count = 0; } acc_updated = true; } int acctime = hrt_absolute_time() - start_acc; if (acctime > 500) printf("ACC: %d us\n", acctime); /* MAGNETOMETER */ if (magcounter == 4) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); /* start calibration mode if requested */ if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) { ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0); printf("[sensors] enabling mag calibration mode\n"); mag_calibration_enabled = true; } else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) { ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0); printf("[sensors] disabling mag calibration mode\n"); mag_calibration_enabled = false; } ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); int errcode_mag = (int) * get_errno_ptr(); int magtime = hrt_absolute_time() - start_mag; if (magtime > 2000) { printf("MAG (pure read): %d us\n", magtime); } if (ret_magnetometer != sizeof(buf_magnetometer)) { mag_fail_count++; if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) { fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) { // global_data_send_subsystem_info(&magn_present_enabled); magn_healthy = false; mag_success_count = 0; } } else { mag_success_count++; if (!magn_healthy && mag_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) { // global_data_send_subsystem_info(&magn_present_enabled_healthy); magn_healthy = true; mag_fail_count = 0; } magn_updated = true; } magtime = hrt_absolute_time() - start_mag; if (magtime > 2000) { printf("MAG (overall time): %d us\n", magtime); fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); } magcounter = 0; } magcounter++; /* BAROMETER */ if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */ uint64_t start_baro = hrt_absolute_time(); *get_errno_ptr() = 0; ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer)); if (ret_barometer != sizeof(buf_barometer)) { baro_fail_count++; if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (baro_healthy && baro_fail_count >= BARO_HEALTH_COUNTER_LIMIT_ERROR) { /* switched from healthy to unhealthy */ baro_healthy = false; baro_success_count = 0; // global_data_send_subsystem_info(&baro_present_enabled); } } else { baro_success_count++; if (!baro_healthy && baro_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) { /* switched from unhealthy to healthy */ baro_healthy = true; baro_fail_count = 0; // global_data_send_subsystem_info(&baro_present_enabled_healthy); } baro_updated = true; } barocounter = 0; int barotime = hrt_absolute_time() - start_baro; if (barotime > 2000) printf("BARO: %d us\n", barotime); } barocounter++; /* ADC */ if (adccounter == 5) { ret_adc = read(fd_adc, &buf_adc, adc_readsize); nsamples_adc = ret_adc / sizeof(struct adc_msg_s); if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) { adc_fail_count++; if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) { adc_healthy = false; adc_success_count = 0; } } else { adc_success_count++; if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) { adc_healthy = true; adc_fail_count = 0; } adc_updated = true; } adccounter = 0; } adccounter++; #ifdef CONFIG_HRT_PPM bool ppm_updated = false; /* PPM */ if (ppmcounter == 5) { /* require at least two channels * to consider the signal valid */ if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { /* Read out values from HRT */ for (int i = 0; i < ppm_decoded_channels; i++) { rc.chan[i].raw = ppm_buffer[i]; /* Set the range to +-, then scale up */ rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; } rc.chan_count = ppm_decoded_channels; rc.timestamp = ppm_last_valid_decode; /* publish a few lines of code later if set to true */ ppm_updated = true; } ppmcounter = 0; } ppmcounter++; #endif /* Copy values of gyro, acc, magnetometer & barometer */ /* GYROSCOPE */ if (gyro_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor /* scale measurements */ // XXX request scaling from driver instead of hardcoding it /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f; raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f; raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f; raw.gyro_raw_counter++; } /* ACCELEROMETER */ if (acc_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor // XXX read range from sensor float range_g = 4.0f; /* scale from 14 bit to m/s2 */ raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; raw.accelerometer_raw_counter++; } /* MAGNETOMETER */ if (magn_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f; raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f; raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f; /* store mode */ raw.magnetometer_mode = buf_magnetometer[3]; raw.magnetometer_raw_counter++; } /* BAROMETER */ if (baro_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius raw.baro_raw_counter++; } /* ADC */ if (adc_updated) { /* copy sensor readings to global data*/ if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { /* Voltage in volts */ raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (uint16_t)(buf_adc.am_data1 * battery_voltage_conversion))); if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { raw.battery_voltage_valid = false; raw.battery_voltage_v = 0.f; } else { raw.battery_voltage_valid = true; } raw.battery_voltage_counter++; } } uint64_t total_time = hrt_absolute_time() - current_time; /* Inform other processes that new data is available to copy */ if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) { /* Values changed, publish */ orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw); } #ifdef CONFIG_HRT_PPM if (ppm_updated) { orb_publish(ORB_ID(rc_channels), rc_pub, &rc); } #endif if (total_time > 2600) { excessive_readout_time_counter++; } if (total_time > 2600 && excessive_readout_time_counter > 100 && excessive_readout_time_counter % 100 == 0) { fprintf(stderr, "[sensors] slow update (>2600 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter); } else if (total_time > 6000) { if (excessive_readout_time_counter < 100 || excessive_readout_time_counter % 100 == 0) fprintf(stderr, "[sensors] WARNING: Slow update (>6000 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter); } read_loop_counter++; #ifdef CONFIG_SENSORS_DEBUG_ENABLED if (read_loop_counter % 1000 == 0) printf("[sensors] read loop counter: %d\n", read_loop_counter); fflush(stdout); if (sensors_timer_loop_counter % 1000 == 0) printf("[sensors] timer/trigger loop counter: %d\n", sensors_timer_loop_counter); #endif } } /* Never really getting here */ printf("[sensors] sensor readout stopped\n"); close(fd_gyro); close(fd_accelerometer); close(fd_magnetometer); close(fd_barometer); close(fd_adc); printf("[sensors] exiting.\n"); return ret; }
void failsafe_led_init(void) { /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); }
__EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ stm32_configgpio(GPIO_ADC1_IN11); /* BATT2_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN13); /* BATT2_CURRENT_SENS */ /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_3V3_PERIPH_EN); stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); stm32_configgpio(GPIO_VDD_5V_HIPOWER_EN); stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_BRICK2_VALID); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VBUS_VALID); // stm32_configgpio(GPIO_SBUS_INV); // stm32_configgpio(GPIO_8266_GPIO0); // stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); // stm32_configgpio(GPIO_8266_PD); // stm32_configgpio(GPIO_8266_RST); // stm32_configgpio(GPIO_BTN_SAFETY_FMU); /* configure the GPIO pins to outputs and keep them low */ stm32_configgpio(GPIO_GPIO0_OUTPUT); stm32_configgpio(GPIO_GPIO1_OUTPUT); stm32_configgpio(GPIO_GPIO2_OUTPUT); stm32_configgpio(GPIO_GPIO3_OUTPUT); stm32_configgpio(GPIO_GPIO4_OUTPUT); stm32_configgpio(GPIO_GPIO5_OUTPUT); /* configure the high-resolution time/callout interface */ hrt_init(); /* configure the DMA allocator */ dma_alloc_init(); /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); #endif /* set up the serial DMA polling */ static struct hrt_call serial_dma_call; struct timespec ts; /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. */ ts.tv_sec = 0; ts.tv_nsec = 1000000; hrt_call_every(&serial_dma_call, ts_to_abstime(&ts), ts_to_abstime(&ts), (hrt_callout)stm32_serial_dma_poll, NULL); /* initial LED state */ drv_led_start(); led_off(LED_AMBER); /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); if (!spi1) { message("[boot] FAILED to initialize SPI port 1\n"); up_ledon(LED_AMBER); return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_ICM, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); SPI_SELECT(spi1, PX4_SPIDEV_LIS, false); SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); SPI_SELECT(spi1, PX4_SPIDEV_EEPROM, false); up_udelay(20); /* Get the SPI port for the FRAM */ spi2 = up_spiinitialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); up_ledon(LED_AMBER); return -ENODEV; } /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) * and de-assert the known chip selects. */ // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); /* Configure SPI 5-based devices */ spi5 = up_spiinitialize(PX4_SPI_EXT0); if (!spi5) { message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_EXT0); up_ledon(LED_RED); return -ENODEV; } /* Default SPI5 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi5, 10000000); SPI_SETBITS(spi5, 8); SPI_SETMODE(spi5, SPIDEV_MODE3); SPI_SELECT(spi5, PX4_SPIDEV_EXT0, false); /* Configure SPI 6-based devices */ spi6 = up_spiinitialize(PX4_SPI_EXT1); if (!spi6) { message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_EXT1); up_ledon(LED_RED); return -ENODEV; } /* Default SPI6 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi6, 10000000); SPI_SETBITS(spi6, 8); SPI_SETMODE(spi6, SPIDEV_MODE3); SPI_SELECT(spi6, PX4_SPIDEV_EXT1, false); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); #endif return OK; }