#!nsh

# APM startup script for NuttX on PX4

# To disable APM startup add a /fs/microsd/APM/nostart file

set deviceA /dev/ttyACM0
set deviceC /dev/ttyS2

# 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/ArduPlane ]
then
	set sketch ArduPlane
fi

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

if [ -f /bin/APMrover2 ]
then
	set sketch APMrover2
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"
	      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
	      else
	      	echo "Failed loading /fs/microsd/px4io.bin" >> /fs/microsd/APM/px4io_update.log
		echo "Failed to upgrade PX4IO firmware - check PX4IO is in bootloader mode"
	      fi
	   fi
	fi

	echo "Starting APM sensors"
	echo "Starting APM sensors" > $logfile
	uorb start
	mpu6000 start
	ms5611 start
	hmc5883 start
	adc start

	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
		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 >> $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
fi

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