restore FMUv2 board versioning

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

@@ -116,6 +116,20 @@ extern void led_on(int led);
 extern void led_off(int led);
 __END_DECLS
 
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
+static int hw_version = -1;
+static int hw_revision = -1;
+static char hw_type[4] = HW_VER_TYPE_INIT;
+#endif
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
 /************************************************************************************
  * Name: board_peripheral_reset
  *
@@ -180,6 +194,123 @@ __EXPORT void board_on_reset(int status)
 	}
 }
 
+/************************************************************************************
+  * Name: determin_hw_version
+ *
+ * Description:
+ *
+ * This function looks at HW deltas to determine what the
+ * build is running on using the following criteria:
+ *
+ * MSN  PB12   FMUv2                Cube             MINI
+ * CAN2_RX       CONECTOR             MX30521          NC
+ * PU.PD         1,0                   1,1             1,0
+ *
+ * LSN  PB4    FMUv2                Cube             MINI
+ *               ACCEL_DRDY LSM303D    NC              NC
+ * PU.PD         0,0                   1,0             1,0
+
+ *  PB12:PB4
+ *  ud   ud
+ *  10   00 - 0x8 FMUv2
+ *  11   10 - 0xE Cube AKA V2.0
+ *  10   10 - 0xA PixhawkMini
+ *
+ *  This will return OK on success and -1 on not supported
+ *
+ *  hw_type Initial state is {'V','2',0, 0}
+ *   V 2    - FMUv2
+ *   V 3 0  - FMUv3 2.0
+ *   V 3 1  - FMUv3 2.1 - not differentiateable,
+ *   V 2 M  - FMUv2 Mini
+ *
+ ************************************************************************************/
+
+#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
+static int determin_hw_version(int *version, int *revision)
+{
+	*revision = -1; /* unknown */
+	int rv = 0;
+	int pos = 0;
+	stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));
+	up_udelay(10);
+	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
+	stm32_configgpio(HW_VER_PB4);
+	up_udelay(10);
+	rv |= stm32_gpioread(HW_VER_PB4) << pos++;
+
+	int votes = 16;
+	int ones[2] = {0, 0};
+	int zeros[2] = {0, 0};
+
+	while (votes--) {
+		stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB12 & ~GPIO_PUPD_MASK));
+		up_udelay(10);
+		stm32_gpioread(HW_VER_PB12) ? ones[0]++ : zeros[0]++;
+		stm32_configgpio(HW_VER_PB12);
+		up_udelay(10);
+		stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
+	}
+
+	if (ones[0] > zeros[0]) {
+		rv |= 1 << pos;
+	}
+
+	pos++;
+
+	if (ones[1] > zeros[1]) {
+		rv |= 1 << pos;
+	}
+
+	stm32_configgpio(HW_VER_PB4_INIT);
+	stm32_configgpio(HW_VER_PB12_INIT);
+	*version = rv;
+	return OK;
+}
+
+/************************************************************************************
+ * Name: board_get_hw_type_name
+ *
+ * Description:
+ *   Optional returns a string defining the HW type
+ *
+ *
+ ************************************************************************************/
+
+__EXPORT const char *board_get_hw_type_name()
+{
+	return (const char *) hw_type;
+}
+
+/************************************************************************************
+ * Name: board_get_hw_version
+ *
+ * Description:
+ *   Optional returns a integer HW version
+ *
+ *
+ ************************************************************************************/
+
+__EXPORT int board_get_hw_version()
+{
+	return  HW_VER_SIMPLE(hw_version);
+}
+
+/************************************************************************************
+ * Name: board_get_hw_revision
+ *
+ * Description:
+ *   Optional returns a integer HW revision
+ *
+ *
+ ************************************************************************************/
+
+__EXPORT int board_get_hw_revision()
+{
+	return  hw_revision;
+}
+#endif // BOARD_HAS_SIMPLE_HW_VERSIONING
+
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -273,6 +404,29 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 #  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)
+
+	if (OK == determin_hw_version(&hw_version, & hw_revision)) {
+		switch (hw_version) {
+		default:
+		case 0x8:
+			break;
+
+		case 0xE:
+			hw_type[1]++;
+			hw_type[2] = '0';
+			break;
+
+		case 0xA:
+			hw_type[2] = 'M';
+			break;
+		}
+
+		PX4_INFO("Ver 0x%1X : Rev %x %s", hw_version, hw_revision, hw_type);
+	}
+
+#endif // BOARD_HAS_SIMPLE_HW_VERSIONING
+
 	/* Bring up the Sensor power */
 
 	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);

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

@@ -82,11 +82,30 @@ 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()) {
+		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
 
@@ -99,7 +118,23 @@ static void stm32_spi1_initialize(void)
 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);
