From 5bea264a5f014d0468293c19081a295c4f87573e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 4 Sep 2017 04:54:35 +0200 Subject: [PATCH] Matrix Quaternions: Apply simpler call for constructor and copying to all remaining modules --- src/modules/ekf2/ekf2_main.cpp | 2 +- .../local_position_estimator/BlockLocalPositionEstimator.cpp | 2 +- src/modules/vtol_att_control/standard.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index d0a2636649..8ea0c24319 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -841,7 +841,7 @@ void Ekf2::run() _integrated_time_us += sensors.gyro_integral_dt; } - matrix::Quaternion q; + matrix::Quatf q; _ekf.copy_quaternion(q.data()); float velocity[3]; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 1f8374209a..cfb9749821 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -807,7 +807,7 @@ void BlockLocalPositionEstimator::updateSSParams() void BlockLocalPositionEstimator::predict() { // get acceleration - matrix::Quaternion q(&_sub_att.get().q[0]); + matrix::Quatf q(&_sub_att.get().q[0]); _eul = matrix::Euler(q); _R_att = matrix::Dcm(q); Vector3f a(_sub_sensor.get().accelerometer_m_s2); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index a0b20b76b3..cf90f1bb5f 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -474,7 +474,7 @@ void Standard::update_mc_state() _v_att_sp->roll_body = -asinf(tilt_new(1)); R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)); matrix::Quatf q_sp(R_sp); - memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d)); + q_sp.copyTo(_v_att_sp->q_d); } _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;