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

#!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 mavlink on ttys0 add a /fs/microsd/APM/mavlink-ttys0 file

set deviceA /dev/ttyACM0

if [ -f /fs/microsd/APM/mavlink-ttys0 ]
then
   set deviceC /dev/ttyS0
else
   set deviceC /dev/ttyS2
fi

# 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"
else

# mount binfs so we can find the built-in apps
if [ -f /bin/reboot ]
then
	echo "binfs already mounted"
else
	echo "Mounting binfs"
	mount -t binfs /dev/null /bin
fi

set sketch NONE
set logfile /fs/microsd/APM/boot.log

if [ -f /bin/ArduPilot ]
then
	set sketch ArduPilot
fi

if [ $sketch != NONE ]
then
	if mkdir /fs/microsd/APM
	then
	     echo "Created APM directory"
	fi

	if [ -f /fs/microsd/px4io.bin ]
	then
	   echo "Checking for new px4io firmware"
	   if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.loaded
	   then
	      echo "No new px4io firmware"
	   else
	      echo "Loading /fs/microsd/px4io.bin"
              tone_alarm MBABGP
	      if px4io update /fs/microsd/px4io.bin > /fs/microsd/APM/px4io_update.log
	      then
	      	cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.loaded
	      	echo "Loaded /fs/microsd/px4io.bin OK" >> /fs/microsd/APM/px4io_update.log
                tone_alarm MSPAA
	      else
	        echo "Failed loading /fs/microsd/px4io.bin (check PX4IO is in bootloader mode - hold down the Safety Button while powering up)" >> /fs/microsd/APM/px4io_update.log
		echo "Failed to upgrade PX4IO firmware - check PX4IO is in bootloader mode"
                tone_alarm MNGGG
	      fi
	   fi
	fi

	echo "Starting APM sensors"
	echo "Starting APM sensors" > $logfile
	uorb start
	mpu6000 start
	ms5611 start
	hmc5883 start
	adc start
        if ets_airspeed start
        then
                echo "Found ETS airspeed sensor" >> $logfile
        fi

	echo "Trying PX4IO board"
	echo "Trying PX4IO board" >> $logfile
	if px4io start
	then
		echo "PX4IO board OK"
		echo "PX4IO board OK" >> $logfile
		echo "Setting FMU mode_pwm"
		fmu mode_pwm
		echo "Loading FMU_pass mixer"
		mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
		if [ $deviceC == /dev/ttyS1 ]
		then
			# ttyS1 is used for PWM output for 4 extra channels
			set deviceC /dev/ttyS2
		fi
	else
		echo "No PX4IO board found"
		echo "No PX4IO board found" >> $logfile

		if [ -f /fs/microsd/APM/mkblctrl ]
		then
	   	echo "APM/mkblctrl found - switch to MK I2C ESCs"
	   	echo "APM/mkblctrl found - switch to MK I2C ESCs" >> $logfile
			echo "Setting up mkblctrl driver"
			echo "Setting up mkblctrl driver" >> $logfile
         		sleep 1
			mkblctrl -mkmode x
			sleep 1
	

		fi

		echo "Setting up PX4FMU direct mode"
		fmu mode_pwm
		if [ $deviceC == /dev/ttyS1 ]
		then
			# ttyS1 is used for PWM output when there
			# is no IO board
			set deviceC /dev/ttyS2
		fi
	fi

	echo Starting $sketch
	echo Starting $sketch $deviceA $deviceC >> $logfile
	$sketch -d $deviceA -d2 $deviceC start

	# if starting on the console, tell nsh to exit
	# this prevents it from chewing bytes
	if [ $deviceC == /dev/ttyS0 ]
	then
		echo "Exiting from nsh shell"
		exit
	fi
else
	echo "No APM sketch found"
fi

echo "rc.APM finished"
echo "rc.APM finished" >> $logfile
fi