#!/bin/sh
#
# PX4 FMUv3 specific board sensors init
#------------------------------------------------------------------------------

adc start

# External I2C bus
hmc5883 -C -T -X start
lis3mdl -X start
ist8310 -X start
qmc5883 -X start

# Internal I2C bus
hmc5883 -C -T -I -R 4 start

# Internal SPI bus ICM-20608-G
mpu6000 -s -T 20608 start


# Internal SPI (auto detect ms5611 or ms5607)
if ! ms5611 -T 5607 -s start
then
	ms5611 -s start
fi

set BOARD_FMUV3 0

# V3 build hwtypecmp supports V2|V2M|V30
if ver hwtypecmp V30
then
	# Check for Pixhawk 2.0 cube
	# MPU6K is rotated 180 degrees yaw
	if mpu6000 -s -R 4 start
	then
		set BOARD_FMUV3 20
	else
		# Check for Pixhawk 2.1 cube
		# external MPU9250 is rotated 180 degrees yaw
		if mpu9250 -S -R 4 start
		then
			set BOARD_FMUV3 21
		fi
	fi
fi

# Check if a Pixhack (which reports as V2M) is present
if ver hwtypecmp V2M
then
	# Pixhawk Mini doesn't have these sensors,
	# so if they are found we know its a Pixhack

	# MPU6K is rotated 180 degrees yaw
	if mpu6000 -s -R 4 start
	then
		set BOARD_FMUV3 20
	else
		# Check for Pixhack 3.1
		# external MPU9250 is rotated 180 degrees yaw
		if mpu9250 -S -R 4 start
		then
			set BOARD_FMUV3 21
		fi
	fi
fi

if [ $BOARD_FMUV3 != 0 ]
then
	# sensor heating is available, but we disable it for now
	param set SENS_EN_THERMAL 0

	# ICM20948 as external magnetometer on I2C (e.g. Here GPS)
	if ! icm20948 -X -M -R 6 start
	then
		# external emulated AK09916 (Here2) is rotated 270 degrees yaw
		ak09916 -X -R 6 start
	fi

	# external L3GD20H is rotated 180 degrees yaw
	l3gd20 -X -R 4 start

	# external LSM303D is rotated 270 degrees yaw
	lsm303d -X -R 6 start

	if [ $BOARD_FMUV3 = 20 ]
	then
		# v2.0 internal MPU6000 is rotated 180 deg roll, 270 deg yaw
		mpu6000 -s -R 14 start

		# v2.0 Has internal hmc5883 on SPI1
		hmc5883 -C -T -S -R 8 start
	fi

	if [ $BOARD_FMUV3 = 21 ]
	then
		# v2.1 internal MPU9250 is rotated 180 deg roll, 270 deg yaw
		mpu9250 -s -R 14 start
	fi

else
	# $BOARD_FMUV3 = 0 -> FMUv2

	mpu6000 -s start

	# As we will use the external mag and the ICM-20608-G
	# V2 build hwtypecmp is always false
	# V3 build hwtypecmp supports V2|V2M|V30
	if ! ver hwtypecmp V2M
	then
		mpu9250 start
	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
	fi

	l3gd20 start
	lsm303d start
fi

# External RM3100 (I2C or SPI)
rm3100 start

unset BOARD_FMUV3