PX4: better error handling in startup script

mk/PX4/ROMFS/init.d/rc.APM

@@ -18,8 +18,9 @@ fi
 
 if [ -f /fs/microsd/APM/nostart ]
 then
-   echo "APM/nostart found - skipping"
-else
+   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 ]
@@ -27,50 +28,56 @@ then
 	echo "binfs already mounted"
 else
 	echo "Mounting binfs"
-	mount -t binfs /dev/null /bin
+        if mount -t binfs /dev/null /bin
+        then
+                echo "binfs mounted OK"
+        else
+                sh /etc/init.d/rc.error
+        fi
 fi
 
 set sketch NONE
 set logfile /fs/microsd/APM/boot.log
 
-if [ -f /bin/ArduPilot ]
+if [ ! -f /bin/ArduPilot ]
 then
-	set sketch ArduPilot
+   echo "/bin/ardupilot not found"
+   sh /etc/init.d/rc.error
 fi
 
-if [ $sketch != NONE ]
+if mkdir /fs/microsd/APM
 then
-	if mkdir /fs/microsd/APM
-	then
      echo "Created APM directory"
-	fi
+fi
 
-	uorb start
-        if mpu6000 start
-        then
+uorb start
+if mpu6000 start
+then
      echo "Detected FMUv1 board"
      set BOARD FMUv1
-        else
+else
      l3gd20 start
      echo "Detected FMUv2 board"
      set BOARD FMUv2
-        fi
+fi
 
-	if [ -f /fs/microsd/APM/mavlink-ttys0 ]
-	then
+if [ -f /fs/microsd/APM/mavlink-ttys0 ]
+then
    set deviceC /dev/ttyS0
-	else
+else
    if [ $BOARD == FMUv1 ]
    then
 	set deviceC /dev/ttyS2
    else
 	set deviceC /dev/ttyS1
    fi
-	fi
+fi
 
-        echo checking for /etc/px4io/px4io.bin
-	if [ -f /etc/px4io/px4io.bin ]
-	then
+########################
+# 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
@@ -90,45 +97,71 @@ then
         tone_alarm MNGGG
       fi
    fi
-	fi
+fi
 
-	echo "Starting APM sensors"
-	echo "Starting APM sensors" > $logfile
-        if [ $BOARD == FMUv1 ]
+echo "Starting APM sensors"
+echo "Starting APM sensors" > $logfile
+if ms5611 start
+then
+    echo "ms5611 started OK"
+else
+    sh /etc/init.d/rc.error
+fi
+
+if adc start
+then
+    echo "adc started OK"
+else
+    sh /etc/init.d/rc.error
+fi
+
+if [ $BOARD == FMUv1 ]
+then
+    echo "Starting FMUv1 sensors"
+    if hmc5883 start
     then
-        	ms5611 start
-	        hmc5883 start
-                adc start
+        echo "hmc5883 started OK"
     else
-        	ms5611 start
+        sh /etc/init.d/rc.error
+    fi
+else
+    echo "Starting FMUv2 sensors"
     if hmc5883 start
     then
         echo "Using external magnetometer"
     fi
-                lsm303d start
-                adc start
-        fi
-        if ets_airspeed start
+    if lsm303d start
     then
-                echo "Found ETS airspeed sensor" >> $logfile
+       echo "lsm303d started OK"
+    else
+       sh /etc/init.d/rc.error
     fi
+fi
 
-	echo "Trying PX4IO board"
-	echo "Trying PX4IO board" >> $logfile
-	if px4io start
-	then
+# why is this sleep needed?
+sleep 1
+
+# optional ETS airspeed sensor
+if ets_airspeed start
+then
+    echo "Found ETS airspeed sensor" >> $logfile
+fi
+
+echo "Trying PX4IO board"
+echo "Trying PX4IO board" >> $logfile
+if px4io start
+then
     echo "PX4IO board OK"
     echo "PX4IO board OK" >> $logfile
     echo "Setting FMU mode_pwm"
     fmu mode_pwm
-		#echo "Loading FMU_pass mixer"
-		#mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
-		if [ $BOARD == FMUv1 ] && [ $deviceC == /dev/ttyS1 ]
+    if [ $BOARD == FMUv1 -a $deviceC == /dev/ttyS1 ]
     then
 	# ttyS1 is used for PWM output for 4 extra channels
 	set deviceC /dev/ttyS2
     fi
-	else
+    sleep 1
+else
     echo "No PX4IO board found"
     echo "No PX4IO board found" >> $logfile
 
@@ -138,9 +171,7 @@ then
    	echo "APM/mkblctrl found - switch to MK I2C ESCs" >> $logfile
 	echo "Setting up mkblctrl driver"
 	echo "Setting up mkblctrl driver" >> $logfile
-         		sleep 1
 	mkblctrl -mkmode x
-			sleep 1
     fi
 
     echo "Setting up PX4FMU direct mode"
@@ -151,23 +182,26 @@ then
 	# is no IO board
 	set deviceC /dev/ttyS2
     fi
-	fi
+fi
 
-	echo Starting $sketch
-	echo Starting $sketch $deviceA $deviceC >> $logfile
-	$sketch -d $deviceA -d2 $deviceC start
+echo Starting ArduPilot
+echo Starting ArduPilot $deviceA $deviceC >> $logfile
+if ArduPilot -d $deviceA -d2 $deviceC start
+then
+    echo ArduPilot started OK
+else
+    sh /etc/init.d/rc.error
+fi
+sleep 1
 
-	# if starting on the console, tell nsh to exit
-	# this prevents it from chewing bytes
-	if [ $deviceC == /dev/ttyS0 ]
-	then
+# 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
-else
-	echo "No APM sketch found"
 fi
 
 echo "rc.APM finished"
 echo "rc.APM finished" >> $logfile
-fi
+sleep 1

mk/PX4/ROMFS/init.d/rc.error

@@ -0,0 +1,7 @@
+echo "Error in startup"
+tone_alarm MNCC
+nshterm /dev/ttyACM0 &
+sleep 1
+nshterm /dev/ttyS0 &
+sleep 1
+exit

mk/PX4/ROMFS/init.d/rcS

@@ -32,10 +32,12 @@ echo "[init] looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	echo "[init] card mounted at /fs/microsd"
+        set HAVE_MICROSD 1
 	# Start playing the startup tune
 	tone_alarm start
 else
 	echo "[init] no microSD card found"
+        set HAVE_MICROSD 0
 	# Play SOS
 	tone_alarm 2
 fi
@@ -75,9 +77,10 @@ fi
 
 # if this is an APM build then there will be a rc.APM script
 # from an EXTERNAL_SCRIPTS build option
-if [ -f /etc/init.d/rc.APM ]
+if [ -f /etc/init.d/rc.APM -a $HAVE_MICROSD == 1 ]
 then
 	echo Running rc.APM
 	# if APM startup is successful then nsh will exit
 	sh /etc/init.d/rc.APM
+        sleep 1
 fi