fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 15:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

precompute cos and sin of yaw once before mutliple use

Browse Source
This commit is contained in:
Kamil Ritz
2020-08-24 19:24:27 +10:00
committed by Paul Riseborough
parent 114516b784
commit 0fafd4d68b
2 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+4 -2
View File
@@ -270,8 +270,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
// calculate delta velocity in a horizontal front-right frame
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
const float dvx = del_vel_NED(0) * cosf(_ekf_gsf[model_index].X(2)) + del_vel_NED(1) * sinf(_ekf_gsf[model_index].X(2));
const float dvy = - del_vel_NED(0) * sinf(_ekf_gsf[model_index].X(2)) + del_vel_NED(1) * cosf(_ekf_gsf[model_index].X(2));
const float cos_yaw = cosf(_ekf_gsf[model_index].X(2));
const float sin_yaw = sinf(_ekf_gsf[model_index].X(2));
const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;
const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw;
// sum delta velocities in earth frame:
_ekf_gsf[model_index].X(0) += del_vel_NED(0);