Cut case MAVLINK_MSG_ID_HIL_STATE_QUATERNION content and paste into handle_message_hil_state_quaternion() method.

src/modules/simulator/simulator.h

@@ -332,6 +332,7 @@ private:
 
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void handle_message_distance_sensor(const mavlink_message_t *msg);
+	void handle_message_hil_state_quaternion(const mavlink_message_t *msg);
 	void handle_message_landing_target(const mavlink_message_t *msg);
 	void handle_message_optical_flow(const mavlink_message_t *msg);
 

src/modules/simulator/simulator_mavlink.cpp

@@ -399,6 +399,21 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		}
 
 	case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
+		handle_message_hil_state_quaternion(msg);
+		break;
+	}
+
+}
+
+void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg)
+{
+	mavlink_distance_sensor_t dist;
+	mavlink_msg_distance_sensor_decode(msg, &dist);
+	publish_distance_topic(&dist);
+}
+
+void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
+{
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
@@ -489,17 +504,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
 				 ORB_PRIO_HIGH);
 	}
-
-		break;
-	}
-
-}
-
-void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg)
-{
-	mavlink_distance_sensor_t dist;
-	mavlink_msg_distance_sensor_decode(msg, &dist);
-	publish_distance_topic(&dist);
 }
 
 void Simulator::handle_message_landing_target(const mavlink_message_t *msg)