From fdf4b668eb20372345459690b2d567b036791789 Mon Sep 17 00:00:00 2001
From: alexklimaj <alex@arkelectron.com>
Date: Fri, 26 Aug 2022 11:19:57 -0600
Subject: [PATCH] Ark flow rev 3 with PAA3905"

Add AFBR-S50LX85D
---
 boards/ark/can-flow/default.px4board                |  2 ++
 boards/ark/can-flow/init/rc.board_sensors           | 12 +++++++++---
 boards/ark/can-flow/src/spi.cpp                     |  2 ++
 .../distance_sensor/broadcom/afbrs50/AFBRS50.cpp    | 13 +++++++++++--
 4 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board
index 48b3132cc3..1d84adebf0 100644
--- a/boards/ark/can-flow/default.px4board
+++ b/boards/ark/can-flow/default.px4board
@@ -8,6 +8,8 @@ CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
diff --git a/boards/ark/can-flow/init/rc.board_sensors b/boards/ark/can-flow/init/rc.board_sensors
index b7502f98fd..9099c97c41 100644
--- a/boards/ark/can-flow/init/rc.board_sensors
+++ b/boards/ark/can-flow/init/rc.board_sensors
@@ -6,12 +6,18 @@
 param set-default IMU_GYRO_RATEMAX 1000
 
 # Internal SPI
-paw3902 -s start -Y 180
+if ! paw3902 -s start -Y 180
+then
+	paa3905 -s start -Y 180
+fi
 
 if ! icm42688p -R 0 -s start
 then
-	bmi088 -A -s -R 4 start
-	bmi088 -G -s -R 4 start
+	if ! iim42652 -R 0 -s start
+	then
+		bmi088 -A -s -R 4 start
+		bmi088 -G -s -R 4 start
+	fi
 fi
 
 afbrs50 start
diff --git a/boards/ark/can-flow/src/spi.cpp b/boards/ark/can-flow/src/spi.cpp
index 92d24684a8..4d8f5469db 100644
--- a/boards/ark/can-flow/src/spi.cpp
+++ b/boards/ark/can-flow/src/spi.cpp
@@ -40,7 +40,9 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
 		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}),
+		initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}),
 		initSPIDevice(DRV_FLOW_DEVTYPE_PAW3902, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
+		initSPIDevice(DRV_FLOW_DEVTYPE_PAA3905, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin12}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
index 01e9634073..b66eeeba3d 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
@@ -148,6 +148,7 @@ int AFBRS50::init()
 		PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c);
 
 		argus_module_version_t mv = Argus_GetModuleVersion(_hnd);
+		argus_laser_type_t lt = Argus_GetLaserType(_hnd);
 
 		switch (mv) {
 		case AFBR_S50MV85G_V1:
@@ -167,11 +168,19 @@ int AFBRS50::init()
 
 		case AFBR_S50LV85D_V1:
 			_min_distance = 0.08f;
-			_max_distance = 30.f;
+
+			if (lt == LASER_H_V2X) {
+				_max_distance = 50.f;
+				PX4_INFO_RAW("AFBR-S50LX85D (v2)\n");
+
+			} else {
+				_max_distance = 30.f;
+				PX4_INFO_RAW("AFBR-S50LV85D (v1)\n");
+			}
+
 			_px4_rangefinder.set_min_distance(_min_distance);
 			_px4_rangefinder.set_max_distance(_max_distance);
 			_px4_rangefinder.set_fov(math::radians(6.f));
-			PX4_INFO_RAW("AFBR-S50LV85D (v1)\n");
 			break;
 
 		case AFBR_S50MV68B_V1: