Crazyflie drivers

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -252,6 +252,11 @@ then
 
 	# I2C bypass of mpu
 	lps25h start
+
+	# Optical flow deck
+	vl53lxx start
+	pmw3901 start
+
 fi
 
 if ver hwcmp AEROFC_V1

cmake/configs/nuttx_crazyflie_default.cmake

@@ -10,6 +10,8 @@ set(config_module_list
 	drivers/imu/mpu9250
 	drivers/barometer/lps25h
 	drivers/gps
+	drivers/distance_sensor/vl53lxx
+	drivers/pmw3901
 	modules/sensors
 
 	#
@@ -29,6 +31,8 @@ set(config_module_list
 	systemcmds/dumpfile
 	systemcmds/ver
 	systemcmds/hardfault_log
+	systemcmds/topic_listener
+	systemcmds/sd_bench
 
 	#
 	# General system control

platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig

@@ -880,7 +880,17 @@ CONFIG_WATCHDOG_DEVPATH="/dev/watchdog0"
 # CONFIG_RGBLED is not set
 # CONFIG_PCA9635PW is not set
 # CONFIG_NCP5623C is not set
-# CONFIG_MMCSD is not set
+
+
+#
+# SD Card Support
+#
+CONFIG_MMCSD=y
+CONFIG_MMCSD_NSLOTS=1
+CONFIG_MMCSD_SPI=y
+CONFIG_MMCSD_SPICLOCK=20000000
+CONFIG_MMCSD_SPIMODE=0
+
 # CONFIG_MODEM is not set
 CONFIG_MTD=y
 
@@ -1104,8 +1114,9 @@ CONFIG_FS_WRITABLE=y
 CONFIG_FS_MQUEUE_MPATH="/var/mqueue"
 # CONFIG_FS_RAMMAP is not set
 CONFIG_FS_FAT=y
-# CONFIG_FAT_LCNAMES is not set
-# CONFIG_FAT_LFN is not set
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
 # CONFIG_FS_FATTIME is not set
 # CONFIG_FAT_FORCE_INDIRECT is not set
 # CONFIG_FAT_DMAMEMORY is not set
@@ -1490,6 +1501,8 @@ CONFIG_NSH_DISABLE_UNAME=y
 CONFIG_NSH_DISABLE_WGET=y
 CONFIG_NSH_DISABLE_XD=y
 CONFIG_NSH_MMCSDMINOR=0
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDSPIPORTNO=1
 
 #
 # Configure Command Options

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
 )

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_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN12)
+#define GPIO_SPI1_CS1_EXT    		/* PB4  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_SPI1_CS2_EXT    		/* PB5  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|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)		// 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_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__ */

src/drivers/boards/crazyflie/init.c

@@ -57,6 +57,7 @@
 #include "platform/cxxinitialize.h"
 #include <nuttx/board.h>
 #include <nuttx/analog/adc.h>
+#include <nuttx/spi/spi.h>
 
 #include "board_config.h"
 #include <stm32_uart.h>
@@ -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;
 }

src/drivers/boards/crazyflie/spi.c

@@ -0,0 +1,166 @@
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+#include <px4_log.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+#include <unistd.h>
+
+#include <nuttx/spi/spi.h>
+#include <arch/board/board.h>
+#include <systemlib/px4_macros.h>
+#include <nuttx/mmcsd.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32_gpio.h>
+#include "board_config.h"
+#include <systemlib/err.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
+	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 external SPI port */
+	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;
+	}
+
+#ifdef CONFIG_MMCSD
+	int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_expansion);
+
+	if (ret != OK) {
+		PX4_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);
+	warnx("reset done, %d ms", 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);
+
+}

src/drivers/distance_sensor/CMakeLists.txt

@@ -41,3 +41,4 @@ add_subdirectory(teraranger)
 add_subdirectory(tfmini)
 add_subdirectory(ulanding)
 add_subdirectory(leddar_one)
+add_subdirectory(vl53lxx)

src/drivers/distance_sensor/vl53lxx/CMakeLists.txt

@@ -0,0 +1,7 @@
+px4_add_module(
+	MODULE drivers__vl53lxx
+	MAIN vl53lxx
+	SRCS
+		vl53lxx.cpp
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :