mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-29 19:57:12 +08:00
crazyflie: add spi expansion to board configurations (modify spi bus and device in pmw3901 driver)
This commit is contained in:
DanielePettenuzzo
committed by
Beat Küng
Beat Küng
parent
2d20f31a70
commit
8a4e144e86
@@ -34,6 +34,7 @@
|
||||
px4_add_library(drivers_board
|
||||
init.c
|
||||
led.c
|
||||
spi.c
|
||||
timer_config.c
|
||||
usb.c
|
||||
)
|
||||
|
||||
@@ -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__ */
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,174 @@
|
||||
/************************************************************************************
|
||||
* 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 <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 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);
|
||||
|
||||
}
|
||||
@@ -77,17 +77,8 @@
|
||||
#include <board_config.h>
|
||||
|
||||
/* 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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user