#!nsh

# APM startup script for NuttX on VRBRAIN

# To disable APM startup add a /fs/microsd/APM/nostart 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] APM file found - renaming"
   mv /fs/microsd/APM /fs/microsd/APM.old
fi

if [ -f /fs/microsd/APM/nostart ]
then
   echo "[APM] 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 "[APM] binfs already mounted"
else
	echo "[APM] Mounting binfs"
    if mount -t binfs /dev/null /bin
    then
        echo "[APM] binfs mounted OK"
    else
        sh /etc/init.d/rc.error
    fi
fi

if rm /fs/microsd/APM/boot.log
then
   echo "[APM] Removed old boot.log"
fi
set logfile /fs/microsd/APM/BOOT.LOG

if [ ! -f /bin/ArduPilot ]
then
   echo "[APM] /bin/ArduPilot not found"
   sh /etc/init.d/rc.error
fi

if mkdir /fs/microsd/APM > /dev/null
then
     echo "[APM] Created APM directory"
fi

echo "[APM] Starting UORB"
if uorb start
then
    echo "[APM] UORB started OK"
else
    sh /etc/init.d/rc.error
fi

echo "[APM] Starting ADC"
if adc start
then
    echo "[APM] ADC started OK"
else
    sh /etc/init.d/rc.error
fi

echo "[APM] Starting APM sensors"
if ms5611 start
then
    echo "[APM] MS5611 started OK"
else
    echo "[APM] MS5611 start failed"
    sh /etc/init.d/rc.error
fi

if hmc5883_onboard start
then
    echo "[APM] HMC5883 onboard started OK"
#    if hmc5883 calibrate
#    then
#      echo "[APM] HMC5883 onboard calibrate OK"
#    else 
#      echo "[APM] HMC5883 onboard calibrate failed"
#    fi
else
    echo "[APM] HMC5883 onboard start failed"
    sh /etc/init.d/rc.error
fi

if hmc5883_external start
then
    echo "[APM] HMC5883 external started OK"
#    if hmc5883 calibrate
#    then
#      echo "[APM] HMC5883 external calibrate OK"
#    else 
#      echo "[APM] HMC5883 external calibrate failed"
#    fi
else
    echo "[APM] HMC5883 external start failed"
#    sh /etc/init.d/rc.error
fi

if mpu6000_onboard start
then
    echo "[APM] MPU6000 onboard started OK"
else
    echo "[APM] MPU6000 onboard start failed"
    sh /etc/init.d/rc.error
fi

if mpu6000_external start
then
    echo "[APM] MPU6000 external started OK"
else
    echo "[APM] MPU6000 external start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APM] Starting MTD driver"
if mtd start /fs/mtd
then
    echo "[APM] MTD driver started OK"
else
    echo "[APM] MTD driver start failed"
    sh /etc/init.d/rc.error           
fi

echo "[APM] MTD driver read test"
if mtd readtest /fs/mtd
then
    echo "[APM] MTD driver readtest OK"
else
    echo "[APM] MTD driver failed to read"
    sh /etc/init.d/rc.error           
fi

echo "[APM] Starting VROUTPUT driver"
vroutput mode_pwm
#if vroutput mode_pwm
#then
#    echo "[APM] VROUTPUT driver started OK"
#else
#    echo "[APM] VROUTPUT driver start failed"
#    sh /etc/init.d/rc.error           
#fi

echo "[APM] Starting VRINPUT driver"
vrinput start
#if vrinput start
#then
#    echo "[APM] VRINPUT driver started OK"
#else
#    echo "[APM] VRINPUT driver start failed"
#    sh /etc/init.d/rc.error           
#fi

set sketch NONE
set deviceA /dev/ttyACM0
set deviceC /dev/ttyS2

echo "[APM] Starting ArduPilot"
#if ArduPilot -d $deviceA -d2 $deviceC start
if ArduPilot start
then
    echo "[APM] ArduPilot started OK"
else
    echo "[APM] ArduPilot start failed"
    sh /etc/init.d/rc.error
fi

echo "[APM] Exiting from nsh shell"
exit

echo "[APM] Script finished"