#!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
# To enable PWM on FMUv1 on ttyS1 add a /fs/microsd/APM/AUXPWM.en file

set deviceA /dev/ttyACM0

# 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 [ -f /bin/px4io ]
then
  if [ -f /bin/lsm303d ]
  then
     echo "Detected FMUv2 board"
     set BOARD FMUv2
  else
     echo "Detected FMUv1 board"
     set BOARD FMUv1
  fi
else
  echo "Detected FMUv4 board"
  set BOARD FMUv4   
fi

if [ $BOARD == FMUv1 ]
then
   set deviceC /dev/ttyS2
   if [ -f /fs/microsd/APM/AUXPWM.en ]
   then
      set deviceD /dev/null
   else
      set deviceD /dev/ttyS1
   fi
else
   set deviceC /dev/ttyS1
   set deviceD /dev/ttyS2
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 [ -f /bin/px4io ]
then
  echo "Trying PX4IO board"

  # try the px4io start twice. Some FMUv2 board don't
  # come up the first time
  set HAVE_PX4IO false
  if px4io start norc
  then
    set HAVE_PX4IO true
  else
    # it may be in bootloader mode
    echo Loading /etc/px4io/px4io.bin
    tone_alarm MBABGP
    if px4io update /etc/px4io/px4io.bin
    then
	echo "upgraded PX4IO firmware OK"
        tone_alarm MSPAA
    else
	echo "Failed to upgrade PX4IO firmware"
        tone_alarm MNGGG
    fi
    sleep 1
    if px4io start norc
    then
        set HAVE_PX4IO true
        # play happy tune again
	tone_alarm 1
    fi
  fi
else
    set HAVE_PX4IO false
    echo "No PX4IO support"
fi

if [ $HAVE_PX4IO == true ]
then
    echo "PX4IO board OK"
    if px4io checkcrc /etc/px4io/px4io.bin
    then
        echo "PX4IO CRC OK"
    else
        echo "PX4IO CRC failure"
        echo "PX4IO CRC failure" >> $logfile    
        tone_alarm MBABGP

        if px4io safety_on
        then
            echo "PX4IO disarm OK"
        else
            echo "PX4IO disarm failed"
        fi

        sleep 1

        if px4io forceupdate 14662 /etc/px4io/px4io.bin
        then
               sleep 1
               if px4io start norc
               then
                  echo "PX4IO restart OK"
                  echo "PX4IO restart OK" >> $logfile    
                  tone_alarm MSPAA
               else
                  echo "PX4IO restart failed"
                  echo "PX4IO restart failed" >> $logfile    
                  tone_alarm MNGGG
                  sh /etc/init.d/rc.error
               fi
        else
               echo "PX4IO update failed"
               echo "PX4IO update failed" >> $logfile    
               tone_alarm MNGGG
        fi
    fi
else
    echo "No PX4IO board found"
    echo "No PX4IO board found" >> $logfile

    if [ $BOARD == FMUv2 ]
    then
       sh /etc/init.d/rc.error
    fi
fi

if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
then
        echo "Setting FMU mode_serial"
        fmu mode_serial
else
        if fmu mode_pwm4
        then
                echo "Set FMU mode_pwm4"
        fi
fi


echo "Starting APM sensors"

if ms5611 start
then
   echo "ms5611 started OK"
else
   echo "no ms5611 found"
   echo "No ms5611 found" >> $logfile
   sh /etc/init.d/rc.error
fi

if adc start
then
   echo "adc started OK"
else
   echo "No adc" >> $logfile
   sh /etc/init.d/rc.error
fi

if [ $BOARD == FMUv1 ]
then
    echo "Starting FMUv1 sensors"
    if hmc5883 -C -T -X start
    then
        echo "Have external hmc5883"
    else
        echo "No external hmc5883"
    fi
    if hmc5883 -C -T -I  start
    then
        echo "Have internal hmc5883"
    else
        echo "No internal hmc5883"
    fi
    if mpu6000 start
    then
       echo "mpu6000  started OK"
    else
       sh /etc/init.d/rc.error
    fi
    if l3gd20 start
    then
       echo "l3gd20 started OK"
    else
       echo "No l3gd20"
       echo "No l3gd20" >> $logfile
    fi
fi

