forked from Archive/PX4-Autopilot
214 lines
2.6 KiB
Plaintext
214 lines
2.6 KiB
Plaintext
#!nsh
|
|
#
|
|
# Standard startup script for PX4FMU v1, v2, v3, v4 onboard sensor drivers.
|
|
#
|
|
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
if ms5611 start
|
|
then
|
|
fi
|
|
else
|
|
# Configure all I2C buses to 100 KHz as they
|
|
# are all external or slow
|
|
fmu i2c 1 100000
|
|
fmu i2c 2 100000
|
|
|
|
if ms5611 -s start
|
|
then
|
|
fi
|
|
|
|
# Blacksheep telemetry
|
|
if bst start
|
|
then
|
|
fi
|
|
fi
|
|
|
|
if adc start
|
|
then
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V2
|
|
then
|
|
# External I2C bus
|
|
if hmc5883 -C -T -X start
|
|
then
|
|
fi
|
|
|
|
# External I2C bus
|
|
if lis3mdl -X start
|
|
then
|
|
fi
|
|
|
|
# Internal I2C bus
|
|
if hmc5883 -C -T -I -R 4 start
|
|
then
|
|
fi
|
|
|
|
# external MPU6K is rotated 180 degrees yaw
|
|
if mpu6000 -X -R 4 start
|
|
then
|
|
set BOARD_FMUV3 true
|
|
else
|
|
set BOARD_FMUV3 false
|
|
fi
|
|
|
|
if [ $BOARD_FMUV3 == true ]
|
|
then
|
|
# external L3GD20H is rotated 180 degrees yaw
|
|
if l3gd20 -X -R 4 start
|
|
then
|
|
fi
|
|
|
|
# external LSM303D is rotated 270 degrees yaw
|
|
if lsm303d -X -R 6 start
|
|
then
|
|
fi
|
|
|
|
# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
|
|
if mpu6000 -R 14 start
|
|
then
|
|
fi
|
|
|
|
if hmc5883 -C -T -S -R 8 start
|
|
then
|
|
fi
|
|
else
|
|
# FMUv2
|
|
if mpu6000 start
|
|
then
|
|
fi
|
|
|
|
if l3gd20 start
|
|
then
|
|
fi
|
|
|
|
if lsm303d start
|
|
then
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
# External I2C bus
|
|
if hmc5883 -C -T -X start
|
|
then
|
|
fi
|
|
|
|
if lis3mdl -R 2 start
|
|
then
|
|
fi
|
|
|
|
# Internal SPI bus is rotated 90 deg yaw
|
|
if hmc5883 -C -T -S -R 2 start
|
|
then
|
|
fi
|
|
|
|
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
|
|
if mpu6000 -R 2 -T 20608 start
|
|
then
|
|
fi
|
|
|
|
# Internal SPI bus mpu9250 is rotated 90 deg yaw
|
|
if mpu9250 -R 2 start
|
|
then
|
|
fi
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
# FMUv1
|
|
if mpu6000 start
|
|
then
|
|
fi
|
|
|
|
if l3gd20 start
|
|
then
|
|
fi
|
|
|
|
# MAG selection
|
|
if param compare SENS_EXT_MAG 2
|
|
then
|
|
if hmc5883 -C -I start
|
|
then
|
|
fi
|
|
else
|
|
# Use only external as primary
|
|
if param compare SENS_EXT_MAG 1
|
|
then
|
|
if hmc5883 -C -X start
|
|
then
|
|
fi
|
|
else
|
|
# auto-detect the primary, prefer external
|
|
if hmc5883 start
|
|
then
|
|
fi
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
if ver hwcmp MINDPX_V2
|
|
then
|
|
# External I2C bus
|
|
if hmc5883 -C -T -X start
|
|
then
|
|
fi
|
|
|
|
# Internal I2C bus
|
|
if hmc5883 -C -T -I -R 12 start
|
|
then
|
|
fi
|
|
|
|
if mpu9250 -s -R 8 start
|
|
then
|
|
fi
|
|
|
|
if lsm303d -R 10 start
|
|
then
|
|
fi
|
|
|
|
if l3gd20 -R 14 start
|
|
then
|
|
fi
|
|
fi
|
|
|
|
if sdp3x_airspeed start
|
|
then
|
|
fi
|
|
|
|
if ms5525_airspeed start
|
|
then
|
|
fi
|
|
|
|
if ms5525_airspeed start -b 2
|
|
then
|
|
fi
|
|
|
|
if ms4525_airspeed start
|
|
then
|
|
fi
|
|
|
|
if ms4525_airspeed start -b 2
|
|
then
|
|
fi
|
|
|
|
if ets_airspeed start
|
|
then
|
|
fi
|
|
|
|
if ets_airspeed start -b 1
|
|
then
|
|
fi
|
|
|
|
if sf1xx start
|
|
then
|
|
fi
|
|
|
|
# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
|
|
usleep 20000
|
|
if sensors start
|
|
then
|
|
fi
|