dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fmu-v2/v3: fix HW detection on CubeBlack 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
boards/px4/fmu-v2/src/init.c
View File

@ -190,6 +190,9 @@ __EXPORT void board_on_reset(int status)
*
* 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}
* V 2 - FMUv2
* V 3 0 - FMUv3 2.0
@ -209,7 +212,7 @@ static int determine_hw_info(int *revision, int *version)
up_udelay(10);
*version |= stm32_gpioread(HW_VER_PB4) << pos++;
int votes = 16;
int votes = 100;
int ones[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]++;
}
if (ones[0] > zeros[0]) {
*version |= 1 << pos;
}
const int margin = 50;
// 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]) {
*version |= 1 << pos;
} else {
if (ones[0] > zeros[0]) {
*version |= 1 << pos;
}
pos++;
if (ones[1] > zeros[1] + margin) {
*version |= 1 << pos;
}
}
stm32_configgpio(HW_VER_PB4_INIT);

29
boards/px4/fmu-v3/src/init.c
View File

@ -190,6 +190,9 @@ __EXPORT void board_on_reset(int status)
*
* 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}
* V 2 - FMUv2
* V 3 0 - FMUv3 2.0
@ -209,7 +212,7 @@ static int determine_hw_info(int *revision, int *version)
up_udelay(10);
*version |= stm32_gpioread(HW_VER_PB4) << pos++;
int votes = 16;
int votes = 100;
int ones[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]++;
}
if (ones[0] > zeros[0]) {
*version |= 1 << pos;
}
const int margin = 50;
// 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]) {
*version |= 1 << pos;
} else {
if (ones[0] > zeros[0]) {
*version |= 1 << pos;
}
pos++;
if (ones[1] > zeros[1] + margin) {
*version |= 1 << pos;
}
}
stm32_configgpio(HW_VER_PB4_INIT);