fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4fmu-v2 version detection and init cleanup

Browse Source 
- if an invalid version is detected reinit, then reset
This commit is contained in:
Daniel Agar 2018-03-13 18:11:27 -04:00 committed by Lorenz Meier
parent 24be68b27a
commit de85acb8b0
2 changed files with 19 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/drivers/boards/px4fmu-v2/init.c
View File

@ -119,11 +119,9 @@ __END_DECLS
/****************************************************************************
* Private Data
****************************************************************************/
#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
static int hw_version = 0;
static int hw_revision = 0;
static char hw_type[4] = HW_VER_TYPE_INIT;
#endif
/************************************************************************************
* Name: board_peripheral_reset
@ -213,6 +211,7 @@ __EXPORT void board_on_reset(int status)
* 10 00 - 0x8 FMUv2
* 11 10 - 0xE Cube AKA V2.0
* 10 10 - 0xA PixhawkMini
* 10 11 - 0xB FMUv2 questionable hardware (should be treated like regular FMUv2)
*
* This will return OK on success and -1 on not supported
*
@ -224,7 +223,6 @@ __EXPORT void board_on_reset(int status)
*
************************************************************************************/
#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
static int determin_hw_version(int *version, int *revision)
{
*revision = 0; /* default revision */
@ -307,7 +305,6 @@ __EXPORT int board_get_hw_revision()
{
return hw_revision;
}
#endif // BOARD_HAS_SIMPLE_HW_VERSIONING
/************************************************************************************
* Name: stm32_boardinitialize
@ -344,7 +341,6 @@ stm32_boardinitialize(void)
stm32_configgpio(GPIO_VDD_USB_VALID);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
}
/****************************************************************************
@ -380,24 +376,18 @@ static struct sdio_dev_s *sdio;
__EXPORT int board_app_initialize(uintptr_t arg)
{
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
/* run C++ ctors before we go any further */
up_cxxinitialize();
# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
# endif
#else
# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif
#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
/* Ensure the power is on 1 ms before we drive the GPIO pins */
usleep(1000);
if (OK == determin_hw_version(&hw_version, & hw_revision)) {
switch (hw_version) {
default:
case 0x8:
break;
@ -409,16 +399,17 @@ __EXPORT int board_app_initialize(uintptr_t arg)
case 0xA:
hw_type[2] = 'M';
break;
default:
// questionable px4fmu-v2 hardware, try forcing regular FMUv2 (not much else we can do)
message("bad version detected, forcing to fmu-v2\n");
hw_version = 0x8;
break;
}
PX4_INFO("Ver 0x%1X : Rev %x %s", hw_version, hw_revision, hw_type);
message("\nFMUv2 ver 0x%1X : Rev %x %s\n", hw_version, hw_revision, hw_type);
}
#endif // BOARD_HAS_SIMPLE_HW_VERSIONING
/* Ensure the power is on 1 ms before we drive the GPIO pins */
usleep(1000);
/* configure SPI interfaces */
stm32_spiinitialize();
@ -646,8 +637,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
message("[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}

120
src/drivers/boards/px4fmu-v2/spi.c
View File

@ -82,30 +82,20 @@ static void stm32_spi1_initialize(void)
stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PD15);
# if !defined(BOARD_HAS_VERSIONING)
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);
# else
if (HW_VER_FMUV2 == board_get_hw_version()) {
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);
} else if (HW_VER_FMUV2MINI == board_get_hw_version()) {
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);
}
# endif
} 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
@ -119,10 +109,6 @@ static void stm32_spi4_initialize(void)
{
stm32_configgpio(GPIO_SPI4_NSS_PE4);
# if !defined(BOARD_HAS_VERSIONING)
stm32_configgpio(GPIO_SPI4_GPIO_PC14);
# else
if (HW_VER_FMUV3 == board_get_hw_version()) {
stm32_configgpio(GPIO_SPI4_EXTI_DRDY_PB0);
stm32_configgpio(GPIO_SPI4_CS_PB1);
@ -133,8 +119,6 @@ static void stm32_spi4_initialize(void)
if (HW_VER_FMUV2MINI != board_get_hw_version()) {
stm32_configgpio(GPIO_SPI4_GPIO_PC14);
}
# endif
}
#endif //CONFIG_STM32_SPI4
@ -157,52 +141,6 @@ __EXPORT void stm32_spiinitialize(void)
__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 */
# if !defined(BOARD_HAS_VERSIONING)
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI1_CS_PC13, !selected);
stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
break;
# if defined(PX4_SPIDEV_ICM_20608)
case PX4_SPIDEV_ICM_20608:
# endif
case PX4_SPIDEV_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PC15, !selected);
stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
break;
case PX4_SPIDEV_BARO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PD7, !selected);
stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
stm32_gpiowrite(GPIO_SPI1_CS_PC2, !selected);
break;
default:
break;
}
# else // defined(BOARD_HAS_VERSIONING)
/* 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
@ -230,10 +168,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
break;
# if defined(PX4_SPIDEV_ICM_20608)
case PX4_SPIDEV_ICM_20608:
# endif
case PX4_SPIDEV_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
@ -315,8 +250,6 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
default:
break;
}
# endif // defined(BOARD_HAS_VERSIONING)
}
__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid)
@ -344,28 +277,6 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid)
__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 */
# if !defined(BOARD_HAS_VERSIONING)
switch (devid) {
case PX4_SPIDEV_EXT_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI4_NSS_PE4, !selected);
stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1);
break;
case PX4_SPIDEV_EXT_BARO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1);
stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, !selected);
break;
default:
break;
}
# else // defined(BOARD_HAS_VERSIONING)
/* 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
@ -403,10 +314,7 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
break;
# if defined(PX4_SPIDEV_ICM_20608)
case PX4_SPIDEV_ICM_20608:
# endif
case PX4_SPIDEV_EXT_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1);
@ -442,9 +350,6 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
break;
}
# endif // defined(BOARD_HAS_VERSIONING)
}
__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid)
{
@ -489,8 +394,6 @@ __EXPORT void board_spi_reset(int ms)
stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PB4), 0);
stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_EXTI_DRDY_PD15), 0);
#if defined(BOARD_HAS_VERSIONING)
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);
@ -510,11 +413,8 @@ __EXPORT void board_spi_reset(int ms)
stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_SCK), 0);
stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_MISO), 0);
stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_MOSI), 0);
}
#endif
/* set the sensor rail off */
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
@ -536,8 +436,6 @@ __EXPORT void board_spi_reset(int ms)
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
#if defined(BOARD_HAS_VERSIONING)
if (HW_VER_FMUV3 == board_get_hw_version()) {
stm32_configgpio(GPIO_SPI4_SCK);
stm32_configgpio(GPIO_SPI4_MISO);
@ -545,8 +443,6 @@ __EXPORT void board_spi_reset(int ms)
stm32_spi4_initialize();
}
#endif
stm32_spi1_initialize();
}