#!/bin/sh
#
# PX4 FMUv6xrt specific board sensors init
#------------------------------------------------------------------------------
#
# UART mapping on PX4 FMU-V6XRT:
#
# LPUART1       /dev/ttyS0      CONSOLE
# LPUART3       /dev/ttyS1      GPS
# LPUART4       /dev/ttyS2      TELEM1
# LPUART5       /dev/ttyS3      GPS2
# LPUART6       /dev/ttyS4      PX4IO
# LPUART8       /dev/ttyS5      TELEM2
# LPUART10      /dev/ttyS6      TELEM3
# LPUART11      /dev/ttyS7      EXT2
#
#------------------------------------------------------------------------------

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

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

if ver hwtypecmp V6XRT000 V6XRT001
then
	# Internal SPI bus ICM42686p (hard-mounted)
	icm42688p -6 -R 12 -b 1 -s start

	# Internal on IMU SPI BMI088
	bmi088 -A -R 4 -s start
	bmi088 -G -R 4 -s start

	# Internal on IMU SPI bus ICM42688p
	icm42688p -R 6 -b 2 -s start
fi

if ver hwtypecmp V6XRT002
then
	# Internal SPI bus icm45686 (hard-mounted)
	icm45686 -R 12 -b 1 -s start

	# Internal on IMU SPI BMI088
	bmi088 -A -R 4 -s start
	bmi088 -G -R 4 -s start

	# Internal on IMU SPI bus icm45686
	icm45686 -R 6 -b 2 -s start
fi

if ver hwtypecmp V6XRT000
then
	# Internal magnetometer on I2c
	bmm150 -I start
fi

if ver hwtypecmp V6XRT001 V6XRT002
then
	# Internal magnetometer on I2c
	bmm350 -I 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

# Disable startup of internal baros if param is set to false
if param compare SENS_INT_BARO_EN 1
then
	bmp388 -I -b 3 -a 0x77 start
fi

bmp388 -X -b 2 start

unset INA_CONFIGURED
unset HAVE_PM2