diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 53f500910a3a6e1d43332ac3dc5a9c4e6ee45d96..4aae03b0f6ed2b784538f576550def25f4772d47 100644
--- a/src/modules/simulator/simulator.h
+++ b/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);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 6801da32f768c2a9f22efea84d02e4be2d749394..99acf86e7c8196cf9e2b512a1c7a0845ee02eae4 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/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)