#!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/ttyS1

# 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

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"
	uorb start
	mpu6000 start
	ms5611 start
	hmc5883 start
	adc start

	echo "Trying PX4IO board"
	if px4io start
	then
		echo "PX4IO board OK"
		echo "Loading FMU_pass mixer"
		mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
		echo "Setting FMU mode_serial"
		fmu mode_serial
	else
		echo "No PX4IO board found"
		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
	$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"