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

restore FMUv2 board versioning

Browse Source
This commit is contained in:
Daniel Agar
2017-11-22 23:20:55 -05:00
committed by Lorenz Meier
parent ab51a41793
commit 08a108ee1b
2 changed files with 429 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4fmu-v2/px4fmu_init.c
+154
View File
@@ -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
+275
View File
@@ -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();
}