#!nsh

# APM startup script for NuttX on VRBRAIN

# mount binfs so we can find the built-in apps
if [ -f /bin/reboot ]
then
	echo "[APMNS] binfs already mounted"
else
	echo "[APMNS] Mounting binfs"
    if mount -t binfs /dev/null /bin
    then
        echo "[APMNS] binfs mounted OK"
    else
        sh /etc/init.d/rc.error
    fi
fi

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

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

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

echo "[APMNS] Starting APM sensors"

echo "[APMNS] Starting MS5611 Internal"
if ms5611 -I start
then
    echo "[APMNS] MS5611 onboard started OK"
else
    echo "[APMNS] MS5611 onboard start failed"
    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting MS5611 External EXP"
if ms5611 -X start
then
    echo "[APMNS] MS5611 external EXP started OK"
else
    echo "[APMNS] MS5611 external EXP start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting MS5611 External IMU"
if ms5611 -U start
then
    echo "[APMNS] MS5611 external IMU started OK"
else
    echo "[APMNS] MS5611 external IMU start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting HMC5883 External GPS"
if hmc5883 -X start
then
    echo "[APMNS] HMC5883 External GPS started OK"
    if hmc5883 -X calibrate
    then
      echo "[APMNS] HMC5883 External GPS calibrate OK"
    else 
      echo "[APMNS] HMC5883 External GPS calibrate failed"
    fi
else
    echo "[APMNS] HMC5883 External GPS start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting HMC5883 Internal"
if hmc5883 -I start
then
    echo "[APMNS] HMC5883 Internal started OK"
    if hmc5883 -I calibrate
    then
      echo "[APMNS] HMC5883 Internal calibrate OK"
    else 
      echo "[APMNS] HMC5883 Internal calibrate failed"
    fi
else
    echo "[APMNS] HMC5883 Internal start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting HMC5983 External EXP"
if hmc5983 -X start
then
    echo "[APMNS] HMC5983 external EXP started OK"
    if hmc5983 -X calibrate
    then
      echo "[APMNS] HMC5983 external EXP calibrate OK"
    else 
      echo "[APMNS] HMC5983 external EXP calibrate failed"
    fi
else
    echo "[APMNS] HMC5983 external EXP start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting HMC5983 External IMU"
if hmc5983 -U start
then
    echo "[APMNS] HMC5983 external IMU started OK"
    if hmc5983 -U calibrate
    then
      echo "[APMNS] HMC5983 external IMU calibrate OK"
    else 
      echo "[APMNS] HMC5983 external IMU calibrate failed"
    fi
else
    echo "[APMNS] HMC5983 external IMU start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting MPU6000 Internal"
if mpu6000 -I start
then
    echo "[APMNS] MPU6000 onboard started OK"
else
    echo "[APMNS] MPU6000 onboard start failed"
    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting MPU6000 External EXP"
if mpu6000 -X start
then
    echo "[APMNS] MPU6000 external EXP started OK"
else
    echo "[APMNS] MPU6000 external EXP start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting MPU6000 External IMU"
if mpu6000 -U start
then
    echo "[APMNS] MPU6000 external IMU started OK"
else
    echo "[APMNS] MPU6000 external IMU start failed"
#    sh /etc/init.d/rc.error
fi

echo "[APMNS] Starting MEAS I2C Airspeed"
if meas_airspeed start
then
    echo "Found MEAS airspeed sensor"
fi

echo "[APMNS] Starting MB12XX Range Finder Sensor"
if mb12xx start
then
    echo "Found mb12xx sensor"
fi

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

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

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

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

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

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

echo "[APMNS] Exiting from nsh shell"
exit

echo "[APMNS] Script finished"