From 6e2060ddb6b1a8f672d882ad98dca92e708c87f9 Mon Sep 17 00:00:00 2001 From: wangwwno1 Date: Thu, 28 Oct 2021 15:11:09 +0800 Subject: [PATCH] Add accel to vehicle_local_position_groundtruth Related Issue: #18527 According to [this doc](https://github.com/mavlink/c_library_v2/blob/92b1a43468e8737da2d4cc1e72304e6443dcfdd3/common/mavlink_msg_hil_state_quaternion.h#L102), the *acc value are mili-Gee, so multiply the value with `CONSTANT_ONE_G/1000.0f` would get the acceleration in `m/s`. --- src/modules/simulator/simulator_mavlink.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index dad520015e..1c6981fd0d 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -562,6 +562,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.heading = euler.psi(); hil_lpos.xy_global = true; hil_lpos.z_global = true;