|
|
|
@ -562,8 +562,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
@@ -562,8 +562,7 @@ 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); |
|
|
|
|
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); |
|
|
|
|
matrix::Vector3f acc(hil_state.xacc / 1000.f, hil_state.yacc / 1000.f, hil_state.zacc / 1000.f); |
|
|
|
|
hil_lpos.ax = acc(0); |
|
|
|
|
hil_lpos.ay = acc(1); |
|
|
|
|
hil_lpos.az = acc(2); |
|
|
|
|