From 4c9b2c65b5e9e9433a0ee5ba06a2c4bb2a6d2e27 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sat, 13 Feb 2021 23:23:12 +0100
Subject: [PATCH] Support v5X build variants This adds support for the
 different implementation variants of the v5X standard.

---
 boards/px4/fmu-v5x/init/rc.board_sensors      | 56 ++++++++++++++-----
 .../fmu-v5x/nuttx-config/scripts/script.ld    |  8 +++
 boards/px4/fmu-v5x/src/board_config.h         |  1 +
 boards/px4/fmu-v5x/src/init.cpp               | 26 +++++++++
 4 files changed, 78 insertions(+), 13 deletions(-)

diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors
index 8449a80dc9..633784ecb8 100644
--- a/boards/px4/fmu-v5x/init/rc.board_sensors
+++ b/boards/px4/fmu-v5x/init/rc.board_sensors
@@ -8,22 +8,52 @@ board_adc start
 ina226 -X -b 1 -t 1 -k start
 ina226 -X -b 2 -t 2 -k start
 
-# Internal SPI BMI088
-bmi088 -A -R 4 -s start
-bmi088 -G -R 4 -s start
+if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1
+then
+	#SKYNODE base fmu board orientation
 
-# Internal SPI bus ICM42688p
-icm42688p -R 6 -s start
+	# Internal SPI BMI088
+	bmi088 -A -R 2 -s start
+	bmi088 -G -R 2 -s start
 
-# Internal SPI bus ICM-20602 (hard-mounted)
-icm20602 -R 10 -s start
+	# Internal SPI bus ICM42688p
+	icm42688p -R 4 -s start
 
-# Internal magnetometer on I2c
-bmm150 -I start
+	# Internal SPI bus ICM-20602 (hard-mounted)
+	icm20602 -R 8 -s start
+
+	# Internal magnetometer on I2c
+	bmm150 -I -R 6 start
+
+else
+	#FMUv5Xbase board orientation
+
+	# Internal SPI BMI088
+	bmi088 -A -R 4 -s start
+	bmi088 -G -R 4 -s start
+
+	# Internal SPI bus ICM42688p
+	icm42688p -R 6 -s start
+
+	# Internal SPI bus ICM-20602 (hard-mounted)
+	icm20602 -R 10 -s start
+
+	# Internal magnetometer on I2c
+	bmm150 -I start
+
+fi
 
 # Possible internal Baro
-bmp388 -I -a 0x77 start
-bmp388 -I start
 
-# Baro on I2C3
-ms5611 -X start
+# Disable startup of internal baros if param is set to false
+if param compare SENS_INT_BARO_EN 1
+then
+	bmp388 -I -a 0x77 start
+	if ver hwtypecmp V5X91 V5Xa1
+	then
+		bmp388 -X -b 2 start
+	else
+		bmp388 -I start
+	fi
+
+fi
diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
index 297385ddcb..8d5a78da6b 100644
--- a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
+++ b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
@@ -127,6 +127,14 @@ SECTIONS
 		_einit = ABSOLUTE(.);
 	} > FLASH_AXIM
 
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > FLASH_AXIM
 
 	.ARM.extab : {
 		*(.ARM.extab*)
diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h
index 61c62097d7..64bcd8e4fb 100644
--- a/boards/px4/fmu-v5x/src/board_config.h
+++ b/boards/px4/fmu-v5x/src/board_config.h
@@ -434,6 +434,7 @@
 
 #define BOARD_NUM_IO_TIMERS 5
 
+
 #define PX4_I2C_BUS_MTD      4,5
 
 __BEGIN_DECLS
diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp
index fee67d3d80..395684f690 100644
--- a/boards/px4/fmu-v5x/src/init.cpp
+++ b/boards/px4/fmu-v5x/src/init.cpp
@@ -286,5 +286,31 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 	px4_platform_configure();
 
+	int hw_version = board_get_hw_version();
+
+	if (hw_version == 0x9 || hw_version == 0xa) {
+		static MCP23009 mcp23009{3, 0x25};
+
+		// No USB
+		if (hw_version == 0x9) {
+			// < P8
+			ret = mcp23009.init(0xf0, 0xf0, 0x0f);
+			// >= P8
+			//ret = mcp23009.init(0xf1, 0xf0, 0x0f);
+		}
+
+		if (hw_version == 0xa) {
+			// < P6
+			//ret = mcp23009.init(0xf0, 0xf0, 0x0f);
+			// >= P6
+			ret = mcp23009.init(0xf1, 0xf0, 0x0f);
+		}
+
+		if (ret != OK) {
+			led_on(LED_RED);
+			return ret;
+		}
+	}
+
 	return OK;
 }