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

rgbled start -I

adc start

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

# Internal I2C bus
hmc5883 -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

	# (external) L3GD20H is rotated 180 degrees yaw
	l3gd20 -s -R 4 start

	# (external) LSM303D is rotated 270 degrees yaw
	lsm303d -s -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 -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 -s start
	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
	fi

	l3gd20 -s start
	lsm303d -s start
fi

unset BOARD_FMUV3