ardupilot/mk/PX4/ROMFS/init.d/rc.APM

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"