quad hil + rotor configuration startup script

ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil

@@ -0,0 +1,110 @@
+#!nsh
+#
+# USB HIL start
+#
+
+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/
+#
+param set MAV_TYPE 2
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+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"
+

ROMFS/px4fmu_common/init.d/rcS

@@ -110,23 +110,33 @@ then
 	then
 		sh /etc/init.d/1000_rc_fw.hil		
 		set MODE custom
-	else
-		if param compare SYS_AUTOSTART 1001
-		then
-			sh /etc/init.d/1001_rc_quad.hil
-			set MODE custom
-		else
-			if param compare SYS_AUTOSTART 1002
-			then
-				sh /etc/init.d/1002_rc_fw_state.hil
-				set MODE custom
-			else
-				# Try to get an USB console
-				nshterm /dev/ttyACM0 &
-			fi	
-		fi
 	fi
 	
+	if param compare SYS_AUTOSTART 1001
+	then
+		sh /etc/init.d/1001_rc_quad.hil
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 1002
+	then
+		sh /etc/init.d/1002_rc_fw_state.hil
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 1003
+	then
+		sh /etc/init.d/1003_rc_quad_+.hil
+		set MODE custom
+	fi
+	
+	if [ $MODE != custom ]
+	then
+		# Try to get an USB console
+		nshterm /dev/ttyACM0 &
+	fi
+
+	
 	#
 	# Upgrade PX4IO firmware
 	#