From 160fec5f2572a6960881b28912a8fd23bd748ef5 Mon Sep 17 00:00:00 2001 From: wangwwno1 Date: Thu, 28 Oct 2021 17:30:40 +0800 Subject: [PATCH] Update simulator_mavlink.cpp --- src/modules/simulator/simulator_mavlink.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 1c6981fd0d..8ff38842dd 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -533,6 +533,9 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg // always publish ground truth attitude message _gpos_ground_truth_pub.publish(hil_gpos); } + + matrix::Vector3f acc(hil_state.xacc / 1000.f, hil_state.yacc / 1000.f, hil_state.zacc / 1000.f); + acc = matrix::Quatf(hil_state.attitude_quaternion).conjugate(acc); /* local position */ vehicle_local_position_s hil_lpos{}; @@ -562,9 +565,9 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg 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.ax = hil_state.xacc * CONSTANTS_ONE_G / 1000.0f; - hil_lpos.ay = hil_state.yacc * CONSTANTS_ONE_G / 1000.0f; - hil_lpos.az = hil_state.zacc * CONSTANTS_ONE_G / 1000.0f; + hil_lpos.ax = acc(0); + hil_lpos.ay = acc(1); + hil_lpos.az = acc(2); hil_lpos.heading = euler.psi(); hil_lpos.xy_global = true; hil_lpos.z_global = true;