px4fmu-v2 version detection and init cleanup

- if an invalid version is detected reinit, then reset

src/drivers/boards/px4fmu-v2/init.c

@@ -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,15 +399,16 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		case 0xA:
 			hw_type[2] = 'M';
 			break;
-		}
 
-		PX4_INFO("Ver 0x%1X : Rev %x %s", hw_version, hw_revision, hw_type);
+		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;
 		}
 
-#endif // BOARD_HAS_SIMPLE_HW_VERSIONING
+		message("\nFMUv2 ver 0x%1X : Rev %x %s\n", hw_version, hw_revision, hw_type);
-
+	}
-	/* 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",
+		message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO);
-			CONFIG_NSH_MMCSDSLOTNO);
 		return -ENODEV;
 	}
 

src/drivers/boards/px4fmu-v2/spi.c

@@ -82,30 +82,20 @@ static void stm32_spi1_initialize(void)
 
 	stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PD15);
 
-#  if !defined(BOARD_HAS_VERSIONING)
+	if (HW_VER_FMUV2MINI == board_get_hw_version()) {
-	stm32_configgpio(GPIO_SPI1_EXTI_DRDY_PB0);
+		stm32_configgpio(GPIO_SPI1_EXTI_20608_DRDY_PC14);
-	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()) {
+	} else if (HW_VER_FMUV3 == board_get_hw_version()) {
+		stm32_configgpio(GPIO_SPI1_CS_PC1);
+
+	} 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);
-
-	} else 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
 }
 #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();
 }