From d542735b2a945ed874fab8b004f9c31e87bc1346 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@student.ethz.ch>
Date: Thu, 19 Sep 2013 11:10:37 +0200
Subject: [PATCH] re-enable state hil

---
 .../init.d/{1000_rc.hil => 1000_rc_fw.hil}    |  0
 .../px4fmu_common/init.d/1002_rc_fw_state.hil | 84 +++++++++++++++++++
 ROMFS/px4fmu_common/init.d/rcS                | 13 ++-
 src/modules/mavlink/mavlink_receiver.cpp      |  1 +
 4 files changed, 95 insertions(+), 3 deletions(-)
 rename ROMFS/px4fmu_common/init.d/{1000_rc.hil => 1000_rc_fw.hil} (100%)
 create mode 100644 ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil

diff --git a/ROMFS/px4fmu_common/init.d/1000_rc.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
similarity index 100%
rename from ROMFS/px4fmu_common/init.d/1000_rc.hil
rename to ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
new file mode 100644
index 0000000000..d5782a89b8
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
@@ -0,0 +1,84 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILStar starting in state-HIL mode.."
+
+#
+# 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 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
+
+#
+# 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
+ 
+#
+# 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
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c2a8c0c6ae..9ec346465a 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -108,15 +108,22 @@ then
 
 	if param compare SYS_AUTOSTART 1000
 	then
-		sh /etc/init.d/1000_rc.hil
+		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
-			# Try to get an USB console
-			nshterm /dev/ttyACM0 &
+			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
 	
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 222d1f45f2..a3ef1d63b9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -574,6 +574,7 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
+			hil_global_pos.valid = true;
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;