diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_init.c b/src/drivers/boards/px4fmu-v2/px4fmu_init.c index 7d8b460d3a..9490100774 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_init.c +++ b/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); diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 48726c0579..8ad297712f 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/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(); }