Merge pull request #412 from thomasgubler/state_hil

re-enable state hil

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"
+

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
 	

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;