Cut case MAVLINK_MSG_ID_HIL_STATE_QUATERNION content and paste into...

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,107 +399,111 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		}
 
 	case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
-		mavlink_hil_state_quaternion_t hil_state;
-		mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
+		handle_message_hil_state_quaternion(msg);
+		break;
+	}
 
-		uint64_t timestamp = hrt_absolute_time();
+}
 
-		/* attitude */
-		struct vehicle_attitude_s hil_attitude = {};
-		{
-			hil_attitude.timestamp = timestamp;
+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);
+}
 
-			matrix::Quatf q(hil_state.attitude_quaternion);
-			q.copyTo(hil_attitude.q);
+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);
 
-			hil_attitude.rollspeed = hil_state.rollspeed;
-			hil_attitude.pitchspeed = hil_state.pitchspeed;
-			hil_attitude.yawspeed = hil_state.yawspeed;
+	uint64_t timestamp = hrt_absolute_time();
 
-			// always publish ground truth attitude message
-			int hilstate_multi;
-			orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH);
-		}
+	/* attitude */
+	struct vehicle_attitude_s hil_attitude = {};
+	{
+		hil_attitude.timestamp = timestamp;
 
-		/* global position */
-		struct vehicle_global_position_s hil_gpos = {};
-		{
-			hil_gpos.timestamp = timestamp;
+		matrix::Quatf q(hil_state.attitude_quaternion);
+		q.copyTo(hil_attitude.q);
 
-			hil_gpos.lat = hil_state.lat / 1E7;//1E7
-			hil_gpos.lon = hil_state.lon / 1E7;//1E7
-			hil_gpos.alt = hil_state.alt / 1E3;//1E3
+		hil_attitude.rollspeed = hil_state.rollspeed;
+		hil_attitude.pitchspeed = hil_state.pitchspeed;
+		hil_attitude.yawspeed = hil_state.yawspeed;
 
-			hil_gpos.vel_n = hil_state.vx / 100.0f;
-			hil_gpos.vel_e = hil_state.vy / 100.0f;
-			hil_gpos.vel_d = hil_state.vz / 100.0f;
+		// always publish ground truth attitude message
+		int hilstate_multi;
+		orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH);
+	}
 
-			// always publish ground truth attitude message
-			int hil_gpos_multi;
-			orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi,
-					 ORB_PRIO_HIGH);
-		}
+	/* global position */
+	struct vehicle_global_position_s hil_gpos = {};
+	{
+		hil_gpos.timestamp = timestamp;
 
-		/* local position */
-		struct vehicle_local_position_s hil_lpos = {};
-		{
-			hil_lpos.timestamp = timestamp;
-
-			double lat = hil_state.lat * 1e-7;
-			double lon = hil_state.lon * 1e-7;
-
-			if (!_hil_local_proj_inited) {
-				_hil_local_proj_inited = true;
-				map_projection_init(&_hil_local_proj_ref, lat, lon);
-				_hil_ref_timestamp = timestamp;
-				_hil_ref_lat = lat;
-				_hil_ref_lon = lon;
-				_hil_ref_alt = hil_state.alt / 1000.0f;
-			}
+		hil_gpos.lat = hil_state.lat / 1E7;//1E7
+		hil_gpos.lon = hil_state.lon / 1E7;//1E7
+		hil_gpos.alt = hil_state.alt / 1E3;//1E3
 
-			float x;
-			float y;
-			map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
-			hil_lpos.timestamp = timestamp;
-			hil_lpos.xy_valid = true;
-			hil_lpos.z_valid = true;
-			hil_lpos.v_xy_valid = true;
-			hil_lpos.v_z_valid = true;
-			hil_lpos.x = x;
-			hil_lpos.y = y;
-			hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
-			hil_lpos.vx = hil_state.vx / 100.0f;
-			hil_lpos.vy = hil_state.vy / 100.0f;
-			hil_lpos.vz = hil_state.vz / 100.0f;
-			matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
-			hil_lpos.yaw = euler.psi();
-			hil_lpos.xy_global = true;
-			hil_lpos.z_global = true;
-			hil_lpos.ref_lat = _hil_ref_lat;
-			hil_lpos.ref_lon = _hil_ref_lon;
-			hil_lpos.ref_alt = _hil_ref_alt;
-			hil_lpos.ref_timestamp = _hil_ref_timestamp;
-			hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
-			hil_lpos.vz_max = std::numeric_limits<float>::infinity();
-			hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
-			hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
-
-			// always publish ground truth attitude message
-			int hil_lpos_multi;
-			orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
-					 ORB_PRIO_HIGH);
-		}
+		hil_gpos.vel_n = hil_state.vx / 100.0f;
+		hil_gpos.vel_e = hil_state.vy / 100.0f;
+		hil_gpos.vel_d = hil_state.vz / 100.0f;
 
-		break;
+		// always publish ground truth attitude message
+		int hil_gpos_multi;
+		orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi,
+				 ORB_PRIO_HIGH);
 	}
 
-}
+	/* local position */
+	struct vehicle_local_position_s hil_lpos = {};
+	{
+		hil_lpos.timestamp = timestamp;
+
+		double lat = hil_state.lat * 1e-7;
+		double lon = hil_state.lon * 1e-7;
+
+		if (!_hil_local_proj_inited) {
+			_hil_local_proj_inited = true;
+			map_projection_init(&_hil_local_proj_ref, lat, lon);
+			_hil_ref_timestamp = timestamp;
+			_hil_ref_lat = lat;
+			_hil_ref_lon = lon;
+			_hil_ref_alt = hil_state.alt / 1000.0f;
+		}
 
-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);
+		float x;
+		float y;
+		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
+		hil_lpos.timestamp = timestamp;
+		hil_lpos.xy_valid = true;
+		hil_lpos.z_valid = true;
+		hil_lpos.v_xy_valid = true;
+		hil_lpos.v_z_valid = true;
+		hil_lpos.x = x;
+		hil_lpos.y = y;
+		hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
+		hil_lpos.vx = hil_state.vx / 100.0f;
+		hil_lpos.vy = hil_state.vy / 100.0f;
+		hil_lpos.vz = hil_state.vz / 100.0f;
+		matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
+		hil_lpos.yaw = euler.psi();
+		hil_lpos.xy_global = true;
+		hil_lpos.z_global = true;
+		hil_lpos.ref_lat = _hil_ref_lat;
+		hil_lpos.ref_lon = _hil_ref_lon;
+		hil_lpos.ref_alt = _hil_ref_alt;
+		hil_lpos.ref_timestamp = _hil_ref_timestamp;
+		hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
+		hil_lpos.vz_max = std::numeric_limits<float>::infinity();
+		hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
+		hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
+
+		// always publish ground truth attitude message
+		int hil_lpos_multi;
+		orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
+				 ORB_PRIO_HIGH);
+	}
 }
 
 void Simulator::handle_message_landing_target(const mavlink_message_t *msg)