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

rgbled start -I
rgbled_ncp5623c start -I

board_adc start

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

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

set BOARD_FMUV3 0

# V3 build hwtypecmp supports V2|V2M|V30
if ver hwtypecmp V30
then
	# Check for Pixhawk 2.0 cube (isolated IMU on SPI4)
	#  mpu6000 on v2.0, mpu9250 on v2.1
	if mpu6000 -s -b 4 -R 10 start
	then
		set BOARD_FMUV3 20
	else
		# Check for Pixhawk 2.1 cube
		# isolated/external mpu9250 (SPI4)
		if mpu9250 -s -b 4 -R 10 start
		then
			set BOARD_FMUV3 21
		fi
	fi
fi

# Check if a Pixhack (which reports as V2M) is present
if ver hwtypecmp V2M
then
	# Internal SPI bus ICM-20608-G
	icm20608g -s -b 1 -R 14 start

	# Pixhawk Mini doesn't have these sensors,
	# so if they are found we know its a Pixhack

	# mppu6000 internal (SPI1)
	if mpu6000 -s -b 1 -R 10 start
	then
		set BOARD_FMUV3 20
	else
		# Check for Pixhack 3.1
		#  mpu9250 external (SPI4)
		if mpu9250 -s -b 1 -R 10 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-default SENS_EN_THERMAL 0

	# l3gd20 (external/isolated SPI4)
	l3gd20 -s -b 4 -R 4 start

	# lsm303d (external/isolated SPI4)
	lsm303d -s -b 4 -R 6 start

	# ms5611 (external/isolated SPI4)
	ms5611 -s -b 4 start

	if [ $BOARD_FMUV3 = 20 ]
	then
		# v2.0 internal mpu6000
		mpu6000 -s -b 1 start

		# v2.0 Has internal hmc5883 on SPI1
		hmc5883 -T -s -b 1 -R 8 start
	fi

	if [ $BOARD_FMUV3 = 21 ]
	then
		# v2.1 mpu9250 on SPI1
		mpu9250 -s -b 1 start
	fi

else
	# $BOARD_FMUV3 = 0 -> FMUv2

	mpu6000 -s -b 1 -R 14 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 -b 1 -R 14 start
	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
	fi

	l3gd20 -s -b 1 start
	lsm303d -s -b 1 start
fi

unset BOARD_FMUV3