Bottle drop: Added HIL startup

2 files changed, 104 insertions, 0 deletions

diff --git a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil
new file mode 100644
index 000000000..060c7d0b5
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil
@@ -0,0 +1,98 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILStar starting.."
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
+
+	param set SYS_AUTOCONFIG 0
+	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 1
+
+#
+# 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_AET.mix
+fw_pos_control_l1 start
+fw_att_control start
+
+#fmu mode_pwm
+
+mixer load /dev/px4io /etc/mixers/FMU_pass.mix
+
+pwm min -d /dev/px4io -c 123 -p 900
+pwm max -d /dev/px4io -c 123 -p 2100
+
+pwm arm -d /dev/px4io
+
+bottle_drop start
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 46bb6d866..5327ee7ca 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -324,6 +324,12 @@ then
 		sh /etc/init.d/900_bottle_drop_test
 		set MODE custom
 	fi
+
+	if param compare SYS_AUTOSTART 901
+	then
+		sh /etc/init.d/901_bottle_drop_test.hil
+		set MODE custom
+	fi
 	
 	# Start any custom extensions that might be missing
 	if [ -f /fs/microsd/etc/rc.local ]