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

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

echo Starting ArduPilot
if ArduPilot start
then
    echo ArduPilot started OK
else
    sh /etc/init.d/rc.error
fi

echo "rc.APM finished"