#!/bin/sh
#
# Standard startup script for sensor drivers.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#

if ! ver hwcmp OMNIBUS_F4SD
then
	if ! ver hwcmp BITCRAZE_CRAZYFLIE
	then
		# Configure all I2C buses to 100 KHz as they
		# are all external or slow
		# TODO: move this
		pwm_out i2c 1 100000
		pwm_out i2c 2 100000
	fi
fi

###############################################################################
#                           Begin Optional drivers                            #
###############################################################################

if param compare -s SENS_EN_BATT 1
then
	batt_smbus start -X
fi

# Start batmon driver if enabled using BATMON_DRIVER_EN
if param compare -s BATMON_DRIVER_EN 1
then
	batmon start -X  #start on external bus
fi

# Sensors on the PWM interface bank
if param compare -s SENS_EN_LL40LS 1
then
	if pwm_input start
	then
		ll40ls_pwm start
	fi
fi

# External automatic trigger system
if param compare FD_EXT_ATS_EN 1
then
	pwm_input start
fi

# Lidar-Lite on I2C
if param compare -s SENS_EN_LL40LS 2
then
	ll40ls start -X
fi

# mappydot lidar sensor
if param compare -s SENS_EN_MPDT 1
then
	mappydot start -X
fi

# mb12xx sonar sensor
if param greater -s SENS_EN_MB12XX 0
then
	mb12xx start -X
fi

# pga460 sonar sensor
if param greater -s SENS_EN_PGA460 0
then
	pga460 start
fi

# Lightware i2c lidar sensor
if param greater -s SENS_EN_SF1XX 0
then
	lightware_laser_i2c start -X
fi

# Sensor HY-SRF05 or HC-SR05 ultrasonic sensor
if param compare -s SENS_EN_SR05 1
then
	srf05 start
fi


# Heater driver for temperature regulated IMUs.
if param compare -s SENS_EN_THERMAL 1
then
	heater start
fi

# Teraranger one tof sensor
if param greater -s SENS_EN_TRANGER 0
then
	teraranger start -X
fi

# Possible external pmw3901 optical flow sensor
if param greater -s SENS_EN_PMW3901 0
then
	pmw3901 -S start
fi

# Possible external paw3902 optical flow sensor
if param greater -s SENS_EN_PAW3902 0
then
	paw3902 -S start
fi

# vl53l1x i2c distance sensor
if param compare -s SENS_EN_VL53L1X 1
then
	vl53l1x start -X
fi

# ADIS16448 spi external IMU
if param compare -s SENS_EN_ADIS164X 1
then
	if param compare -s SENS_OR_ADIS164X 0
	then
		adis16448 -S start
	fi
	if param compare -s SENS_OR_ADIS164X 4
	then
		adis16448 -S start -R 4
	fi
fi

# Eagle Tree airspeed sensor external I2C
if param compare -s SENS_EN_ETSASPD 1
then
	ets_airspeed start -X
fi

# Sensirion SDP3X differential pressure sensor external I2C
if param compare -s SENS_EN_SDP3X 1
then
	if ! sdp3x_airspeed start -X
	then
		# try another common address
		sdp3x_airspeed start -X -a 0x22
	fi
fi

# TE MS4525 differential pressure sensor external I2C
if param compare -s SENS_EN_MS4525 1
then
	ms4525_airspeed start -X
fi

# TE MS5525 differential pressure sensor external I2C
if param compare -s SENS_EN_MS5525 1
then
	ms5525_airspeed start -X
fi

# probe for optional external I2C devices
if param compare SENS_EXT_I2C_PRB 1
then
	icm20948_i2c_passthrough -X -q start

	# compasses
	hmc5883 -T -X -q start
	ist8308 -X -q start
	ist8310 -X -q start
	lis2mdl -X -q start
	lis3mdl -X -q start
	qmc5883l -X -q start
	rm3100 -X -q start

	# start last (wait for possible icm20948 passthrough mode)
	ak09916 -X -q start
fi

###############################################################################
#                            End Optional drivers                             #
###############################################################################

sensors start