mirror of https://github.com/ArduPilot/ardupilot
504 lines
10 KiB
Plaintext
Executable File
504 lines
10 KiB
Plaintext
Executable File
#!nsh
|
|
|
|
# APM startup script for NuttX on PX4
|
|
|
|
# To disable APM startup add a /fs/microsd/APM/nostart file
|
|
# To enable mkblctrl startup add a /fs/microsd/APM/mkblctrl file
|
|
# To enable mkblctrl_+ startup add a /fs/microsd/APM/mkblctrl_+ file
|
|
# To enable mkblctrl_x startup add a /fs/microsd/APM/mkblctrl_x file
|
|
# To enable PWM on FMUv1 on ttyS1 add a /fs/microsd/APM/AUXPWM.en file
|
|
|
|
set deviceA /dev/ttyACM0
|
|
|
|
# check for an old file called APM, caused by
|
|
# a bug in an earlier firmware release
|
|
if [ -f /fs/microsd/APM ]
|
|
then
|
|
echo "APM file found - renaming"
|
|
mv /fs/microsd/APM /fs/microsd/APM.old
|
|
fi
|
|
|
|
if [ -f /fs/microsd/APM/nostart ]
|
|
then
|
|
echo "APM/nostart found - skipping APM startup"
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
# mount binfs so we can find the built-in apps
|
|
if [ -f /bin/reboot ]
|
|
then
|
|
echo "binfs already mounted"
|
|
else
|
|
echo "Mounting binfs"
|
|
if mount -t binfs /dev/null /bin
|
|
then
|
|
echo "binfs mounted OK"
|
|
else
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
fi
|
|
|
|
set sketch NONE
|
|
if rm /fs/microsd/APM/boot.log
|
|
then
|
|
echo "removed old boot.log"
|
|
fi
|
|
set logfile /fs/microsd/APM/BOOT.LOG
|
|
|
|
if [ ! -f /bin/ArduPilot ]
|
|
then
|
|
echo "/bin/ardupilot not found"
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
if mkdir /fs/microsd/APM > /dev/null
|
|
then
|
|
echo "Created APM directory"
|
|
fi
|
|
|
|
if [ -f /bin/px4io ]
|
|
then
|
|
if [ -f /bin/lsm303d ]
|
|
then
|
|
echo "Detected FMUv2 board"
|
|
set BOARD FMUv2
|
|
else
|
|
echo "Detected FMUv1 board"
|
|
set BOARD FMUv1
|
|
fi
|
|
else
|
|
echo "Detected FMUv4 board"
|
|
set BOARD FMUv4
|
|
fi
|
|
|
|
if [ $BOARD == FMUv1 ]
|
|
then
|
|
set deviceC /dev/ttyS2
|
|
if [ -f /fs/microsd/APM/AUXPWM.en ]
|
|
then
|
|
set deviceD /dev/null
|
|
else
|
|
set deviceD /dev/ttyS1
|
|
fi
|
|
else
|
|
set deviceC /dev/ttyS1
|
|
set deviceD /dev/ttyS2
|
|
fi
|
|
|
|
if uorb start
|
|
then
|
|
echo "uorb started OK"
|
|
else
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
# start mkblctrl driver if configured
|
|
if [ -f /fs/microsd/APM/mkblctrl ]
|
|
then
|
|
echo "Setting up mkblctrl driver"
|
|
echo "Setting up mkblctrl driver" >> $logfile
|
|
mkblctrl -d /dev/pwm_output
|
|
fi
|
|
|
|
if [ -f /fs/microsd/APM/mkblctrl_+ ]
|
|
then
|
|
echo "Setting up mkblctrl driver +"
|
|
echo "Setting up mkblctrl driver +" >> $logfile
|
|
mkblctrl -mkmode + -d /dev/pwm_output
|
|
fi
|
|
|
|
if [ -f /fs/microsd/APM/mkblctrl_x ]
|
|
then
|
|
echo "Setting up mkblctrl driver x"
|
|
echo "Setting up mkblctrl driver x" >> $logfile
|
|
mkblctrl -mkmode x -d /dev/pwm_output
|
|
fi
|
|
|
|
if [ -f /bin/px4io ]
|
|
then
|
|
echo "Trying PX4IO board"
|
|
|
|
# try the px4io start twice. Some FMUv2 board don't
|
|
# come up the first time
|
|
set HAVE_PX4IO false
|
|
if px4io start norc
|
|
then
|
|
set HAVE_PX4IO true
|
|
else
|
|
# it may be in bootloader mode
|
|
echo Loading /etc/px4io/px4io.bin
|
|
tone_alarm MBABGP
|
|
if px4io update /etc/px4io/px4io.bin
|
|
then
|
|
echo "upgraded PX4IO firmware OK"
|
|
tone_alarm MSPAA
|
|
else
|
|
echo "Failed to upgrade PX4IO firmware"
|
|
tone_alarm MNGGG
|
|
fi
|
|
sleep 1
|
|
if px4io start norc
|
|
then
|
|
set HAVE_PX4IO true
|
|
# play happy tune again
|
|
tone_alarm 1
|
|
fi
|
|
fi
|
|
else
|
|
set HAVE_PX4IO false
|
|
echo "No PX4IO support"
|
|
fi
|
|
|
|
if [ $HAVE_PX4IO == true ]
|
|
then
|
|
echo "PX4IO board OK"
|
|
if px4io checkcrc /etc/px4io/px4io.bin
|
|
then
|
|
echo "PX4IO CRC OK"
|
|
else
|
|
echo "PX4IO CRC failure"
|
|
echo "PX4IO CRC failure" >> $logfile
|
|
tone_alarm MBABGP
|
|
|
|
if px4io safety_on
|
|
then
|
|
echo "PX4IO disarm OK"
|
|
else
|
|
echo "PX4IO disarm failed"
|
|
fi
|
|
|
|
sleep 1
|
|
|
|
if px4io forceupdate 14662 /etc/px4io/px4io.bin
|
|
then
|
|
sleep 1
|
|
if px4io start norc
|
|
then
|
|
echo "PX4IO restart OK"
|
|
echo "PX4IO restart OK" >> $logfile
|
|
tone_alarm MSPAA
|
|
else
|
|
echo "PX4IO restart failed"
|
|
echo "PX4IO restart failed" >> $logfile
|
|
tone_alarm MNGGG
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
else
|
|
echo "PX4IO update failed"
|
|
echo "PX4IO update failed" >> $logfile
|
|
tone_alarm MNGGG
|
|
fi
|
|
fi
|
|
else
|
|
echo "No PX4IO board found"
|
|
echo "No PX4IO board found" >> $logfile
|
|
|
|
if [ $BOARD == FMUv2 ]
|
|
then
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
fi
|
|
|
|
if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
|
|
then
|
|
echo "Setting FMU mode_serial"
|
|
fmu mode_serial
|
|
else
|
|
if fmu mode_pwm4
|
|
then
|
|
echo "Set FMU mode_pwm4"
|
|
fi
|
|
fi
|
|
|
|
|
|
if mtd start /fs/mtd
|
|
then
|
|
echo "started mtd driver OK"
|
|
else
|
|
echo "failed to start mtd driver"
|
|
echo "failed to start mtd driver" >> $logfile
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
if mtd readtest /fs/mtd
|
|
then
|
|
echo "mtd readtest OK"
|
|
else
|
|
echo "failed to read mtd"
|
|
echo "failed to read mtd" >> $logfile
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
if [ $BOARD == FMUv2 -o $BOARD == FMUv4 ]
|
|
then
|
|
# the ramtron on FMUv2 is very fast and can handle trillions of
|
|
# writes. This full rw test on each boot ensures it is working
|
|
# properly. We have one board that failed this, so
|
|
# the test is arguably worth having
|
|
if mtd rwtest /fs/mtd
|
|
then
|
|
echo "mtd rwtest OK"
|
|
else
|
|
echo "failed to test mtd"
|
|
echo "failed to test mtd" >> $logfile
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
fi
|
|
|
|
|
|
echo "Starting APM sensors"
|
|
|
|
if ms5611 start
|
|
then
|
|
echo "ms5611 started OK"
|
|
else
|
|
echo "no ms5611 found"
|
|
echo "No ms5611 found" >> $logfile
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
if adc start
|
|
then
|
|
echo "adc started OK"
|
|
else
|
|
echo "No adc" >> $logfile
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
if [ $BOARD == FMUv1 ]
|
|
then
|
|
echo "Starting FMUv1 sensors"
|
|
if hmc5883 -C -T -X start
|
|
then
|
|
echo "Have external hmc5883"
|
|
else
|
|
echo "No external hmc5883"
|
|
fi
|
|
if hmc5883 -C -T -I start
|
|
then
|
|
echo "Have internal hmc5883"
|
|
else
|
|
echo "No internal hmc5883"
|
|
fi
|
|
if mpu6000 start
|
|
then
|
|
echo "mpu6000 started OK"
|
|
else
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
if l3gd20 start
|
|
then
|
|
echo "l3gd20 started OK"
|
|
else
|
|
echo "No l3gd20"
|
|
echo "No l3gd20" >> $logfile
|
|
fi
|
|
fi
|
|
|
|
if [ $BOARD == FMUv2 ]
|
|
then
|
|
echo "Starting FMUv2 sensors"
|
|
if hmc5883 -C -T -X start
|
|
then
|
|
echo "Have external hmc5883"
|
|
else
|
|
echo "No external hmc5883"
|
|
fi
|
|
if hmc5883 -C -T -I -R 4 start
|
|
then
|
|
echo "Have internal hmc5883"
|
|
else
|
|
echo "No internal hmc5883"
|
|
fi
|
|
|
|
# external MPU6000 is rotated YAW_180 from standard
|
|
if mpu6000 -X -R 4 start
|
|
then
|
|
echo "Found MPU6000 external"
|
|
set HAVE_FMUV3 true
|
|
else
|
|
if mpu9250 -X -R 4 start
|
|
then
|
|
echo "Found MPU9250 external"
|
|
set HAVE_FMUV3 true
|
|
else
|
|
echo "No MPU6000 or MPU9250 external"
|
|
set HAVE_FMUV3 false
|
|
fi
|
|
fi
|
|
if [ $HAVE_FMUV3 == true ]
|
|
then
|
|
# external L3GD20 is rotated YAW_180 from standard
|
|
if l3gd20 -X -R 4 start
|
|
then
|
|
echo "l3gd20 external started OK"
|
|
else
|
|
echo "No l3gd20"
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
# external LSM303D is rotated YAW_270 from standard
|
|
if lsm303d -X -R 6 start
|
|
then
|
|
echo "lsm303d external started OK"
|
|
else
|
|
echo "No lsm303d"
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
# internal MPU6000 is rotated ROLL_180_YAW_270 from standard
|
|
if mpu6000 -R 14 start
|
|
then
|
|
echo "Found MPU6000 internal"
|
|
else
|
|
if mpu9250 -R 14 start
|
|
then
|
|
echo "Found MPU9250 internal"
|
|
else
|
|
echo "No MPU6000 or MPU9250"
|
|
echo "No MPU6000 or MPU9250" >> $logfile
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
fi
|
|
if hmc5883 -C -T -S -R 8 start
|
|
then
|
|
echo "Found SPI hmc5883"
|
|
fi
|
|
else
|
|
if mpu6000 start
|
|
then
|
|
echo "Found MPU6000"
|
|
else
|
|
if mpu9250 start
|
|
then
|
|
echo "Found MPU9250"
|
|
else
|
|
echo "No MPU6000 or MPU9250"
|
|
echo "No MPU9250" >> $logfile
|
|
fi
|
|
fi
|
|
if l3gd20 start
|
|
then
|
|
echo "l3gd20 started OK"
|
|
else
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
if lsm303d start
|
|
then
|
|
echo "lsm303d started OK"
|
|
else
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
|
|
if [ $BOARD == FMUv4 ]
|
|
then
|
|
echo "Starting FMUv4 sensors"
|
|
if hmc5883 -C -T -X start
|
|
then
|
|
echo "Have external hmc5883"
|
|
else
|
|
echo "No external hmc5883"
|
|
fi
|
|
if hmc5883 -C -T -S -R 2 start
|
|
then
|
|
echo "Have SPI hmc5883"
|
|
else
|
|
echo "No SPI hmc5883"
|
|
fi
|
|
|
|
if mpu6000 -R 2 -T 20608 start
|
|
then
|
|
echo "Found ICM-20608 internal"
|
|
fi
|
|
|
|
if mpu9250 -R 2 start
|
|
then
|
|
echo "Found mpu9250 internal"
|
|
fi
|
|
fi
|
|
|
|
# optional ETS airspeed sensor
|
|
if ets_airspeed start
|
|
then
|
|
echo "Found ETS airspeed sensor"
|
|
fi
|
|
|
|
if meas_airspeed start
|
|
then
|
|
echo "Found MEAS airspeed sensor"
|
|
else
|
|
if meas_airspeed start -b 2
|
|
then
|
|
echo "Found MEAS airspeed sensor (bus2)"
|
|
fi
|
|
fi
|
|
|
|
# optional Range Finder sensor
|
|
if ll40ls -X start
|
|
then
|
|
echo "Found external ll40ls sensor"
|
|
fi
|
|
|
|
if ll40ls -I start
|
|
then
|
|
echo "Found internal ll40ls sensor"
|
|
fi
|
|
|
|
if trone start
|
|
then
|
|
echo "Found trone sensor"
|
|
fi
|
|
|
|
if mb12xx start
|
|
then
|
|
echo "Found mb12xx sensor"
|
|
fi
|
|
|
|
# optional PX4Flow sensor
|
|
if [ -f /bin/px4flow ]
|
|
then
|
|
if px4flow start
|
|
then
|
|
echo "Found px4flow sensor"
|
|
fi
|
|
fi
|
|
|
|
# optional PWM input driver
|
|
if pwm_input start
|
|
then
|
|
echo "started pwm_input driver"
|
|
fi
|
|
|
|
# optional oreo leds
|
|
if [ -f /bin/oreoled ]
|
|
then
|
|
if oreoled start autoupdate
|
|
then
|
|
echo "oreoled started OK"
|
|
fi
|
|
fi
|
|
|
|
# optional smbus battery monitor
|
|
if batt_smbus -b 2 start
|
|
then
|
|
echo "Found batt_smbus"
|
|
fi
|
|
|
|
# optional irlock
|
|
if irlock start
|
|
then
|
|
echo "irlock started"
|
|
fi
|
|
|
|
echo Starting ArduPilot $deviceA $deviceC $deviceD
|
|
if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start
|
|
then
|
|
echo ArduPilot started OK
|
|
else
|
|
sh /etc/init.d/rc.error
|
|
fi
|
|
|
|
echo "rc.APM finished"
|
|
|