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

	uorb start
        if mpu6000 start
        then
                echo "Detected FMUv1 board"
                set BOARD FMUv1
                set PX4IOBIN /etc/px4io/px4io-v1.bin
        else
                l3gd20 start
                echo "Detected FMUv2 board"
                set BOARD FMUv2
                set PX4IOBIN /etc/px4io/px4io-v2.bin
        fi

        echo checking for $PX4IOBIN
	if [ -f $PX4IOBIN ]
	then
	   echo "Checking for new px4io firmware"
	   if cmp $PX4IOBIN /fs/microsd/px4io.loaded
	   then
	      echo "No new px4io firmware"
	   else
	      echo Loading $PX4IOBIN
              tone_alarm MBABGP
	      if px4io update $PX4IOBIN > /fs/microsd/APM/px4io_update.log
	      then
	      	cp $PX4IOBIN /fs/microsd/px4io.loaded
	      	echo Loaded $PX4IOBIN OK >> /fs/microsd/APM/px4io_update.log
                tone_alarm MSPAA
	      else
	        echo Failed loading $PX4IOBIN >> /fs/microsd/APM/px4io_update.log
                echo "check Safety Button" >> /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
        if [ $BOARD == FMUv1 ]
        then
        	ms5611 start
	        hmc5883 start
                adc start
        else
        	ms5611 start
                if hmc5883 start
                then
                        echo "Using external magnetometer"
                fi
                lsm303d start
                adc start
        fi
        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