From 50f8c30f102a8cf949146676cc923dd3c15d1782 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 21 Mar 2023 16:26:38 +1100 Subject: [PATCH] fmu-v2/v3: refactor detection function This consolidates the version/revision detection function. This should allow for actual changes in a follow up commit. Signed-off-by: Julian Oes --- boards/px4/fmu-v2/src/init.c | 20 +++++++++---------- boards/px4/fmu-v3/src/init.c | 19 +++++++++--------- .../imxrt/board_hw_info/board_hw_rev_ver.c | 13 ------------ .../board_hw_info/board_hw_rev_ver.c | 13 ------------ 4 files changed, 18 insertions(+), 47 deletions(-) diff --git a/boards/px4/fmu-v2/src/init.c b/boards/px4/fmu-v2/src/init.c index 61cc923fd6..59948777f7 100644 --- a/boards/px4/fmu-v2/src/init.c +++ b/boards/px4/fmu-v2/src/init.c @@ -166,7 +166,7 @@ __EXPORT void board_on_reset(int status) } /************************************************************************************ - * Name: determin_hw_version + * Name: determine_hw_info * * Description: * @@ -188,7 +188,7 @@ __EXPORT void board_on_reset(int status) * 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 + * This will return OK on success. * * hw_type Initial state is {'V','2',0, 0} * V 2 - FMUv2 @@ -198,17 +198,16 @@ __EXPORT void board_on_reset(int status) * ************************************************************************************/ -static int determin_hw_version(int *version, int *revision) +static int determine_hw_info(int *revision, int *version) { *revision = 0; /* default revision */ - 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++; + *version |= stm32_gpioread(HW_VER_PB4) << pos++; stm32_configgpio(HW_VER_PB4); up_udelay(10); - rv |= stm32_gpioread(HW_VER_PB4) << pos++; + *version |= stm32_gpioread(HW_VER_PB4) << pos++; int votes = 16; int ones[2] = {0, 0}; @@ -224,18 +223,18 @@ static int determin_hw_version(int *version, int *revision) } if (ones[0] > zeros[0]) { - rv |= 1 << pos; + *version |= 1 << pos; } pos++; if (ones[1] > zeros[1]) { - rv |= 1 << pos; + *version |= 1 << pos; } stm32_configgpio(HW_VER_PB4_INIT); stm32_configgpio(HW_VER_PB12_INIT); - *version = rv; + return OK; } @@ -366,7 +365,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Ensure the power is on 1 ms before we drive the GPIO pins */ usleep(1000); - if (OK == determin_hw_version(&hw_version, & hw_revision)) { + if (OK == determine_hw_info(&hw_revision, &hw_version)) { switch (hw_version) { case HW_VER_FMUV2_STATE: break; @@ -514,7 +513,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif /* Configure the HW based on the manifest */ - px4_platform_configure(); return OK; diff --git a/boards/px4/fmu-v3/src/init.c b/boards/px4/fmu-v3/src/init.c index 2973184961..59948777f7 100644 --- a/boards/px4/fmu-v3/src/init.c +++ b/boards/px4/fmu-v3/src/init.c @@ -166,7 +166,7 @@ __EXPORT void board_on_reset(int status) } /************************************************************************************ - * Name: determin_hw_version + * Name: determine_hw_info * * Description: * @@ -188,7 +188,7 @@ __EXPORT void board_on_reset(int status) * 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 + * This will return OK on success. * * hw_type Initial state is {'V','2',0, 0} * V 2 - FMUv2 @@ -198,17 +198,16 @@ __EXPORT void board_on_reset(int status) * ************************************************************************************/ -static int determin_hw_version(int *version, int *revision) +static int determine_hw_info(int *revision, int *version) { *revision = 0; /* default revision */ - 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++; + *version |= stm32_gpioread(HW_VER_PB4) << pos++; stm32_configgpio(HW_VER_PB4); up_udelay(10); - rv |= stm32_gpioread(HW_VER_PB4) << pos++; + *version |= stm32_gpioread(HW_VER_PB4) << pos++; int votes = 16; int ones[2] = {0, 0}; @@ -224,18 +223,18 @@ static int determin_hw_version(int *version, int *revision) } if (ones[0] > zeros[0]) { - rv |= 1 << pos; + *version |= 1 << pos; } pos++; if (ones[1] > zeros[1]) { - rv |= 1 << pos; + *version |= 1 << pos; } stm32_configgpio(HW_VER_PB4_INIT); stm32_configgpio(HW_VER_PB12_INIT); - *version = rv; + return OK; } @@ -366,7 +365,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Ensure the power is on 1 ms before we drive the GPIO pins */ usleep(1000); - if (OK == determin_hw_version(&hw_version, & hw_revision)) { + if (OK == determine_hw_info(&hw_revision, &hw_version)) { switch (hw_version) { case HW_VER_FMUV2_STATE: break; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c index bb85fc47d3..b04ac6b34e 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -66,19 +66,6 @@ static char hw_info[HW_INFO_SIZE] = {0}; /**************************************************************************** * Protected Functions ****************************************************************************/ -/**************************************************************************** - * Name: determin_hw_version - * - * Description: - * - * This function fist determines if revision and version resistors are in place. - * if they it will read the ADC channels and decode the DN to ordinal numbers - * that will be returned by board_get_hw_version and board_get_hw_revision API - * - * This will return OK on success and -1 on not supported -* - * - ****************************************************************************/ static int dn_to_ordinal(uint16_t dn) { diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 700d1dacb4..54c8a836c9 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -71,19 +71,6 @@ static char hw_info[HW_INFO_SIZE] = {0}; /**************************************************************************** * Protected Functions ****************************************************************************/ -/**************************************************************************** - * Name: determin_hw_version - * - * Description: - * - * This function fist determines if revision and version resistors are in place. - * if they it will read the ADC channels and decode the DN to ordinal numbers - * that will be returned by board_get_hw_version and board_get_hw_revision API - * - * This will return OK on success and -1 on not supported -* - * - ****************************************************************************/ static int dn_to_ordinal(uint16_t dn) {