diff --git a/src/drivers/boards/crazyflie/CMakeLists.txt b/src/drivers/boards/crazyflie/CMakeLists.txt index 4b64e6e1b45..4505fd107a4 100644 --- a/src/drivers/boards/crazyflie/CMakeLists.txt +++ b/src/drivers/boards/crazyflie/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_library(drivers_board init.c led.c + spi.c timer_config.c usb.c ) diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h index 2000b7e81ae..966deb83293 100644 --- a/src/drivers/boards/crazyflie/board_config.h +++ b/src/drivers/boards/crazyflie/board_config.h @@ -97,6 +97,38 @@ #define BOARD_I2C_BUS_CLOCK_INIT {PX4_I2C_BUS_ONBOARD_HZ, 100000, PX4_I2C_BUS_EXPANSION_HZ} +/* + * 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_2MHz)) + + +/* SPI Busses */ + +/* SPI1 Bus */ +#define PX4_SPI_BUS_EXPANSION 1 + +/* SPI1 CS */ +#define GPIO_SPI1_CS0_EXT /* PC12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN12) +#define GPIO_SPI1_CS1_EXT /* PB4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SPI1_CS2_EXT /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) + +#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) +#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_FLOW_BUS_FIRST_CS PX4_SPIDEV_EXPANSION_1 +#define PX4_FLOW_BUS_LAST_CS PX4_SPIDEV_EXPANSION_3 + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) /* Devices on the onboard bus. @@ -204,14 +236,13 @@ __BEGIN_DECLS ****************************************************************************************************/ /**************************************************************************************************** - * Name: board_spi_reset board_peripheral_reset + * Name: board_peripheral_reset * * Description: - * Called to reset SPI and the perferal bus + * Called to reset the periferal bus * ****************************************************************************************************/ -#define board_spi_reset(ms) #define board_peripheral_reset(ms) /**************************************************************************************************** @@ -234,6 +265,29 @@ extern void stm32_usbinitialize(void); int board_i2c_initialize(void); +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +/************************************************************************************ + * Name: stm32_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +extern int stm32_spi_bus_initialize(void); + +void board_spi_reset(int ms); + + #include "../common/board_common.h" #endif /* __ASSEMBLY__ */ diff --git a/src/drivers/boards/crazyflie/init.c b/src/drivers/boards/crazyflie/init.c index 7a51ce6f745..b5781ea4af3 100644 --- a/src/drivers/boards/crazyflie/init.c +++ b/src/drivers/boards/crazyflie/init.c @@ -57,6 +57,7 @@ #include "platform/cxxinitialize.h" #include #include +#include #include "board_config.h" #include @@ -133,6 +134,10 @@ stm32_boardinitialize(void) board_autoled_initialize(); + /* configure SPI interfaces */ + + stm32_spiinitialize(); + stm32_usbinitialize(); } @@ -327,5 +332,14 @@ __EXPORT int board_app_initialize(uintptr_t arg) led_off(LED_TX); led_off(LED_RX); +#ifdef CONFIG_SPI + int ret = stm32_spi_bus_initialize(); + + if (ret != OK) { + return ret; + } + +#endif + return OK; } diff --git a/src/drivers/boards/crazyflie/spi.c b/src/drivers/boards/crazyflie/spi.c new file mode 100644 index 00000000000..6e92cd6659c --- /dev/null +++ b/src/drivers/boards/crazyflie/spi.c @@ -0,0 +1,174 @@ +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include "board_config.h" +#include + +/**************************************************************************** + * 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 + board_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 SPI port for the Sensors */ + + spi_expansion = stm32_spibus_initialize(PX4_SPI_BUS_EXPANSION); + + if (!spi_expansion) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXPANSION); + return -ENODEV; + } + + /* Default PX4_SPI_BUS_SENSORS to 1MHz and de-assert the known chip selects. */ + + SPI_SETFREQUENCY(spi_expansion, 10000000); + SPI_SETBITS(spi_expansion, 8); + SPI_SETMODE(spi_expansion, SPIDEV_MODE3); + + for (int cs = PX4_FLOW_BUS_FIRST_CS; cs <= PX4_FLOW_BUS_LAST_CS; cs++) { + SPI_SELECT(spi_expansion, cs, false); + } + + 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; + ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_EXPANSION); + + /* 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); + + /* set the sensor rail off */ + //stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", 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 (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/src/drivers/pmw3901/pmw3901.cpp b/src/drivers/pmw3901/pmw3901.cpp index 740650dd9e5..7d01b7c9447 100644 --- a/src/drivers/pmw3901/pmw3901.cpp +++ b/src/drivers/pmw3901/pmw3901.cpp @@ -77,17 +77,8 @@ #include /* Configuration Constants */ -#ifdef PX4_SPI_BUS_EXT1 -#define PMW3901_BUS PX4_SPI_BUS_EXT1 // fmu-v4pro -#else -#define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1 // fmu-v5 -#endif - -#ifdef PX4_SPIDEV_EXT0 -#define PMW3901_SPIDEV PX4_SPIDEV_EXT0 // fmu-v4pro -#else -#define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 -#endif +#define PMW3901_BUS PX4_SPI_BUS_EXPANSION /* fmu-v4pro: PX4_SPI_BUS_EXT1 | fmu-v5: PX4_SPI_BUS_EXTERNAL1 */ +#define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 /* fmu-v4pro: PX4_SPIDEV_EXT0 | fmu-v5: PX4_SPIDEV_EXTERNAL1_1 */ #define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz @@ -571,8 +562,8 @@ PMW3901::collect() readMotionCount(delta_x_raw, delta_y_raw); - delta_x = (float)delta_x_raw / 500.0f; // proportional factor + convert from pixels to radians - delta_y = (float)delta_y_raw / 500.0f; // proportional factor + convert from pixels to radians + delta_x = (float)delta_x_raw / 500.0f; // proportional factor + convert from pixels to radians + delta_y = (float)delta_y_raw / 500.0f; // proportional factor + convert from pixels to radians struct optical_flow_s report;