aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorAsh Charles <ashcharles@gmail.com>2014-05-13 09:41:41 -0700
committerAsh Charles <ashcharles@gmail.com>2014-05-13 09:41:41 -0700
commite5508a1aa07fb4a0e76dfd077c5beaa114f21695 (patch)
treea7af9c3587c5eb57f70b3b5ce24f065b46605b6a /src
parent178a3e8567b3e721771fffcb8f32df140ad1038b (diff)
downloadpx4-firmware-e5508a1aa07fb4a0e76dfd077c5beaa114f21695.tar.gz
px4-firmware-e5508a1aa07fb4a0e76dfd077c5beaa114f21695.tar.bz2
px4-firmware-e5508a1aa07fb4a0e76dfd077c5beaa114f21695.zip
Add Gumstix AeroCore device
Based on the work of Andrew Smith [1], add board configuration and device drivers to support the Gumstix AeroCore (previously Aerodroid) board [2]. The AeroCore is an autopilot board based on a STM32F427 similar to the FMUv2. [1] https://github.com/smithandrewc/Firmware [2] https://store.gumstix.com/index.php/products/585/ Signed-off-by: Ash Charles <ashcharles@gmail.com>
Diffstat (limited to 'src')
-rw-r--r--src/drivers/boards/aerocore/aerocore_init.c282
-rw-r--r--src/drivers/boards/aerocore/aerocore_led.c121
-rw-r--r--src/drivers/boards/aerocore/aerocore_pwm_servo.c117
-rw-r--r--src/drivers/boards/aerocore/aerocore_spi.c183
-rw-r--r--src/drivers/boards/aerocore/board_config.h176
-rw-r--r--src/drivers/boards/aerocore/module.mk8
-rw-r--r--src/drivers/boards/px4fmu-v1/board_config.h2
-rw-r--r--src/drivers/boards/px4fmu-v2/board_config.h2
-rw-r--r--src/drivers/drv_gpio.h10
-rw-r--r--src/drivers/drv_gps.h4
-rw-r--r--src/drivers/gps/gps.cpp11
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp2
-rw-r--r--src/drivers/ms5611/ms5611_spi.cpp2
-rw-r--r--src/drivers/px4fmu/fmu.cpp84
-rw-r--r--src/drivers/stm32/adc/adc.cpp4
-rw-r--r--src/drivers/stm32/drv_hrt.c4
-rw-r--r--src/lib/version/version.h4
-rw-r--r--src/modules/commander/commander_helper.cpp2
-rw-r--r--src/modules/sensors/sensor_params.c9
-rw-r--r--src/modules/sensors/sensors.cpp10
-rw-r--r--src/systemcmds/mtd/mtd.c4
21 files changed, 1031 insertions, 10 deletions
diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c
new file mode 100644
index 000000000..4e3ba2d7e
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_init.c
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file aerocore_init.c
+ *
+ * AeroCore-specific early startup code. This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+# ifdef CONFIG_DEBUG
+# define message(...) lowsyslog(__VA_ARGS__)
+# else
+# define message(...) printf(__VA_ARGS__)
+# endif
+#else
+# ifdef CONFIG_DEBUG
+# define message lowsyslog
+# else
+# define message printf
+# endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+# error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/*
+ * The DMA heap size constrains the total number of things that can be
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file.
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+ dma_allocator = gran_initialize(g_dma_heap,
+ sizeof(g_dma_heap),
+ 7, /* 128B granule - must be > alignment (XXX bug?) */
+ 6); /* 64B alignment */
+ if (dma_allocator == NULL) {
+ message("[boot] DMA allocator setup FAILED");
+ } else {
+ g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+ }
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+ perf_count(g_dma_perf);
+ return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+ gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ * All STM32 architectures must provide the following entry point. This entry point
+ * is called early in the intitialization -- after all memory has been configured
+ * and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+ /* configure SPI interfaces */
+ stm32_spiinitialize();
+
+ /* configure LEDs */
+ up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ * Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi3;
+static struct spi_dev_s *spi4;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+ return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+ return 1;
+}
+#endif
+
+__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 */
+
+ /* 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");
+ up_ledon(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");
+ up_ledon(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;
+}
diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c
new file mode 100644
index 000000000..e40d1730c
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_led.c
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file aerocore_led.c
+ *
+ * AeroCore LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+ stm32_configgpio(GPIO_LED0);
+ stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+ switch (led) {
+ case 0:
+ stm32_gpiowrite(GPIO_LED0, true);
+ break;
+
+ case 1:
+ stm32_gpiowrite(GPIO_LED1, true);
+ break;
+
+ default:
+ warnx("LED ID not recognized\n");
+ }
+}
+
+__EXPORT void led_off(int led)
+{
+ switch (led) {
+ case 0:
+ stm32_gpiowrite(GPIO_LED0, false);
+ break;
+
+ case 1:
+ stm32_gpiowrite(GPIO_LED1, false);
+ break;
+
+ default:
+ warnx("LED ID not recognized\n");
+ }
+}
+
+__EXPORT void led_toggle(int led)
+{
+ switch (led) {
+ case 0:
+ if (stm32_gpioread(GPIO_LED0))
+ stm32_gpiowrite(GPIO_LED0, false);
+ else
+ stm32_gpiowrite(GPIO_LED0, true);
+ break;
+
+ case 1:
+ if (stm32_gpioread(GPIO_LED1))
+ stm32_gpiowrite(GPIO_LED1, false);
+ else
+ stm32_gpiowrite(GPIO_LED1, true);
+ break;
+
+ default:
+ warnx("LED ID not recognized\n");
+ }
+}
diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c
new file mode 100644
index 000000000..251eaff7b
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file aerocore_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+ {
+ .base = STM32_TIM1_BASE,
+ .clock_register = STM32_RCC_APB2ENR,
+ .clock_bit = RCC_APB2ENR_TIM1EN,
+ .clock_freq = STM32_APB2_TIM1_CLKIN
+ },
+ {
+ .base = STM32_TIM3_BASE,
+ .clock_register = STM32_RCC_APB1ENR,
+ .clock_bit = RCC_APB1ENR_TIM3EN,
+ .clock_freq = STM32_APB1_TIM3_CLKIN
+ }
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+ {
+ .gpio = GPIO_TIM1_CH1OUT,
+ .timer_index = 0,
+ .timer_channel = 1,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM1_CH2OUT,
+ .timer_index = 0,
+ .timer_channel = 2,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM1_CH3OUT,
+ .timer_index = 0,
+ .timer_channel = 3,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM1_CH4OUT,
+ .timer_index = 0,
+ .timer_channel = 4,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM3_CH1OUT,
+ .timer_index = 1,
+ .timer_channel = 1,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM3_CH2OUT,
+ .timer_index = 1,
+ .timer_channel = 2,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM3_CH3OUT,
+ .timer_index = 1,
+ .timer_channel = 3,
+ .default_value = 1500,
+ },
+ {
+ .gpio = GPIO_TIM3_CH4OUT,
+ .timer_index = 1,
+ .timer_channel = 4,
+ .default_value = 1500,
+ }
+};
diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c
new file mode 100644
index 000000000..e329bd9d1
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_spi.c
@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file aerocore_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ * Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+ stm32_configgpio(GPIO_SPI1_NSS);
+ stm32_gpiowrite(GPIO_SPI1_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+ stm32_configgpio(GPIO_SPI2_NSS);
+ stm32_gpiowrite(GPIO_SPI2_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+ stm32_configgpio(GPIO_SPI_CS_GYRO);
+ stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+ stm32_configgpio(GPIO_SPI_CS_BARO);
+
+ /* De-activate all peripherals,
+ * required for some peripheral
+ * state machines
+ */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+
+ stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+ stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+ stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+#endif
+
+#ifdef CONFIG_STM32_SPI4
+ stm32_configgpio(GPIO_SPI4_NSS);
+ stm32_gpiowrite(GPIO_SPI4_NSS, 1);
+#endif
+}
+
+#ifdef CONFIG_STM32_SPI1
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* there is only one device broken-out so select it */
+ stm32_gpiowrite(GPIO_SPI1_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* there is only one device broken-out so select it */
+ stm32_gpiowrite(GPIO_SPI2_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* SPI select is active low, so write !selected to select the device */
+
+ switch (devid) {
+ case PX4_SPIDEV_GYRO:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ break;
+
+ case PX4_SPIDEV_ACCEL_MAG:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+ break;
+
+ case PX4_SPIDEV_BARO:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+ break;
+
+ default:
+ break;
+ }
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
+#endif
+
+
+#ifdef CONFIG_STM32_SPI4
+__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* there can only be one device on this bus, so always select it */
+ stm32_gpiowrite(GPIO_SPI4_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ /* FRAM is always present */
+ return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
new file mode 100644
index 000000000..a3257f339
--- /dev/null
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * AeroCore internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START 0x1FFF7A10
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+/* LEDs */
+#define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10)
+
+/* Gyro */
+#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0)
+
+/* Accel & Mag */
+#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2)
+
+/* GPS */
+#define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4)
+
+/* SPI3--Sensors */
+#define PX4_SPI_BUS_SENSORS 3
+#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+
+/* Nominal chip selects for devices on SPI bus #3 */
+#define PX4_SPIDEV_ACCEL_MAG 0
+#define PX4_SPIDEV_GYRO 1
+#define PX4_SPIDEV_BARO 2
+
+/* User GPIOs broken out on J11 */
+#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1)
+//#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)
+
+#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+//#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+
+/* PWM
+ *
+ * Eight PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PA8 : TIM1_CH1
+ * CH2 : PA9 : TIM1_CH2
+ * CH3 : PA10 : TIM1_CH3
+ * CH4 : PA11 : TIM1_CH4
+ * CH5 : PC6 : TIM3_CH1
+ * CH6 : PC7 : TIM3_CH2
+ * CH7 : PC8 : TIM3_CH3
+ * CH8 : PC9 : TIM3_CH4
+ */
+#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1
+#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_1
+#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_1
+#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_1
+#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_3
+#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3
+#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2
+#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2
+
+/* High-resolution timer */
+#define HRT_TIMER 8 /* use timer 8 for the HRT */
+#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
+
+/* Tone Alarm (no onboard speaker )*/
+#define TONE_ALARM_TIMER 2 /* timer 2 */
+#define TONE_ALARM_CHANNEL 1 /* channel 1 */
+#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
+
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ * Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
new file mode 100644
index 000000000..b53fe0a29
--- /dev/null
+++ b/src/drivers/boards/aerocore/module.mk
@@ -0,0 +1,8 @@
+#
+# Board-specific startup code for the AeroCore
+#
+
+SRCS = aerocore_init.c \
+ aerocore_pwm_servo.c \
+ aerocore_spi.c \
+ aerocore_led.c
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 02c26b5c0..58273f2d2 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -85,6 +85,8 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+#define PX4_SPI_BUS_SENSORS 1
+
/*
* Use these in place of the spi_dev_e enumeration to
* select a specific SPI device on SPI1
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 7cfca7656..c2de1bfba 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -107,6 +107,8 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
+#define PX4_SPI_BUS_SENSORS 1
+
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index f60964c2b..5acd0d343 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -94,6 +94,14 @@
#endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+/*
+ * AeroCore GPIO numbers and configuration.
+ *
+ */
+# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
+#endif
+
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
/* no GPIO driver on the PX4IOv1 board */
#endif
@@ -146,4 +154,4 @@
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
-#endif /* _DRV_GPIO_H */ \ No newline at end of file
+#endif /* _DRV_GPIO_H */
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 06e3535b3..96669a021 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -46,7 +46,11 @@
#include "drv_sensor.h"
#include "drv_orb_dev.h"
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define GPS_DEFAULT_UART_PORT "/dev/ttyS0"
+#else
#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+#endif
#define GPS_DEVICE_PATH "/dev/gps"
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a902bdf2f..337705570 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -63,6 +63,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <board_config.h>
+
#include "ubx.h"
#include "mtk.h"
@@ -421,7 +423,14 @@ GPS::task_main()
void
GPS::cmd_reset()
{
- //XXX add reset?
+#ifdef GPIO_GPS_NRESET
+ warnx("Toggling GPS reset pin");
+ stm32_configgpio(GPIO_GPS_NRESET);
+ stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+ usleep(100);
+ stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+ warnx("Toggled GPS reset pin");
+#endif
}
void
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 4dee7649b..4ca8b5e42 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1793,7 +1793,7 @@ start()
errx(0, "already started");
/* create the driver */
- g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+ g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 26216e840..8759d16a1 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -117,7 +117,7 @@ private:
device::Device *
MS5611_spi_interface(ms5611::prom_u &prom_buf)
{
- return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+ return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4d72ead9b..1b6b136eb 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -92,6 +92,7 @@ public:
MODE_2PWM,
MODE_4PWM,
MODE_6PWM,
+ MODE_8PWM,
};
PX4FMU();
virtual ~PX4FMU();
@@ -113,6 +114,9 @@ private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
static const unsigned _max_actuators = 6;
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ static const unsigned _max_actuators = 8;
+#endif
Mode _mode;
unsigned _pwm_default_rate;
@@ -203,6 +207,21 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_VDD_5V_HIPOWER_OC, 0, 0},
{GPIO_VDD_5V_PERIPH_OC, 0, 0},
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ /* AeroCore breaks out User GPIOs on J11 */
+ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+ {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+// {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
+ {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
+ {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
+ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
+ {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0},
+ {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0},
+ {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0},
+ {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0},
+ {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0},
+ {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0},
+#endif
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -382,6 +401,20 @@ PX4FMU::set_mode(Mode mode)
break;
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
+ debug("MODE_8PWM");
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0xff);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+ break;
+#endif
+
case MODE_NONE:
debug("MODE_NONE");
@@ -602,6 +635,9 @@ PX4FMU::task_main()
num_outputs = 6;
break;
+ case MODE_8PWM:
+ num_outputs = 8;
+ break;
default:
num_outputs = 0;
break;
@@ -757,6 +793,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_2PWM:
case MODE_4PWM:
case MODE_6PWM:
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case MODE_8PWM:
+#endif
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -986,6 +1025,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case PWM_SERVO_SET(7):
+ case PWM_SERVO_SET(6):
+ if (_mode < MODE_8PWM) {
+ ret = -EINVAL;
+ break;
+ }
+#endif
+
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
@@ -1013,6 +1061,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case PWM_SERVO_GET(7):
+ case PWM_SERVO_GET(6):
+ if (_mode < MODE_8PWM) {
+ ret = -EINVAL;
+ break;
+ }
+#endif
+
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) {
@@ -1040,12 +1097,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_RATEGROUP(3):
case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5):
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case PWM_SERVO_GET_RATEGROUP(6):
+ case PWM_SERVO_GET_RATEGROUP(7):
+#endif
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ case MODE_8PWM:
+ *(unsigned *)arg = 8;
+ break;
+#endif
+
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
@@ -1091,6 +1158,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_6PWM);
break;
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ case 8:
+ set_mode(MODE_8PWM);
+ break;
+#endif
default:
ret = -EINVAL;
@@ -1181,10 +1253,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
unsigned count = len / 2;
uint16_t values[6];
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ if (count > 8) {
+ // we have at most 8 outputs
+ count = 8;
+ }
+#else
if (count > 6) {
// we have at most 6 outputs
count = 6;
}
+#endif
// allow for misaligned values
memcpy(values, buffer, count * 2);
@@ -1459,6 +1538,9 @@ fmu_new_mode(PortMode new_mode)
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
servo_mode = PX4FMU::MODE_6PWM;
#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+ servo_mode = PX4FMU::MODE_8PWM;
+#endif
break;
/* mixed modes supported on v1 board only */
@@ -1776,7 +1858,7 @@ fmu_main(int argc, char *argv[])
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 3a60d2cae..de13b8969 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -419,6 +419,10 @@ adc_main(int argc, char *argv[])
g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) |
(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
#endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ /* XXX this hardcodes the default channel set for AeroCore - should be configurable */
+ g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 5bb550279..281f918d7 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -141,7 +141,7 @@
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP
-# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN
+# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN
# if CONFIG_STM32_TIM10
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
# endif
@@ -150,7 +150,7 @@
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM
-# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN
+# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN
# if CONFIG_STM32_TIM11
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index af733aaf0..d8ccb6774 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,4 +59,8 @@
#define HW_ARCH "PX4FMU_V2"
#endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define HW_ARCH "AEROCORE"
+#endif
+
#endif /* VERSION_H_ */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 86f77cdb8..940a04aa1 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -206,7 +206,7 @@ int led_init()
return ERROR;
}
- /* the blue LED is only available on FMUv1 but not FMUv2 */
+ /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE);
/* we consider the amber led mandatory */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 07be3560a..c021e1fde 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#elif CONFIG_ARCH_BOARD_AEROCORE
+/**
+ * Scaling factor for battery voltage sensor on AeroCore.
+ *
+ * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
#else
/**
* Scaling factor for battery voltage sensor on FMU v1.
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 18bf97f8d..d31016111 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -126,6 +126,12 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define ADC_BATTERY_VOLTAGE_CHANNEL 10
+#define ADC_BATTERY_CURRENT_CHANNEL -1
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1
+#endif
+
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
@@ -797,7 +803,7 @@ Sensors::accel_init()
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
-#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -806,7 +812,7 @@ Sensors::accel_init()
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#else
-#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE
#endif
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index 0a88d40f3..a57eaafe7 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -160,7 +160,11 @@ static void
ramtron_attach(void)
{
/* find the right spi */
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+ struct spi_dev_s *spi = up_spiinitialize(4);
+#else
struct spi_dev_s *spi = up_spiinitialize(2);
+#endif
/* this resets the spi bus, set correct bus speed again */
SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
SPI_SETBITS(spi, 8);