__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)
{
/* 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)
{
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 init_once()
{
_shell_task_id = pthread_self();
//printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
work_queues_init();
hrt_work_queue_init();
hrt_init();
param_init();
}
__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;
}
void init_once(void)
{
_shell_task_id = pthread_self();
//printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
work_queues_init();
hrt_work_queue_init();
hrt_init();
#ifdef CONFIG_SHMEM
PX4_DEBUG("Syncing params to shared memory\n");
init_params();
#endif
}
void init_once(void)
{
// Required for QuRT
//_posix_init();
// _shell_task_id = pthread_self();
// PX4_INFO("Shell id is %lu", _shell_task_id);
work_queues_init();
hrt_work_queue_init();
hrt_init();
/* Shared memory param sync*/
init_params();
}
void init_once(void)
{
// Required for QuRT
//_posix_init();
PX4_WARN( "Before calling work_queue_init" );
// _shell_task_id = pthread_self();
// PX4_INFO("Shell id is %lu", _shell_task_id);
work_queues_init();
PX4_WARN( "Before calling hrt_init" );
hrt_work_queue_init();
hrt_init();
PX4_WARN( "after calling hrt_init" );
}
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));
/* default to 50 Hz PWM outputs */
system_state.servo_rate = 50;
/* configure the high-resolution time/callout interface */
hrt_init();
/* print some startup info */
lib_lowprintf("\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);
/* start the flight control signal handler */
task_create("FCon",
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)controls_main,
NULL);
struct mallinfo minfo = mallinfo();
lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
/* we're done here, go run the communications loop */
comms_main();
}
__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 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;
}
__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;
}
__EXPORT int nsh_archinitialize(void)
{
/* configure ADC pins */
stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */
stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */
stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */
stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */
#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 Sensors on SPI bus #3 */
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
board_led_on(LED_AMBER);
return -ENODEV;
}
/* Default: 1MHz, 8 bits, Mode 3 */
SPI_SETFREQUENCY(spi3, 10000000);
SPI_SETBITS(spi3, 8);
SPI_SETMODE(spi3, SPIDEV_MODE3);
SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi3, PX4_SPIDEV_BARO, false);
up_udelay(20);
message("[boot] Initialized SPI port 3 (SENSORS)\n");
/* Configure FRAM on SPI bus #4 */
spi4 = up_spiinitialize(4);
if (!spi4) {
message("[boot] FAILED to initialize SPI port 4\n");
board_led_on(LED_AMBER);
return -ENODEV;
}
/* Default: ~10MHz, 8 bits, Mode 3 */
SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000);
SPI_SETBITS(spi4, 8);
SPI_SETMODE(spi4, SPIDEV_MODE0);
SPI_SELECT(spi4, SPIDEV_FLASH, false);
message("[boot] Initialized SPI port 4 (FRAM)\n");
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 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;
}
__EXPORT int nsh_archinitialize(void)
{
int result;
message("\n");
/* configure always-on ADC pins */
stm32_configgpio(GPIO_ADC1_IN0);
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
stm32_configgpio(GPIO_UART_SBUS_INVERTER);
#ifdef CONFIG_RC_INPUTS_TYPE(RC_INPUT_SBUS)
stm32_gpiowrite(GPIO_UART_SBUS_INVERTER, 1);
#else
stm32_gpiowrite(GPIO_UART_SBUS_INVERTER, 0);
#endif
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* 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, GPIO_SPI_CS_MS5611, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_MS5611, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_MPU6000, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_HMC5983, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_WIFI_EEPROM, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
// message("[boot] Initializing Wireless Module\n");
// wireless_archinitialize();
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, GPIO_SPI_CS_MPU6000, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_MS5611, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_MPU6000, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_HMC5983, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_EEPROM, 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, GPIO_SPI_CS_DATAFLASH, false);
SPI_SELECT(spi3, GPIO_SPI_CS_EEPROM, false);
SPI_SELECT(spi3, GPIO_SPI_CS_SDCARD, 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;
}
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();
}
}
}
void init_once(void)
{
work_queues_init();
hrt_work_queue_init();
hrt_init();
}
__EXPORT int nsh_archinitialize(void)
{
int result;
message("\r\n");
/* configure ADC pins */
stm32_configgpio(GPIO_ADC1_IN10); /* BATT_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN11); /* BATT_CURRENT_SENS */
stm32_configgpio(GPIO_ADC1_IN14); /* SONAR */
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_SBUS_INV);
#ifdef GPIO_RC_OUT
stm32_configgpio(GPIO_RC_OUT); /* Serial RC output pin */
stm32_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */
#endif
/* 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);
stm32_configgpio(GPIO_GPIO6_OUTPUT);
stm32_configgpio(GPIO_GPIO7_OUTPUT);
stm32_configgpio(GPIO_GPIO8_OUTPUT);
stm32_configgpio(GPIO_GPIO9_OUTPUT);
stm32_configgpio(GPIO_GPI10_OUTPUT);
stm32_configgpio(GPIO_GPI11_OUTPUT);
stm32_configgpio(GPIO_GPI12_OUTPUT);
stm32_configgpio(GPIO_GPI13_OUTPUT);
stm32_configgpio(GPIO_GPI14_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_RED);
led_off(LED_GREEN);
led_off(LED_BLUE);
/* Configure SPI-based devices */
message("[boot] Initializing SPI port 1\r\n");
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\r\n");
up_ledon(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, SPIDEV_MS5611, false);
SPI_SELECT(spi1, SPIDEV_EXP_MS5611, false);
SPI_SELECT(spi1, SPIDEV_EXP_MPU6000, false);
SPI_SELECT(spi1, SPIDEV_EXP_HMC5983, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
message("[boot] Initializing SPI port 2\r\n");
spi2 = up_spiinitialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\r\n");
up_ledon(LED_RED);
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_MPU9250, false);
SPI_SELECT(spi2, SPIDEV_IMU_MS5611, false);
SPI_SELECT(spi2, SPIDEV_IMU_MPU6000, false);
SPI_SELECT(spi2, SPIDEV_IMU_HMC5983, false);
message("[boot] Successfully initialized SPI port 2\r\n");
/* Get the SPI port for the microSD slot */
#ifdef CONFIG_MMCSD
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_RED);
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\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");
led_on(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n");
#endif
return OK;
}
__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();
param_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);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
/* Configure SPI-based devices */
spi3 = px4_spibus_initialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
board_autoled_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, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi3, PX4_SPIDEV_BARO, false);
up_udelay(20);
/* Get the SPI port for the FRAM */
spi4 = px4_spibus_initialize(4);
if (!spi4) {
message("[boot] FAILED to initialize SPI port 4\n");
board_autoled_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI4 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(spi4, 12 * 1000 * 1000);
SPI_SETBITS(spi4, 8);
SPI_SETMODE(spi4, SPIDEV_MODE3);
SPI_SELECT(spi4, SPIDEV_FLASH(0), false);
return OK;
}
__EXPORT int nsh_archinitialize(void)
{
int result;
/* 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");
// initial LED state
drv_led_start();
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* 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;
}
// 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, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\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");
up_ledon(LED_AMBER);
return -ENODEV;
}
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");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
//stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available?
//stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
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;
}
__EXPORT int nsh_archinitialize(void)
{
int result;
/* configure always-on ADC pins */
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
/* IN12 and IN13 further below */
/* 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);
/* 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;
}
/* 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, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
/*
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
* Keep the SPI2 init optional and conditionally initialize the ADC pins
*/
#ifdef CONFIG_STM32_SPI2
spi2 = up_spiinitialize(2);
/* 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, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
#else
spi2 = NULL;
message("[boot] Enabling IN12/13 instead of SPI2\n");
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
#endif
/* Get the SPI port for the microSD slot */
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* 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");
up_ledon(LED_AMBER);
return -ENODEV;
}
return OK;
}
__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;
}
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();
i2c2 = up_i2cinitialize(2);
if (!i2c2) {
message("[boot] FAILED to initialize I2C bus 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C3 speed */
I2C_SETFREQUENCY(i2c2, 400000);
/* try to attach, don't fail if device is not responding */
(void)eeprom_attach(i2c2, 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 (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 */
/* 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
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();
}
}
}
__EXPORT int nsh_archinitialize(void)
{
int result;
message("\n");
/* configure always-on ADC pins */
stm32_configgpio(GPIO_ADC1_IN10); //VBAT
stm32_configgpio(GPIO_ADC1_IN11); //IBAT
stm32_configgpio(GPIO_ADC1_IN14); //SONAR_IN_1
stm32_configgpio(GPIO_ADC1_IN15); //SONAR_IN_2
stm32_configgpio(GPIO_USB_PRESENT);
stm32_configgpio(GPIO_SHUTDOWN);
stm32_configgpio(GPIO_SHUTDOWN_INT);
// stm32_configgpio(GPIO_AUX_OUT1);
// stm32_configgpio(GPIO_AUX_OUT2);
// stm32_configgpio(GPIO_AUX_OUT3);
// stm32_configgpio(GPIO_AUX_IN1);
// stm32_configgpio(GPIO_AUX_IN2);
stm32_configgpio(GPIO_AUX_IO1);
stm32_configgpio(GPIO_AUX_IO2);
stm32_configgpio(GPIO_AUX_IO3);
stm32_configgpio(GPIO_AUX_IO4);
stm32_configgpio(GPIO_AUX_IO5);
stm32_configgpio(GPIO_AUX_IO6);
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_off(LED_BLUE);
led_off(LED_GREEN);
message("[boot] Initializing USB detect\n");
stm32_usbinitialize();
/* 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, GPIO_SPI_CS_DATAFLASH, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EEPROM, false);
SPI_SELECT(spi1, GPIO_SPI_CS_DF_EXT1, false);
SPI_SELECT(spi1, GPIO_SPI_CS_DF_EXT2, false);
SPI_SELECT(spi1, GPIO_SPI_CS_DF_EXT3, false);
SPI_SELECT(spi1, GPIO_SPI_CS_DF_EXT4, false);
SPI_SELECT(spi1, GPIO_SPI_CS_DF_EXT5, false);
SPI_SELECT(spi1, GPIO_SPI_CS_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, GPIO_SPI_CS_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;
}
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)
{
#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 board_app_initialize(uintptr_t arg)
{
/* configure ADC pins */
/* configure power supply control/sense pins */
#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();
param_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
/* initial LED state */
drv_led_start();
led_on(LED_AMBER);
led_off(LED_AMBER);
/* Configure SPI-based devices */
spi0 = px4_spibus_initialize(PX4_SPI_BUS_SENSORS);
if (!spi0) {
message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS);
board_autoled_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi0, 10000000);
SPI_SETBITS(spi0, 8);
SPI_SETMODE(spi0, SPIDEV_MODE3);
SPI_SELECT(spi0, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi0, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi0, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi0, PX4_SPIDEV_MPU, false);
up_udelay(20);
#if defined(CONFIG_SAMV7_SPI1_MASTER)
spi1 = px4_spibus_initialize(PX4_SPI_BUS_MEMORY);
/* Default SPI4 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_EXT0, false);
SPI_SELECT(spi1, PX4_SPIDEV_EXT1, false);
#endif
#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;
}