rcS and autostart scripts cleanup, WIP

9 files changed, 182 insertions, 208 deletions

diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index 610d482c1..24f1099d3 100644
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -1,8 +1,9 @@
 #!nsh
-
-# Maintainers: Anton Babushkin <anton.babushkin@me.com>
-
-echo "[init] Team Blacksheep Discovery Quad"
+#
+# Team Blacksheep Discovery Quadcopter
+#
+# Maintainers: Simon Wilks <sjwilks@gmail.com>
+#
 
 if [ $DO_AUTOCONFIG == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index de5028052..b8fc5e606 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -1,11 +1,9 @@
 #!nsh
-
+#
+# 3DR Iris Quadcopter
+#
 # Maintainers: Anton Babushkin <anton.babushkin@me.com>
-
-echo "[init] 3DR Iris Quad"
-
-# Use generic wide arm quad X PWM as base
-sh /etc/init.d/10001_quad_w
+#
 
 if [ $DO_AUTOCONFIG == yes ]
 then
@@ -41,3 +39,8 @@ then
 	param set MPC_Z_VEL_MAX 2
 	param set MPC_Z_VEL_P 0.20
 fi
+
+set VEHICLE_TYPE mc
+set FRAME_GEOMETRY quad_w
+
+set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
index 7d21176f2..c5b92d7d4 100644
--- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
@@ -1,6 +1,9 @@
 #!nsh
-
-echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad"
+#
+# HIL Quadcopter X
+#
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
+#
 
 if [ $DO_AUTOCONFIG == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
index 0cc07ad34..5ec70043a 100644
--- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -1,105 +1,46 @@
 #!nsh
 #
-# USB HIL start
+# HIL Quadcopter +
 #
-
-echo "[HIL] HILS quadrotor + starting.."
-
-#
-# Load default params for this platform
-#
-if param compare SYS_AUTOCONFIG 1
-then
-        # Set all params here, then disable autoconfig
-        param set SYS_AUTOCONFIG 0
-
-        param set MC_ATTRATE_D 0.0
-        param set MC_ATTRATE_I 0.0
-        param set MC_ATTRATE_P 0.05
-        param set MC_ATT_D 0.0
-        param set MC_ATT_I 0.0
-        param set MC_ATT_P 3.0
-        param set MC_YAWPOS_D 0.0
-        param set MC_YAWPOS_I 0.0
-        param set MC_YAWPOS_P 2.1
-        param set MC_YAWRATE_D 0.0
-        param set MC_YAWRATE_I 0.0
-        param set MC_YAWRATE_P 0.05
-        param set NAV_TAKEOFF_ALT 3.0
-        param set MPC_TILT_MAX 0.5
-        param set MPC_THR_MAX 0.5
-        param set MPC_THR_MIN 0.1
-        param set MPC_XY_D 0
-        param set MPC_XY_P 0.5
-        param set MPC_XY_VEL_D 0
-        param set MPC_XY_VEL_I 0
-        param set MPC_XY_VEL_MAX 3
-        param set MPC_XY_VEL_P 0.2
-        param set MPC_Z_D 0
-        param set MPC_Z_P 1
-        param set MPC_Z_VEL_D 0
-        param set MPC_Z_VEL_I 0.1
-        param set MPC_Z_VEL_MAX 2
-        param set MPC_Z_VEL_P 0.20
-
-        param save
-fi
-
-# Allow USB some time to come up
-sleep 1
-# Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Create a fake HIL /dev/pwm_output interface
-hil mode_pwm
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-#              see https://pixhawk.ethz.ch/mavlink/
+# Maintainers: Anton Babushkin <anton.babushkin@me.com>
 #
-param set MAV_TYPE 2
 
-#
-# Check if we got an IO
-#
-if px4io start
+if [ $DO_AUTOCONFIG == yes ]
 then
-	echo "IO started"
-else
-	fmu mode_serial
-	echo "FMU started"
+	#
+	# Default parameters for this platform
+	#
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_D 0.004
+	param set MC_ATT_P 7.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_D 0.0
+	param set MC_YAWPOS_P 2.0
+	param set MC_YAWPOS_I 0.0
+	param set MC_YAWPOS_D 0.0
+	param set MC_YAWRATE_P 0.3
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.005
+	
+	param set MPC_TILT_MAX 0.5
+	param set MPC_THR_MAX 0.8
+	param set MPC_THR_MIN 0.2
+	param set MPC_XY_D 0
+	param set MPC_XY_P 0.5
+	param set MPC_XY_VEL_D 0
+	param set MPC_XY_VEL_I 0
+	param set MPC_XY_VEL_MAX 3
+	param set MPC_XY_VEL_P 0.2
+	param set MPC_Z_D 0
+	param set MPC_Z_P 1
+	param set MPC_Z_VEL_D 0
+	param set MPC_Z_VEL_I 0.1
+	param set MPC_Z_VEL_MAX 2
+	param set MPC_Z_VEL_P 0.20
 fi
 
-#
-# Start the sensors (depends on orb, px4io)
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start the attitude estimator (depends on orb)
-#
-att_pos_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
-
-#
-# Start position estimator
-#
-position_estimator_inav start
-
-#
-# Start attitude control
-#
-multirotor_att_control start
-
-#
-# Start position control
-#
-multirotor_pos_control start
-
-echo "[HIL] setup done, running"
+set HIL yes
 
+set VEHICLE_TYPE mc
+set FRAME_GEOMETRY quad_+
diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 0cfe68b27..94afe91ae 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -1,8 +1,9 @@
 #!nsh
-
+#
+# DJI Flame Wheel F330 Quadcopter
+#
 # Maintainers: Anton Babushkin <anton.babushkin@me.com>
-
-echo "[init] DJI F330"
+#
 
 if [ $DO_AUTOCONFIG == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index 4a0953a2e..21b3088d3 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -1,8 +1,9 @@
 #!nsh
-
+#
+# DJI Flame Wheel F450 Quadcopter
+#
 # Maintainers: Lorenz Meier <lm@inf.ethz.ch>
-
-echo "[init] DJI F450"
+#
 
 if [ $DO_AUTOCONFIG == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550
index 396349d9c..27f73471d 100644
--- a/ROMFS/px4fmu_common/init.d/4012_hk_x550
+++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550
@@ -1,8 +1,9 @@
 #!nsh
-
+#
+# HobbyKing X550 Quadcopter
+#
 # Maintainers: Todd Stellanova <tstellanova@gmail.com>
-
-echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550"
+#
 
 if [ $DO_AUTOCONFIG == yes ]
 then
@@ -21,6 +22,8 @@ then
 	param set MC_YAWRATE_P 0.08
 	param set MC_YAWRATE_I 0
 	param set MC_YAWRATE_D 0
+
+	# TODO add default MPC parameters
 fi
 
 set VEHICLE_TYPE mc
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 9da0135b3..352a916ac 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -28,7 +28,7 @@ fi
 
 if param compare SYS_AUTOSTART 1001
 then
-	sh /etc/init.d/1001_rc_quad.hil
+	sh /etc/init.d/1001_rc_quad_x.hil
 fi
 
 if param compare SYS_AUTOSTART 1002
@@ -38,7 +38,7 @@ fi
 
 if param compare SYS_AUTOSTART 1003
 then
-	#sh /etc/init.d/1003_rc_quad_+.hil
+	sh /etc/init.d/1003_rc_quad_+.hil
 fi
 
 if param compare SYS_AUTOSTART 1004
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 09d66cf41..d6d6ec352 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -13,19 +13,32 @@ set logfile /fs/microsd/bootlog.txt
 #
 # Try to mount the microSD card.
 #
-echo "[init] looking for microSD..."
+echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
-	echo "[init] card mounted at /fs/microsd"
+	echo "[init] microSD card mounted at /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
-	echo "[init] no microSD card found"
+	echo "[init] No microSD card found"
 	# Play SOS
 	tone_alarm error
 fi
 
 #
+# Set default values here, can be overriden in rc.txt from SD card
+#
+set HIL no
+set VEHICLE_TYPE none
+set FRAME_GEOMETRY none
+set OUTPUT_MODE none
+set VEHICLE_TYPE none
+set PWM_RATE none
+set PWM_DISARMED none
+set PWM_MIN none
+set PWM_MAX none
+
+#
 # Look for an init script on the microSD card.
 #
 # To prevent automatic startup in the current flight mode,
@@ -70,27 +83,18 @@ then
 	uorb start
 	
 	#
-	# Load microSD params
+	# Load parameters
 	#
-	#if ramtron start
-	#then
-	#	param select /ramtron/params
-	#	if [ -f /ramtron/params ]
-	#	then
-	#		param load /ramtron/params
-	#	fi
-	#else
-		param select /fs/microsd/params
-		if [ -f /fs/microsd/params ]
+	param select /fs/microsd/params
+	if [ -f /fs/microsd/params ]
+	then
+		if param load /fs/microsd/params
 		then
-			if param load /fs/microsd/params
-			then
-				echo "[init] Parameters loaded"
-			else
-				echo "[init] Parameter file corrupt - ignoring"
-			fi
+			echo "[init] Parameters loaded"
+		else
+			echo "[init] Parameter file corrupt - ignoring"
 		fi
-	#fi
+	fi
 	
 	#
 	# Start system state indicator
@@ -104,13 +108,9 @@ then
 			blinkm systemstate
 		fi
 	fi
-
-	# Use FMU PWM output by default
-	set OUTPUT_MODE fmu_pwm
-	set IO_PRESENT no
 	
 	#
-	# Upgrade PX4IO firmware
+	# Check if PX4IO present and update firmware if needed
 	#
 	if [ -f /etc/extras/px4io-v2_default.bin ]
 	then
@@ -119,15 +119,14 @@ then
 		set io_file /etc/extras/px4io-v1_default.bin
 	fi
 
+	set IO_PRESENT no
+	
 	if px4io checkcrc $io_file
 	then
 		echo "[init] PX4IO CRC OK"
 		echo "PX4IO CRC OK" >> $logfile
 
 		set IO_PRESENT yes
-		
-		# If PX4IO present, use it as primary PWM output by default
-		set OUTPUT_MODE io_pwm
 	else
 		echo "[init] PX4IO CRC failure"
 		echo "PX4IO CRC failure" >> $logfile
@@ -142,9 +141,6 @@ then
 				tone_alarm MSPAA
 
 				set IO_PRESENT yes
-
-				# If PX4IO present, use it as primary PWM output by default
-				set OUTPUT_MODE io_pwm
 			else
 				echo "[init] PX4IO restart failed"
 				echo "PX4IO restart failed" >> $logfile
@@ -164,16 +160,27 @@ then
 			fi
 		fi
 	fi
-	
-	set HIL no
-	set FRAME_TYPE none
-	set PWM_RATE none
-	set PWM_DISARMED none
-	set PWM_MIN none
-	set PWM_MAX none
+
+	#
+	# Set default output if it was not defined in rc.txt
+	#
+	if [ $OUTPUT_MODE == none ]
+	then
+		if [ $IO_PRESENT == yes ]
+		then
+			# If PX4IO present, use it as primary PWM output by default
+			set OUTPUT_MODE io_pwm
+		else
+			# Else use PX4FMU PWM output
+			set OUTPUT_MODE fmu_pwm
+		fi
+	fi
 	
 	set EXIT_ON_END no
 	
+	#
+	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
+	#
 	if param compare SYS_AUTOCONFIG 1
 	then
 		set DO_AUTOCONFIG yes
@@ -182,40 +189,34 @@ then
 	fi
 	
 	#
-	# Start the Commander (needs to be this early for in-air-restarts)
-	#
-	commander start
-	
-	#
 	# Set parameters and env variables for selected AUTOSTART
 	#
 	sh /etc/init.d/rc.autostart
 
-	# Custom configuration
-	if [ -f /fs/microsd/etc/rc.conf ]
-	then
-		sh /fs/microsd/etc/rc.conf
-	fi
-
-	if [ $HIL == yes ]
-	then
-		set OUTPUT_MODE hil
-	else
-		# Try to get an USB console if not in HIL mode
-		nshterm /dev/ttyACM0 &
-	fi
-	
-	#
-	# If autoconfig parameter was set, reset it and save parameters
-	#
-	if [ $DO_AUTOCONFIG == yes ]
-	then
-		param set SYS_AUTOCONFIG 0
-		param save
-	fi
-
 	if [ $MODE == autostart ]
 	then	
+		if [ $HIL == yes ]
+		then
+			set OUTPUT_MODE hil
+		else
+			# Try to get an USB console if not in HIL mode
+			nshterm /dev/ttyACM0 &
+		fi
+		
+		#
+		# If autoconfig parameter was set, reset it and save parameters
+		#
+		if [ $DO_AUTOCONFIG == yes ]
+		then
+			param set SYS_AUTOCONFIG 0
+			param save
+		fi
+
+		#
+		# Start the Commander (needs to be this early for in-air-restarts)
+		#
+		commander start
+		
 		#
 		# Start primary output
 		#
@@ -224,29 +225,47 @@ then
 			echo "[init] Use PX4IO PWM as primary output"
 			if px4io start
 			then
-				echo "[init] PX4IO OK"
-				echo "PX4IO OK" >> $logfile
+				echo "[init] PX4IO started"
 				sh /etc/init.d/rc.io
 			else
 				echo "[init] PX4IO start error"
-				echo "PX4IO start error" >> $logfile
 				tone_alarm MNGGG
 			fi
 		fi
 		if [ $OUTPUT_MODE == fmu_pwm ]
 		then
 			echo "[init] Use PX4FMU PWM as primary output"
-			fmu mode_pwm
+			if fmu mode_pwm
+			then
+				echo "[init] PX4FMU PWM output started"
+				sh /etc/init.d/rc.io
+			else
+				echo "[init] PX4FMU PWM output start error"
+				tone_alarm MNGGG
+			fi
 		fi
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			echo "[init] Use MKBLCTRL as primary output"
-		    mkblctrl
+			if mkblctrl
+			then
+				echo "[init] MKBLCTRL started"
+			else
+				echo "[init] MKBLCTRL start error"
+				tone_alarm MNGGG
+			fi
+			
 		fi		
 		if [ $OUTPUT_MODE == hil ]
 		then
 			echo "[init] Use HIL as primary output"
-			hil mode_pwm
+			if hil mode_pwm
+			then
+				echo "[init] HIL output started"
+			else
+				echo "[init] HIL output error"
+				tone_alarm MNGGG
+			fi
 		fi
 		
 		#
@@ -257,12 +276,10 @@ then
 			echo "[init] Use PX4IO PWM as secondary output"
 			if px4io start
 			then
-				echo "[init] PX4IO OK"
-				echo "PX4IO OK" >> $logfile
+				echo "[init] PX4IO started"
 				sh /etc/init.d/rc.io
 			else
 				echo "[init] PX4IO start error"
-				echo "PX4IO start error" >> $logfile
 				tone_alarm MNGGG
 			fi
 		fi
@@ -270,18 +287,24 @@ then
 		#
 		# MAVLink
 		#
-		if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ]
+		if [ $HIL == yes ]
 		then
-			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output
-			mavlink start -d /dev/ttyS0
+			mavlink start -b 230400 -d /dev/ttyACM0 
 			usleep 5000
-			
-			# Exit from nsh to free port for mavlink
-			set EXIT_ON_END yes
 		else
-			# Start MAVLink on default port: ttyS1
-			mavlink start
-			usleep 5000
+			if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ]
+			then
+				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output
+				mavlink start -d /dev/ttyS0
+				usleep 5000
+				
+				# Exit from nsh to free port for mavlink
+				set EXIT_ON_END yes
+			else
+				# Start MAVLink on default port: ttyS1
+				mavlink start
+				usleep 5000
+			fi
 		fi
 		
 		#
@@ -294,10 +317,7 @@ then
 		then
 			echo "[init] Start logging"
 			sh /etc/init.d/rc.logging
-		fi
-
-		if [ $HIL == no ]
-		then
+			
 			echo "[init] Start GPS"
 			gps start
 		fi
@@ -336,6 +356,7 @@ then
 		if [ $VEHICLE_TYPE == none ]
 		then
 			echo "[init] Vehicle type: GENERIC"
+			
 			attitude_estimator_ekf start
 			position_estimator_inav start
 		fi