Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

fmu-v2/v3: fix HW detection on CubeBlack

Browse Source 
This fixes the case where the hardware detection fails on CubeBlack when
a CAN device is connected to CAN1 and talking, and therefore preventing
the check from getting a clear result.

Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
Julian Oes
2023-03-21 16:53:52 +11:00
committed by Ramon Roche
parent 50f8c30f10
commit 19752a7670
2 changed files with 44 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/fmu-v2/src/init.c
+22 -7
View File
@@ -190,6 +190,9 @@ __EXPORT void board_on_reset(int status)
* *
* This will return OK on success. * This will return OK on success.
* *
* If PB12 is not returning a consistent result we assume that it's a Cube Black
* with something connected to CAN1 and talking to it.
*
* hw_type Initial state is {'V','2',0, 0} * hw_type Initial state is {'V','2',0, 0}
* V 2 - FMUv2 * V 2 - FMUv2
* V 3 0 - FMUv3 2.0 * V 3 0 - FMUv3 2.0
@@ -209,7 +212,7 @@ static int determine_hw_info(int *revision, int *version)
up_udelay(10); up_udelay(10);
*version |= stm32_gpioread(HW_VER_PB4) << pos++; *version |= stm32_gpioread(HW_VER_PB4) << pos++;
int votes = 16; int votes = 100;
int ones[2] = {0, 0}; int ones[2] = {0, 0};
int zeros[2] = {0, 0}; int zeros[2] = {0, 0};
@@ -222,14 +225,26 @@ static int determine_hw_info(int *revision, int *version)
stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++; stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
} }
if (ones[0] > zeros[0]) { const int margin = 50;
*version |= 1 << pos; // On Cube the detection does not work as expected when something
} // is connected to CAN1. In that case, there is no clear winner
// between ones and zeros.
pos++; if (*version == 0x2 && abs(ones[0] - zeros[0]) < margin && abs(ones[1] - zeros[1]) < margin) {
*version = HW_VER_FMUV3_STATE;
syslog(LOG_DEBUG, "Ambiguous board detection, assuming Pixhawk Cube\n");
if (ones[1] > zeros[1]) { } else {
*version |= 1 << pos;
if (ones[0] > zeros[0]) {
*version |= 1 << pos;
}
pos++;
if (ones[1] > zeros[1] + margin) {
*version |= 1 << pos;
}
} }
stm32_configgpio(HW_VER_PB4_INIT); stm32_configgpio(HW_VER_PB4_INIT);
boards/px4/fmu-v3/src/init.c
+22 -7
View File
@@ -190,6 +190,9 @@ __EXPORT void board_on_reset(int status)
* *
* This will return OK on success. * This will return OK on success.
* *
* If PB12 is not returning a consistent result we assume that it's a Cube Black
* with something connected to CAN1 and talking to it.
*
* hw_type Initial state is {'V','2',0, 0} * hw_type Initial state is {'V','2',0, 0}
* V 2 - FMUv2 * V 2 - FMUv2
* V 3 0 - FMUv3 2.0 * V 3 0 - FMUv3 2.0
@@ -209,7 +212,7 @@ static int determine_hw_info(int *revision, int *version)
up_udelay(10); up_udelay(10);
*version |= stm32_gpioread(HW_VER_PB4) << pos++; *version |= stm32_gpioread(HW_VER_PB4) << pos++;
int votes = 16; int votes = 100;
int ones[2] = {0, 0}; int ones[2] = {0, 0};
int zeros[2] = {0, 0}; int zeros[2] = {0, 0};
@@ -222,14 +225,26 @@ static int determine_hw_info(int *revision, int *version)
stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++; stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
} }
if (ones[0] > zeros[0]) { const int margin = 50;
*version |= 1 << pos; // On Cube the detection does not work as expected when something
} // is connected to CAN1. In that case, there is no clear winner
// between ones and zeros.
pos++; if (*version == 0x2 && abs(ones[0] - zeros[0]) < margin && abs(ones[1] - zeros[1]) < margin) {
*version = HW_VER_FMUV3_STATE;
syslog(LOG_DEBUG, "Ambiguous board detection, assuming Pixhawk Cube\n");
if (ones[1] > zeros[1]) { } else {
*version |= 1 << pos;
if (ones[0] > zeros[0]) {
*version |= 1 << pos;
}
pos++;
if (ones[1] > zeros[1] + margin) {
*version |= 1 << pos;
}
} }
stm32_configgpio(HW_VER_PB4_INIT); stm32_configgpio(HW_VER_PB4_INIT);