Hi,
I’m working on a project to implement a TDoA location system using a UWB chip (DWM1000).
I’m using a Pixracer board but the DWM1000 is connected through the wi-fi port instead of the SPI port, as follows:
Now I’m trying to set the wi-fi port as it was set in the pixhawk (px4-v2) firmware but so far it doesn’t work.
Here’s a list of the changes I made:
-
https://github.com/ArduPilot/PX4Firmware/blob/master/nuttx-configs/px4fmu-v4/include/board.h
added:
#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz)
#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz)
#define GPIO_SPI4_NSS (GPIO_SPI4_NSS_1|GPIO_SPEED_50MHz)
#define GPIO_SPI4_CS_EXT (GPIO_SPI_CS_EXT1|GPIO_SPEED_50MHz)
-
https://github.com/ArduPilot/PX4Firmware/blob/master/src/drivers/boards/px4fmu-v4/board_config.h
added:
#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
/* External bus */
#define PX4_SPIDEV_EXT0 1
#define PX4_SPIDEV_EXT1 2
-
https://github.com/ArduPilot/PX4Firmware/blob/master/src/drivers/boards/px4fmu-v4/px4fmu_init.c
added:
static struct spi_dev_s *spi4;
spi4 = up_spiinitialize(4);
SPI_SETFREQUENCY(spi4, 10000000);
SPI_SETBITS(spi4, 8);
SPI_SETMODE(spi4, SPIDEV_MODE3);
SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
-
https://github.com/ArduPilot/PX4Firmware/blob/master/src/drivers/boards/px4fmu-v4/px4fmu_spi.c
added:
#ifdef CONFIG_STM32_SPI4
stm32_configgpio(GPIO_SPI_CS_EXT1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_configgpio(GPIO_SPI_CS_EXT0);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
#endif
__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
{
return SPI_STATUS_PRESENT;
}
__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
switch (devid) {
case PX4_SPIDEV_EXT0:
stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
case PX4_SPIDEV_EXT1:
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
default:
break;
}
}
-
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp#L68
changed:
#define UARTF_DEFAULT_DEVICE "/dev/ttyS0" // wifi
to:
#define UARTF_DEFAULT_DEVICE "/dev/null" // wifi
Please could you tell me if I did right so far and if I need to do something else?
Thank you very much.