diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
new file mode 100644
index 0000000000..cc13110e40
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
@@ -0,0 +1,61 @@
+#!nsh
+#
+# HIL Malolo 1 (Flightgear)
+#
+# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
+#
+
+echo "HIL Malolo 1 starting.."
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	# Set all params here, then disable autoconfig
+	
+	param set FW_AIRSPD_MIN 12
+	param set FW_AIRSPD_TRIM 25
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 15
+	param set FW_PR_FF 0.8
+	param set FW_PR_I 0.05
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.1
+	param set FW_P_LIM_MAX 45
+	param set FW_P_LIM_MIN -45
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 0
+	param set FW_RR_FF 0.6
+	param set FW_RR_I 0.02
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.1
+	param set FW_R_LIM 45
+	param set FW_R_RMAX 0
+	param set FW_THR_CRUISE 0.6
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_CLMB_MAX 5
+	param set FW_T_HRATE_P 0.02
+	param set FW_T_PTCH_DAMP 0
+	param set FW_T_RLL2THR 15
+	param set FW_T_SINK_MAX 5
+	param set FW_T_SINK_MIN 2
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 3
+	param set FW_T_VERT_ACC 7
+	param set FW_YCO_VMIN 1000
+	param set FW_YR_FF 0.0
+	param set FW_YR_I 0
+	param set FW_YR_IMAX 0.2
+	param set FW_YR_P	 0.0
+	param set FW_Y_RMAX 0
+
+	param set SYS_AUTOCONFIG 0
+	param save
+fi
+
+set HIL yes
+
+set VEHICLE_TYPE fw
+# Set the AERT mixer for HIL (even if the malolo is a flying wing)
+set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 030806fd7e..7fa2afbc8b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -46,6 +46,11 @@ then
 	sh /etc/init.d/1004_rc_fw_Rascal110.hil
 fi
 
+if param compare SYS_AUTOSTART 1005
+then
+	sh /etc/init.d/1005_rc_fw_Malolo1.hil
+fi
+
 #
 # Standard plane
 #
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a62b53221c..3ef1871a8b 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -785,26 +785,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_tecs.set_speed_weight(_parameters.speed_weight);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
 
 		/* previous waypoint */
 		math::Vector<2> prev_wp;
 
 		if (pos_sp_triplet.previous.valid) {
-			prev_wp(0) = pos_sp_triplet.previous.lat;
-			prev_wp(1) = pos_sp_triplet.previous.lon;
+			prev_wp(0) = (float)pos_sp_triplet.previous.lat;
+			prev_wp(1) = (float)pos_sp_triplet.previous.lon;
 
 		} else {
 			/*
 			 * No valid previous waypoint, go for the current wp.
 			 * This is automatically handled by the L1 library.
 			 */
-			prev_wp(0) = pos_sp_triplet.current.lat;
-			prev_wp(1) = pos_sp_triplet.current.lon;
+			prev_wp(0) = (float)pos_sp_triplet.current.lat;
+			prev_wp(1) = (float)pos_sp_triplet.current.lon;
 
 		}
 
@@ -1263,7 +1263,7 @@ FixedwingPositionControl::task_main()
 			// vehicle_baro_poll();
 
 			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
-			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
 			 * Attempt to control position, on success (= sensors present and not in manual mode),
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 6e4b5f0a06..114776327e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1078,7 +1078,7 @@ Navigator::start_loiter()
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
 		}
 
-		_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
+		_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	}
 
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;