diff --git a/boards/aerotenna/ocpoc/src/CMakeLists.txt b/boards/aerotenna/ocpoc/src/CMakeLists.txt index 8cc8e0994d91..42ca05a19b10 100644 --- a/boards/aerotenna/ocpoc/src/CMakeLists.txt +++ b/boards/aerotenna/ocpoc/src/CMakeLists.txt @@ -32,5 +32,6 @@ ############################################################################ add_library(drivers_board - empty.c + i2c.cpp + spi.cpp ) diff --git a/boards/aerotenna/ocpoc/src/board_config.h b/boards/aerotenna/ocpoc/src/board_config.h index 5c840e61cd86..6182198eef83 100644 --- a/boards/aerotenna/ocpoc/src/board_config.h +++ b/boards/aerotenna/ocpoc/src/board_config.h @@ -61,10 +61,11 @@ #define PX4_I2C_BUS_LED 1 // SPI +#include #define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) // spidev1.0 - mpu9250 -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) // spidev1.1 - ms5611 -//#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) // TODO: where is the 2nd mpu9250? +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) // spidev1.0 - mpu9250 +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) // spidev1.1 - ms5611 +//#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(0, 2) // TODO: where is the 2nd mpu9250? #define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS diff --git a/boards/aerotenna/ocpoc/src/empty.c b/boards/aerotenna/ocpoc/src/empty.c deleted file mode 100644 index e69de29bb2d1..000000000000 diff --git a/boards/aerotenna/ocpoc/src/i2c.cpp b/boards/aerotenna/ocpoc/src/i2c.cpp new file mode 100644 index 000000000000..eab2e5580d1a --- /dev/null +++ b/boards/aerotenna/ocpoc/src/i2c.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(2), // i2c-2: Air Data Probe or I2C Splitter + initI2CBusExternal(4), // i2c-4: GPS/Compass #1 + initI2CBusExternal(5), // i2c-5: GPS/Compass #2 + initI2CBusExternal(3), // i2c-3: GPS/Compass #3 +}; + diff --git a/boards/aerotenna/ocpoc/src/spi.cpp b/boards/aerotenna/ocpoc/src/spi.cpp new file mode 100644 index 000000000000..219d30ec6fc0 --- /dev/null +++ b/boards/aerotenna/ocpoc/src/spi.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, 0), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, 1), + }), +}; diff --git a/boards/airmind/mindpx-v2/src/CMakeLists.txt b/boards/airmind/mindpx-v2/src/CMakeLists.txt index ca5759063b3e..e28e360c2329 100644 --- a/boards/airmind/mindpx-v2/src/CMakeLists.txt +++ b/boards/airmind/mindpx-v2/src/CMakeLists.txt @@ -33,15 +33,17 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/airmind/mindpx-v2/src/board_config.h b/boards/airmind/mindpx-v2/src/board_config.h index a3cea3a654cc..4237c83b7fa8 100644 --- a/boards/airmind/mindpx-v2/src/board_config.h +++ b/boards/airmind/mindpx-v2/src/board_config.h @@ -118,13 +118,14 @@ #define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI4 */ -#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#include +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_L3GD20) +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(0, DRV_ACC_DEVTYPE_LSM303D) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) /* External bus */ -#define PX4_SPIDEV_EXT0 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 1) +#define PX4_SPIDEV_EXT0 PX4_MK_SPI_SEL(0, 0) /* I2C busses */ @@ -152,29 +153,6 @@ #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) -/* User GPIOs - * - * GPIO0-7 are the PWM servo outputs. - */ -#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) -#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) -#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) -#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) -#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12) -#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) -#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) - - -#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) -#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) -#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) -#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) -#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) -#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) - /* Power supply control and monitoring GPIOs */ // #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) // #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -285,7 +263,6 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); extern void stm32_usbinitialize(void); diff --git a/boards/airmind/mindpx-v2/src/i2c.cpp b/boards/airmind/mindpx-v2/src/i2c.cpp new file mode 100644 index 000000000000..5802883bfa13 --- /dev/null +++ b/boards/airmind/mindpx-v2/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(1), + initI2CBusExternal(2), +}; + diff --git a/boards/airmind/mindpx-v2/src/spi.c b/boards/airmind/mindpx-v2/src/spi.c deleted file mode 100644 index 4384d03475a3..000000000000 --- a/boards/airmind/mindpx-v2/src/spi.c +++ /dev/null @@ -1,249 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015, 2016 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 mindpx_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include -#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_SPI4 - px4_arch_configgpio(GPIO_SPI_CS_GYRO); - px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); - px4_arch_configgpio(GPIO_SPI_CS_BARO); - px4_arch_configgpio(GPIO_SPI_CS_MPU); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - - px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY); - px4_arch_configgpio(GPIO_EXTI_MAG_DRDY); - px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY); - px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); -#endif - -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); -#endif - -#ifdef CONFIG_STM32_SPI2 - px4_arch_configgpio(GPIO_SPI_CS_EXT0); -#endif -} - -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t 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); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 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); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - - px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - break; - - case PX4_SPIDEV_MPU: - /* 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, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - - px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - - px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF); - px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); - px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF); - px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF); - - px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); - - px4_arch_configgpio(GPIO_SPI4_SCK_OFF); - px4_arch_configgpio(GPIO_SPI4_MISO_OFF); - px4_arch_configgpio(GPIO_SPI4_MOSI_OFF); - - px4_arch_gpiowrite(GPIO_SPI4_SCK_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI4_MISO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); - - px4_arch_configgpio(GPIO_GYRO_DRDY_OFF); - px4_arch_configgpio(GPIO_MAG_DRDY_OFF); - px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF); - px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); - px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0); - px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); - px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); - - // /* set the sensor rail off */ - // stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - // stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - // - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - // - // /* re-enable power */ - // - // /* switch the sensor rail back on */ - // stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - // - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32_SPI4 - px4_arch_configgpio(GPIO_SPI_CS_GYRO); - px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); - px4_arch_configgpio(GPIO_SPI_CS_BARO); - px4_arch_configgpio(GPIO_SPI_CS_MPU); - - /* 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_gpiowrite(GPIO_SPI_CS_FRAM, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); - - // XXX bring up the EXTI pins again - // px4_arch_configgpio(GPIO_GYRO_DRDY); - // px4_arch_configgpio(GPIO_MAG_DRDY); - // px4_arch_configgpio(GPIO_ACCEL_DRDY); - -#endif - -} diff --git a/boards/airmind/mindpx-v2/src/spi.cpp b/boards/airmind/mindpx-v2/src/spi.cpp new file mode 100644 index 000000000000..3604e4d2ac18 --- /dev/null +++ b/boards/airmind/mindpx-v2/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortE, GPIO::Pin12}), + }), + initSPIBusExternal(2, { + initSPIConfigExternal(SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(4, { + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortB, GPIO::Pin2}, SPI::DRDY{GPIO::PortE, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortD, GPIO::Pin11}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin3}, SPI::DRDY{GPIO::PortE, GPIO::Pin10}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 532dff48628f..3b94afab437a 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -16,6 +16,7 @@ ms4525_airspeed -T 4515 -b 3 start if ! param greater SENS_EN_PMW3901 0 then # try to start adis16497 only if pmw3901 isn't enabled, or parameter doesn't exists + # TODO: this one is external SPI adis16497 start fi diff --git a/boards/av/x-v1/src/CMakeLists.txt b/boards/av/x-v1/src/CMakeLists.txt index 2fde5e91eca1..1eccb518cc63 100644 --- a/boards/av/x-v1/src/CMakeLists.txt +++ b/boards/av/x-v1/src/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_library(drivers_board + i2c.cpp init.c sdio.c spi.cpp @@ -40,6 +41,7 @@ add_library(drivers_board target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/av/x-v1/src/board_config.h b/boards/av/x-v1/src/board_config.h index 7493398403bf..c21013d726dc 100644 --- a/boards/av/x-v1/src/board_config.h +++ b/boards/av/x-v1/src/board_config.h @@ -123,21 +123,22 @@ #define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) +#include /* SPI1 */ -#define PX4_SPIDEV_ADIS16477 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSOR1,0) +#define PX4_SPIDEV_ADIS16477 PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_ADIS16477) #define PX4_SENSOR1_BUS_CS_GPIO {GPIO_SPI1_CS1_ADIS16477} /* SPI2 */ -#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,0) +#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(0,0) #define PX4_EXTERNAL1_BUS_CS_GPIO {SPI2_CS1_EXTERNAL1} /* SPI4 */ -#define PX4_SPIDEV_LPS22HB PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSOR4,0) +#define PX4_SPIDEV_LPS22HB PX4_MK_SPI_SEL(0,DRV_BARO_DEVTYPE_LPS22HB) #define PX4_SENSOR4_BUS_CS_GPIO {GPIO_SPI4_CS1_LPS22HB} /* SPI5 */ -#define PX4_SPIDEV_LSM303A_M PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSOR5,0) -#define PX4_SPIDEV_LSM303A_X PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSOR5,1) +#define PX4_SPIDEV_LSM303A_M PX4_MK_SPI_SEL(0,DRV_MAG_DEVTYPE_LSM303AGR) +#define PX4_SPIDEV_LSM303A_X PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_LSM303AGR) #define PX4_SENSOR5_BUS_CS_GPIO {GPIO_SPI5_CS1_LSM303A_M, GPIO_SPI5_CS1_LSM303A_X} /* I2C busses */ @@ -287,7 +288,6 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); #define board_peripheral_reset(ms) diff --git a/boards/av/x-v1/src/i2c.cpp b/boards/av/x-v1/src/i2c.cpp new file mode 100644 index 000000000000..165af3522308 --- /dev/null +++ b/boards/av/x-v1/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(2), + initI2CBusInternal(3), + initI2CBusExternal(4), +}; + diff --git a/boards/av/x-v1/src/spi.cpp b/boards/av/x-v1/src/spi.cpp index 317fe2d2956c..cc60f82d16eb 100644 --- a/boards/av/x-v1/src/spi.cpp +++ b/boards/av/x-v1/src/spi.cpp @@ -31,306 +31,25 @@ * ****************************************************************************/ -/** - * @file px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include - -/* Define CS GPIO array */ -static constexpr uint32_t spi1selects_gpio[] = PX4_SENSOR1_BUS_CS_GPIO; -static constexpr uint32_t spi2selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; -static constexpr uint32_t spi4selects_gpio[] = PX4_SENSOR4_BUS_CS_GPIO; -static constexpr uint32_t spi5selects_gpio[] = PX4_SENSOR5_BUS_CS_GPIO; - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32F7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI1 - - -#if defined(CONFIG_STM32F7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI2 - - -#ifdef CONFIG_STM32F7_SPI4 - - for (auto gpio : spi4selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI4 - - -#ifdef CONFIG_STM32F7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI5 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSOR1); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_EXTERNAL1); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI2 - -/************************************************************************************ - * Name: stm32_spi4select and stm32_spi4status - * - * Description: - * Called by stm32 spi driver on bus 4. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSOR4); - - // Making sure the other peripherals are not selected - for (auto cs : spi4selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi4selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI4 - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSOR5); - - // Making sure the other peripherals are not selected - for (auto cs : spi5selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI5 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int ms) -{ -#ifdef CONFIG_STM32F7_SPI1 - - // disable SPI1 - for (auto cs : spi1selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); -#endif // CONFIG_STM32F7_SPI1 - -#ifdef CONFIG_STM32F7_SPI2 - - // disable SPI2 - for (auto cs : spi2selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI2_SCK_OFF); - stm32_configgpio(GPIO_SPI2_MISO_OFF); - stm32_configgpio(GPIO_SPI2_MOSI_OFF); -#endif // CONFIG_STM32F7_SPI2 - -#ifdef CONFIG_STM32F7_SPI4 - - // disable SPI4 - for (auto cs : spi4selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI4_SCK_OFF); - stm32_configgpio(GPIO_SPI4_MISO_OFF); - stm32_configgpio(GPIO_SPI4_MOSI_OFF); -#endif // CONFIG_STM32F7_SPI4 - -#ifdef CONFIG_STM32F7_SPI5 - - // disable SPI5 - for (auto cs : spi5selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI5_SCK_OFF); - stm32_configgpio(GPIO_SPI5_MISO_OFF); - stm32_configgpio(GPIO_SPI5_MOSI_OFF); -#endif // CONFIG_STM32F7_SPI5 - - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - -#ifdef CONFIG_STM32F7_SPI1 - - // re-enable SPI1 - for (auto cs : spi1selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); -#endif // CONFIG_STM32F7_SPI1 - -#ifdef CONFIG_STM32F7_SPI2 - - // re-enable SPI2 - for (auto cs : spi2selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI2_SCK); - stm32_configgpio(GPIO_SPI2_MISO); - stm32_configgpio(GPIO_SPI2_MOSI); -#endif // CONFIG_STM32F7_SPI2 - -#ifdef CONFIG_STM32F7_SPI4 - - // re-enable SPI4 - for (auto cs : spi4selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); -#endif // CONFIG_STM32F7_SPI4 - -#ifdef CONFIG_STM32F7_SPI5 - // re-enable SPI5 - for (auto cs : spi5selects_gpio) { - stm32_configgpio(cs); - } +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_GYR_DEVTYPE_ADIS16477, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortJ, GPIO::Pin0}), + }), + initSPIBusExternal(2, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin0}), + }), + initSPIBus(4, { + initSPIDevice(DRV_BARO_DEVTYPE_LPS22HB, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortK, GPIO::Pin1}), + }), + initSPIBus(5, { + initSPIDevice(DRV_MAG_DEVTYPE_LSM303AGR, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortK, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_LSM303AGR, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); - stm32_configgpio(GPIO_SPI5_SCK); - stm32_configgpio(GPIO_SPI5_MISO); - stm32_configgpio(GPIO_SPI5_MOSI); -#endif // CONFIG_STM32F7_SPI5 -} diff --git a/boards/beaglebone/blue/src/CMakeLists.txt b/boards/beaglebone/blue/src/CMakeLists.txt index f8245bbdf271..315f2580d219 100644 --- a/boards/beaglebone/blue/src/CMakeLists.txt +++ b/boards/beaglebone/blue/src/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ px4_add_library(drivers_board + i2c.cpp init.cpp ) target_link_libraries(drivers_board PRIVATE robotics_cape) diff --git a/boards/beaglebone/blue/src/i2c.cpp b/boards/beaglebone/blue/src/i2c.cpp new file mode 100644 index 000000000000..abd240f7ae73 --- /dev/null +++ b/boards/beaglebone/blue/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), // i2c-1: pins P9 17,18 + initI2CBusInternal(2), // i2c-2: pins P9 19,20 - bmp280, mpu9250 +}; + diff --git a/boards/bitcraze/crazyflie/src/CMakeLists.txt b/boards/bitcraze/crazyflie/src/CMakeLists.txt index db9cd1ca4c78..d967b47016b0 100644 --- a/boards/bitcraze/crazyflie/src/CMakeLists.txt +++ b/boards/bitcraze/crazyflie/src/CMakeLists.txt @@ -32,14 +32,16 @@ ############################################################################ add_library(drivers_board + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start px4_layer ) diff --git a/boards/bitcraze/crazyflie/src/board_config.h b/boards/bitcraze/crazyflie/src/board_config.h index 3e039670c566..6e29ea966e61 100644 --- a/boards/bitcraze/crazyflie/src/board_config.h +++ b/boards/bitcraze/crazyflie/src/board_config.h @@ -121,9 +121,9 @@ #define PX4_FLOW_BUS_CS_GPIO { GPIO_SPI1_CS0_EXT, GPIO_SPI1_CS1_EXT, GPIO_SPI1_CS2_EXT } /* SPI1 Devices */ -#define PX4_SPIDEV_EXPANSION_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 0) // SD CARD BREAKOUT -#define PX4_SPIDEV_EXPANSION_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 1) // OPTICAL FLOW BREAKOUT -#define PX4_SPIDEV_EXPANSION_3 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 2) +#define PX4_SPIDEV_EXPANSION_1 SPIDEV_MMCSD(0) // SD CARD BREAKOUT +#define PX4_SPIDEV_EXPANSION_2 PX4_MK_SPI_SEL(0, DRV_FLOW_DEVTYPE_PMW3901) // OPTICAL FLOW BREAKOUT +#define PX4_SPIDEV_EXPANSION_3 PX4_MK_SPI_SEL(0, DRV_DEVTYPE_UNUSED) #define PX4_FLOW_BUS_FIRST_CS PX4_SPIDEV_EXPANSION_1 #define PX4_FLOW_BUS_LAST_CS PX4_SPIDEV_EXPANSION_3 @@ -261,8 +261,6 @@ extern void stm32_spiinitialize(void); extern int stm32_spi_bus_initialize(void); -void board_spi_reset(int ms); - #include diff --git a/boards/bitcraze/crazyflie/src/i2c.cpp b/boards/bitcraze/crazyflie/src/i2c.cpp new file mode 100644 index 000000000000..a1dcf2807d54 --- /dev/null +++ b/boards/bitcraze/crazyflie/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(3), +}; + diff --git a/boards/bitcraze/crazyflie/src/spi.c b/boards/bitcraze/crazyflie/src/spi.c deleted file mode 100644 index 64ec846972c4..000000000000 --- a/boards/bitcraze/crazyflie/src/spi.c +++ /dev/null @@ -1,165 +0,0 @@ -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -/* Define CS GPIO array */ -static const uint32_t spi1selects_gpio[] = PX4_FLOW_BUS_CS_GPIO; - - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - px4_gpio_init(spi1selects_gpio, arraySize(spi1selects_gpio)); -#endif - -} - -/************************************************************************************ - * Name: stm32_spi_bus_initialize - * - * Description: - * Called to configure SPI buses on PX4FMU board. - * - ************************************************************************************/ -static struct spi_dev_s *spi_expansion; - -__EXPORT int stm32_spi_bus_initialize(void) -{ - /* Configure SPI-based devices */ - - /* Get the external SPI port */ - spi_expansion = stm32_spibus_initialize(PX4_SPI_BUS_EXPANSION); - - if (!spi_expansion) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXPANSION); - return -ENODEV; - } - -#ifdef CONFIG_MMCSD - int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_expansion); - - if (ret != OK) { - syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); - return -ENODEV; - } - -#endif - - - return OK; - -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - int sel = (int) devid; - - /* Making sure the other peripherals are not selected */ - - for (size_t cs = 0; arraySize(spi1selects_gpio) > 1 && cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - stm32_gpiowrite(spi1selects_gpio[cs], 1); - } - } - - uint32_t gpio = spi1selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - stm32_gpiowrite(gpio, !selected); - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - for (size_t cs = 0; arraySize(spi1selects_gpio) > 1 && cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - stm32_configgpio(_PIN_OFF(spi1selects_gpio[cs])); - } - } - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - for (size_t cs = 0; arraySize(spi1selects_gpio) > 1 && cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - stm32_configgpio(spi1selects_gpio[cs]); - } - } - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - -} diff --git a/boards/intel/aerofc-v1/src/spi.c b/boards/bitcraze/crazyflie/src/spi.cpp similarity index 60% rename from boards/intel/aerofc-v1/src/spi.c rename to boards/bitcraze/crazyflie/src/spi.cpp index ecf85f85f162..a1c5a10965cd 100644 --- a/boards/intel/aerofc-v1/src/spi.c +++ b/boards/bitcraze/crazyflie/src/spi.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * Author: David Sidrane + * Copyright (C) 2020 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 @@ -32,58 +31,68 @@ * ****************************************************************************/ -/** - * @file spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - #include +#include #include #include #include +#include #include #include +#include +#include -#include "up_arch.h" -#include "chip.h" -#include "stm32.h" +#include +#include +#include #include "board_config.h" -/************************************************************************************ - * Public Functions - ************************************************************************************/ +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortC, GPIO::Pin12}), + initSPIDevice(DRV_FLOW_DEVTYPE_PMW3901, SPI::CS{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin5}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); /************************************************************************************ - * Name: stm32_spiinitialize + * Name: stm32_spi_bus_initialize * * Description: - * Called to configure SPI chip select GPIO pins for the AEROFC-v1 board. + * Called to configure SPI buses on PX4FMU board. * ************************************************************************************/ +static struct spi_dev_s *spi_expansion; -__EXPORT void stm32_spiinitialize(void) +__EXPORT int stm32_spi_bus_initialize(void) { -#ifdef CONFIG_STM32_SPI1 - px4_arch_configgpio(GPIO_SPI_CS_MPU6500); + /* Configure SPI-based devices */ -#endif -} + /* Get the external SPI port */ + spi_expansion = stm32_spibus_initialize(1); + if (!spi_expansion) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); + return -ENODEV; + } -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - px4_arch_gpiowrite(GPIO_SPI_CS_MPU6500, !selected); -} +#ifdef CONFIG_MMCSD + int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_expansion); -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; + if (ret != OK) { + syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); + return -ENODEV; + } + +#endif + + return OK; } diff --git a/boards/emlid/navio2/src/CMakeLists.txt b/boards/emlid/navio2/src/CMakeLists.txt index 8cc8e0994d91..42ca05a19b10 100644 --- a/boards/emlid/navio2/src/CMakeLists.txt +++ b/boards/emlid/navio2/src/CMakeLists.txt @@ -32,5 +32,6 @@ ############################################################################ add_library(drivers_board - empty.c + i2c.cpp + spi.cpp ) diff --git a/boards/emlid/navio2/src/board_config.h b/boards/emlid/navio2/src/board_config.h index 3aa36522d4fb..dc0f0dbf9124 100644 --- a/boards/emlid/navio2/src/board_config.h +++ b/boards/emlid/navio2/src/board_config.h @@ -58,11 +58,12 @@ // SPI +#include #define PX4_SPI_BUS_SENSORS 0 -#define PX4_SPIDEV_UBLOX PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) // spidev0.0 - ublox m8n -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) // spidev0.1 - mpu9250 -#define PX4_SPIDEV_LSM9DS1_M PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) // spidev0.2 - lsm9ds1 mag -#define PX4_SPIDEV_LSM9DS1_AG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) // spidev0.3 - lsm9ds1 accel/gyro +#define PX4_SPIDEV_UBLOX PX4_MK_SPI_SEL(0, 0) // spidev0.0 - ublox m8n +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) // spidev0.1 - mpu9250 +#define PX4_SPIDEV_LSM9DS1_M PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_ST_LSM9DS1_M) // spidev0.2 - lsm9ds1 mag +#define PX4_SPIDEV_LSM9DS1_AG PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ST_LSM9DS1_AG) // spidev0.3 - lsm9ds1 accel/gyro // ADC channels: diff --git a/boards/emlid/navio2/src/empty.c b/boards/emlid/navio2/src/empty.c deleted file mode 100644 index e69de29bb2d1..000000000000 diff --git a/boards/emlid/navio2/src/i2c.cpp b/boards/emlid/navio2/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/emlid/navio2/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/emlid/navio2/src/spi.cpp b/boards/emlid/navio2/src/spi.cpp new file mode 100644 index 000000000000..a5d046a65259 --- /dev/null +++ b/boards/emlid/navio2/src/spi.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(0, { + // spidev0.0 - ublox m8n + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, 1), + initSPIDevice(DRV_MAG_DEVTYPE_ST_LSM9DS1_M, 2), + initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG, 3), + }), +}; diff --git a/boards/holybro/durandal-v1/src/CMakeLists.txt b/boards/holybro/durandal-v1/src/CMakeLists.txt index cfac87b66666..2be96b170df9 100644 --- a/boards/holybro/durandal-v1/src/CMakeLists.txt +++ b/boards/holybro/durandal-v1/src/CMakeLists.txt @@ -46,6 +46,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") else() add_library(drivers_board can.c + i2c.cpp init.c led.c manifest.c @@ -58,6 +59,7 @@ else() target_link_libraries(drivers_board PRIVATE + arch_spi arch_board_hw_info drivers__led # drv_led_start nuttx_arch # sdio diff --git a/boards/holybro/durandal-v1/src/board_config.h b/boards/holybro/durandal-v1/src/board_config.h index a53d1b1be7a2..f69423b3954e 100644 --- a/boards/holybro/durandal-v1/src/board_config.h +++ b/boards/holybro/durandal-v1/src/board_config.h @@ -167,20 +167,21 @@ #define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_MEMORY /* ^ END Legacy SPI defines TODO: fix this with enumeration */ -#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) -#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,1) -#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,2) -#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,3) +#include +#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20689) +#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_BMI088) +#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_BMI088) +#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(0,DRV_DEVTYPE_UNUSED) #define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI1_CS1_ICM20689, GPIO_SPI1_CS3_BMI088_GYRO, GPIO_SPI1_CS4_BMI088_ACC, GPIO_SPI1_CS5_AUX_MEM} -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) #define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI2_CS_FRAM} -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO,0) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0,DRV_BARO_DEVTYPE_MS5611) #define PX4_BARO_BUS_CS_GPIO {GPIO_SPI4_CS1_MS5611} -#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,0) -#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,1) +#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(0,0) +#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(0,1) #define PX4_EXTERNAL1_BUS_CS_GPIO {SPI5_CS1_EXTERNAL1, SPI5_CS2_EXTERNAL1} #define PX4_SPIDEV_EXTERNAL2_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL2,0) @@ -333,7 +334,6 @@ #define GPIO_nVDD_5V_PERIPH_OC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) #define GPIO_nVDD_5V_HIPOWER_EN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) #define GPIO_nVDD_5V_HIPOWER_OC /* PG13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) -#define GPIO_VDD_3V3_SENSORS_EN /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define GPIO_VDD_3V3_SD_CARD_EN /* PG7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN7) @@ -342,7 +342,6 @@ #define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_PERIPH_EN, !(on_true)) #define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_HIPOWER_EN, !(on_true)) -#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, (on_true)) #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) #define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) @@ -474,7 +473,6 @@ GPIO_nVDD_5V_PERIPH_OC, \ GPIO_nVDD_5V_HIPOWER_EN, \ GPIO_nVDD_5V_HIPOWER_OC, \ - GPIO_VDD_3V3_SENSORS_EN, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_VDD_3V3_SD_CARD_EN, \ GPIO_TONE_ALARM_IDLE, \ @@ -522,8 +520,6 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/holybro/durandal-v1/src/i2c.cpp b/boards/holybro/durandal-v1/src/i2c.cpp new file mode 100644 index 000000000000..63e1172b7b69 --- /dev/null +++ b/boards/holybro/durandal-v1/src/i2c.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(3), + initI2CBusExternal(4), +}; + diff --git a/boards/holybro/durandal-v1/src/init.c b/boards/holybro/durandal-v1/src/init.c index 54c0468aba36..356953af1566 100644 --- a/boards/holybro/durandal-v1/src/init.c +++ b/boards/holybro/durandal-v1/src/init.c @@ -105,7 +105,7 @@ __EXPORT void board_peripheral_reset(int ms) /* set the peripheral rails off */ VDD_5V_PERIPH_EN(false); - VDD_3V3_SENSORS_EN(false); + board_control_spi_sensors_power(false, 0xffff); bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); /* Keep Spektum on to discharge rail*/ @@ -119,7 +119,7 @@ __EXPORT void board_peripheral_reset(int ms) /* switch the peripheral rail back on */ VDD_3V3_SPEKTRUM_POWER_EN(last); - VDD_3V3_SENSORS_EN(true); + board_control_spi_sensors_power(true, 0xffff); VDD_5V_PERIPH_EN(true); } @@ -170,9 +170,7 @@ stm32_boardinitialize(void) const uint32_t gpio[] = PX4_GPIO_INIT_LIST; px4_gpio_init(gpio, arraySize(gpio)); - /* configure SPI interfaces */ - - stm32_spiinitialize(); + board_control_spi_sensors_power_configgpio(); /* configure USB interfaces */ @@ -212,7 +210,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); - VDD_3V3_SENSORS_EN(true); + board_control_spi_sensors_power(true, 0xffff); VDD_3V3_SPEKTRUM_POWER_EN(true); /* Need hrt running before using the ADC */ @@ -228,6 +226,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); } + /* configure SPI interfaces (after we determined the HW version) */ + + stm32_spiinitialize(); + /* configure the DMA allocator */ if (board_dma_alloc_init() < 0) { diff --git a/boards/holybro/durandal-v1/src/spi.cpp b/boards/holybro/durandal-v1/src/spi.cpp index 5e8c60eb70b5..bcd3afb10e40 100644 --- a/boards/holybro/durandal-v1/src/spi.cpp +++ b/boards/holybro/durandal-v1/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2020 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 @@ -31,291 +31,33 @@ * ****************************************************************************/ -/** - * @file px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/* Define CS GPIO array */ -static constexpr uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; -static constexpr uint32_t spi2selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; -static constexpr uint32_t spi4selects_gpio[] = PX4_BARO_BUS_CS_GPIO; -static constexpr uint32_t spi5selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; -static constexpr uint32_t spi6selects_gpio[] = PX4_EXTERNAL2_BUS_CS_GPIO; - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32H7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32H7_SPI1 - - -#if defined(CONFIG_STM32H7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32H7_SPI2 - - -#ifdef CONFIG_STM32H7_SPI4 - - for (auto gpio : spi4selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32H7_SPI4 - - -#ifdef CONFIG_STM32H7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32H7_SPI5 - - -#ifdef CONFIG_STM32H7_SPI6 - - for (auto gpio : spi6selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32H7_SPI6 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32H7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32H7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32H7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == SPIDEV_FLASH(0)) { - devid = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_MEMORY); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32H7_SPI2 && GPIO_SPI2_CS_FRAM - -/************************************************************************************ - * Name: stm32_spi4select and stm32_spi4status - * - * Description: - * Called by stm32 spi driver on bus 4. - * - ************************************************************************************/ -#ifdef CONFIG_STM32H7_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_BARO); - - // Making sure the other peripherals are not selected - for (auto cs : spi4selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi4selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32H7_SPI4 - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ -#ifdef CONFIG_STM32H7_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_EXTERNAL1); - - // Making sure the other peripherals are not selected - for (auto cs : spi5selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32H7_SPI5 - -/************************************************************************************ - * Name: stm32_spi6select and stm32_spi6status - * - * Description: - * Called by stm32 spi driver on bus 6. - * - ************************************************************************************/ -#ifdef CONFIG_STM32H7_SPI6 -__EXPORT void stm32_spi6select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_EXTERNAL2); - - // Making sure the other peripherals are not selected - for (auto cs : spi6selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi6selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32H7_SPI6 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int ms) -{ - // disable SPI bus - for (auto cs : spi1selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY1_ICM20689); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY2_BMI088_GYRO); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY3_BMI088_ACC); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY4_ICM20602); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY5_BMI088_GYRO); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY6_BMI088_ACC); -#endif - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - for (auto cs : spi1selects_gpio) { - stm32_configgpio(cs); - } - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + }), + initSPIBusExternal(6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI1_DRDY1_ICM20689); - stm32_configgpio(GPIO_SPI1_DRDY2_BMI055_GYRO); - stm32_configgpio(GPIO_SPI1_DRDY3_BMI055_ACC); - stm32_configgpio(GPIO_SPI1_DRDY4_ICM20602); - stm32_configgpio(GPIO_SPI1_DRDY5_BMI055_GYRO); - stm32_configgpio(GPIO_SPI1_DRDY6_BMI055_ACC); -#endif -} diff --git a/boards/holybro/kakutef7/src/CMakeLists.txt b/boards/holybro/kakutef7/src/CMakeLists.txt index 8b8eb8952150..d4a02322dc78 100644 --- a/boards/holybro/kakutef7/src/CMakeLists.txt +++ b/boards/holybro/kakutef7/src/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_library(drivers_board + i2c.cpp init.c led.c spi.cpp @@ -41,6 +42,7 @@ add_library(drivers_board target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/holybro/kakutef7/src/board_config.h b/boards/holybro/kakutef7/src/board_config.h index cd3fc40f8de4..744f4bf0d94c 100644 --- a/boards/holybro/kakutef7/src/board_config.h +++ b/boards/holybro/kakutef7/src/board_config.h @@ -70,6 +70,8 @@ #define PX4_SPI_BUS_OSD 2 #define PX4_SPI_BUS_SENSORS 4 +#include + /* Define the Chip Selects, Data Ready and Control signals per SPI bus */ #define GPIO_SPI1_CS1_SD_CARD /* PA4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) @@ -77,7 +79,7 @@ #define PX4_SD_CARD_BUS_CS_GPIO {GPIO_SPI1_CS1_SD_CARD} #define GPIO_SPI2_CS1_OSD /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12) -#define PX4_SPIDEV_OSD 1 +#define PX4_SPIDEV_OSD PX4_MK_SPI_SEL(0,DRV_OSD_DEVTYPE_ATXXXX) #define PX4_OSD_BUS_CS_GPIO {GPIO_SPI2_CS1_OSD} @@ -91,8 +93,8 @@ #define GPIO_SPI4_DRDY1_ICM20689 /* PE1 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN1) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) -#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_MPU6000) +#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20689) #define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI4_CS1_ICM20689} /* I2C busses */ @@ -206,8 +208,6 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/holybro/kakutef7/src/i2c.cpp b/boards/holybro/kakutef7/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/holybro/kakutef7/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/holybro/kakutef7/src/spi.cpp b/boards/holybro/kakutef7/src/spi.cpp index 03296da18c7f..8d3b7192abb3 100644 --- a/boards/holybro/kakutef7/src/spi.cpp +++ b/boards/holybro/kakutef7/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2020 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 @@ -31,181 +31,22 @@ * ****************************************************************************/ -/** - * @file spi.cpp - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/* Define CS GPIO array */ -static constexpr uint32_t spi1selects_gpio[] = PX4_SD_CARD_BUS_CS_GPIO; -static constexpr uint32_t spi2selects_gpio[] = PX4_OSD_BUS_CS_GPIO; -static constexpr uint32_t spi4selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32F7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI1 - - -#if defined(CONFIG_STM32F7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI2 - - -#ifdef CONFIG_STM32F7_SPI4 - - for (auto gpio : spi4selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI4 - - -#ifdef CONFIG_STM32F7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI5 +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortA, GPIO::Pin4}) + }), + initSPIBus(2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); -#ifdef CONFIG_STM32F7_SPI6 - - for (auto gpio : spi6selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI6 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(devid == SPIDEV_MMCSD(0)); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_OSD); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI2 && GPIO_SPI2_CS_FRAM - -/************************************************************************************ - * Name: stm32_spi4select and stm32_spi4status - * - * Description: - * Called by stm32 spi driver on bus 4. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS); - - // Making sure the other peripherals are not selected - for (auto cs : spi4selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi4selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI4 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int ms) -{ -} diff --git a/boards/intel/aerofc-v1/src/CMakeLists.txt b/boards/intel/aerofc-v1/src/CMakeLists.txt index f121e41cc77b..b94b7e828943 100644 --- a/boards/intel/aerofc-v1/src/CMakeLists.txt +++ b/boards/intel/aerofc-v1/src/CMakeLists.txt @@ -33,13 +33,15 @@ ############################################################################ add_library(drivers_board + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start px4_layer modules__dataman # dm_flash_sector_description_set # TODO: fix this diff --git a/boards/intel/aerofc-v1/src/board_config.h b/boards/intel/aerofc-v1/src/board_config.h index fc744bfd1b72..6359fec1e909 100644 --- a/boards/intel/aerofc-v1/src/board_config.h +++ b/boards/intel/aerofc-v1/src/board_config.h @@ -87,9 +87,10 @@ #define PX4_I2C_BUS_EXPANSION1 2 #define PX4_I2C_BUS_ONBOARD 3 +#include #define GPIO_SPI_CS_MPU6500 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) /* * STM32 ADC channels @@ -167,7 +168,6 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); -#define board_spi_reset(ms) #define board_peripheral_reset(ms) /************************************************************************************ diff --git a/boards/intel/aerofc-v1/src/i2c.cpp b/boards/intel/aerofc-v1/src/i2c.cpp new file mode 100644 index 000000000000..a79588da4671 --- /dev/null +++ b/boards/intel/aerofc-v1/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(3), +}; diff --git a/boards/intel/aerofc-v1/src/spi.cpp b/boards/intel/aerofc-v1/src/spi.cpp new file mode 100644 index 000000000000..37a0b47e3e45 --- /dev/null +++ b/boards/intel/aerofc-v1/src/spi.cpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortA, GPIO::Pin4}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/modalai/fc-v1/src/CMakeLists.txt b/boards/modalai/fc-v1/src/CMakeLists.txt index f1244df5782e..3ccb6124bf3a 100644 --- a/boards/modalai/fc-v1/src/CMakeLists.txt +++ b/boards/modalai/fc-v1/src/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c manifest.c @@ -45,6 +46,7 @@ add_dependencies(drivers_board arch_board_hw_info) target_link_libraries(drivers_board PRIVATE + arch_spi arch_board_hw_info drivers__led # drv_led_start nuttx_arch # sdio diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index 903c4a032488..176178592311 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -181,17 +181,19 @@ #define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) #define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_MEMORY -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS1,0) +#include + +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20602) #define PX4_SENSORS1_BUS_CS_GPIO {GPIO_SPI1_nCS1_ICM20602} -#define PX4_SPIDEV_ICM_42688 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS2,0) +#define PX4_SPIDEV_ICM_42688 PX4_MK_SPI_SEL(0,DRV_DEVTYPE_UNUSED) #define PX4_SENSORS2_BUS_CS_GPIO {GPIO_SPI2_nCS1_ICM_42688} -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) #define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI5_nCS1_FRAM} -#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS3,0) -#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS3,1) +#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_BMI088) +#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_BMI088) #define PX4_SENSORS3_BUS_CS_GPIO {GPIO_SPI6_nCS2_BMI088, GPIO_SPI6_nCS1_BMI088} /* I2C busses */ @@ -451,8 +453,6 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/modalai/fc-v1/src/i2c.cpp b/boards/modalai/fc-v1/src/i2c.cpp new file mode 100644 index 000000000000..65211836db96 --- /dev/null +++ b/boards/modalai/fc-v1/src/i2c.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusExternal(3), + initI2CBusInternal(4), +}; + diff --git a/boards/modalai/fc-v1/src/spi.cpp b/boards/modalai/fc-v1/src/spi.cpp index adcad87e1075..9150ac5c29d8 100644 --- a/boards/modalai/fc-v1/src/spi.cpp +++ b/boards/modalai/fc-v1/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2020 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 @@ -31,463 +31,26 @@ * ****************************************************************************/ -/** - * @file spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/* Define CS GPIO array */ -#ifdef CONFIG_STM32F7_SPI1 -static constexpr uint32_t spi1selects_gpio[] = PX4_SENSORS1_BUS_CS_GPIO; -#endif -#ifdef CONFIG_STM32F7_SPI2 -static constexpr uint32_t spi2selects_gpio[] = PX4_SENSORS2_BUS_CS_GPIO; -#endif -#ifdef CONFIG_STM32F7_SPI3 -static constexpr uint32_t spi3selects_gpio[] = 0; // Not used -#endif -#ifdef CONFIG_STM32F7_SPI4 -static constexpr uint32_t spi4selects_gpio[] = 0; // Not used -#endif -#ifdef CONFIG_STM32F7_SPI5 -static constexpr uint32_t spi5selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; -#endif -#ifdef CONFIG_STM32F7_SPI6 -static constexpr uint32_t spi6selects_gpio[] = PX4_SENSORS3_BUS_CS_GPIO; -#endif - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32F7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI1 - -#if defined(CONFIG_STM32F7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI2 - -#if defined(CONFIG_STM32F7_SPI3) - - for (auto gpio : spi3selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI3 - - -#ifdef CONFIG_STM32F7_SPI4 - - for (auto gpio : spi4selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI4 - - -#ifdef CONFIG_STM32F7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI5 - - -#ifdef CONFIG_STM32F7_SPI6 - - for (auto gpio : spi6selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI6 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS1); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS2); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI2 - -/************************************************************************************ - * Name: stm32_spi3select and stm32_spi3status - * - * Description: - * Called by stm32 spi driver on bus 3. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI3) -__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS3); - - // Making sure the other peripherals are not selected - for (auto cs : spi3selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi3selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI3 - -/************************************************************************************ - * Name: stm32_spi4select and stm32_spi4status - * - * Description: - * Called by stm32 spi driver on bus 4. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS3); - - // Making sure the other peripherals are not selected - for (auto cs : spi4selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi4selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI4 - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == SPIDEV_FLASH(0)) { - devid = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_MEMORY); - - // Making sure the other peripherals are not selected - for (auto cs : spi5selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI5 - -/************************************************************************************ - * Name: stm32_spi6select and stm32_spi6status - * - * Description: - * Called by stm32 spi driver on bus 6. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI6 -__EXPORT void stm32_spi6select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS3); - - // Making sure the other peripherals are not selected - for (auto cs : spi6selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi6selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI6 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * TODO:Add 4 bit MASK active LOW for Bus 1-4 - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int mask_ms) -{ - int ms = mask_ms & 0x00ffffff; - int mask = ((mask_ms & 0xff000000) >> 24) ^ 0xff; - - // disable SPI bus - -#ifdef CONFIG_STM32F7_SPI1 - - if (mask & 1) { - for (auto cs : spi1selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY1_ICM20602); -#endif - - /* set the sensor rail off */ - - // NA, not controlling rails in ModalAI FC-v1 - - } - -#endif - -#ifdef CONFIG_STM32F7_SPI2 - - if (mask & 2) { - for (auto cs : spi2selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI2_SCK_OFF); - stm32_configgpio(GPIO_SPI2_MISO_OFF); - stm32_configgpio(GPIO_SPI2_MOSI_OFF); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI2_DRDY1_ISM330); -#endif - /* set the sensor rail off */ - - // NA, not controlling rails in ModalAI FC-v1 - } - -#endif - -#ifdef CONFIG_STM32F7_SPI3 - - if (mask & 4) { - for (auto cs : spi3selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI3_SCK_OFF); - stm32_configgpio(GPIO_SPI3_MISO_OFF); - stm32_configgpio(GPIO_SPI3_MOSI_OFF); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI3_DRDY1_BMI088); - stm32_configgpio(GPIO_DRDY_OFF_SPI3_DRDY2_BMI088); -#endif - /* set the sensor rail off */ - - // NA, not controlling rails in ModalAI FC-v1 - } - -#endif - -#ifdef CONFIG_STM32F7_SPI4 - - if (mask & 8) { - for (auto cs : spi4selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI4_SCK_OFF); - stm32_configgpio(GPIO_SPI4_MISO_OFF); - stm32_configgpio(GPIO_SPI4_MOSI_OFF); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI4_DRDY1_BMM150); -#endif - /* set the sensor rail off */ - - // NA, not controlling rails in ModalAI FC-v1 - } - -#endif - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - // NA, not controlling rails in ModalAI FC-v1 - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - -#ifdef CONFIG_STM32F7_SPI1 - - if (mask & 1) { - /* reconfigure the SPI pins */ - for (auto cs : spi1selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI1_DRDY1_ICM20602); -#endif - - } - -#endif - -#ifdef CONFIG_STM32F7_SPI2 - - if (mask & 2) { - /* reconfigure the SPI pins */ - for (auto cs : spi2selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI2_SCK); - stm32_configgpio(GPIO_SPI2_MISO); - stm32_configgpio(GPIO_SPI2_MOSI); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI2_DRDY1_ISM330); -#endif - } - -#endif - -#ifdef CONFIG_STM32F7_SPI3 - - if (mask & 4) { - /* reconfigure the SPI pins */ - for (auto cs : spi3selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI3_SCK); - stm32_configgpio(GPIO_SPI3_MISO); - stm32_configgpio(GPIO_SPI3_MOSI); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI3_DRDY1_BMI088); - stm32_configgpio(GPIO_SPI3_DRDY2_BMI088); -#endif - } - -#endif - -#ifdef CONFIG_STM32F7_SPI4 - - if (mask & 8) { - /* reconfigure the SPI pins */ - for (auto cs : spi4selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI4_DRDY1_BMM150); -#endif - } +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }), + initSPIBus(2, { + // ICM-42688 + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }), + initSPIBus(5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBus(6, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); -#endif -} diff --git a/boards/mro/ctrl-zero-f7/src/CMakeLists.txt b/boards/mro/ctrl-zero-f7/src/CMakeLists.txt index 6cac6bd89eb9..a533df7c69f0 100644 --- a/boards/mro/ctrl-zero-f7/src/CMakeLists.txt +++ b/boards/mro/ctrl-zero-f7/src/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(drivers_board init.c + i2c.cpp led.c sdio.c spi.cpp @@ -42,6 +43,7 @@ add_library(drivers_board target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/mro/ctrl-zero-f7/src/board_config.h b/boards/mro/ctrl-zero-f7/src/board_config.h index 4e6aea6a06bf..5eec0042c075 100644 --- a/boards/mro/ctrl-zero-f7/src/board_config.h +++ b/boards/mro/ctrl-zero-f7/src/board_config.h @@ -103,16 +103,17 @@ #define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_2 /* ^ END Legacy SPI defines TODO: fix this with enumeration */ -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_1,0) -#define PX4_SPIDEV_ICM_20948 PX4_MK_SPI_SEL(PX4_SPI_BUS_1,1) +#include +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20602) +#define PX4_SPIDEV_ICM_20948 PX4_MK_SPI_SEL(0,DRV_DEVTYPE_UNUSED) #define PX4_SPI_BUS_1_CS_GPIO {GPIO_SPI1_CS1_ICM20602, GPIO_SPI1_CS2_ICM20948} -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_2,0) -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_2,1) +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0,DRV_DEVTYPE_DPS310) #define PX4_SPI_BUS_2_CS_GPIO {GPIO_SPI2_CS1_FRAM, GPIO_SPI2_CS2_BARO} -#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_5,0) -#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_5,1) +#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_BMI088) +#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_BMI088) #define PX4_SPI_BUS_5_CS_GPIO {GPIO_SPI5_CS1_BMI088_ACCEL, GPIO_SPI5_CS2_BMI088_GYRO} @@ -176,12 +177,10 @@ #define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define BOARD_NUMBER_BRICKS 1 -#define GPIO_VDD_3V3_SENSORS_EN /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) /* Define True logic Power Control in arch agnostic form */ -#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, (on_true)) #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) @@ -260,7 +259,6 @@ GPIO_CAN1_RX, \ GPIO_CAN1_SILENT_S0, \ GPIO_nPOWER_IN_A, \ - GPIO_VDD_3V3_SENSORS_EN, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_TONE_ALARM_IDLE, \ GPIO_SAFETY_SWITCH_IN, \ @@ -298,8 +296,6 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/mro/ctrl-zero-f7/src/i2c.cpp b/boards/mro/ctrl-zero-f7/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/mro/ctrl-zero-f7/src/init.c b/boards/mro/ctrl-zero-f7/src/init.c index 4b05132747e6..2e38e18b023f 100644 --- a/boards/mro/ctrl-zero-f7/src/init.c +++ b/boards/mro/ctrl-zero-f7/src/init.c @@ -101,7 +101,7 @@ __END_DECLS __EXPORT void board_peripheral_reset(int ms) { /* set the peripheral rails off */ - VDD_3V3_SENSORS_EN(false); + board_control_spi_sensors_power(false, 0xffff); bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); /* Keep Spektum on to discharge rail*/ @@ -115,7 +115,7 @@ __EXPORT void board_peripheral_reset(int ms) /* switch the peripheral rail back on */ VDD_3V3_SPEKTRUM_POWER_EN(last); - VDD_3V3_SENSORS_EN(true); + board_control_spi_sensors_power(true, 0xffff); } /************************************************************************************ @@ -198,7 +198,7 @@ stm32_boardinitialize(void) __EXPORT int board_app_initialize(uintptr_t arg) { /* Power on Interfaces */ - VDD_3V3_SENSORS_EN(true); + board_control_spi_sensors_power(true, 0xffff); VDD_3V3_SPEKTRUM_POWER_EN(true); px4_platform_init(); diff --git a/boards/mro/ctrl-zero-f7/src/spi.cpp b/boards/mro/ctrl-zero-f7/src/spi.cpp index 92d306f1f1c4..90a71ec830dd 100644 --- a/boards/mro/ctrl-zero-f7/src/spi.cpp +++ b/boards/mro/ctrl-zero-f7/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2020 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 @@ -31,234 +31,25 @@ * ****************************************************************************/ -/** - * @file spi.cpp - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/* Define CS GPIO array */ -static constexpr uint32_t spi1selects_gpio[] = PX4_SPI_BUS_1_CS_GPIO; -static constexpr uint32_t spi2selects_gpio[] = PX4_SPI_BUS_2_CS_GPIO; -static constexpr uint32_t spi5selects_gpio[] = PX4_SPI_BUS_5_CS_GPIO; - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32F7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI1 - - -#if defined(CONFIG_STM32F7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI2 - -#ifdef CONFIG_STM32F7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI5 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_1); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == SPIDEV_FLASH(0)) { - devid = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_2); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI2 && GPIO_SPI2_CS_FRAM - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_5); - - // Making sure the other peripherals are not selected - for (auto cs : spi5selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI5 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int ms) -{ - // disable SPI bus - - // SPI1 - for (auto cs : spi1selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(_PIN_OFF(GPIO_SPI1_SCK)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_MISO)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_MOSI)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_DRDY1_ICM20602)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_DRDY2_ICM20948)); - - - // SPI5 - for (auto cs : spi5selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(_PIN_OFF(GPIO_SPI5_SCK)); - stm32_configgpio(_PIN_OFF(GPIO_SPI5_MISO)); - stm32_configgpio(_PIN_OFF(GPIO_SPI5_MOSI)); - stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT1_ACCEL)); - stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT2_ACCEL)); - stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT3_GYRO)); - stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT4_GYRO)); - - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - - // SPI1 - for (auto cs : spi1selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - stm32_configgpio(GPIO_SPI1_DRDY1_ICM20602); - stm32_configgpio(GPIO_SPI1_DRDY2_ICM20948); - - // SPI5 - for (auto cs : spi5selects_gpio) { - stm32_configgpio(cs); - } +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + // ICM-20948 + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }, {GPIO::PortE, GPIO::Pin3}), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); - stm32_configgpio(GPIO_SPI5_SCK); - stm32_configgpio(GPIO_SPI5_MISO); - stm32_configgpio(GPIO_SPI5_MOSI); - stm32_configgpio(GPIO_DRDY_BMI088_INT1_ACCEL); - stm32_configgpio(GPIO_DRDY_BMI088_INT2_ACCEL); - stm32_configgpio(GPIO_DRDY_BMI088_INT3_GYRO); - stm32_configgpio(GPIO_DRDY_BMI088_INT4_GYRO); -} diff --git a/boards/mro/x21-777/src/CMakeLists.txt b/boards/mro/x21-777/src/CMakeLists.txt index e4b502dacf52..76ac1a835eb0 100644 --- a/boards/mro/x21-777/src/CMakeLists.txt +++ b/boards/mro/x21-777/src/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(drivers_board init.c + i2c.cpp led.c sdio.c spi.cpp @@ -43,6 +44,7 @@ add_dependencies(drivers_board arch_board_hw_info) target_link_libraries(drivers_board PRIVATE + arch_spi arch_board_hw_info drivers__led # drv_led_start nuttx_arch # sdio diff --git a/boards/mro/x21-777/src/board_config.h b/boards/mro/x21-777/src/board_config.h index 3bc35f0f9e8f..8eb6a8c4c910 100644 --- a/boards/mro/x21-777/src/board_config.h +++ b/boards/mro/x21-777/src/board_config.h @@ -107,10 +107,11 @@ #define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#include +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20602) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) /* I2C busses */ @@ -219,7 +220,7 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); +extern void board_peripheral_reset(int ms); extern void stm32_usbinitialize(void); diff --git a/boards/mro/x21-777/src/i2c.cpp b/boards/mro/x21-777/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/mro/x21-777/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/mro/x21-777/src/spi.cpp b/boards/mro/x21-777/src/spi.cpp index 46ddb85b5fe2..9c22f3251d8b 100644 --- a/boards/mro/x21-777/src/spi.cpp +++ b/boards/mro/x21-777/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2020 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 @@ -31,167 +31,21 @@ * ****************************************************************************/ -/** - * @file spi.cpp - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32F7_SPI1 - px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X); - px4_arch_configgpio(GPIO_SPI_CS_BARO); - px4_arch_configgpio(GPIO_SPI_CS_MPU); - - px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); - px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY); -#endif /* CONFIG_STM32F7_SPI1 */ - -#ifdef CONFIG_STM32F7_SPI2 - px4_arch_configgpio(GPIO_SPI_CS_FRAM); -#endif /* CONFIG_STM32F7_SPI2 */ - -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - - /* intended fallthrough */ - case PX4_SPIDEV_ICM_20602: - - /* intended fallthrough */ - case PX4_SPIDEV_ICM_20608: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, !selected); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - break; - - case PX4_SPIDEV_MPU: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - -#ifdef CONFIG_STM32F7_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif /* CONFIG_STM32F7_SPI2 */ - - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X_OFF); - px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF); - px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF); - - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); - - px4_arch_configgpio(GPIO_SPI1_SCK_OFF); - px4_arch_configgpio(GPIO_SPI1_MISO_OFF); - px4_arch_configgpio(GPIO_SPI1_MOSI_OFF); - - px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - - px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY_OFF); - px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - px4_arch_gpiowrite(GPIO_EXTI_ICM_2060X_DRDY_OFF, 0); - px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); - - /* set the sensor rail off */ - px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); - px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32F7_SPI1 - px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X); - px4_arch_configgpio(GPIO_SPI_CS_BARO); - px4_arch_configgpio(GPIO_SPI_CS_MPU); - px4_arch_configgpio(GPIO_SPI1_SCK); - px4_arch_configgpio(GPIO_SPI1_MISO); - px4_arch_configgpio(GPIO_SPI1_MOSI); +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + }), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + }), +}; - // // XXX bring up the EXTI pins again - // px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); - // px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY); +static constexpr bool unused = validateSPIConfig(px4_spi_buses); -#endif /* CONFIG_STM32F7_SPI1 */ -} diff --git a/boards/mro/x21/src/CMakeLists.txt b/boards/mro/x21/src/CMakeLists.txt index 26cde8bf5cf7..a7db4590f0da 100644 --- a/boards/mro/x21/src/CMakeLists.txt +++ b/boards/mro/x21/src/CMakeLists.txt @@ -33,14 +33,16 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/mro/x21/src/board_config.h b/boards/mro/x21/src/board_config.h index b0ec3b088862..1646687d5681 100644 --- a/boards/mro/x21/src/board_config.h +++ b/boards/mro/x21/src/board_config.h @@ -105,10 +105,11 @@ #define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#include +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20602) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) /* I2C busses */ @@ -198,15 +199,7 @@ __BEGIN_DECLS /**************************************************************************************************** * Public Functions ****************************************************************************************************/ -/**************************************************************************************************** - * Name: board_spi_reset board_peripheral_reset - * - * Description: - * Called to reset SPI and the perferal bus - * - ****************************************************************************************************/ -extern void board_spi_reset(int ms); extern void board_peripheral_reset(int ms); /**************************************************************************************************** diff --git a/boards/mro/x21/src/i2c.cpp b/boards/mro/x21/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/mro/x21/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/mro/x21/src/spi.c b/boards/mro/x21/src/spi.c deleted file mode 100644 index 95d45d845c9f..000000000000 --- a/boards/mro/x21/src/spi.c +++ /dev/null @@ -1,203 +0,0 @@ -/**************************************************************************** - * - * 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 spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - - px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X); - px4_arch_configgpio(GPIO_SPI_CS_BARO); - px4_arch_configgpio(GPIO_SPI_CS_MPU); - - px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); - px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY); -#endif - -#ifdef CONFIG_STM32_SPI2 - px4_arch_configgpio(GPIO_SPI_CS_FRAM); -#endif - -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - - /* intended fallthrough */ - case PX4_SPIDEV_ICM_20602: - - /* intended fallthrough */ - case PX4_SPIDEV_ICM_20608: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, !selected); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1); - break; - - case PX4_SPIDEV_MPU: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif - - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X_OFF); - px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF); - px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF); - - px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); - - px4_arch_configgpio(GPIO_SPI1_SCK_OFF); - px4_arch_configgpio(GPIO_SPI1_MISO_OFF); - px4_arch_configgpio(GPIO_SPI1_MOSI_OFF); - - px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - - px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY_OFF); - px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - px4_arch_gpiowrite(GPIO_EXTI_ICM_2060X_DRDY_OFF, 0); - px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); - - /* set the sensor rail off */ - px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN); - px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32_SPI1 - px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X); - px4_arch_configgpio(GPIO_SPI_CS_BARO); - px4_arch_configgpio(GPIO_SPI_CS_MPU); - - px4_arch_configgpio(GPIO_SPI1_SCK); - px4_arch_configgpio(GPIO_SPI1_MISO); - px4_arch_configgpio(GPIO_SPI1_MOSI); - - // // XXX bring up the EXTI pins again - // px4_arch_configgpio(GPIO_EXTI_MPU_DRDY); - // px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY); - -#endif -} diff --git a/boards/mro/x21/src/spi.cpp b/boards/mro/x21/src/spi.cpp new file mode 100644 index 000000000000..9c22f3251d8b --- /dev/null +++ b/boards/mro/x21/src/spi.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + }), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + diff --git a/boards/nxp/fmuk66-v3/src/CMakeLists.txt b/boards/nxp/fmuk66-v3/src/CMakeLists.txt index c4d0609dfcf1..2c563c1f8346 100644 --- a/boards/nxp/fmuk66-v3/src/CMakeLists.txt +++ b/boards/nxp/fmuk66-v3/src/CMakeLists.txt @@ -35,10 +35,11 @@ add_library(drivers_board autoleds.c automount.c can.c + i2c.cpp init.c led.c sdhc.c - spi.c + spi.cpp timer_config.cpp usb.c ) diff --git a/boards/nxp/fmuk66-v3/src/board_config.h b/boards/nxp/fmuk66-v3/src/board_config.h index 400f8d4ba71d..78883595896e 100644 --- a/boards/nxp/fmuk66-v3/src/board_config.h +++ b/boards/nxp/fmuk66-v3/src/board_config.h @@ -53,6 +53,8 @@ __BEGIN_DECLS #include #include +__END_DECLS + /* FMUK66 GPIOs ***********************************************************************************/ /* LEDs */ /* An RGB LED is connected through GPIO as shown below: @@ -233,20 +235,21 @@ __BEGIN_DECLS /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#include +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) #define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI_CS_MEMORY} #define PX4_MEMORY_BUS_FIRST_CS PX4_SPIDEV_MEMORY #define PX4_MEMORY_BUS_LAST_CS PX4_SPIDEV_MEMORY -#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) -#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,1) -#define PX4_SPIDEV_CALMEM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,2) +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_FXOS8701C) +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_FXAS2100C) +#define PX4_SPIDEV_CALMEM PX4_MK_SPI_SEL(0,DRV_DEVTYPE_UNUSED) #define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI_CS_FXOS8700CQ_ACCEL_MAG, GPIO_SPI_CS_FXAS21002CQ_GYRO, GPIO_SPI1_CS_CALMEM} #define PX4_SENSOR_BUS_FIRST_CS PX4_SPIDEV_ACCEL_MAG #define PX4_SENSOR_BUS_LAST_CS PX4_SPIDEV_CALMEM -#define PX4_SPIDEV_EXTERNAL1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL,0) -#define PX4_SPIDEV_EXTERNAL2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL,1) +#define PX4_SPIDEV_EXTERNAL1 PX4_MK_SPI_SEL(0,0) +#define PX4_SPIDEV_EXTERNAL2 PX4_MK_SPI_SEL(0,1) #define PX4_EXTERNAL_BUS_CS_GPIO {GPIO_SPI2_CS, GPIO_SPI2_EXT} #define PX4_EXTERNAL_BUS_FIRST_CS PX4_SPIDEV_EXTERNAL1 #define PX4_EXTERNAL_BUS_LAST_CS PX4_SPIDEV_EXTERNAL2 @@ -482,6 +485,8 @@ __BEGIN_DECLS * Public data ************************************************************************************/ +__BEGIN_DECLS + #ifndef __ASSEMBLY__ /************************************************************************************ @@ -515,7 +520,6 @@ int fmuk66_spi_bus_initialize(void); * Called to reset SPI and the perferal bus * ****************************************************************************************************/ -void board_spi_reset(int ms); void board_peripheral_reset(int ms); /************************************************************************************ diff --git a/boards/nxp/fmuk66-v3/src/i2c.cpp b/boards/nxp/fmuk66-v3/src/i2c.cpp new file mode 100644 index 000000000000..cff9faf05228 --- /dev/null +++ b/boards/nxp/fmuk66-v3/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)), + initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)), +}; + diff --git a/boards/nxp/fmuk66-v3/src/spi.c b/boards/nxp/fmuk66-v3/src/spi.cpp similarity index 67% rename from boards/nxp/fmuk66-v3/src/spi.c rename to boards/nxp/fmuk66-v3/src/spi.cpp index 1bff27a93fa4..7d0ea804885d 100644 --- a/boards/nxp/fmuk66-v3/src/spi.c +++ b/boards/nxp/fmuk66-v3/src/spi.cpp @@ -33,9 +33,9 @@ * ************************************************************************************/ -/************************************************************************************ - * Included Files - ************************************************************************************/ +#include +#include +#include #include @@ -55,18 +55,31 @@ #if defined(CONFIG_KINETIS_SPI0) || defined(CONFIG_KINETIS_SPI1) || defined(CONFIG_KINETIS_SPI2) -#define PX4_MK_GPIO(pin_ftmx, io) ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io)) +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(PX4_BUS_NUMBER_TO_PX4(0), { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortC, GPIO::Pin2}) + }), + initSPIBus(PX4_BUS_NUMBER_TO_PX4(1), { + initSPIDevice(DRV_ACC_DEVTYPE_FXOS8701C, SPI::CS{GPIO::PortB, GPIO::Pin10}), + initSPIDevice(DRV_GYR_DEVTYPE_FXAS2100C, SPI::CS{GPIO::PortB, GPIO::Pin9}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortA, GPIO::Pin19}), + }, {GPIO::PortB, GPIO::Pin8}), + initSPIBusExternal(PX4_BUS_NUMBER_TO_PX4(2), { + initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin20}), + initSPIConfigExternal(SPI::CS{GPIO::PortD, GPIO::Pin15}), + }), +}; -/* Define CS GPIO array */ -static const uint32_t spi0selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; -static const uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; -static const uint32_t spi2selects_gpio[] = PX4_EXTERNAL_BUS_CS_GPIO; +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + + +#define PX4_MK_GPIO(pin_ftmx, io) ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io)) /************************************************************************************ * Public Functions ************************************************************************************/ -__EXPORT void board_spi_reset(int ms) +__EXPORT void board_spi_reset(int ms, int bus_mask) { /* Goal not to back feed the chips on the bus via IO lines */ @@ -76,10 +89,13 @@ __EXPORT void board_spi_reset(int ms) kinetis_gpiowrite(PIN_SPI1_SIN, 1); /* Next Change CS to inputs with pull downs */ - - for (unsigned int cs = 0; cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - kinetis_pinconfig(PX4_MK_GPIO(spi1selects_gpio[cs], GPIO_PULLDOWN)); + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + kinetis_pinconfig(PX4_MK_GPIO(px4_spi_buses[bus].devices[i].cs_gpio, GPIO_PULLDOWN)); + } + } } } @@ -110,9 +126,13 @@ __EXPORT void board_spi_reset(int ms) /* Restore all the CS to ouputs inactive */ - for (unsigned int cs = 0; cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - kinetis_pinconfig(spi1selects_gpio[cs]); + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + kinetis_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio); + } + } } } @@ -155,23 +175,13 @@ __EXPORT void board_spi_reset(int ms) void fmuk66_spidev_initialize(void) { - board_spi_reset(10); - - for (unsigned int cs = 0; cs < arraySize(spi0selects_gpio); cs++) { - if (spi0selects_gpio[cs] != 0) { - kinetis_pinconfig(spi0selects_gpio[cs]); - } - } - - for (unsigned int cs = 0; cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - kinetis_pinconfig(spi1selects_gpio[cs]); - } - } + board_spi_reset(10, 0xffff); - for (unsigned int cs = 0; cs < arraySize(spi2selects_gpio); cs++) { - if (spi2selects_gpio[cs] != 0) { - kinetis_pinconfig(spi2selects_gpio[cs]); + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + kinetis_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio); + } } } } @@ -183,73 +193,79 @@ void fmuk66_spidev_initialize(void) * Called to configure SPI chip select GPIO pins for the NXP FMUK66 v3 board. * ************************************************************************************/ -static struct spi_dev_s *spi_sensors; -static struct spi_dev_s *spi_memory; -static struct spi_dev_s *spi_ext; +static const px4_spi_bus_t *_spi_bus0; +static const px4_spi_bus_t *_spi_bus1; +static const px4_spi_bus_t *_spi_bus2; __EXPORT int fmuk66_spi_bus_initialize(void) { + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case PX4_BUS_NUMBER_TO_PX4(0): _spi_bus0 = &px4_spi_buses[i]; break; + + case PX4_BUS_NUMBER_TO_PX4(1): _spi_bus1 = &px4_spi_buses[i]; break; + + case PX4_BUS_NUMBER_TO_PX4(2): _spi_bus2 = &px4_spi_buses[i]; break; + } + } + /* Configure SPI-based devices */ - spi_sensors = px4_spibus_initialize(PX4_SPI_BUS_SENSORS); + struct spi_dev_s *spi_sensors = px4_spibus_initialize(1); if (!spi_sensors) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); return -ENODEV; } - /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. + /* Default bus 1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi_sensors, 1 * 1000 * 1000); SPI_SETBITS(spi_sensors, 8); SPI_SETMODE(spi_sensors, SPIDEV_MODE0); - for (int cs = PX4_SENSOR_BUS_FIRST_CS; cs <= PX4_SENSOR_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_sensors, cs, false); - } - /* Get the SPI port for the Memory */ - spi_memory = px4_spibus_initialize(PX4_SPI_BUS_MEMORY); + struct spi_dev_s *spi_memory = px4_spibus_initialize(0); if (!spi_memory) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_MEMORY); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 0); return -ENODEV; } - /* Default PX4_SPI_BUS_MEMORY to 12MHz and de-assert the known chip selects. + /* Default bus 0 to 12MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi_memory, 12 * 1000 * 1000); SPI_SETBITS(spi_memory, 8); SPI_SETMODE(spi_memory, SPIDEV_MODE3); - for (int cs = PX4_MEMORY_BUS_FIRST_CS; cs <= PX4_MEMORY_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_memory, cs, false); - } - /* Configure EXTERNAL SPI-based devices */ - spi_ext = px4_spibus_initialize(PX4_SPI_BUS_EXTERNAL); + struct spi_dev_s *spi_ext = px4_spibus_initialize(2); if (!spi_ext) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); return -ENODEV; } - /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. + /* Default external bus to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi_ext, 8 * 1000 * 1000); SPI_SETBITS(spi_ext, 8); SPI_SETMODE(spi_ext, SPIDEV_MODE3); - for (int cs = PX4_EXTERNAL_BUS_FIRST_CS; cs <= PX4_EXTERNAL_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_ext, cs, false); + /* deselect all */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + SPI_SELECT(spi_ext, px4_spi_buses[bus].devices[i].devid, false); + } + } } - return OK; } @@ -282,35 +298,37 @@ __EXPORT int fmuk66_spi_bus_initialize(void) * ************************************************************************************/ -void kinetis_spi0select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +static inline void kinetis_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) { - spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); - - /* SPI select is active low, so write !selected to select the device */ + int matched_dev_idx = -1; - uint32_t sel = devid; - - if (devid == SPIDEV_FLASH(0)) { - sel = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_MEMORY); + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } - /* Making sure the other peripherals are not selected */ + if (devid == bus->devices[i].devid) { + matched_dev_idx = i; - for (unsigned int cs = 0; arraySize(spi0selects_gpio) > 1 && cs < arraySize(spi0selects_gpio); cs++) { - if (spi0selects_gpio[cs] != 0) { - kinetis_gpiowrite(spi0selects_gpio[cs], 1); + } else { + // Making sure the other peripherals are not selected + kinetis_gpiowrite(bus->devices[i].cs_gpio, 1); } } - uint32_t gpio = spi0selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - kinetis_gpiowrite(gpio, !selected); + // different devices might use the same CS, so make sure to configure the one we want last + if (matched_dev_idx != -1) { + // SPI select is active low, so write !selected to select the device + kinetis_gpiowrite(bus->devices[matched_dev_idx].cs_gpio, !selected); } } +void kinetis_spi0select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); + kinetis_spixselect(_spi_bus0, dev, devid, selected); +} + uint8_t kinetis_spi0status(FAR struct spi_dev_s *dev, uint32_t devid) { return SPI_STATUS_PRESENT; @@ -319,25 +337,7 @@ uint8_t kinetis_spi0status(FAR struct spi_dev_s *dev, uint32_t devid) void kinetis_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) { spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); - - /* SPI select is active low, so write !selected to select the device */ - - int sel = (int) devid; - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_SENSORS); - - /* Making sure the other peripherals are not selected */ - - for (unsigned int cs = 0; arraySize(spi1selects_gpio) > 1 && cs < arraySize(spi1selects_gpio); cs++) { - if (spi1selects_gpio[cs] != 0) { - kinetis_gpiowrite(spi1selects_gpio[cs], 1); - } - } - - uint32_t gpio = spi1selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - kinetis_gpiowrite(gpio, !selected); - } + kinetis_spixselect(_spi_bus1, dev, devid, selected); } uint8_t kinetis_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) @@ -348,25 +348,7 @@ uint8_t kinetis_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) void kinetis_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) { spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); - - /* SPI select is active low, so write !selected to select the device */ - - int sel = (int) devid; - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_EXTERNAL); - - /* Making sure the other peripherals are not selected */ - - for (unsigned int cs = 0; arraySize(spi2selects_gpio) > 1 && cs < arraySize(spi2selects_gpio); cs++) { - if (spi2selects_gpio[cs] != 0) { - kinetis_gpiowrite(spi2selects_gpio[cs], 1); - } - } - - uint32_t gpio = spi2selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - kinetis_gpiowrite(gpio, !selected); - } + kinetis_spixselect(_spi_bus2, dev, devid, selected); } uint8_t kinetis_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) diff --git a/boards/nxp/fmurt1062-v1/src/CMakeLists.txt b/boards/nxp/fmurt1062-v1/src/CMakeLists.txt index a0831d9edcba..d62c0da5eff5 100644 --- a/boards/nxp/fmurt1062-v1/src/CMakeLists.txt +++ b/boards/nxp/fmurt1062-v1/src/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_library(drivers_board autoleds.c automount.c can.c + i2c.cpp init.c led.c sdhc.c diff --git a/boards/nxp/fmurt1062-v1/src/board_config.h b/boards/nxp/fmurt1062-v1/src/board_config.h index 79c1429bf6f7..341873a3ad8f 100644 --- a/boards/nxp/fmurt1062-v1/src/board_config.h +++ b/boards/nxp/fmurt1062-v1/src/board_config.h @@ -186,29 +186,30 @@ #define PX4_SPIDEV_BMI 0 /* ^ END Legacy SPI defines TODO: fix this with enumeration */ +#include -#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) -#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,1) -#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,2) -#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,3) +#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20689) +#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_BMI055) +#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_BMI055) +#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(0,DRV_DEVTYPE_UNUSED) #define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI1_CS1_ICM20689, GPIO_SPI1_CS3_BMI055_GYRO, GPIO_SPI1_CS4_BMI055_ACCEL, SPI1_CS5_AUX_MEM} #define PX4_SENSORS_BUS_FIRST_CS PX4_SPIDEV_ICM_20689 #define PX4_SENSORS_BUS_LAST_CS PX4_SPIDEV_AUX_MEM -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) #define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI2_CS_FRAM} #define PX4_MEMORY_BUS_FIRST_CS PX4_SPIDEV_MEMORY #define PX4_MEMORY_BUS_LAST_CS PX4_SPIDEV_MEMORY -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO,0) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0,DRV_BARO_DEVTYPE_MS5611) #define PX4_BARO_BUS_CS_GPIO {GPIO_SPI3_CS1_MS5611} #define PX4_BARO_BUS_FIRST_CS PX4_SPIDEV_BARO #define PX4_BARO_BUS_LAST_CS PX4_SPIDEV_BARO -#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,0) -#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,1) -#define PX4_SPIDEV_EXTERNAL1_3 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,2) +#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(0,0) +#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(0,1) +#define PX4_SPIDEV_EXTERNAL1_3 PX4_MK_SPI_SEL(0,2) #define PX4_EXTERNAL1_BUS_CS_GPIO {SPI4_CS1_EXTERNAL1, SPI4_CS2_EXTERNAL1, SPI4_CS3_EXTERNAL1} #define PX4_EXTERNAL1_BUS_FIRST_CS PX4_SPIDEV_EXTERNAL1_1 #define PX4_EXTERNAL1_BUS_LAST_CS PX4_SPIDEV_EXTERNAL1_3 @@ -581,8 +582,6 @@ extern int imxrt1062_spi_bus_initialize(void); extern int imxrt_usb_initialize(void); -void board_spi_reset(int ms); - extern void imxrt_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/nxp/fmurt1062-v1/src/i2c.cpp b/boards/nxp/fmurt1062-v1/src/i2c.cpp new file mode 100644 index 000000000000..40b15c1abc53 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(3), +}; + diff --git a/boards/nxp/fmurt1062-v1/src/init.c b/boards/nxp/fmurt1062-v1/src/init.c index 11290f30f6eb..b920d614cd1a 100644 --- a/boards/nxp/fmurt1062-v1/src/init.c +++ b/boards/nxp/fmurt1062-v1/src/init.c @@ -216,7 +216,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_3V3_SENSORS_EN(true); VDD_3V3_SPEKTRUM_POWER_EN(true); - board_spi_reset(10); + board_spi_reset(10, 0xffff); if (OK == board_determine_hw_info()) { syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), diff --git a/boards/nxp/fmurt1062-v1/src/spi.cpp b/boards/nxp/fmurt1062-v1/src/spi.cpp index bc3744c41f4c..69f172256d9b 100644 --- a/boards/nxp/fmurt1062-v1/src/spi.cpp +++ b/boards/nxp/fmurt1062-v1/src/spi.cpp @@ -37,6 +37,10 @@ * Included Files ************************************************************************************/ +#include +#include +#include + #include #include @@ -59,20 +63,30 @@ #if defined(CONFIG_IMXRT_LPSPI1) || defined(CONFIG_IMXRT_LPSPI2) || \ defined(CONFIG_IMXRT_LPSPI3) || defined(CONFIG_IMXRT_LPSPI4) -/* Define CS GPIO array */ -#if defined(CONFIG_IMXRT_LPSPI1) -static const uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; -#endif -#if defined(CONFIG_IMXRT_LPSPI2) -static const uint32_t spi2selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; -#endif -#if defined(CONFIG_IMXRT_LPSPI3) -static const uint32_t spi3selects_gpio[] = PX4_BARO_BUS_CS_GPIO; -#endif -#if defined(CONFIG_IMXRT_LPSPI4) -static const uint32_t spi4selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; -#endif +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::Port3, GPIO::Pin26}, SPI::DRDY{GPIO::Port4, GPIO::Pin15}), /* GPIO_EMC_40 GPIO3_IO26 */ + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::Port2, GPIO::Pin26}, SPI::DRDY{GPIO::Port4, GPIO::Pin16}), /* GPIO_B1_10 GPIO2_IO26 */ + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::Port2, GPIO::Pin31}, SPI::DRDY{GPIO::Port3, GPIO::Pin23}), /* GPIO_B1_15 GPIO2_IO31 */ + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::Port3, GPIO::Pin0}), /* GPIO_SD_B1_00 GPIO3_IO00, AUX_MEM */ + }, {GPIO::Port3, GPIO::Pin27}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::Port3, GPIO::Pin20}) /* GPIO_EMC_34 G GPIO3_IO20 */ + }), + initSPIBus(3, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::Port4, GPIO::Pin14}), /* GPIO_EMC_14 GPIO4_IO14 */ + }), + initSPIBusExternal(4, { + initSPIConfigExternal(SPI::CS{GPIO::Port4, GPIO::Pin7}), /* GPIO_EMC_07 GPIO4_IO07 */ + initSPIConfigExternal(SPI::CS{GPIO::Port2, GPIO::Pin30}), /* GPIO_B1_14 GPIO2_IO30 */ + initSPIConfigExternal(SPI::CS{GPIO::Port4, GPIO::Pin11}), /* GPIO_EMC_11 GPIO4_IO011 */ + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT)) /************************************************************************************ * Public Functions @@ -87,18 +101,13 @@ static const uint32_t spi4selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; void imxrt_spidev_initialize(void) { -#if defined(CONFIG_IMXRT_LPSPI1) - px4_gpio_init(spi1selects_gpio, arraySize(spi1selects_gpio)); -#endif -#if defined(CONFIG_IMXRT_LPSPI2) - px4_gpio_init(spi2selects_gpio, arraySize(spi2selects_gpio)); -#endif -#if defined(CONFIG_IMXRT_LPSPI3) - px4_gpio_init(spi3selects_gpio, arraySize(spi3selects_gpio)); -#endif -#if defined(CONFIG_IMXRT_LPSPI4) - px4_gpio_init(spi4selects_gpio, arraySize(spi4selects_gpio)); -#endif + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } } /************************************************************************************ @@ -108,39 +117,48 @@ void imxrt_spidev_initialize(void) * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. * ************************************************************************************/ -static struct spi_dev_s *spi_sensors; -static struct spi_dev_s *spi_memory; -static struct spi_dev_s *spi_baro; -static struct spi_dev_s *spi_ext; + +static const px4_spi_bus_t *_spi_bus1; +static const px4_spi_bus_t *_spi_bus2; +static const px4_spi_bus_t *_spi_bus3; +static const px4_spi_bus_t *_spi_bus4; __EXPORT int imxrt1062_spi_bus_initialize(void) { + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 1: _spi_bus1 = &px4_spi_buses[i]; break; + + case 2: _spi_bus2 = &px4_spi_buses[i]; break; + + case 3: _spi_bus3 = &px4_spi_buses[i]; break; + + case 4: _spi_bus4 = &px4_spi_buses[i]; break; + } + } + /* Configure SPI-based devices */ - spi_sensors = px4_spibus_initialize(PX4_SPI_BUS_SENSORS); + struct spi_dev_s *spi_sensors = px4_spibus_initialize(1); if (!spi_sensors) { - PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 1); return -ENODEV; } - /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. + /* Default bus 1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi_sensors, 1 * 1000 * 1000); SPI_SETBITS(spi_sensors, 8); SPI_SETMODE(spi_sensors, SPIDEV_MODE3); - for (int cs = PX4_SENSORS_BUS_FIRST_CS; cs <= PX4_SENSORS_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_sensors, cs, false); - } - /* Get the SPI port for the Memory */ - spi_memory = px4_spibus_initialize(PX4_SPI_BUS_MEMORY); + struct spi_dev_s *spi_memory = px4_spibus_initialize(2); if (!spi_memory) { - PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_MEMORY); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 2); return -ENODEV; } @@ -151,16 +169,12 @@ __EXPORT int imxrt1062_spi_bus_initialize(void) SPI_SETBITS(spi_memory, 8); SPI_SETMODE(spi_memory, SPIDEV_MODE3); - for (int cs = PX4_MEMORY_BUS_FIRST_CS; cs <= PX4_MEMORY_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_memory, cs, false); - } - /* Get the SPI port for the BARO */ - spi_baro = px4_spibus_initialize(PX4_SPI_BUS_BARO); + struct spi_dev_s *spi_baro = px4_spibus_initialize(3); if (!spi_baro) { - PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_BARO); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 3); return -ENODEV; } @@ -171,28 +185,29 @@ __EXPORT int imxrt1062_spi_bus_initialize(void) SPI_SETBITS(spi_baro, 8); SPI_SETMODE(spi_baro, SPIDEV_MODE3); - for (int cs = PX4_BARO_BUS_FIRST_CS; cs <= PX4_BARO_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_baro, cs, false); - } - /* Get the SPI port for the PX4_SPI_EXTERNAL1 */ - spi_ext = px4_spibus_initialize(PX4_SPI_BUS_EXTERNAL1); + struct spi_dev_s *spi_ext = px4_spibus_initialize(4); if (!spi_ext) { - PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL1); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 4); return -ENODEV; } - /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. + /* Default ext bus to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi_ext, 8 * 1000 * 1000); SPI_SETBITS(spi_ext, 8); SPI_SETMODE(spi_ext, SPIDEV_MODE3); - for (int cs = PX4_EXTERNAL1_BUS_FIRST_CS; cs <= PX4_EXTERNAL1_BUS_LAST_CS; cs++) { - SPI_SELECT(spi_ext, cs, false); + /* deselect all */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + SPI_SELECT(spi_ext, px4_spi_buses[bus].devices[i].devid, false); + } + } } return OK; @@ -224,27 +239,37 @@ __EXPORT int imxrt1062_spi_bus_initialize(void) * ****************************************************************************/ -#if defined(CONFIG_IMXRT_LPSPI1) -__EXPORT void imxrt_lpspi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +static inline void imxrt_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) { - /* SPI select is active low, so write !selected to select the device */ + int matched_dev_idx = -1; - int sel = (int) devid; - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_SENSORS); + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } - /* Making sure the other peripherals are not selected */ + if (devid == bus->devices[i].devid) { + matched_dev_idx = i; - for (auto cs : spi1selects_gpio) { - imxrt_gpio_write(cs, 1); + } else { + // Making sure the other peripherals are not selected + imxrt_gpio_write(bus->devices[i].cs_gpio, 1); + } } - uint32_t gpio = spi1selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - imxrt_gpio_write(gpio, !selected); + // different devices might use the same CS, so make sure to configure the one we want last + if (matched_dev_idx != -1) { + // SPI select is active low, so write !selected to select the device + imxrt_gpio_write(bus->devices[matched_dev_idx].cs_gpio, !selected); } } +#if defined(CONFIG_IMXRT_LPSPI1) +__EXPORT void imxrt_lpspi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_spixselect(_spi_bus1, dev, devid, selected); +} + __EXPORT uint8_t imxrt_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid) { return SPI_STATUS_PRESENT; @@ -254,27 +279,7 @@ __EXPORT uint8_t imxrt_lpspi1status(FAR struct spi_dev_s *dev, uint32_t devid) #if defined(CONFIG_IMXRT_LPSPI2) __EXPORT void imxrt_lpspi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) { - /* SPI select is active low, so write !selected to select the device */ - - int sel = (int) devid; - - if (devid == SPIDEV_FLASH(0)) { - sel = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_MEMORY); - - /* Making sure the other peripherals are not selected */ - - for (auto cs : spi2selects_gpio) { - imxrt_gpio_write(cs, 1); - } - - uint32_t gpio = spi2selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - imxrt_gpio_write(gpio, !selected); - } + imxrt_spixselect(_spi_bus2, dev, devid, selected); } __EXPORT uint8_t imxrt_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid) @@ -286,22 +291,7 @@ __EXPORT uint8_t imxrt_lpspi2status(FAR struct spi_dev_s *dev, uint32_t devid) #if defined(CONFIG_IMXRT_LPSPI3) __EXPORT void imxrt_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) { - /* SPI select is active low, so write !selected to select the device */ - - int sel = (int) devid; - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_BARO); - - /* Making sure the other peripherals are not selected */ - - for (auto cs : spi3selects_gpio) { - imxrt_gpio_write(cs, 1); - } - - uint32_t gpio = spi3selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - imxrt_gpio_write(gpio, !selected); - } + imxrt_spixselect(_spi_bus3, dev, devid, selected); } __EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) @@ -313,20 +303,7 @@ __EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) #if defined(CONFIG_IMXRT_LPSPI4) __EXPORT void imxrt_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) { - int sel = (int) devid; - - ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_EXTERNAL1); - - /* Making sure the other peripherals are not selected */ - for (auto cs : spi4selects_gpio) { - imxrt_gpio_write(cs, 1); - } - - uint32_t gpio = spi4selects_gpio[PX4_SPI_DEV_ID(sel)]; - - if (gpio) { - imxrt_gpio_write(gpio, !selected); - } + imxrt_spixselect(_spi_bus4, dev, devid, selected); } __EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) @@ -343,23 +320,29 @@ __EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) * ************************************************************************************/ -__EXPORT void board_spi_reset(int ms) +__EXPORT void board_spi_reset(int ms, int bus_mask) { #ifdef CONFIG_IMXRT_LPSPI1 /* Goal not to back feed the chips on the bus via IO lines */ - for (auto cs : spi1selects_gpio) { - imxrt_config_gpio(_PIN_OFF(cs)); + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio)); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio)); + } + } + } } imxrt_config_gpio(GPIO_SPI1_SCK_OFF); imxrt_config_gpio(GPIO_SPI1_MISO_OFF); imxrt_config_gpio(GPIO_SPI1_MOSI_OFF); - for (auto cs : spi3selects_gpio) { - imxrt_config_gpio(_PIN_OFF(cs)); - } - imxrt_config_gpio(GPIO_SPI3_SCK_OFF); imxrt_config_gpio(GPIO_SPI3_MISO_OFF); imxrt_config_gpio(GPIO_SPI3_MOSI_OFF); @@ -368,12 +351,6 @@ __EXPORT void board_spi_reset(int ms) imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SDA_RESET)); imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SCL_RESET)); -# if BOARD_USE_DRDY - imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY1_ICM20689); - imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY2_BMI055_GYRO); - imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY3_BMI055_ACC); - imxrt_config_gpio(GPIO_DRDY_OFF_SPI1_DRDY4_ICM20602); -# endif /* set the sensor rail off */ imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 0); @@ -390,19 +367,24 @@ __EXPORT void board_spi_reset(int ms) usleep(100); /* reconfigure the SPI pins */ - for (auto cs : spi1selects_gpio) { - imxrt_config_gpio(cs); + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + imxrt_config_gpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + imxrt_config_gpio(px4_spi_buses[bus].devices[i].drdy_gpio); + } + } + } } imxrt_config_gpio(GPIO_LPSPI1_SCK); imxrt_config_gpio(GPIO_LPSPI1_MISO); imxrt_config_gpio(GPIO_LPSPI1_MOSI); - /* reconfigure the SPI pins */ - for (auto cs : spi3selects_gpio) { - imxrt_config_gpio(cs); - } - imxrt_config_gpio(GPIO_LPSPI3_SCK); imxrt_config_gpio(GPIO_LPSPI3_MISO); imxrt_config_gpio(GPIO_LPSPI3_MOSI); @@ -410,12 +392,6 @@ __EXPORT void board_spi_reset(int ms) imxrt_config_gpio(GPIO_LPI2C3_SDA); imxrt_config_gpio(GPIO_LPI2C3_SCL); -# if BOARD_USE_DRDY - imxrt_config_gpio(GPIO_SPI1_DRDY1_ICM20689); - imxrt_config_gpio(GPIO_SPI1_DRDY2_BMI055_GYRO); - imxrt_config_gpio(GPIO_SPI1_DRDY3_BMI055_ACC); - imxrt_config_gpio(GPIO_SPI1_DRDY4_ICM20602); -# endif #endif /* CONFIG_IMXRT_LPSPI1 */ } diff --git a/boards/omnibus/f4sd/init/rc.board_sensors b/boards/omnibus/f4sd/init/rc.board_sensors index 78813842348c..76e43ae1b13e 100644 --- a/boards/omnibus/f4sd/init/rc.board_sensors +++ b/boards/omnibus/f4sd/init/rc.board_sensors @@ -16,4 +16,3 @@ hmc5883 -X start bmp280 start -adc start diff --git a/boards/omnibus/f4sd/src/CMakeLists.txt b/boards/omnibus/f4sd/src/CMakeLists.txt index ef9809a96e61..4212b7e6d27b 100644 --- a/boards/omnibus/f4sd/src/CMakeLists.txt +++ b/boards/omnibus/f4sd/src/CMakeLists.txt @@ -32,15 +32,17 @@ ############################################################################ add_library(drivers_board + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/omnibus/f4sd/src/board_config.h b/boards/omnibus/f4sd/src/board_config.h index 1962168a6a61..00b6295b1bbc 100644 --- a/boards/omnibus/f4sd/src/board_config.h +++ b/boards/omnibus/f4sd/src/board_config.h @@ -112,6 +112,8 @@ /* OMNIBUSF4SD SPI chip selects and DRDY */ /*----------------------------------------------------------*/ +#include + /* SPI chip selects */ /* * Define the Chip Selects for SPI1 @@ -171,11 +173,11 @@ // One device per bus #define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPIDEV_MPU 1 -#define PX4_SPIDEV_ICM_20602 1 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20602) #define PX4_SPIDEV_BARO_BUS 3 -#define PX4_SPIDEV_BARO 1 -#define PX4_SPIDEV_OSD 2 +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_BMP280) +#define PX4_SPIDEV_OSD PX4_MK_SPI_SEL(0, DRV_OSD_DEVTYPE_ATXXXX) /* USB OTG FS * @@ -259,14 +261,9 @@ __BEGIN_DECLS * Description: * Called to configure SPI chip select GPIO pins for the PX4FMU board. * - * mask - is bus selection - * 1 - 1 << 0 - * 2 - 1 << 1 - * ****************************************************************************************************/ extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); /**************************************************************************************************** diff --git a/boards/omnibus/f4sd/src/i2c.cpp b/boards/omnibus/f4sd/src/i2c.cpp new file mode 100644 index 000000000000..7a508b91e646 --- /dev/null +++ b/boards/omnibus/f4sd/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(2), +}; + diff --git a/boards/omnibus/f4sd/src/spi.c b/boards/omnibus/f4sd/src/spi.c deleted file mode 100644 index 4e894a8f83dc..000000000000 --- a/boards/omnibus/f4sd/src/spi.c +++ /dev/null @@ -1,187 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018 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 spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * mask - is bus selection - * 1 - 1 << 0 - * 2 - 1 << 1 - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ - stm32_configgpio(GPIO_SPI_CS_MEMS); - stm32_configgpio(GPIO_SPI_CS_SDCARD); - stm32_configgpio(GPIO_SPI3_CS_BARO); - stm32_configgpio(GPIO_SPI3_CS_OSD); -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - UNUSED(devid); - px4_arch_gpiowrite(GPIO_SPI_CS_MEMS, !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - UNUSED(devid); - px4_arch_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - -__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - UNUSED(devid); - /* SPI select is active low, so write !selected to select the device */ - px4_arch_gpiowrite(GPIO_SPI3_CS_BARO, !selected); - px4_arch_gpiowrite(GPIO_SPI3_CS_OSD, !selected); -} - -__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} - -__EXPORT void board_spi_reset(int ms) -{ - // TODO: DRDY - ///* disable SPI bus 1 DRDY */ - //stm32_configgpio(GPIO_DRDY_OFF_PORTD_PIN15); - //stm32_configgpio(GPIO_DRDY_OFF_PORTC_PIN14); - //stm32_configgpio(GPIO_DRDY_OFF_PORTE_PIN12); - - //stm32_gpiowrite(GPIO_DRDY_OFF_PORTD_PIN15, 0); - //stm32_gpiowrite(GPIO_DRDY_OFF_PORTC_PIN14, 0); - //stm32_gpiowrite(GPIO_DRDY_OFF_PORTE_PIN12, 0); - - /* disable SPI bus 1 CS */ - stm32_configgpio(GPIO_SPI1_CS_MEMS_OFF); - stm32_gpiowrite(GPIO_SPI1_CS_MEMS_OFF, 0); - - /* disable SPI bus 2 CS */ - stm32_configgpio(GPIO_SPI2_CS_SDCARD_OFF); - stm32_gpiowrite(GPIO_SPI2_CS_SDCARD_OFF, 0); - - /* disable SPI bus 3 CS */ - stm32_configgpio(GPIO_SPI3_CS_BARO_OFF); - stm32_gpiowrite(GPIO_SPI3_CS_BARO_OFF, 0); - stm32_configgpio(GPIO_SPI3_CS_OSD_OFF); - stm32_gpiowrite(GPIO_SPI3_CS_OSD_OFF, 0); - - /* disable SPI bus 1*/ - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - - /* disable SPI bus 2*/ - stm32_configgpio(GPIO_SPI2_SCK_OFF); - stm32_configgpio(GPIO_SPI2_MISO_OFF); - stm32_configgpio(GPIO_SPI2_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI2_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI2_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI2_MOSI_OFF, 0); - - /* disable SPI bus 3*/ - stm32_configgpio(GPIO_SPI3_SCK_OFF); - stm32_configgpio(GPIO_SPI3_MISO_OFF); - stm32_configgpio(GPIO_SPI3_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI3_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI3_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI3_MOSI_OFF, 0); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - stm32_spiinitialize(); - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - - // TODO: why do we not enable SPI2 here? - - stm32_configgpio(GPIO_SPI3_SCK); - stm32_configgpio(GPIO_SPI3_MISO); - stm32_configgpio(GPIO_SPI3_MOSI); -} diff --git a/boards/omnibus/f4sd/src/spi.cpp b/boards/omnibus/f4sd/src/spi.cpp new file mode 100644 index 000000000000..1504bf85b8be --- /dev/null +++ b/boards/omnibus/f4sd/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2018 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortA, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}), + }), + initSPIBus(2, { + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortB, GPIO::Pin12}) + }), + initSPIBus(3, { + initSPIDevice(DRV_BARO_DEVTYPE_BMP280, SPI::CS{GPIO::PortB, GPIO::Pin3}), + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortA, GPIO::Pin15}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + diff --git a/boards/px4/fmu-v2/src/CMakeLists.txt b/boards/px4/fmu-v2/src/CMakeLists.txt index beb8034f8a88..ae93a84ce71f 100644 --- a/boards/px4/fmu-v2/src/CMakeLists.txt +++ b/boards/px4/fmu-v2/src/CMakeLists.txt @@ -33,10 +33,10 @@ add_library(drivers_board can.c - i2c.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c manifest.c @@ -44,6 +44,7 @@ add_library(drivers_board target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/px4/fmu-v2/src/board_config.h b/boards/px4/fmu-v2/src/board_config.h index 7d0c52c0668b..9e8448baf662 100644 --- a/boards/px4/fmu-v2/src/board_config.h +++ b/boards/px4/fmu-v2/src/board_config.h @@ -60,6 +60,7 @@ #define HW_VER_FMUV2MINI_STATE 0xA /* PB12:PU:1 PB12:PD:0 PB4:PU:1 PB4PD:0 */ #define HW_VER_FMUV2X_STATE 0xB /* PB12:PU:1 PB12:PD:0 PB4:PU:1 PB4PD:1 */ #define HW_VER_TYPE_INIT {'V','2',0, 0} +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 /**************************************************************************************************** * Definitions @@ -87,12 +88,7 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) #define BOARD_OVERLOAD_LED LED_AMBER -/* - * Define the ability to shut off off the sensor signals - * by changing the signals to inputs - */ - -#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_50MHz)) +#include /* Due to inconsistent use of chip select and dry signal on * different board that use this build. We are defining the GPIO @@ -208,24 +204,27 @@ /* Use these to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_L3GD20) +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(0, DRV_ACC_DEVTYPE_LSM303D) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) /* FMUv3 SPI on external bus */ -#define PX4_SPIDEV_EXT_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 1) -#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 2) -#define PX4_SPIDEV_EXT_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 3) -#define PX4_SPIDEV_EXT_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 4) -#define PX4_SPIDEV_EXT_BMI PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 5) +#define PX4_SPIDEV_EXT_GYRO PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_L3GD20) +#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_MK_SPI_SEL(0, DRV_ACC_DEVTYPE_LSM303D) +#define PX4_SPIDEV_EXT_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_EXT_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) +#define PX4_SPIDEV_EXT_BMI PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_BMI055) /* I2C busses */ +#define BOARD_OVERRIDE_I2C_BUS_EXTERNAL #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_ONBOARD 2 #define PX4_I2C_BUS_LED PX4_I2C_BUS_ONBOARD +#define BOARD_SPI_BUS_MAX_BUS_ITEMS 3 + /*----------------------------------------------------------*/ /* FMUv3 Cube SPI chip selects and DRDY */ /*----------------------------------------------------------*/ @@ -292,7 +291,7 @@ #define GPIO_SPI4_EXTERN_CS GPIO_SPI4_CS_PB1 /* PB1 is an External CS on V3 */ -#define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_HMC5883) /*----------------------------------------------------------*/ /* End FMUv3 Cube SPI chip selects and DRDY */ @@ -351,7 +350,7 @@ */ #define GPIO_SPI1_EXTI_20608_DRDY_PC14 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) -#define PX4_SPIDEV_ICM_20608 6 /* ICM_20608 on PC15 */ +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) @@ -379,7 +378,6 @@ #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) #define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) -#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) @@ -463,7 +461,6 @@ extern void stm32_spiinitialize(void); * ****************************************************************************************************/ -void board_spi_reset(int ms); extern void board_peripheral_reset(int ms); /**************************************************************************************************** diff --git a/boards/px4/fmu-v2/src/i2c.c b/boards/px4/fmu-v2/src/i2c.cpp similarity index 88% rename from boards/px4/fmu-v2/src/i2c.c rename to boards/px4/fmu-v2/src/i2c.cpp index 55d559976e76..aaf09696e273 100644 --- a/boards/px4/fmu-v2/src/i2c.c +++ b/boards/px4/fmu-v2/src/i2c.cpp @@ -38,15 +38,23 @@ */ #include "board_config.h" +#include +#include -__EXPORT bool px4_i2c_bus_external(int bus) +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(2), +}; + + +bool px4_i2c_bus_external(const px4_i2c_bus_t &bus) { if (HW_VER_FMUV3 == board_get_hw_version()) { /* All FMUV3 2.1 i2c buses are external */ return true; } else { - if (bus != PX4_I2C_BUS_ONBOARD) { + if (bus.bus != 2) { return true; } } diff --git a/boards/px4/fmu-v2/src/init.c b/boards/px4/fmu-v2/src/init.c index 7fb8f876639f..9d71cd3d49c1 100644 --- a/boards/px4/fmu-v2/src/init.c +++ b/boards/px4/fmu-v2/src/init.c @@ -160,7 +160,7 @@ __EXPORT void board_on_reset(int status) up_mdelay(400); /* on reboot (status >= 0) reset sensors and peripherals */ - board_spi_reset(10); + board_spi_reset(10, 0xffff); } } @@ -309,7 +309,8 @@ stm32_boardinitialize(void) /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + board_control_spi_sensors_power_configgpio(); + board_control_spi_sensors_power(true, 0xffff); stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_USB_VALID); @@ -419,7 +420,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_DEBUG, "\nFMUv2 ver 0x%1X : Rev %x %s\n", hw_version, hw_revision, hw_type); } - /* configure SPI interfaces */ + /* configure SPI interfaces (after the hw is determined) */ stm32_spiinitialize(); px4_platform_init(); @@ -457,10 +458,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Configure SPI-based devices */ - spi1 = stm32_spibus_initialize(PX4_SPI_BUS_SENSORS); + spi1 = stm32_spibus_initialize(1); if (!spi1) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); led_on(LED_AMBER); return -ENODEV; } @@ -473,10 +474,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Get the SPI port for the FRAM */ - spi2 = stm32_spibus_initialize(PX4_SPI_BUS_RAMTRON); + spi2 = stm32_spibus_initialize(2); if (!spi2) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_RAMTRON); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); led_on(LED_AMBER); return -ENODEV; } @@ -489,10 +490,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); - spi4 = stm32_spibus_initialize(PX4_SPI_BUS_EXT); + spi4 = stm32_spibus_initialize(4); if (!spi4) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXT); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 4); led_on(LED_AMBER); return -ENODEV; } diff --git a/boards/px4/fmu-v2/src/spi.c b/boards/px4/fmu-v2/src/spi.c deleted file mode 100644 index 3269a2838eae..000000000000 --- a/boards/px4/fmu-v2/src/spi.c +++ /dev/null @@ -1,461 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -#ifdef CONFIG_STM32_SPI1 -/* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local V2 v2 V2 V3 a V2 V2M V2x a a 4 - */ -static void stm32_spi1_initialize(void) -{ - stm32_configgpio(GPIO_SPI1_CS_PC2); - stm32_configgpio(GPIO_SPI1_CS_PD7); - - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PD15); - - if (HW_VER_FMUV2MINI == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI1_EXTI_20608_DRDY_PC14); - stm32_configgpio(GPIO_SPI1_CS_PC15); - - } else if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI1_CS_PC1); - - } else { - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB0); - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB1); - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB4); - stm32_configgpio(GPIO_SPI1_CS_PC13); - stm32_configgpio(GPIO_SPI1_CS_PC15); - } -} -#endif // CONFIG_STM32_SPI1 - -#ifdef CONFIG_STM32_SPI4 -/* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local V3 V3 - V3 a V3 V23 V3 - - - */ -static void stm32_spi4_initialize(void) -{ - stm32_configgpio(GPIO_SPI4_NSS_PE4); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI4_EXTI_DRDY_PB0); - stm32_configgpio(GPIO_SPI4_CS_PB1); - stm32_configgpio(GPIO_SPI4_CS_PC13); - stm32_configgpio(GPIO_SPI4_CS_PC15); - } - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_configgpio(GPIO_SPI4_GPIO_PC14); - } -} -#endif //CONFIG_STM32_SPI4 - -__EXPORT void stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - stm32_spi1_initialize(); -#endif - -#ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI2_CS_PD10); -#endif - -#ifdef CONFIG_STM32_SPI4 - stm32_spi4_initialize(); -#endif -} - -#ifdef CONFIG_STM32_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - /* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local - - - V3 a V2 - V2M a - - - */ - - switch (devid) { - case PX4_SPIDEV_GYRO: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, !selected); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_ICM_20608: - case PX4_SPIDEV_ACCEL_MAG: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, !selected); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_BARO: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, !selected); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_MPU: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, !selected); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_HMC: - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, !selected); - } - - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32_SPI1 - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI2_CS_PD10, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif - - -#ifdef CONFIG_STM32_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - /* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local - - - - - V3 !V2M V3 - - a - */ - - switch (devid) { - case PX4_SPIDEV_EXT_MPU: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, !selected); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1); - } - - break; - - case PX4_SPIDEV_EXT_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, !selected); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1); - } - - break; - - case PX4_SPIDEV_ICM_20608: - case PX4_SPIDEV_EXT_ACCEL_MAG: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, !selected); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1); - } - - break; - - case PX4_SPIDEV_EXT_BMI: - case PX4_SPIDEV_EXT_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, !selected); - } - - break; - - default: - break; - - } -} -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32_SPI4 - -/* V2, V2M SPI1 All signals SPI4, V3 ALL signals */ -/* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * local A A A A A A V3 A A !V2 A A A V3 V3 V3 V3 - */ - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC2)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC13)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC15)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PD7)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC2), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC13), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC15), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PD7), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI1_SCK)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_MISO)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_MOSI)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_SCK), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_MISO), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_MOSI), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB0)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB1)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB4)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PD15)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB0), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB1), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB4), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PD15), 0); - - if (HW_VER_FMUV2 != board_get_hw_version()) { - stm32_configgpio(_PIN_OFF(GPIO_SPI4_CS_PC14)); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_CS_PC14), 0); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC1)); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC1), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI4_NSS_PE4)); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_NSS_PE4), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI4_SCK)); - stm32_configgpio(_PIN_OFF(GPIO_SPI4_MISO)); - stm32_configgpio(_PIN_OFF(GPIO_SPI4_MOSI)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_SCK), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_MISO), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_MOSI), 0); - } - - /* set the sensor rail off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); - stm32_spi4_initialize(); - } - - stm32_spi1_initialize(); -} - -__EXPORT bool px4_spi_bus_external(int bus) -{ - if (HW_VER_FMUV3 == board_get_hw_version()) { - /* all FMUv3 2.1 spi buses are internal */ - return false; - - } else { - if (bus == PX4_SPI_BUS_EXT) { - return true; - } - } - - return false; -} diff --git a/boards/px4/fmu-v2/src/spi.cpp b/boards/px4/fmu-v2/src/spi.cpp new file mode 100644 index 000000000000..8294d660c63b --- /dev/null +++ b/boards/px4/fmu-v2/src/spi.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(HW_VER_FMUV2, { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), + initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) + }), + initSPIBusExternal(4, { + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), + initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}), + }), + }), + + initSPIHWVersion(HW_VER_FMUV3, { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_MAG_DEVTYPE_HMC5883, SPI::CS{GPIO::PortC, GPIO::Pin1}), // HMC5983 + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) + }), + initSPIBus(4, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), + }), + }), + + initSPIHWVersion(HW_VER_FMUV2MINI, { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) + }), + initSPIBusExternal(4, { // unused, but we must at least define it here + }), + }), + + // HW_VER_FMUV2X: treat as HW_VER_FMUV2 +}; +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); + diff --git a/boards/px4/fmu-v3/src/CMakeLists.txt b/boards/px4/fmu-v3/src/CMakeLists.txt index beb8034f8a88..ae93a84ce71f 100644 --- a/boards/px4/fmu-v3/src/CMakeLists.txt +++ b/boards/px4/fmu-v3/src/CMakeLists.txt @@ -33,10 +33,10 @@ add_library(drivers_board can.c - i2c.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c manifest.c @@ -44,6 +44,7 @@ add_library(drivers_board target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/px4/fmu-v3/src/board_config.h b/boards/px4/fmu-v3/src/board_config.h index 7d0c52c0668b..9e8448baf662 100644 --- a/boards/px4/fmu-v3/src/board_config.h +++ b/boards/px4/fmu-v3/src/board_config.h @@ -60,6 +60,7 @@ #define HW_VER_FMUV2MINI_STATE 0xA /* PB12:PU:1 PB12:PD:0 PB4:PU:1 PB4PD:0 */ #define HW_VER_FMUV2X_STATE 0xB /* PB12:PU:1 PB12:PD:0 PB4:PU:1 PB4PD:1 */ #define HW_VER_TYPE_INIT {'V','2',0, 0} +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 /**************************************************************************************************** * Definitions @@ -87,12 +88,7 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) #define BOARD_OVERLOAD_LED LED_AMBER -/* - * Define the ability to shut off off the sensor signals - * by changing the signals to inputs - */ - -#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_50MHz)) +#include /* Due to inconsistent use of chip select and dry signal on * different board that use this build. We are defining the GPIO @@ -208,24 +204,27 @@ /* Use these to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_L3GD20) +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(0, DRV_ACC_DEVTYPE_LSM303D) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) /* FMUv3 SPI on external bus */ -#define PX4_SPIDEV_EXT_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 1) -#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 2) -#define PX4_SPIDEV_EXT_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 3) -#define PX4_SPIDEV_EXT_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 4) -#define PX4_SPIDEV_EXT_BMI PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT, 5) +#define PX4_SPIDEV_EXT_GYRO PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_L3GD20) +#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_MK_SPI_SEL(0, DRV_ACC_DEVTYPE_LSM303D) +#define PX4_SPIDEV_EXT_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_EXT_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) +#define PX4_SPIDEV_EXT_BMI PX4_MK_SPI_SEL(0, DRV_GYR_DEVTYPE_BMI055) /* I2C busses */ +#define BOARD_OVERRIDE_I2C_BUS_EXTERNAL #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_ONBOARD 2 #define PX4_I2C_BUS_LED PX4_I2C_BUS_ONBOARD +#define BOARD_SPI_BUS_MAX_BUS_ITEMS 3 + /*----------------------------------------------------------*/ /* FMUv3 Cube SPI chip selects and DRDY */ /*----------------------------------------------------------*/ @@ -292,7 +291,7 @@ #define GPIO_SPI4_EXTERN_CS GPIO_SPI4_CS_PB1 /* PB1 is an External CS on V3 */ -#define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_HMC5883) /*----------------------------------------------------------*/ /* End FMUv3 Cube SPI chip selects and DRDY */ @@ -351,7 +350,7 @@ */ #define GPIO_SPI1_EXTI_20608_DRDY_PC14 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) -#define PX4_SPIDEV_ICM_20608 6 /* ICM_20608 on PC15 */ +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) @@ -379,7 +378,6 @@ #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) #define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) -#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) @@ -463,7 +461,6 @@ extern void stm32_spiinitialize(void); * ****************************************************************************************************/ -void board_spi_reset(int ms); extern void board_peripheral_reset(int ms); /**************************************************************************************************** diff --git a/boards/px4/fmu-v3/src/i2c.c b/boards/px4/fmu-v3/src/i2c.cpp similarity index 88% rename from boards/px4/fmu-v3/src/i2c.c rename to boards/px4/fmu-v3/src/i2c.cpp index 55d559976e76..aaf09696e273 100644 --- a/boards/px4/fmu-v3/src/i2c.c +++ b/boards/px4/fmu-v3/src/i2c.cpp @@ -38,15 +38,23 @@ */ #include "board_config.h" +#include +#include -__EXPORT bool px4_i2c_bus_external(int bus) +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(2), +}; + + +bool px4_i2c_bus_external(const px4_i2c_bus_t &bus) { if (HW_VER_FMUV3 == board_get_hw_version()) { /* All FMUV3 2.1 i2c buses are external */ return true; } else { - if (bus != PX4_I2C_BUS_ONBOARD) { + if (bus.bus != 2) { return true; } } diff --git a/boards/px4/fmu-v3/src/init.c b/boards/px4/fmu-v3/src/init.c index 7fb8f876639f..9d71cd3d49c1 100644 --- a/boards/px4/fmu-v3/src/init.c +++ b/boards/px4/fmu-v3/src/init.c @@ -160,7 +160,7 @@ __EXPORT void board_on_reset(int status) up_mdelay(400); /* on reboot (status >= 0) reset sensors and peripherals */ - board_spi_reset(10); + board_spi_reset(10, 0xffff); } } @@ -309,7 +309,8 @@ stm32_boardinitialize(void) /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + board_control_spi_sensors_power_configgpio(); + board_control_spi_sensors_power(true, 0xffff); stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_USB_VALID); @@ -419,7 +420,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_DEBUG, "\nFMUv2 ver 0x%1X : Rev %x %s\n", hw_version, hw_revision, hw_type); } - /* configure SPI interfaces */ + /* configure SPI interfaces (after the hw is determined) */ stm32_spiinitialize(); px4_platform_init(); @@ -457,10 +458,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Configure SPI-based devices */ - spi1 = stm32_spibus_initialize(PX4_SPI_BUS_SENSORS); + spi1 = stm32_spibus_initialize(1); if (!spi1) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); led_on(LED_AMBER); return -ENODEV; } @@ -473,10 +474,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Get the SPI port for the FRAM */ - spi2 = stm32_spibus_initialize(PX4_SPI_BUS_RAMTRON); + spi2 = stm32_spibus_initialize(2); if (!spi2) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_RAMTRON); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); led_on(LED_AMBER); return -ENODEV; } @@ -489,10 +490,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); - spi4 = stm32_spibus_initialize(PX4_SPI_BUS_EXT); + spi4 = stm32_spibus_initialize(4); if (!spi4) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXT); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 4); led_on(LED_AMBER); return -ENODEV; } diff --git a/boards/px4/fmu-v3/src/spi.c b/boards/px4/fmu-v3/src/spi.c deleted file mode 100644 index 3269a2838eae..000000000000 --- a/boards/px4/fmu-v3/src/spi.c +++ /dev/null @@ -1,461 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -#ifdef CONFIG_STM32_SPI1 -/* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local V2 v2 V2 V3 a V2 V2M V2x a a 4 - */ -static void stm32_spi1_initialize(void) -{ - stm32_configgpio(GPIO_SPI1_CS_PC2); - stm32_configgpio(GPIO_SPI1_CS_PD7); - - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PD15); - - if (HW_VER_FMUV2MINI == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI1_EXTI_20608_DRDY_PC14); - stm32_configgpio(GPIO_SPI1_CS_PC15); - - } else if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI1_CS_PC1); - - } else { - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB0); - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB1); - stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB4); - stm32_configgpio(GPIO_SPI1_CS_PC13); - stm32_configgpio(GPIO_SPI1_CS_PC15); - } -} -#endif // CONFIG_STM32_SPI1 - -#ifdef CONFIG_STM32_SPI4 -/* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local V3 V3 - V3 a V3 V23 V3 - - - */ -static void stm32_spi4_initialize(void) -{ - stm32_configgpio(GPIO_SPI4_NSS_PE4); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI4_EXTI_DRDY_PB0); - stm32_configgpio(GPIO_SPI4_CS_PB1); - stm32_configgpio(GPIO_SPI4_CS_PC13); - stm32_configgpio(GPIO_SPI4_CS_PC15); - } - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_configgpio(GPIO_SPI4_GPIO_PC14); - } -} -#endif //CONFIG_STM32_SPI4 - -__EXPORT void stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - stm32_spi1_initialize(); -#endif - -#ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI2_CS_PD10); -#endif - -#ifdef CONFIG_STM32_SPI4 - stm32_spi4_initialize(); -#endif -} - -#ifdef CONFIG_STM32_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - /* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local - - - V3 a V2 - V2M a - - - */ - - switch (devid) { - case PX4_SPIDEV_GYRO: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, !selected); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_ICM_20608: - case PX4_SPIDEV_ACCEL_MAG: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, !selected); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_BARO: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, !selected); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_MPU: - - /* Making sure the other peripherals are not selected */ - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, !selected); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1); - } - - break; - - case PX4_SPIDEV_HMC: - if (HW_VER_FMUV2 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1); - } - - if (HW_VER_FMUV3 != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1); - } - - stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1); - stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI1_CS_PC1, !selected); - } - - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32_SPI1 - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI2_CS_PD10, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif - - -#ifdef CONFIG_STM32_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - /* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * driver X X X X X X - * local - - - - - V3 !V2M V3 - - a - */ - - switch (devid) { - case PX4_SPIDEV_EXT_MPU: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, !selected); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1); - } - - break; - - case PX4_SPIDEV_EXT_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, !selected); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1); - } - - break; - - case PX4_SPIDEV_ICM_20608: - case PX4_SPIDEV_EXT_ACCEL_MAG: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, !selected); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1); - } - - break; - - case PX4_SPIDEV_EXT_BMI: - case PX4_SPIDEV_EXT_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1); - - if (HW_VER_FMUV2MINI != board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1); - stm32_gpiowrite(GPIO_SPI4_CS_PC13, !selected); - } - - break; - - default: - break; - - } -} -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32_SPI4 - -/* V2, V2M SPI1 All signals SPI4, V3 ALL signals */ -/* Verification - * PA5 PA6 PA7 PB0 PB1 PB4 PC1 PC2 PC13 PC14 PC15 PD7 PD15 PE2 PE4 PE5 PE6 - * local A A A A A A V3 A A !V2 A A A V3 V3 V3 V3 - */ - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC2)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC13)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC15)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PD7)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC2), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC13), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC15), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PD7), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI1_SCK)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_MISO)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_MOSI)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_SCK), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_MISO), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_MOSI), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB0)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB1)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB4)); - stm32_configgpio(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PD15)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB0), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB1), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB4), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PD15), 0); - - if (HW_VER_FMUV2 != board_get_hw_version()) { - stm32_configgpio(_PIN_OFF(GPIO_SPI4_CS_PC14)); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_CS_PC14), 0); - } - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC1)); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC1), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI4_NSS_PE4)); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_NSS_PE4), 0); - - stm32_configgpio(_PIN_OFF(GPIO_SPI4_SCK)); - stm32_configgpio(_PIN_OFF(GPIO_SPI4_MISO)); - stm32_configgpio(_PIN_OFF(GPIO_SPI4_MOSI)); - - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_SCK), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_MISO), 0); - stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_MOSI), 0); - } - - /* set the sensor rail off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - - if (HW_VER_FMUV3 == board_get_hw_version()) { - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); - stm32_spi4_initialize(); - } - - stm32_spi1_initialize(); -} - -__EXPORT bool px4_spi_bus_external(int bus) -{ - if (HW_VER_FMUV3 == board_get_hw_version()) { - /* all FMUv3 2.1 spi buses are internal */ - return false; - - } else { - if (bus == PX4_SPI_BUS_EXT) { - return true; - } - } - - return false; -} diff --git a/boards/px4/fmu-v3/src/spi.cpp b/boards/px4/fmu-v3/src/spi.cpp new file mode 100644 index 000000000000..8294d660c63b --- /dev/null +++ b/boards/px4/fmu-v3/src/spi.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(HW_VER_FMUV2, { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), + initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) + }), + initSPIBusExternal(4, { + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), + initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}), + }), + }), + + initSPIHWVersion(HW_VER_FMUV3, { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_MAG_DEVTYPE_HMC5883, SPI::CS{GPIO::PortC, GPIO::Pin1}), // HMC5983 + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) + }), + initSPIBus(4, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), + }), + }), + + initSPIHWVersion(HW_VER_FMUV2MINI, { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) + }), + initSPIBusExternal(4, { // unused, but we must at least define it here + }), + }), + + // HW_VER_FMUV2X: treat as HW_VER_FMUV2 +}; +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); + diff --git a/boards/px4/fmu-v4/src/CMakeLists.txt b/boards/px4/fmu-v4/src/CMakeLists.txt index 473034931d5a..8730ec995917 100644 --- a/boards/px4/fmu-v4/src/CMakeLists.txt +++ b/boards/px4/fmu-v4/src/CMakeLists.txt @@ -33,15 +33,17 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index d3452e82351d..1d4368bb277e 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -143,25 +143,25 @@ # define SPI_BUS_INIT_MASK_EXT PX4_SPI_BUS_EXTERNAL #endif /* CONFIG_STM32_SPI4 */ -#define SPI_BUS_INIT_MASK (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_SENSORS) +#include /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) -#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 5) -#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 7) -#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 10) -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 11) -#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 14) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_HMC5883) +#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_LIS3MDL) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20602) +#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) /** * Onboard MS5611 and FRAM are both on bus SPI2. * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver. * PX4_MK_SPI_SEL differentiate by adding in PX4_SPI_DEVICE_ID. */ -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO, 3) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) #ifdef CONFIG_STM32_SPI4 -# define PX4_SPIDEV_EXTERNAL PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL, 1) +# define PX4_SPIDEV_EXTERNAL PX4_MK_SPI_SEL(0, 0) #endif /* CONFIG_STM32_SPI4 */ /* I2C busses. */ @@ -188,7 +188,6 @@ /* Power supply control and monitoring GPIOs. */ #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) -#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) /* Tone alarm output. */ #define TONE_ALARM_TIMER 2 /* timer 2 */ @@ -307,14 +306,9 @@ __BEGIN_DECLS * Description: * Called to configure SPI chip select GPIO pins for the PX4FMU board. * - * mask - is bus selection - * 1 - 1 << 0 - * 2 - 1 << 1 - * ****************************************************************************************************/ -extern void stm32_spiinitialize(int mask); -void board_spi_reset(int ms); +extern void stm32_spiinitialize(void); extern void stm32_usbinitialize(void); diff --git a/boards/px4/fmu-v4/src/i2c.cpp b/boards/px4/fmu-v4/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/px4/fmu-v4/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/px4/fmu-v4/src/init.c b/boards/px4/fmu-v4/src/init.c index 340569181595..6c43b273ea2f 100644 --- a/boards/px4/fmu-v4/src/init.c +++ b/boards/px4/fmu-v4/src/init.c @@ -148,7 +148,7 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } - /** + /* * On resets invoked from system (not boot) insure we establish a low * output state (discharge the pins) on PWM pins before they become inputs. */ @@ -193,12 +193,6 @@ stm32_boardinitialize(void) stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_USB_VALID); - /** - * Start with Sensor voltage off We will enable it - * in board_app_initialize. - */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_configgpio(GPIO_SBUS_INV); stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); @@ -209,15 +203,10 @@ stm32_boardinitialize(void) stm32_configgpio(GPIO_BTN_SAFETY); stm32_configgpio(GPIO_PPM_IN); - int spi_init_mask = SPI_BUS_INIT_MASK; - #if defined(CONFIG_STM32_SPI4) /* We have SPI4 is GPIO_8266_GPIO2 PB4 pin 3 Low */ - if (stm32_gpioread(GPIO_8266_GPIO2) == 0) { - spi_init_mask |= SPI_BUS_INIT_MASK_EXT; - - } else { + if (stm32_gpioread(GPIO_8266_GPIO2) != 0) { #endif /* CONFIG_STM32_SPI4 */ stm32_configgpio(GPIO_8266_PD); @@ -228,8 +217,8 @@ stm32_boardinitialize(void) #endif /* CONFIG_STM32_SPI4 */ -// Configure SPI all interfaces GPIO. - stm32_spiinitialize(spi_init_mask); + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); // Configure heater GPIO. stm32_configgpio(GPIO_HEATER_INPUT); @@ -304,9 +293,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) led_on(LED_RED); } - // Power up the sensors. - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - // Power down the heater. stm32_gpiowrite(GPIO_HEATER_OUTPUT, 0); diff --git a/boards/px4/fmu-v4/src/spi.c b/boards/px4/fmu-v4/src/spi.c deleted file mode 100644 index 0efba013cde6..000000000000 --- a/boards/px4/fmu-v4/src/spi.c +++ /dev/null @@ -1,316 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -__EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus) -{ - bool rv = true; - - switch (type) { - case BOARD_SPI_BUS: -#ifdef CONFIG_STM32_SPI4 - rv = bus != PX4_SPI_BUS_EXTERNAL || (stm32_gpioread(GPIO_8266_GPIO2) == 0); -#endif /* CONFIG_STM32_SPI4 */ - break; - - case BOARD_I2C_BUS: - break; - } - - return rv; -} - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * mask - is bus selection - * 1 - 1 << 0 - * 2 - 1 << 1 - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize(int mask) -{ -#ifdef CONFIG_STM32_SPI1 - - if (mask & PX4_SPI_BUS_SENSORS) { - stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN2); - stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN15); - stm32_configgpio(GPIO_SPI1_CS_PORTE_PIN15); - - stm32_configgpio(GPIO_DRDY_PORTD_PIN15); - stm32_configgpio(GPIO_DRDY_PORTC_PIN14); - stm32_configgpio(GPIO_DRDY_PORTE_PIN12); - } - -#endif - -#ifdef CONFIG_STM32_SPI2 - - if (mask & (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_BARO)) { - stm32_configgpio(GPIO_SPI2_CS_MS5611); - stm32_configgpio(GPIO_SPI2_CS_FRAM); - } - -#endif - -#ifdef CONFIG_STM32_SPI4 - - if (mask & PX4_SPI_BUS_EXTERNAL) { - stm32_configgpio(GPIO_SPI4_CS_1); //add cs - } - -#endif /* CONFIG_STM32_SPI4 */ -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - - /* Shared PC2 CS devices */ - - case PX4_SPIDEV_MPU: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, !selected); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); - break; - - /* Shared PC15 CS devices */ - - case PX4_SPIDEV_ICM_20602: - case PX4_SPIDEV_ICM_20608: - case PX4_SPIDEV_MPU2: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, !selected); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); - break; - - /* Shared PE15 CS devices */ - - case PX4_SPIDEV_HMC: - case PX4_SPIDEV_LIS: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - case SPIDEV_FLASH(0): - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI2_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI2_CS_FRAM, !selected); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI2_CS_FRAM, 1); - stm32_gpiowrite(GPIO_SPI2_CS_MS5611, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif - -#ifdef CONFIG_STM32_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == PX4_SPIDEV_EXTERNAL && stm32_gpioread(GPIO_8266_GPIO2) == 0) { - stm32_gpiowrite(GPIO_SPI4_CS_1, !selected); // add cs - } -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif /* CONFIG_STM32_SPI4 */ - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus 1 DRDY */ - - stm32_configgpio(GPIO_DRDY_OFF_PORTD_PIN15); - stm32_configgpio(GPIO_DRDY_OFF_PORTC_PIN14); - stm32_configgpio(GPIO_DRDY_OFF_PORTE_PIN12); - - stm32_gpiowrite(GPIO_DRDY_OFF_PORTD_PIN15, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_PORTC_PIN14, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_PORTE_PIN12, 0); - - /* disable SPI bus 1 CS */ - - stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN2); - stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN15); - stm32_configgpio(GPIO_SPI1_CS_OFF_PORTE_PIN15); - - stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN2, 0); - stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN15, 0); - stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTE_PIN15, 0); - - /* disable SPI bus 1*/ - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - -#ifdef CONFIG_STM32_SPI4 - - /* disable SPI bus 4*/ - if (stm32_gpioread(GPIO_8266_GPIO2) == 0) { - stm32_configgpio(GPIO_SPI4_SCK_OFF); - stm32_configgpio(GPIO_SPI4_MISO_OFF); - stm32_configgpio(GPIO_SPI4_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); - } - -#endif /* CONFIG_STM32_SPI4 */ - - /* N.B we do not have control over the SPI 2 buss powered devices - * so the the ms5611 is not resetable. - */ - - /* set the sensor rail off (default) */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - -#ifdef CONFIG_STM32_SPI4 - - if (stm32_gpioread(GPIO_8266_GPIO2) == 0) { - /* set the periph rail off (default) for SPI4 */ - stm32_configgpio(GPIO_PERIPH_3V3_EN); - } - -#endif /* CONFIG_STM32_SPI4 */ - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - -#ifdef CONFIG_STM32_SPI4 - - if (stm32_gpioread(GPIO_8266_GPIO2) == 0) { - /* switch the periph rail back on */ - stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); - } - -#endif /* CONFIG_STM32_SPI4 */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - stm32_spiinitialize(PX4_SPI_BUS_SENSORS); - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - -#ifdef CONFIG_STM32_SPI4 - - if (stm32_gpioread(GPIO_8266_GPIO2) == 0) { - stm32_spiinitialize(PX4_SPI_BUS_EXTERNAL); - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); - } - -#endif /* CONFIG_STM32_SPI4 */ -} diff --git a/boards/px4/fmu-v4/src/spi.cpp b/boards/px4/fmu-v4/src/spi.cpp new file mode 100644 index 000000000000..90bb1e264586 --- /dev/null +++ b/boards/px4/fmu-v4/src/spi.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_MAG_DEVTYPE_HMC5883, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), // hmc5983 + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBusExternal(4, { + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin8}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +__EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus) +{ + bool rv = true; + + switch (type) { + case BOARD_SPI_BUS: +#ifdef CONFIG_STM32_SPI4 + rv = bus != 4 || (stm32_gpioread(GPIO_8266_GPIO2) == 0); +#endif /* CONFIG_STM32_SPI4 */ + break; + + case BOARD_I2C_BUS: + break; + + default: break; + } + + return rv; +} + diff --git a/boards/px4/fmu-v4pro/src/CMakeLists.txt b/boards/px4/fmu-v4pro/src/CMakeLists.txt index 26cde8bf5cf7..a7db4590f0da 100644 --- a/boards/px4/fmu-v4pro/src/CMakeLists.txt +++ b/boards/px4/fmu-v4pro/src/CMakeLists.txt @@ -33,14 +33,16 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/px4/fmu-v4pro/src/board_config.h b/boards/px4/fmu-v4pro/src/board_config.h index 94cbeee8afd1..fc118cac9f09 100644 --- a/boards/px4/fmu-v4pro/src/board_config.h +++ b/boards/px4/fmu-v4pro/src/board_config.h @@ -162,16 +162,17 @@ /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) -#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) -#define PX4_SPIDEV_EEPROM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 5) -#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 6) -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 7) - -#define PX4_SPIDEV_EXT0 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT0, 1) -#define PX4_SPIDEV_EXT1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT1, 1) +#include +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_HMC5883) +#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_LIS3MDL) +#define PX4_SPIDEV_EEPROM PX4_MK_SPI_SEL(0, DRV_DEVTYPE_UNUSED) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20602) + +#define PX4_SPIDEV_EXT0 PX4_MK_SPI_SEL(0, 0) +#define PX4_SPIDEV_EXT1 PX4_MK_SPI_SEL(0, 1) #define PX4_SPIDEV_RM_EXT PX4_SPIDEV_EXT0 @@ -218,7 +219,6 @@ #define GPIO_nVDD_BRICK2_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN5) #define BOARD_NUMBER_BRICKS 2 #define GPIO_nVDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) -#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_3V3_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN10) #define GPIO_VDD_5V_HIPOWER_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN4) @@ -316,7 +316,6 @@ __BEGIN_DECLS ****************************************************************************************************/ extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); extern void stm32_usbinitialize(void); diff --git a/boards/px4/fmu-v4pro/src/i2c.cpp b/boards/px4/fmu-v4pro/src/i2c.cpp new file mode 100644 index 000000000000..5802883bfa13 --- /dev/null +++ b/boards/px4/fmu-v4pro/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(1), + initI2CBusExternal(2), +}; + diff --git a/boards/px4/fmu-v4pro/src/init.c b/boards/px4/fmu-v4pro/src/init.c index 3f3acd54f259..efdf175b7261 100644 --- a/boards/px4/fmu-v4pro/src/init.c +++ b/boards/px4/fmu-v4pro/src/init.c @@ -110,7 +110,7 @@ __EXPORT void board_peripheral_reset(int ms) { /* set the peripheral and sensor rails off */ stm32_gpiowrite(GPIO_VDD_3V3_PERIPH_EN, 0); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + board_control_spi_sensors_power(false, 0xffff); stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); stm32_gpiowrite(GPIO_VDD_5V_HIPOWER_EN, 1); @@ -128,7 +128,7 @@ __EXPORT void board_peripheral_reset(int ms) /* switch the peripheral rail back on */ // stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); stm32_gpiowrite(GPIO_VDD_3V3_PERIPH_EN, 1); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + board_control_spi_sensors_power(true, 0xffff); stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); stm32_gpiowrite(GPIO_VDD_5V_HIPOWER_EN, 0); } @@ -191,7 +191,7 @@ stm32_boardinitialize(void) board_autoled_initialize(); /* Start with Power off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + board_control_spi_sensors_power_configgpio(); /* configure ADC pins */ stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ @@ -270,13 +270,13 @@ static struct sdio_dev_s *sdio; __EXPORT int board_app_initialize(uintptr_t arg) { - /* Bring up the Sensor power */ - - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); /* Now it is ok to drvie the pins high * so configure SPI CPIO */ + // the temp cal eeprom is unused, so disable the CS from here + stm32_configgpio(GPIO_SPI_CS_TEMPCAL_EEPROM); + stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, 1); stm32_spiinitialize(); px4_platform_init(); @@ -316,10 +316,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Configure SPI-based devices */ - spi1 = stm32_spibus_initialize(PX4_SPI_BUS_SENSORS); + spi1 = stm32_spibus_initialize(1); if (!spi1) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); led_on(LED_RED); return -ENODEV; } @@ -337,10 +337,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Get the SPI port for the FRAM */ - spi2 = stm32_spibus_initialize(PX4_SPI_BUS_RAMTRON); + spi2 = stm32_spibus_initialize(2); if (!spi2) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_RAMTRON); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); led_on(LED_RED); return -ENODEV; } @@ -356,10 +356,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Configure SPI 5-based devices */ - spi5 = stm32_spibus_initialize(PX4_SPI_BUS_EXT0); + spi5 = stm32_spibus_initialize(5); if (!spi5) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXT0); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 5); led_on(LED_RED); return -ENODEV; } @@ -372,10 +372,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Configure SPI 6-based devices */ - spi6 = stm32_spibus_initialize(PX4_SPI_BUS_EXT1); + spi6 = stm32_spibus_initialize(6); if (!spi6) { - syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXT1); + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 6); led_on(LED_RED); return -ENODEV; } diff --git a/boards/px4/fmu-v4pro/src/spi.c b/boards/px4/fmu-v4pro/src/spi.c deleted file mode 100644 index 71b6a6e5ae60..000000000000 --- a/boards/px4/fmu-v4pro/src/spi.c +++ /dev/null @@ -1,283 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_MPU9250); - stm32_configgpio(GPIO_SPI_CS_LIS3MDL); - stm32_configgpio(GPIO_SPI_CS_MS5611); - stm32_configgpio(GPIO_SPI_CS_ICM_2060X); - stm32_configgpio(GPIO_SPI_CS_TEMPCAL_EEPROM); - - stm32_configgpio(GPIO_DRDY_MPU9250); - stm32_configgpio(GPIO_DRDY_LIS3MDL); - stm32_configgpio(GPIO_DRDY_ICM_2060X); -#endif - -#ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI_CS_FRAM); -#endif -#ifdef CONFIG_STM32_SPI5 - stm32_configgpio(GPIO_SPI5_CS); -#endif -#ifdef CONFIG_STM32_SPI6 - stm32_configgpio(GPIO_SPI6_CS); -#endif - -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - case PX4_SPIDEV_ICM_20602: - - // FALLTHROUGH - case PX4_SPIDEV_ICM_20608: - - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_LIS3MDL, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_2060X, !selected); - stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, 1); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_LIS3MDL, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, 1); - break; - - case PX4_SPIDEV_LIS: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_LIS3MDL, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, 1); - break; - - case PX4_SPIDEV_MPU: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); - stm32_gpiowrite(GPIO_SPI_CS_LIS3MDL, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, 1); - break; - - case PX4_SPIDEV_EEPROM: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); - stm32_gpiowrite(GPIO_SPI_CS_LIS3MDL, 1); - stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); - stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ - -#ifdef CONFIG_STM32_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - stm32_gpiowrite(GPIO_SPI5_CS, !selected); - -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif - -/************************************************************************************ - * Name: stm32_spi6select and stm32_spi6status - * - * Description: - * Called by stm32 spi driver on bus 6. - * - ************************************************************************************/ - -#ifdef CONFIG_STM32_SPI6 -__EXPORT void stm32_spi6select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - stm32_gpiowrite(GPIO_SPI6_CS, !selected); - -} - -__EXPORT uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus */ - stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250); - stm32_configgpio(GPIO_SPI_CS_OFF_LIS3MDL); - stm32_configgpio(GPIO_SPI_CS_OFF_MS5611); - stm32_configgpio(GPIO_SPI_CS_OFF_ICM_2060X); - stm32_configgpio(GPIO_SPI_CS_TEMPCAL_EEPROM); - - stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); - stm32_gpiowrite(GPIO_SPI_CS_OFF_LIS3MDL, 0); - stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); - stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_2060X, 0); - stm32_gpiowrite(GPIO_SPI_CS_TEMPCAL_EEPROM, 0); - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - - stm32_configgpio(GPIO_DRDY_OFF_MPU9250); - stm32_configgpio(GPIO_DRDY_OFF_LIS3MDL); - stm32_configgpio(GPIO_DRDY_OFF_ICM_2060X); - - stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_LIS3MDL, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_ICM_2060X, 0); - - /* set the sensor rail off */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_MPU9250); - stm32_configgpio(GPIO_SPI_CS_LIS3MDL); - stm32_configgpio(GPIO_SPI_CS_MS5611); - stm32_configgpio(GPIO_SPI_CS_ICM_2060X); - stm32_configgpio(GPIO_SPI_CS_TEMPCAL_EEPROM); - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - - /* bring up the EXTI pins again */ - stm32_configgpio(GPIO_DRDY_MPU9250); - stm32_configgpio(GPIO_DRDY_LIS3MDL); - stm32_configgpio(GPIO_DRDY_ICM_2060X); - -#endif - -} diff --git a/boards/px4/fmu-v4pro/src/spi.cpp b/boards/px4/fmu-v4pro/src/spi.cpp new file mode 100644 index 000000000000..fe04a740489d --- /dev/null +++ b/boards/px4/fmu-v4pro/src/spi.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_MAG_DEVTYPE_LIS3MDL, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + }), + initSPIBusExternal(5, { + initSPIConfigExternal(SPI::CS{GPIO::PortF, GPIO::Pin6}), + }), + initSPIBusExternal(6, { + initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin11}), + }), +}; + diff --git a/boards/px4/fmu-v5/src/CMakeLists.txt b/boards/px4/fmu-v5/src/CMakeLists.txt index f1244df5782e..722847ed9750 100644 --- a/boards/px4/fmu-v5/src/CMakeLists.txt +++ b/boards/px4/fmu-v5/src/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c manifest.c @@ -46,6 +47,7 @@ add_dependencies(drivers_board arch_board_hw_info) target_link_libraries(drivers_board PRIVATE arch_board_hw_info + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index 157fda67cbbb..97a884c644cd 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -182,18 +182,19 @@ /* v BEGIN Legacy SPI defines TODO: fix this with enumeration */ #define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_MEMORY /* ^ END Legacy SPI defines TODO: fix this with enumeration */ +#include -#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,0) -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,1) -#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,2) -#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,3) -#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,4) +#define PX4_SPIDEV_ICM_20689 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20689) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20602) +#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_BMI055) +#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_BMI055) +#define PX4_SPIDEV_AUX_MEM PX4_MK_SPI_SEL(0,4) #define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI1_CS1_ICM20689, GPIO_SPI1_CS2_ICM20602, GPIO_SPI1_CS3_BMI055_GYRO, GPIO_SPI1_CS4_BMI055_ACC, GPIO_SPI1_CS5_AUX_MEM} -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) #define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI2_CS_FRAM} -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO,0) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0,DRV_BARO_DEVTYPE_MS5611) #define PX4_SPIDEV_SPI4_CS2 PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO,1) #define PX4_BARO_BUS_CS_GPIO {GPIO_SPI4_CS1_MS5611, GPIO_SPI4_CS2} @@ -384,7 +385,6 @@ #define GPIO_nVDD_5V_PERIPH_OC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) #define GPIO_nVDD_5V_HIPOWER_EN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) #define GPIO_nVDD_5V_HIPOWER_OC /* PG13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) -#define GPIO_VDD_3V3_SENSORS_EN /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define GPIO_VDD_5V_RC_EN /* PG5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5) #define GPIO_VDD_5V_WIFI_EN /* PG6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5) @@ -395,7 +395,6 @@ #define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_PERIPH_EN, !(on_true)) #define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_nVDD_5V_HIPOWER_EN, !(on_true)) -#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, (on_true)) #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) #define VDD_5V_RC_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_RC_EN, (on_true)) @@ -575,7 +574,6 @@ GPIO_nVDD_5V_PERIPH_OC, \ GPIO_nVDD_5V_HIPOWER_EN, \ GPIO_nVDD_5V_HIPOWER_OC, \ - GPIO_VDD_3V3_SENSORS_EN, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_VDD_5V_RC_EN, \ GPIO_VDD_5V_WIFI_EN, \ @@ -629,8 +627,6 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/px4/fmu-v5/src/i2c.cpp b/boards/px4/fmu-v5/src/i2c.cpp new file mode 100644 index 000000000000..63e1172b7b69 --- /dev/null +++ b/boards/px4/fmu-v5/src/i2c.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(3), + initI2CBusExternal(4), +}; + diff --git a/boards/px4/fmu-v5/src/init.c b/boards/px4/fmu-v5/src/init.c index 2f982d426c12..cc73c4272589 100644 --- a/boards/px4/fmu-v5/src/init.c +++ b/boards/px4/fmu-v5/src/init.c @@ -105,7 +105,7 @@ __EXPORT void board_peripheral_reset(int ms) /* set the peripheral rails off */ VDD_5V_PERIPH_EN(false); - VDD_3V3_SENSORS_EN(false); + board_control_spi_sensors_power(false, 0xffff); bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); /* Keep Spektum on to discharge rail*/ @@ -119,7 +119,7 @@ __EXPORT void board_peripheral_reset(int ms) /* switch the peripheral rail back on */ VDD_3V3_SPEKTRUM_POWER_EN(last); - VDD_3V3_SENSORS_EN(true); + board_control_spi_sensors_power(true, 0xffff); VDD_5V_PERIPH_EN(true); } @@ -169,10 +169,7 @@ stm32_boardinitialize(void) const uint32_t gpio[] = PX4_GPIO_INIT_LIST; px4_gpio_init(gpio, arraySize(gpio)); - - /* configure SPI interfaces */ - - stm32_spiinitialize(); + board_control_spi_sensors_power_configgpio(); /* configure USB interfaces */ @@ -212,7 +209,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); - VDD_3V3_SENSORS_EN(true); + board_control_spi_sensors_power(true, 0xffff); VDD_3V3_SPEKTRUM_POWER_EN(true); VDD_5V_RC_EN(true); VDD_5V_WIFI_EN(true); @@ -230,6 +227,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); } + /* configure SPI interfaces (after we determined the HW version) */ + + stm32_spiinitialize(); + /* configure the DMA allocator */ if (board_dma_alloc_init() < 0) { diff --git a/boards/px4/fmu-v5/src/spi.cpp b/boards/px4/fmu-v5/src/spi.cpp index 75c1daff7efe..7cb3ffb9ceb4 100644 --- a/boards/px4/fmu-v5/src/spi.cpp +++ b/boards/px4/fmu-v5/src/spi.cpp @@ -31,291 +31,34 @@ * ****************************************************************************/ -/** - * @file spi.cpp - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/* Define CS GPIO array */ -static constexpr uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; -static constexpr uint32_t spi2selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; -static constexpr uint32_t spi4selects_gpio[] = PX4_BARO_BUS_CS_GPIO; -static constexpr uint32_t spi5selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; -static constexpr uint32_t spi6selects_gpio[] = PX4_EXTERNAL2_BUS_CS_GPIO; - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32F7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI1 - - -#if defined(CONFIG_STM32F7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI2 - - -#ifdef CONFIG_STM32F7_SPI4 - - for (auto gpio : spi4selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI4 - - -#ifdef CONFIG_STM32F7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI5 - - -#ifdef CONFIG_STM32F7_SPI6 - - for (auto gpio : spi6selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI6 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == SPIDEV_FLASH(0)) { - devid = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_MEMORY); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI2 && GPIO_SPI2_CS_FRAM - -/************************************************************************************ - * Name: stm32_spi4select and stm32_spi4status - * - * Description: - * Called by stm32 spi driver on bus 4. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_BARO); - - // Making sure the other peripherals are not selected - for (auto cs : spi4selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi4selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI4 - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_EXTERNAL1); - - // Making sure the other peripherals are not selected - for (auto cs : spi5selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI5 - -/************************************************************************************ - * Name: stm32_spi6select and stm32_spi6status - * - * Description: - * Called by stm32 spi driver on bus 6. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI6 -__EXPORT void stm32_spi6select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_EXTERNAL2); - - // Making sure the other peripherals are not selected - for (auto cs : spi6selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi6selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI6 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int ms) -{ - // disable SPI bus - for (auto cs : spi1selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY1_ICM20689); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY2_BMI055_GYRO); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY3_BMI055_ACC); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY4_ICM20602); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY5_BMI055_GYRO); - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY6_BMI055_ACC); -#endif - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - /* reconfigure the SPI pins */ - for (auto cs : spi1selects_gpio) { - stm32_configgpio(cs); - } - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11}) + }), + initSPIBusExternal(6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI1_DRDY1_ICM20689); - stm32_configgpio(GPIO_SPI1_DRDY2_BMI055_GYRO); - stm32_configgpio(GPIO_SPI1_DRDY3_BMI055_ACC); - stm32_configgpio(GPIO_SPI1_DRDY4_ICM20602); - stm32_configgpio(GPIO_SPI1_DRDY5_BMI055_GYRO); - stm32_configgpio(GPIO_SPI1_DRDY6_BMI055_ACC); -#endif -} diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 6edcfc149bf6..8e050cac2df7 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -30,7 +30,7 @@ qmc5883 -X start # Possible internal compass bmm150 start -# Possible internal Barro +# Possible internal Baro bmp388 -I start bmp388 -J start diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index f1244df5782e..3ccb6124bf3a 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c manifest.c @@ -45,6 +46,7 @@ add_dependencies(drivers_board arch_board_hw_info) target_link_libraries(drivers_board PRIVATE + arch_spi arch_board_hw_info drivers__led # drv_led_start nuttx_arch # sdio diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 5d95f90fb7c3..61ed100e0fe3 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -227,21 +227,22 @@ #define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) #define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_MEMORY -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS1,0) +#include +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20602) #define PX4_SENSORS1_BUS_CS_GPIO {GPIO_SPI1_nCS1_ICM20602} -#define PX4_SPIDEV_ISM330 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS2,0) +#define PX4_SPIDEV_ISM330 PX4_MK_SPI_SEL(0,DRV_DEVTYPE_ST_ISM330DLC) #define PX4_SENSORS2_BUS_CS_GPIO {GPIO_SPI2_nCS1_ISM330} -#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS3,0) -#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS3,1) +#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_BMI088) +#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(0,DRV_ACC_DEVTYPE_BMI088) #define PX4_SENSORS3_BUS_CS_GPIO {GPIO_SPI3_nCS2_BMI088_GYRO, GPIO_SPI3_nCS1_BMI088_ACCEL} -#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_MEMORY,0) +#define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) #define PX4_MEMORY_BUS_CS_GPIO {GPIO_SPI5_nCS1_FRAM} -#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,0) -#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL1,1) +#define PX4_SPIDEV_EXTERNAL1_1 PX4_MK_SPI_SEL(0,0) +#define PX4_SPIDEV_EXTERNAL1_2 PX4_MK_SPI_SEL(0,1) #define PX4_EXTERNAL1_BUS_CS_GPIO {GPIO_SPI6_nCS1_EXTERNAL1, GPIO_SPI6_nCS2_EXTERNAL1} @@ -389,9 +390,6 @@ #define GPIO_VDD_5V_PERIPH_nOC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) #define GPIO_VDD_5V_HIPOWER_nEN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) #define GPIO_VDD_5V_HIPOWER_nOC /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) -#define GPIO_VDD_3V3_SENSORS1_EN /* PI11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN11) -#define GPIO_VDD_3V3_SENSORS2_EN /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) -#define GPIO_VDD_3V3_SENSORS3_EN /* PE7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN7) #define GPIO_VDD_3V3_SENSORS4_EN /* PG8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN8) #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) #define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) @@ -413,9 +411,6 @@ #define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) #define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) -#define VDD_3V3_SENSORS1_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS1_EN, (on_true)) -#define VDD_3V3_SENSORS2_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS2_EN, (on_true)) -#define VDD_3V3_SENSORS3_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS3_EN, (on_true)) #define VDD_3V3_SENSORS4_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) @@ -569,9 +564,6 @@ GPIO_VDD_5V_PERIPH_nOC, \ GPIO_VDD_5V_HIPOWER_nEN, \ GPIO_VDD_5V_HIPOWER_nOC, \ - GPIO_VDD_3V3_SENSORS1_EN, \ - GPIO_VDD_3V3_SENSORS2_EN, \ - GPIO_VDD_3V3_SENSORS3_EN, \ GPIO_VDD_3V3_SENSORS4_EN, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_VDD_3V3_SD_CARD_EN, \ @@ -627,8 +619,6 @@ int stm32_sdio_initialize(void); extern void stm32_spiinitialize(void); -void board_spi_reset(int ms); - extern void stm32_usbinitialize(void); extern void board_peripheral_reset(int ms); diff --git a/boards/px4/fmu-v5x/src/i2c.cpp b/boards/px4/fmu-v5x/src/i2c.cpp new file mode 100644 index 000000000000..8a557078e0fd --- /dev/null +++ b/boards/px4/fmu-v5x/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusExternal(3), + initI2CBusInternal(4), +}; diff --git a/boards/px4/fmu-v5x/src/init.c b/boards/px4/fmu-v5x/src/init.c index 7963ead2496d..852ed8a211d4 100644 --- a/boards/px4/fmu-v5x/src/init.c +++ b/boards/px4/fmu-v5x/src/init.c @@ -105,9 +105,7 @@ __EXPORT void board_peripheral_reset(int ms) /* set the peripheral rails off */ VDD_5V_PERIPH_EN(false); - VDD_3V3_SENSORS1_EN(false); - VDD_3V3_SENSORS2_EN(false); - VDD_3V3_SENSORS3_EN(false); + board_control_spi_sensors_power(false, 0xffff); VDD_3V3_SENSORS4_EN(false); bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); @@ -122,9 +120,7 @@ __EXPORT void board_peripheral_reset(int ms) /* switch the peripheral rail back on */ VDD_3V3_SPEKTRUM_POWER_EN(last); - VDD_3V3_SENSORS1_EN(true); - VDD_3V3_SENSORS2_EN(true); - VDD_3V3_SENSORS3_EN(true); + board_control_spi_sensors_power(true, 0xffff); VDD_3V3_SENSORS4_EN(true); VDD_5V_PERIPH_EN(true); @@ -176,7 +172,9 @@ stm32_boardinitialize(void) const uint32_t gpio[] = PX4_GPIO_INIT_LIST; px4_gpio_init(gpio, arraySize(gpio)); - /* configure SPI interfaces */ + /* configure SPI interfaces (we can do this here as long as we only have a single SPI hw config version - + * otherwise we need to move this after board_determine_hw_info()) */ + _Static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support"); stm32_spiinitialize(); @@ -220,7 +218,8 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); - board_spi_reset(0xff00000A); + board_spi_reset(10, 0xffff); + VDD_3V3_SENSORS4_EN(true); VDD_3V3_SPEKTRUM_POWER_EN(true); SE050_RESET(false); diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index fce3116c114e..4ad043eb17c0 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -31,392 +31,33 @@ * ****************************************************************************/ -/** - * @file spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - +#include +#include #include -#include -#include - -#include -#include -#include -#include "board_config.h" - -/* Define CS GPIO array */ -static constexpr uint32_t spi1selects_gpio[] = PX4_SENSORS1_BUS_CS_GPIO; -static constexpr uint32_t spi2selects_gpio[] = PX4_SENSORS2_BUS_CS_GPIO; -static constexpr uint32_t spi3selects_gpio[] = PX4_SENSORS3_BUS_CS_GPIO; -#ifdef CONFIG_STM32F7_SPI4 -static constexpr uint32_t spi4selects_gpio[] = PX4_SENSORS4_BUS_CS_GPIO; -#endif -static constexpr uint32_t spi5selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; -static constexpr uint32_t spi6selects_gpio[] = PX4_EXTERNAL1_BUS_CS_GPIO; - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize() -{ -#ifdef CONFIG_STM32F7_SPI1 - - for (auto gpio : spi1selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI1 - -#if defined(CONFIG_STM32F7_SPI2) - - for (auto gpio : spi2selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI2 - -#if defined(CONFIG_STM32F7_SPI3) - - for (auto gpio : spi3selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI3 - - -#ifdef CONFIG_STM32F7_SPI4 - - for (auto gpio : spi4selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI4 - - -#ifdef CONFIG_STM32F7_SPI5 - - for (auto gpio : spi5selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI5 - - -#ifdef CONFIG_STM32F7_SPI6 - - for (auto gpio : spi6selects_gpio) { - px4_arch_configgpio(gpio); - } - -#endif // CONFIG_STM32F7_SPI6 -} - -/************************************************************************************ - * Name: stm32_spi1select and stm32_spi1status - * - * Description: - * Called by stm32 spi driver on bus 1. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI1 -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS1); - - // Making sure the other peripherals are not selected - for (auto cs : spi1selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI1 - -/************************************************************************************ - * Name: stm32_spi2select and stm32_spi2status - * - * Description: - * Called by stm32 spi driver on bus 2. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI2) -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS2); - - // Making sure the other peripherals are not selected - for (auto cs : spi2selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI2 - -/************************************************************************************ - * Name: stm32_spi3select and stm32_spi3status - * - * Description: - * Called by stm32 spi driver on bus 3. - * - ************************************************************************************/ -#if defined(CONFIG_STM32F7_SPI3) -__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS3); - - // Making sure the other peripherals are not selected - for (auto cs : spi3selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi3selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI3 - -/************************************************************************************ - * Name: stm32_spi4select and stm32_spi4status - * - * Description: - * Called by stm32 spi driver on bus 4. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_SENSORS4); - - // Making sure the other peripherals are not selected - for (auto cs : spi4selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi4selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI4 - -/************************************************************************************ - * Name: stm32_spi5select and stm32_spi5status - * - * Description: - * Called by stm32 spi driver on bus 5. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI5 -__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == SPIDEV_FLASH(0)) { - devid = PX4_SPIDEV_MEMORY; - } - - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_MEMORY); - - // Making sure the other peripherals are not selected - for (auto cs : spi5selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI5 - -/************************************************************************************ - * Name: stm32_spi6select and stm32_spi6status - * - * Description: - * Called by stm32 spi driver on bus 6. - * - ************************************************************************************/ -#ifdef CONFIG_STM32F7_SPI6 -__EXPORT void stm32_spi6select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_EXTERNAL1); - - // Making sure the other peripherals are not selected - for (auto cs : spi6selects_gpio) { - stm32_gpiowrite(cs, 1); - } - - // SPI select is active low, so write !selected to select the device - stm32_gpiowrite(spi6selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); -} - -__EXPORT uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif // CONFIG_STM32F7_SPI6 - -/************************************************************************************ - * Name: board_spi_reset - * - * Description: - * TODO:Add 4 bit MASK active LOW for Bus 1-4 - * - ************************************************************************************/ - -__EXPORT void board_spi_reset(int mask_ms) -{ - int ms = mask_ms & 0x00ffffff; - int mask = ((mask_ms & 0xff000000) >> 24) & 0xff; - - // disable SPI bus - - if (mask & 1) { - for (auto cs : spi1selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI1_DRDY1_ICM20602); -#endif - - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS1_EN, 0); - - } - - if (mask & 2) { - for (auto cs : spi2selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI2_SCK_OFF); - stm32_configgpio(GPIO_SPI2_MISO_OFF); - stm32_configgpio(GPIO_SPI2_MOSI_OFF); - stm32_configgpio(GPIO_DRDY_OFF_SPI2_DRDY1_ISM330); - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS2_EN, 0); - } - - if (mask & 4) { - for (auto cs : spi3selects_gpio) { - stm32_configgpio(_PIN_OFF(cs)); - } - - stm32_configgpio(GPIO_SPI3_SCK_OFF); - stm32_configgpio(GPIO_SPI3_MISO_OFF); - stm32_configgpio(GPIO_SPI3_MOSI_OFF); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_DRDY_OFF_SPI3_DRDY1_BMI088); - stm32_configgpio(GPIO_DRDY_OFF_SPI3_DRDY2_BMI088); -#endif - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS3_EN, 0); - } - - if (mask & 8) { - /* set the sensor rail off */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, 0); - } - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS1_EN, 1); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS2_EN, 1); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS3_EN, 1); - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - if (mask & 1) { - /* reconfigure the SPI pins */ - for (auto cs : spi1selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI1_DRDY1_ICM20602); -#endif - - } - - if (mask & 2) { - /* reconfigure the SPI pins */ - for (auto cs : spi2selects_gpio) { - stm32_configgpio(cs); - } - - stm32_configgpio(GPIO_SPI2_SCK); - stm32_configgpio(GPIO_SPI2_MISO); - stm32_configgpio(GPIO_SPI2_MOSI); - stm32_configgpio(GPIO_SPI2_DRDY1_ISM330); - } - if (mask & 4) { - /* reconfigure the SPI pins */ - for (auto cs : spi3selects_gpio) { - stm32_configgpio(cs); - } +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(2, { + initSPIDevice(DRV_DEVTYPE_ST_ISM330DLC, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), +// initSPIBus(4, { +// // no devices +// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h +// }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); - stm32_configgpio(GPIO_SPI3_SCK); - stm32_configgpio(GPIO_SPI3_MISO); - stm32_configgpio(GPIO_SPI3_MOSI); -#if BOARD_USE_DRDY - stm32_configgpio(GPIO_SPI3_DRDY1_BMI088); - stm32_configgpio(GPIO_SPI3_DRDY2_BMI088); -#endif - } -} diff --git a/boards/px4/io-v2/src/board_config.h b/boards/px4/io-v2/src/board_config.h index 2cc16eecf4c4..715a9f80bec0 100644 --- a/boards/px4/io-v2/src/board_config.h +++ b/boards/px4/io-v2/src/board_config.h @@ -166,3 +166,5 @@ #define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ #define BOARD_NUM_IO_TIMERS 3 + +#define BOARD_DISABLE_I2C_SPI diff --git a/boards/px4/raspberrypi/src/CMakeLists.txt b/boards/px4/raspberrypi/src/CMakeLists.txt index 8cc8e0994d91..42ca05a19b10 100644 --- a/boards/px4/raspberrypi/src/CMakeLists.txt +++ b/boards/px4/raspberrypi/src/CMakeLists.txt @@ -32,5 +32,6 @@ ############################################################################ add_library(drivers_board - empty.c + i2c.cpp + spi.cpp ) diff --git a/boards/px4/raspberrypi/src/board_config.h b/boards/px4/raspberrypi/src/board_config.h index de84c6fe11a4..9f2e3bc25d79 100644 --- a/boards/px4/raspberrypi/src/board_config.h +++ b/boards/px4/raspberrypi/src/board_config.h @@ -55,7 +55,7 @@ // SPI #define PX4_SPI_BUS_SENSORS 0 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) // spidev0.0 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, 0) // spidev0.0 #include #include diff --git a/boards/px4/raspberrypi/src/empty.c b/boards/px4/raspberrypi/src/empty.c deleted file mode 100644 index e69de29bb2d1..000000000000 diff --git a/boards/px4/raspberrypi/src/i2c.cpp b/boards/px4/raspberrypi/src/i2c.cpp new file mode 100644 index 000000000000..37993cfa8498 --- /dev/null +++ b/boards/px4/raspberrypi/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(0), + initI2CBusInternal(1), +}; + diff --git a/boards/px4/raspberrypi/src/spi.cpp b/boards/px4/raspberrypi/src/spi.cpp new file mode 100644 index 000000000000..97343740bab7 --- /dev/null +++ b/boards/px4/raspberrypi/src/spi.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(0, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, 0), + }), +}; + diff --git a/boards/uvify/core/src/CMakeLists.txt b/boards/uvify/core/src/CMakeLists.txt index f78b4061f4b3..8e470f2ead15 100644 --- a/boards/uvify/core/src/CMakeLists.txt +++ b/boards/uvify/core/src/CMakeLists.txt @@ -33,15 +33,17 @@ add_library(drivers_board can.c + i2c.cpp init.c led.c - spi.c + spi.cpp timer_config.cpp usb.c ) target_link_libraries(drivers_board PRIVATE + arch_spi drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h index 14e4e5839f81..b8f17cbd2166 100644 --- a/boards/uvify/core/src/board_config.h +++ b/boards/uvify/core/src/board_config.h @@ -145,32 +145,25 @@ # define SPI_BUS_INIT_MASK_EXT PX4_SPI_BUS_EXTERNAL #endif /* CONFIG_STM32_SPI4 */ -#define SPI_BUS_INIT_MASK (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_SENSORS) +#include /* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ -#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) -#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) -#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 5) -#define PX4_SPIDEV_ICM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 6) -#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 7) -#define PX4_SPIDEV_BMI PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 8) -#define PX4_SPIDEV_BMA PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 9) -#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 10) -#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 11) -#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 12) -#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 13) -#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 14) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU6000) +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_HMC5883) +#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(0, DRV_MAG_DEVTYPE_LIS3MDL) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20608) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_ICM20602) +#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(0, DRV_IMU_DEVTYPE_MPU9250) /** * Onboard MS5611 and FRAM are both on bus SPI2. * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver. * PX4_MK_SPI_SEL differentiate by adding in PX4_SPI_DEVICE_ID. */ -#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO, 3) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(0, DRV_BARO_DEVTYPE_MS5611) #ifdef CONFIG_STM32_SPI4 -# define PX4_SPIDEV_EXTERNAL PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL, 1) +# define PX4_SPIDEV_EXTERNAL PX4_MK_SPI_SEL(0, 0) #endif /* CONFIG_STM32_SPI4 */ /* I2C busses. */ @@ -202,7 +195,6 @@ /* Power supply control and monitoring GPIOs. */ #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) -#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) /* Tone alarm output. */ #define TONE_ALARM_TIMER 2 /* timer 2 */ @@ -323,14 +315,9 @@ __BEGIN_DECLS * Description: * Called to configure SPI chip select GPIO pins for the PX4FMU board. * - * mask - is bus selection - * 1 - 1 << 0 - * 2 - 1 << 1 - * ****************************************************************************************************/ -extern void stm32_spiinitialize(int mask); -void board_spi_reset(int ms); +extern void stm32_spiinitialize(void); extern void stm32_usbinitialize(void); diff --git a/boards/uvify/core/src/i2c.cpp b/boards/uvify/core/src/i2c.cpp new file mode 100644 index 000000000000..711e9cdf2655 --- /dev/null +++ b/boards/uvify/core/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; + diff --git a/boards/uvify/core/src/init.c b/boards/uvify/core/src/init.c index 043a29d0207d..41940559345f 100644 --- a/boards/uvify/core/src/init.c +++ b/boards/uvify/core/src/init.c @@ -191,12 +191,6 @@ stm32_boardinitialize(void) stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_VDD_USB_VALID); - /** - * Start with Sensor voltage off We will enable it - * in board_app_initialize. - */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_configgpio(GPIO_SBUS_INV); stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); @@ -207,15 +201,10 @@ stm32_boardinitialize(void) stm32_configgpio(GPIO_BTN_SAFETY); stm32_configgpio(GPIO_PPM_IN); - int spi_init_mask = SPI_BUS_INIT_MASK; - #if defined(CONFIG_STM32_SPI4) /* We have SPI4 is GPIO_PB4 pin 3 Low */ - if (stm32_gpioread(GPIO_PB4) == 0) { - spi_init_mask |= SPI_BUS_INIT_MASK_EXT; - - } else { + if (stm32_gpioread(GPIO_PB4) != 0) { #endif /* CONFIG_STM32_SPI4 */ stm32_configgpio(GPIO_PE5); @@ -226,8 +215,8 @@ stm32_boardinitialize(void) #endif /* CONFIG_STM32_SPI4 */ -// Configure SPI all interfaces GPIO. - stm32_spiinitialize(spi_init_mask); + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); // Configure heater GPIO. stm32_configgpio(GPIO_HEATER_INPUT); @@ -302,9 +291,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) led_on(LED_RED); } - // Power up the sensors. - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - // Power down the heater. stm32_gpiowrite(GPIO_HEATER_OUTPUT, 0); @@ -322,7 +308,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) 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); diff --git a/boards/uvify/core/src/spi.c b/boards/uvify/core/src/spi.c deleted file mode 100644 index e4553a7622c8..000000000000 --- a/boards/uvify/core/src/spi.c +++ /dev/null @@ -1,320 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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 spi.c - * - * UVify Core specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "board_config.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -__EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus) -{ - bool rv = true; - - switch (type) { - case BOARD_SPI_BUS: -#ifdef CONFIG_STM32_SPI4 - rv = bus != PX4_SPI_BUS_EXTERNAL || (stm32_gpioread(GPIO_PB4) == 0); -#endif /* CONFIG_STM32_SPI4 */ - break; - - case BOARD_I2C_BUS: - break; - } - - return rv; -} - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * mask - is bus selection - * 1 - 1 << 0 - * 2 - 1 << 1 - * - ************************************************************************************/ - -__EXPORT void stm32_spiinitialize(int mask) -{ -#ifdef CONFIG_STM32_SPI1 - - if (mask & PX4_SPI_BUS_SENSORS) { - stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN2); - stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN15); - stm32_configgpio(GPIO_SPI1_CS_PORTE_PIN15); - - stm32_configgpio(GPIO_DRDY_PORTD_PIN15); - stm32_configgpio(GPIO_DRDY_PORTC_PIN14); - stm32_configgpio(GPIO_DRDY_PORTE_PIN12); - } - -#endif - -#ifdef CONFIG_STM32_SPI2 - - if (mask & (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_BARO)) { - stm32_configgpio(GPIO_SPI2_CS_MS5611); - stm32_configgpio(GPIO_SPI2_CS_FRAM); - } - -#endif - -#ifdef CONFIG_STM32_SPI4 - - if (mask & PX4_SPI_BUS_EXTERNAL) { - stm32_configgpio(GPIO_SPI4_CS_1); //add cs - } - -#endif /* CONFIG_STM32_SPI4 */ -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - - /* Shared PC2 CS devices */ - - case PX4_SPIDEV_BMI: - case PX4_SPIDEV_MPU: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, !selected); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); - break; - - /* Shared PC15 CS devices */ - - case PX4_SPIDEV_ICM: - case PX4_SPIDEV_ICM_20602: - case PX4_SPIDEV_ICM_20608: - case PX4_SPIDEV_BMI055_ACC: - case PX4_SPIDEV_MPU2: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, !selected); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); - break; - - /* Shared PE15 CS devices */ - - case PX4_SPIDEV_HMC: - case PX4_SPIDEV_LIS: - case PX4_SPIDEV_BMI055_GYR: - /* Making sure the other peripherals are not selected */ - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); - px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} - - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - case SPIDEV_FLASH(0): - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI2_CS_MS5611, 1); - stm32_gpiowrite(GPIO_SPI2_CS_FRAM, !selected); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI2_CS_FRAM, 1); - stm32_gpiowrite(GPIO_SPI2_CS_MS5611, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif - -#ifdef CONFIG_STM32_SPI4 -__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) -{ - if (devid == PX4_SPIDEV_EXTERNAL && stm32_gpioread(GPIO_PB4) == 0) { - stm32_gpiowrite(GPIO_SPI4_CS_1, !selected); // add cs - } -} - -__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) -{ - return SPI_STATUS_PRESENT; -} -#endif /* CONFIG_STM32_SPI4 */ - -__EXPORT void board_spi_reset(int ms) -{ - /* disable SPI bus 1 DRDY */ - - stm32_configgpio(GPIO_DRDY_OFF_PORTD_PIN15); - stm32_configgpio(GPIO_DRDY_OFF_PORTC_PIN14); - stm32_configgpio(GPIO_DRDY_OFF_PORTE_PIN12); - - stm32_gpiowrite(GPIO_DRDY_OFF_PORTD_PIN15, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_PORTC_PIN14, 0); - stm32_gpiowrite(GPIO_DRDY_OFF_PORTE_PIN12, 0); - - /* disable SPI bus 1 CS */ - - stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN2); - stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN15); - stm32_configgpio(GPIO_SPI1_CS_OFF_PORTE_PIN15); - - stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN2, 0); - stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN15, 0); - stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTE_PIN15, 0); - - /* disable SPI bus 1*/ - - stm32_configgpio(GPIO_SPI1_SCK_OFF); - stm32_configgpio(GPIO_SPI1_MISO_OFF); - stm32_configgpio(GPIO_SPI1_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); - -#ifdef CONFIG_STM32_SPI4 - - /* disable SPI bus 4*/ - if (stm32_gpioread(GPIO_PB4) == 0) { - stm32_configgpio(GPIO_SPI4_SCK_OFF); - stm32_configgpio(GPIO_SPI4_MISO_OFF); - stm32_configgpio(GPIO_SPI4_MOSI_OFF); - - stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0); - stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0); - stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); - } - -#endif /* CONFIG_STM32_SPI4 */ - - /* N.B we do not have control over the SPI 2 buss powered devices - * so the the ms5611 is not resetable. - */ - - /* set the sensor rail off (default) */ - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - -#ifdef CONFIG_STM32_SPI4 - - if (stm32_gpioread(GPIO_PB4) == 0) { - /* set the periph rail off (default) for SPI4 */ - stm32_configgpio(GPIO_PERIPH_3V3_EN); - } - -#endif /* CONFIG_STM32_SPI4 */ - - /* wait for the sensor rail to reach GND */ - usleep(ms * 1000); - syslog(LOG_DEBUG, "reset done, %d ms\n", ms); - - /* re-enable power */ - -#ifdef CONFIG_STM32_SPI4 - - if (stm32_gpioread(GPIO_PB4) == 0) { - /* switch the periph rail back on */ - stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); - } - -#endif /* CONFIG_STM32_SPI4 */ - - /* switch the sensor rail back on */ - stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); - - /* wait a bit before starting SPI, different times didn't influence results */ - usleep(100); - - stm32_spiinitialize(PX4_SPI_BUS_SENSORS); - stm32_configgpio(GPIO_SPI1_SCK); - stm32_configgpio(GPIO_SPI1_MISO); - stm32_configgpio(GPIO_SPI1_MOSI); - -#ifdef CONFIG_STM32_SPI4 - - if (stm32_gpioread(GPIO_PB4) == 0) { - stm32_spiinitialize(PX4_SPI_BUS_EXTERNAL); - stm32_configgpio(GPIO_SPI4_SCK); - stm32_configgpio(GPIO_SPI4_MISO); - stm32_configgpio(GPIO_SPI4_MOSI); - } - -#endif /* CONFIG_STM32_SPI4 */ -} diff --git a/boards/uvify/core/src/spi.cpp b/boards/uvify/core/src/spi.cpp new file mode 100644 index 000000000000..77fdaad3a17d --- /dev/null +++ b/boards/uvify/core/src/spi.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + initSPIDevice(DRV_MAG_DEVTYPE_HMC5883, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), // hmc5983 + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBusExternal(4, { + SPI::CS{GPIO::PortA, GPIO::Pin8}, + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +__EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus) +{ + bool rv = true; + + switch (type) { + case BOARD_SPI_BUS: +#ifdef CONFIG_STM32_SPI4 + rv = bus != 4 || (stm32_gpioread(GPIO_PB4) == 0); +#endif /* CONFIG_STM32_SPI4 */ + break; + + case BOARD_I2C_BUS: + break; + + default: break; + } + + return rv; +} + diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index 0680758b5da4..d7f453833353 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -41,13 +41,13 @@ endif() add_library(px4_platform board_identity.c - #i2c.cpp - #i2c_spi_buses.cpp + i2c.cpp + i2c_spi_buses.cpp module.cpp px4_getopt.c px4_cli.cpp shutdown.cpp - #spi.cpp + spi.cpp ${SRCS} ) add_dependencies(px4_platform prebuild_targets) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 64a1cc78eb8e..9c0e773ed3d1 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -101,27 +101,22 @@ /* I2C PX4 clock configuration * - * A board may override BOARD_NUMBER_I2C_BUSES and BOARD_I2C_BUS_CLOCK_INIT - * simply by defining the #defines. - * - * If none are provided the default number of I2C busses will be taken from - * the px4 micro hal and the init will be from the legacy values of 100K. + * A board may override BOARD_I2C_BUS_CLOCK_INIT simply by defining the #defines. */ -#if !defined(BOARD_NUMBER_I2C_BUSES) -# define BOARD_NUMBER_I2C_BUSES PX4_NUMBER_I2C_BUSES -#endif -#if !defined(BOARD_I2C_BUS_CLOCK_INIT) -# if (BOARD_NUMBER_I2C_BUSES) == 1 -# define BOARD_I2C_BUS_CLOCK_INIT {100000} -# elif (BOARD_NUMBER_I2C_BUSES) == 2 -# define BOARD_I2C_BUS_CLOCK_INIT {100000, 100000} -# elif (BOARD_NUMBER_I2C_BUSES) == 3 -# define BOARD_I2C_BUS_CLOCK_INIT {100000, 100000, 100000} -# elif (BOARD_NUMBER_I2C_BUSES) == 4 -# define BOARD_I2C_BUS_CLOCK_INIT {100000, 100000, 100000, 100000} +#if defined(BOARD_I2C_BUS_CLOCK_INIT) +# define PX4_I2C_BUS_CLOCK_INIT BOARD_I2C_BUS_CLOCK_INIT +#else +# if (PX4_NUMBER_I2C_BUSES) == 1 +# define PX4_I2C_BUS_CLOCK_INIT {100000} +# elif (PX4_NUMBER_I2C_BUSES) == 2 +# define PX4_I2C_BUS_CLOCK_INIT {100000, 100000} +# elif (PX4_NUMBER_I2C_BUSES) == 3 +# define PX4_I2C_BUS_CLOCK_INIT {100000, 100000, 100000} +# elif (PX4_NUMBER_I2C_BUSES) == 4 +# define PX4_I2C_BUS_CLOCK_INIT {100000, 100000, 100000, 100000} # else -# error BOARD_NUMBER_I2C_BUSES not supported +# error PX4_NUMBER_I2C_BUSES not supported # endif #endif @@ -1023,46 +1018,6 @@ int board_shutdown(void); static inline int board_register_power_state_notification_cb(power_button_state_notification_t cb) { return 0; } static inline int board_shutdown(void) { return -EINVAL; } #endif -__END_DECLS - -/************************************************************************************ - * Name: px4_i2c_bus_external - * - ************************************************************************************/ - -#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) - -__EXPORT bool px4_i2c_bus_external(int bus); - -#else - -#ifdef PX4_I2C_BUS_ONBOARD -#define px4_i2c_bus_external(bus) (bus != PX4_I2C_BUS_ONBOARD) -#else -#define px4_i2c_bus_external(bus) true -#endif /* PX4_I2C_BUS_ONBOARD */ - -#endif /* BOARD_HAS_SIMPLE_HW_VERSIONING */ - - -/************************************************************************************ - * Name: px4_spi_bus_external - * - ************************************************************************************/ - -#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) - -__EXPORT bool px4_spi_bus_external(int bus); - -#else - -#ifdef PX4_SPI_BUS_EXT -#define px4_spi_bus_external(bus) (bus == PX4_SPI_BUS_EXT) -#else -#define px4_spi_bus_external(bus) false -#endif /* PX4_SPI_BUS_EXT */ - -#endif /* BOARD_HAS_SIMPLE_HW_VERSIONING */ /************************************************************************************ * Name: board_has_bus @@ -1083,6 +1038,41 @@ __EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus); # define board_has_bus(t, b) true #endif /* BOARD_HAS_BUS_MANIFEST */ + +/************************************************************************************ + * Name: board_spi_reset + * + * Description: + * Reset SPI buses and devices + * + * Input Parameters: + * ms - delay in msbetween powering off the devices and re-enabling power. + * + * bus_mask - bitmask to select buses - use 0xffff to select all. + */ +__EXPORT void board_spi_reset(int ms, int bus_mask); + +/************************************************************************************ + * Name: board_control_spi_sensors_power_configgpio + * + * Description: + * Initialize GPIO pins for all SPI bus power enable pins + */ +__EXPORT void board_control_spi_sensors_power_configgpio(void); + +/************************************************************************************ + * Name: board_control_spi_sensors_power + * + * Description: + * Control the power of SPI buses + * + * Input Parameters: + * enable_power - true to enable power, false to disable + * + * bus_mask - bitmask to select buses - use 0xffff to select all. + */ +__EXPORT void board_control_spi_sensors_power(bool enable_power, int bus_mask); + /************************************************************************************ * Name: board_hardfault_init * @@ -1107,3 +1097,5 @@ __EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus); * 32000 resets. */ int board_hardfault_init(int display_to_console, bool allow_prompt); + +__END_DECLS diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index d5331c6e5e4d..b76650c9e083 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -69,7 +69,7 @@ struct px4_spi_bus_t { struct px4_spi_bus_all_hw_t { px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS]; - int board_hw_version; ///< 0=default, >0 for a specific revision (see board_get_hw_version) + int board_hw_version{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version), -1=unused }; #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 diff --git a/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp b/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp index a08a28889b8c..6e512ab5ecbb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp @@ -106,6 +106,7 @@ __EXPORT void stm32_spiinitialize() { px4_set_spi_buses_from_hw_version(); board_control_spi_sensors_power_configgpio(); + board_control_spi_sensors_power(true, 0xffff); for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { switch (px4_spi_buses[i].bus) { diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5bddeaea7f98..93196679b911 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -65,15 +65,17 @@ #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 -#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 // TODO: remove this #define DRV_ACC_DEVTYPE_ACCELSIM 0x14 -#define DRV_ACC_DEVTYPE_MPU9250 0x16 +#define DRV_ACC_DEVTYPE_MPU9250 0x16 // TOOD: remove this #define DRV_ACC_DEVTYPE_BMI160 0x17 #define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_IMU_DEVTYPE_MPU6000 DRV_GYR_DEVTYPE_MPU6000 #define DRV_GYR_DEVTYPE_L3GD20 0x22 #define DRV_GYR_DEVTYPE_GYROSIM 0x23 #define DRV_GYR_DEVTYPE_MPU9250 0x24 +#define DRV_IMU_DEVTYPE_MPU9250 DRV_GYR_DEVTYPE_MPU9250 #define DRV_GYR_DEVTYPE_BMI160 0x25 #define DRV_RNG_DEVTYPE_MB12XX 0x31 @@ -82,12 +84,15 @@ #define DRV_ACC_DEVTYPE_MPU6500 0x34 #define DRV_GYR_DEVTYPE_MPU6050 0x35 #define DRV_GYR_DEVTYPE_MPU6500 0x36 -#define DRV_ACC_DEVTYPE_ICM20602 0x37 +#define DRV_ACC_DEVTYPE_ICM20602 0x37 // TODO: remove this #define DRV_GYR_DEVTYPE_ICM20602 0x38 +#define DRV_IMU_DEVTYPE_ICM20602 DRV_GYR_DEVTYPE_ICM20602 #define DRV_ACC_DEVTYPE_ICM20608 0x39 -#define DRV_GYR_DEVTYPE_ICM20608 0x3A +#define DRV_GYR_DEVTYPE_ICM20608 0x3A // TODO: remove this +#define DRV_IMU_DEVTYPE_ICM20608 DRV_GYR_DEVTYPE_ICM20608 #define DRV_ACC_DEVTYPE_ICM20689 0x3B -#define DRV_GYR_DEVTYPE_ICM20689 0x3C +#define DRV_GYR_DEVTYPE_ICM20689 0x3C // TODO: remove this +#define DRV_IMU_DEVTYPE_ICM20689 DRV_GYR_DEVTYPE_ICM20689 #define DRV_BARO_DEVTYPE_MS5611 0x3D #define DRV_BARO_DEVTYPE_MS5607 0x3E #define DRV_BARO_DEVTYPE_BMP280 0x3F @@ -120,10 +125,16 @@ #define DRV_ACC_DEVTYPE_ADIS16497 0x63 #define DRV_GYR_DEVTYPE_ADIS16497 0x64 #define DRV_BARO_DEVTYPE_BAROSIM 0x65 -#define DRV_DEVTYPE_BMI088 0x66 +#define DRV_GYR_DEVTYPE_BMI088 0x66 #define DRV_DEVTYPE_BMP388 0x67 #define DRV_DEVTYPE_DPS310 0x68 #define DRV_DEVTYPE_ST_ISM330DLC 0x69 +#define DRV_ACC_DEVTYPE_BMI088 0x6a +#define DRV_OSD_DEVTYPE_ATXXXX 0x6b +#define DRV_FLOW_DEVTYPE_PMW3901 0x6c + + +#define DRV_DEVTYPE_UNUSED 0xff /* * ioctl() definitions diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp index 1bf94243aa66..96182032cd9a 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -62,7 +62,7 @@ BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enu _duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")), _got_duplicate(false) { - _px4_accel.set_device_type(DRV_DEVTYPE_BMI088); + _px4_accel.set_device_type(DRV_GYR_DEVTYPE_BMI088); } BMI088_accel::~BMI088_accel() diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp index a58d4c084845..33f5265988c4 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -63,7 +63,7 @@ BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum R _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")) { - _px4_gyro.set_device_type(DRV_DEVTYPE_BMI088); + _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI088); } BMI088_gyro::~BMI088_gyro() diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 95e7c0ae78de..866962772baa 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1497,7 +1497,7 @@ PX4FMU::sensor_reset(int ms) ms = 1; } - board_spi_reset(ms); + board_spi_reset(ms, 0xffff); } void diff --git a/src/lib/drivers/device/nuttx/I2C.cpp b/src/lib/drivers/device/nuttx/I2C.cpp index 872895d65134..77b58ff18293 100644 --- a/src/lib/drivers/device/nuttx/I2C.cpp +++ b/src/lib/drivers/device/nuttx/I2C.cpp @@ -51,7 +51,7 @@ namespace device * All calls to init() will NOT set the buss frequency */ -unsigned int I2C::_bus_clocks[BOARD_NUMBER_I2C_BUSES] = BOARD_I2C_BUS_CLOCK_INIT; +unsigned int I2C::_bus_clocks[PX4_NUMBER_I2C_BUSES] = PX4_I2C_BUS_CLOCK_INIT; I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency) : CDev(name, devname), diff --git a/src/lib/drivers/device/nuttx/I2C.hpp b/src/lib/drivers/device/nuttx/I2C.hpp index 5f472767ff50..124a3a1a680c 100644 --- a/src/lib/drivers/device/nuttx/I2C.hpp +++ b/src/lib/drivers/device/nuttx/I2C.hpp @@ -41,6 +41,7 @@ #define _DEVICE_I2C_H #include "../CDev.hpp" +#include #include @@ -69,8 +70,6 @@ class __EXPORT I2C : public CDev static int set_bus_clock(unsigned bus, unsigned clock_hz); - static unsigned int _bus_clocks[BOARD_NUMBER_I2C_BUSES]; - protected: /** * The number of times a read or write operation will be retried on @@ -109,10 +108,12 @@ class __EXPORT I2C : public CDev */ int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); - virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } + bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: - uint32_t _frequency{0}; + static unsigned int _bus_clocks[PX4_NUMBER_I2C_BUSES]; + + const uint32_t _frequency; i2c_master_s *_dev{nullptr}; }; diff --git a/src/lib/drivers/device/nuttx/SPI.cpp b/src/lib/drivers/device/nuttx/SPI.cpp index 2f9966dca573..413b7179805e 100644 --- a/src/lib/drivers/device/nuttx/SPI.cpp +++ b/src/lib/drivers/device/nuttx/SPI.cpp @@ -70,6 +70,10 @@ SPI::SPI(const char *name, const char *devname, int bus, uint32_t device, enum s _device_id.devid_s.address = (uint8_t)device; // devtype needs to be filled in by the driver _device_id.devid_s.devtype = 0; + + if (!px4_spi_bus_requires_locking(bus)) { + _locking_mode = LOCK_NONE; + } } SPI::~SPI() diff --git a/src/lib/drivers/device/nuttx/SPI.hpp b/src/lib/drivers/device/nuttx/SPI.hpp index b2f94faada0f..0029185757ab 100644 --- a/src/lib/drivers/device/nuttx/SPI.hpp +++ b/src/lib/drivers/device/nuttx/SPI.hpp @@ -43,6 +43,7 @@ #include "../CDev.hpp" #include +#include namespace device __EXPORT { @@ -166,7 +167,7 @@ class __EXPORT SPI : public CDev int _transferhword(uint16_t *send, uint16_t *recv, unsigned len); - virtual bool external() const override { return px4_spi_bus_external(get_device_bus()); } + bool external() const override { return px4_spi_bus_external(get_device_bus()); } }; diff --git a/src/lib/drivers/device/posix/I2C.hpp b/src/lib/drivers/device/posix/I2C.hpp index b886413a3f1b..aa8bf1f27567 100644 --- a/src/lib/drivers/device/posix/I2C.hpp +++ b/src/lib/drivers/device/posix/I2C.hpp @@ -41,6 +41,7 @@ #define _DEVICE_I2C_H #include "../CDev.hpp" +#include namespace device __EXPORT { diff --git a/src/lib/drivers/device/posix/SPI.hpp b/src/lib/drivers/device/posix/SPI.hpp index ce7e76555b3b..8cfd7526cc9b 100644 --- a/src/lib/drivers/device/posix/SPI.hpp +++ b/src/lib/drivers/device/posix/SPI.hpp @@ -41,6 +41,7 @@ #define _DEVICE_SPI_H #include "../CDev.hpp" +#include #ifdef __PX4_LINUX