if [ $BOARD == FMUv2 ]
then
    echo "Starting FMUv2 sensors"
    if hmc5883 -C -T -X start
    then
        echo "Have external hmc5883"
    else
        echo "No external hmc5883"
    fi
    if hmc5883 -C -T -I -R 4 start
    then
        echo "Have internal hmc5883"
    else
        echo "No internal hmc5883"
    fi

    # external MPU6000 is rotated YAW_180 from standard
    if mpu6000 -X -R 4 start
    then
       echo "Found MPU6000 external"
       set HAVE_FMUV3 true
    else
       if mpu9250 -X -R 4 start
       then
           echo "Found MPU9250 external"
           set HAVE_FMUV3 true
       else
           echo "No MPU6000 or MPU9250 external"
           set HAVE_FMUV3 false
       fi
    fi
    if [ $HAVE_FMUV3 == true ]
    then
        # external L3GD20 is rotated YAW_180 from standard
        if l3gd20 -X -R 4 start
        then
               echo "l3gd20 external started OK"
        else
               echo "No l3gd20"
               sh /etc/init.d/rc.error
        fi
        # external LSM303D is rotated YAW_270 from standard
        if lsm303d -X -R 6 start
        then
               echo "lsm303d external started OK"
        else
               echo "No lsm303d"
               sh /etc/init.d/rc.error
        fi
        # internal MPU6000 is rotated ROLL_180_YAW_270 from standard
        if mpu6000 -R 14 start
        then
               echo "Found MPU6000 internal"
        else
           if mpu9250 -R 14 start
           then
               echo "Found MPU9250 internal"
           else
               echo "No MPU6000 or MPU9250"
               echo "No MPU6000 or MPU9250" >> $logfile
               sh /etc/init.d/rc.error
           fi
        fi
        if hmc5883 -C -T -S -R 8 start
        then
               echo "Found SPI hmc5883"
        fi
    else
        if mpu6000 start
        then
               echo "Found MPU6000"
        else
               if mpu9250 start
               then
                   echo "Found MPU9250"
               else
                   echo "No MPU6000 or MPU9250"
                   echo "No MPU9250" >> $logfile
               fi
        fi
        if l3gd20 start
        then
               echo "l3gd20 started OK"
        else
               sh /etc/init.d/rc.error
        fi
        if lsm303d start
        then
               echo "lsm303d started OK"
        else
               sh /etc/init.d/rc.error
        fi
    fi
fi


if [ $BOARD == FMUv4 ]
then
    echo "Starting FMUv4 sensors"
    if hmc5883 -C -T -X start
    then
        echo "Have external hmc5883"
    else
        echo "No external hmc5883"
    fi
    if hmc5883 -C -T -S -R 2 start
    then
        echo "Have SPI hmc5883"
    else
        echo "No SPI hmc5883"
    fi

    if mpu6000 -R 2 -T 20608 start
    then
       echo "Found ICM-20608 internal"
    fi

    if mpu9250 -R 2 start
    then
       echo "Found mpu9250 internal"
    fi
fi

# optional ETS airspeed sensor
if ets_airspeed start
then
    echo "Found ETS airspeed sensor"
fi

if meas_airspeed start
then
    echo "Found MEAS airspeed sensor"
else
    if meas_airspeed start -b 2
    then
        echo "Found MEAS airspeed sensor (bus2)"
    fi
fi

# optional Range Finder sensor
if ll40ls -X start
then
    echo "Found external ll40ls sensor"
fi

if ll40ls -I start
then
    echo "Found internal ll40ls sensor"
fi

if trone start
then
    echo "Found trone sensor"
fi

if mb12xx start
then
    echo "Found mb12xx sensor"
fi

# optional PX4Flow sensor
if [ -f /bin/px4flow ]
then
  if px4flow start
  then
    echo "Found px4flow sensor"
  fi
fi

# optional PWM input driver
if pwm_input start
then
    echo "started pwm_input driver"
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

# optional oreo leds
if [ -f /bin/oreoled ]
then
  if oreoled start autoupdate
  then
    echo "oreoled started OK"
  fi
fi

# optional smbus battery monitor
if batt_smbus -b 2 start
then
    echo "Found batt_smbus"
fi

# optional irlock
if irlock start
then
    echo "irlock started"
fi

if [ $BOARD == FMUv2 -o $BOARD == FMUv4 ]
then
  # the ramtron on FMUv2 is very fast and can handle trillions of
  # writes. This full rw test on each boot ensures it is working
  # properly. We have one board that failed this, so
  # the test is arguably worth having
  if mtd rwtest /fs/mtd
  then
    echo "mtd rwtest OK"
  else
    echo "failed to test mtd"
    echo "failed to test mtd" >> $logfile
    sh /etc/init.d/rc.error           
  fi
fi

echo Starting ArduPilot $deviceA $deviceC $deviceD
if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start
then
    echo ArduPilot started OK
else
    sh /etc/init.d/rc.error
fi

echo "rc.APM finished"