+		stm32_configgpio(GPIO_SPI4_CS_PC13);
+		stm32_configgpio(GPIO_SPI4_CS_PC15);
+	}
+
+	if (HW_VER_FMUV2MINI != board_get_hw_version()) {
+		stm32_configgpio(GPIO_SPI4_GPIO_PC14);
+	}
+
+#  endif
 }
 #endif //CONFIG_STM32_SPI4
 
@@ -123,6 +158,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
 {
 	/* 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 */
@@ -163,6 +199,124 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
 	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
+	 * local               -   -   -  V3   a   V2   -   V2M   a    -       -
+	 */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+
+		/* Making sure the other peripherals are not selected */
+		if (HW_VER_FMUV2 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC13, !selected);
+		}
+
+		if (HW_VER_FMUV3 != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
+		}
+
+		stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
+		stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC1, 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 */
+		if (HW_VER_FMUV2 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
+		}
+
+		if (HW_VER_FMUV3 != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC15, !selected);
+		}
+
+		stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
+		stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1);
+		}
+
+		break;
+
+	case PX4_SPIDEV_BARO:
+
+		/* Making sure the other peripherals are not selected */
+		if (HW_VER_FMUV2 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
+		}
+
+		if (HW_VER_FMUV3 != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
+		}
+
+		stm32_gpiowrite(GPIO_SPI1_CS_PD7, !selected);
+		stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1);
+		}
+
+		break;
+
+	case PX4_SPIDEV_MPU:
+
+		/* Making sure the other peripherals are not selected */
+		if (HW_VER_FMUV2 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
+		}
+
+		if (HW_VER_FMUV3 != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
+		}
+
+		stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
+		stm32_gpiowrite(GPIO_SPI1_CS_PC2, !selected);
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC1, 1);
+		}
+
+		break;
+
+	case PX4_SPIDEV_HMC:
+		if (HW_VER_FMUV2 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC13, 1);
+		}
+
+		if (HW_VER_FMUV3 != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC15, 1);
+		}
+
+		stm32_gpiowrite(GPIO_SPI1_CS_PD7, 1);
+		stm32_gpiowrite(GPIO_SPI1_CS_PC2, 1);
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI1_CS_PC1, !selected);
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+#  endif // defined(BOARD_HAS_VERSIONING)
 }
 
 __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid)
@@ -190,6 +344,8 @@ __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 */
@@ -207,6 +363,88 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
 		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
+	 * local               -   -   -  -    -   V3  !V2M  V3   -   -        a
+	 */
+
+	switch (devid) {
+	case PX4_SPIDEV_EXT_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI4_NSS_PE4, !selected);
+
+		if (HW_VER_FMUV2MINI != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1);
+		}
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1);
+			stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1);
+		}
+
+		break;
+
+	case PX4_SPIDEV_EXT_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1);
+
+		if (HW_VER_FMUV2MINI != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, !selected);
+		}
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1);
+			stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1);
+		}
+
+		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);
+
+		if (HW_VER_FMUV2MINI != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1);
+		}
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_CS_PC15, !selected);
+			stm32_gpiowrite(GPIO_SPI4_CS_PC13, 1);
+		}
+
+		break;
+
+	case PX4_SPIDEV_EXT_BMI:
+	case PX4_SPIDEV_EXT_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI4_NSS_PE4, 1);
+
+		if (HW_VER_FMUV2MINI != board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_GPIO_PC14, 1);
+		}
+
+		if (HW_VER_FMUV3 == board_get_hw_version()) {
+			stm32_gpiowrite(GPIO_SPI4_CS_PC15, 1);
+			stm32_gpiowrite(GPIO_SPI4_CS_PC13, !selected);
+		}
+
+		break;
+
+	default:
+		break;
+
+	}
+
+#  endif // defined(BOARD_HAS_VERSIONING)
+
 }
 __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid)
 {
@@ -251,6 +489,32 @@ __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);
+	}
+
+	if (HW_VER_FMUV3 == board_get_hw_version()) {
+		stm32_configgpio(_PIN_OFF(GPIO_SPI1_CS_PC1));
+		stm32_gpiowrite(_PIN_OFF(GPIO_SPI1_CS_PC1), 0);
+
+		stm32_configgpio(_PIN_OFF(GPIO_SPI4_NSS_PE4));
+		stm32_gpiowrite(_PIN_OFF(GPIO_SPI4_NSS_PE4), 0);
+
+		stm32_configgpio(_PIN_OFF(GPIO_SPI4_SCK));
+		stm32_configgpio(_PIN_OFF(GPIO_SPI4_MISO));
+		stm32_configgpio(_PIN_OFF(GPIO_SPI4_MOSI));
+
+		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);
@@ -272,5 +536,16 @@ __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);
+		stm32_configgpio(GPIO_SPI4_MOSI);
+		stm32_spi4_initialize();
+	}
+
+#endif
+
 	stm32_spi1_initialize();
 }