#!/bin/sh
#
# ZeroOne X6 specific board sensors init
#------------------------------------------------------------------------------
set HAVE_PM2 yes
set INA_CONFIGURED no

if mft query -q -k MFT -s MFT_PM2 -v 0
then
	set HAVE_PM2 no
fi
if param compare -s ADC_ADS1115_EN 1
then
	ads1115 start -X
	board_adc start -n
else
	board_adc start
fi


if param compare SENS_EN_INA226 1
then
	# Start Digital power monitors
	ina226 -X -b 1 -t 1 -k start

	if [ $HAVE_PM2 = yes ]
	then
		ina226 -X -b 2 -t 2 -k start
	fi

	set INA_CONFIGURED yes
fi

if param compare SENS_EN_INA228 1
then
	# Start Digital power monitors
	ina228 -X -b 1 -t 1 -k start
	if [ $HAVE_PM2 = yes ]
	then
		ina228 -X -b 2 -t 2 -k start
	fi

	set INA_CONFIGURED yes
fi

if param compare SENS_EN_INA238 1
then
	# Start Digital power monitors
	ina238 -X -b 1 -t 1 -k start
	if [ $HAVE_PM2 = yes ]
	then
		ina238 -X -b 2 -t 2 -k start
	fi

	set INA_CONFIGURED yes
fi

#Start Auterion Power Module selector for zeroone boards
if ver hwbasecmp 009 010 011
then
	pm_selector_auterion start
else
	if [ $INA_CONFIGURED = no ]
	then
		# INA226, INA228, INA238 auto-start
		i2c_launcher start -b 1
		if [ $HAVE_PM2 = yes ]
		then
			i2c_launcher start -b 2
		fi
	fi
fi

# Keep nesting shallow
if ver hwtypecmp V6X006 V6X008
then
	if ver hwtypecmp V6X006
	then
		# Internal SPI bus ICM45686
		adis16470 -s -R 0 start
		iim42652 -s -R 6 start
		icm45686 -s -R 10 start
	else
		# Internal SPI bus 3x ICM45686
		icm45686 -b 3 -s -R 0 start
		icm45686 -b 2 -s -R 0 start
		icm45686 -b 1 -s -R 10 start
	fi
else
	if ver hwtypecmp V6X004
	then
		# Internal SPI bus ICM20649
		icm20649 -s -R 6 start
	else
		# Internal SPI BMI088
		if ver hwbasecmp 009 010 011
		then
			bmi088 -A -R 6 -s start
			bmi088 -G -R 6 -s start
		else
			if ver hwtypecmp V6X010
			then
				bmi088 -A -R 0 -s start
				bmi088 -G -R 0 -s start
			else
				bmi088 -A -R 4 -s start
				bmi088 -G -R 4 -s start
			fi
		fi
	fi

	# Internal SPI bus ICM42688p
	if ver hwbasecmp 009 010 011
	then
		icm42688p -R 12 -s start
	else
		if ver hwtypecmp V6X010
		then
			icm42688p -R 14 -s start
		else
			icm45686 -b 1 -s -R 8 start        //ZeroOne
		fi
	fi

	if ver hwtypecmp V6X003 V6X004
	then
		# Internal SPI bus ICM-42670-P (hard-mounted)
		icm42670p -R 10 -s start
	else
		if ver hwbasecmp 009 010 011
		then
			icm20602 -R 6 -s start
		else
			# Internal SPI bus ICM-20649 (hard-mounted)
			icm45686 -b 2 -s -R 6 start        //ZeroOne
		fi
	fi
fi
rm3100 -I -b 4 start
# Internal magnetometer on I2c
# if ver hwtypecmp V6X001
# then
# 	rm3100 -I -b 4 start
# else
# 	# Internal magnetometer on I2C
# 	bmm150 -I -R 0 start
# fi

# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
ist8310 -X -b 1 -R 10 start

# Possible internal Baro
if param compare SENS_INT_BARO_EN 1
then
	icp201xx -I -a 0x64 start
	# if ver hwtypecmp V6X001 V6X006 V6X008
	# then
	# 	icp201xx -I -a 0x64 start
	# else
	# 	bmp388 -I -a 0x77 start
	# fi
fi
icp201xx -X start
#external baro
# if ver hwtypecmp V6X001
# then
# 	icp201xx -X start
# else
# 	bmp388 -X start
# fi

# Baro on I2C3
ms5611 -X start

unset INA_CONFIGURED
unset HAVE_PM2