From 1144b4f3d73b47f183c13d95d53a7514169a2337 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 14 Dec 2013 16:14:28 +1100
Subject: [PATCH] PX4: added automatic IO firmware update

---
 mk/PX4/ROMFS/init.d/rc.APM | 116 ++++++++++++++++++-------------------
 1 file changed, 58 insertions(+), 58 deletions(-)

diff --git a/mk/PX4/ROMFS/init.d/rc.APM b/mk/PX4/ROMFS/init.d/rc.APM
index f118cc8c90..7da5216f59 100644
--- a/mk/PX4/ROMFS/init.d/rc.APM
+++ b/mk/PX4/ROMFS/init.d/rc.APM
@@ -4,7 +4,10 @@
 
 # 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
+# To enable uartD on FMUv1 on ttyS1 add a /fs/microsd/APM/uartD.en
+# file
+
+# enable uartD by default on PX4v1
 
 set deviceA /dev/ttyACM0
 
@@ -45,34 +48,6 @@ then
    sh /etc/init.d/rc.error
 fi
 
-########################
-# PX4IO upgrade handling
-echo checking for /etc/px4io/px4io.bin
-if [ -f /etc/px4io/px4io.bin ]
-then
-   echo "Checking for new px4io firmware"
-   if cmp /etc/px4io/px4io.bin /fs/microsd/px4io.loaded
-   then
-      echo "No new px4io firmware"
-   else
-      echo Loading /etc/px4io/px4io.bin
-      tone_alarm MBABGP
-      echo Loading /etc/px4io/px4io.bin > /fs/microsd/px4io_update.log
-      if px4io update /etc/px4io/px4io.bin
-      then
-      	cp /etc/px4io/px4io.bin /fs/microsd/px4io.loaded
-      	echo Loaded /etc/px4io/px4io.bin OK >> /fs/microsd/px4io_update.log
-        tone_alarm MSPAA
-      else
-        echo Failed loading /etc/px4io/px4io.bin >> /fs/microsd/px4io_update.log
-        echo "check Safety Button" >> /fs/microsd/APM/px4io_update.log
-	echo "Failed to upgrade PX4IO firmware"
-        tone_alarm MNGGG
-      fi
-   fi
-fi
-
-
 if mkdir /fs/microsd/APM > /dev/null
 then
      echo "Created APM directory"
@@ -87,16 +62,18 @@ else
      set BOARD FMUv1
 fi
 
-if [ -f /fs/microsd/APM/mavlink-ttys0 ]
+if [ $BOARD == FMUv1 ]
 then
-   set deviceC /dev/ttyS0
-else
-   if [ $BOARD == FMUv1 ]
+   set deviceC /dev/ttyS2
+   if [ -f /fs/microsd/APM/uartD.en ]
    then
-	set deviceC /dev/ttyS2
+      set deviceD /dev/ttyS1
    else
-	set deviceC /dev/ttyS1
+      set deviceD /dev/null
    fi
+else
+   set deviceC /dev/ttyS1
+   set deviceD /dev/ttyS2
 fi
 
 if uorb start
@@ -114,8 +91,18 @@ if px4io start
 then
     set HAVE_PX4IO true
 else
-    tone_alarm MNGG
-    sleep 2
+    # 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
     then
         set HAVE_PX4IO true
@@ -128,12 +115,33 @@ if [ $HAVE_PX4IO == true ]
 then
     echo "PX4IO board OK"
     echo "PX4IO board OK" >> $logfile
-    echo "Setting FMU mode_pwm"
-    fmu mode_pwm
-    if [ $BOARD == FMUv1 -a $deviceC == /dev/ttyS1 ]
+    if px4io checkcrc /etc/px4io/px4io.bin
     then
-	# ttyS1 is used for PWM output for 4 extra channels
-	set deviceC /dev/ttyS2
+        echo "PX4IO CRC OK"
+        echo "PX4IO CRC OK" >> $logfile    
+    else
+        echo "PX4IO CRC failure"
+        echo "PX4IO CRC failure" >> $logfile    
+        tone_alarm MBABGP
+        if px4io forceupdate 14662 /etc/px4io/px4io.bin
+        then
+               sleep 1
+               if px4io start
+               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"
@@ -152,15 +160,15 @@ else
 	echo "Setting up mkblctrl driver" >> $logfile
 	mkblctrl -mkmode x
     fi
+fi
 
-    echo "Setting up PX4FMU direct mode"
-    fmu mode_pwm
-    if [ $BOARD == FMUv1 -a $deviceC == /dev/ttyS1 ]
-    then
-	# ttyS1 is used for PWM output when there
-	# is no IO board
-	set deviceC /dev/ttyS2
-    fi
+if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
+then
+        echo "Setting FMU mode_serial"
+        fmu mode_serial
+else
+        echo "Setting FMU mode_pwm"
+        fmu mode_pwm
 fi
 
 
@@ -261,14 +269,6 @@ else
     sh /etc/init.d/rc.error
 fi
 
-# 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
-
 echo "rc.APM finished"
 echo "rc.APM finished" >> $logfile