diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-05 13:35:13 +0100 |
|---|---|---|
| committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-11 13:38:59 +0100 |
| commit | ebb111dafa4929ddc3cab75a6202655c39756db0 (patch) | |
| tree | 20bafb0e34f081854a1ed22e2ee09d346c17a2cc /src/modules/ekf_att_pos_estimator | |
| parent | ce07e0de2d7cd2b476c8b8f304606f42bfd809f4 (diff) | |
| download | px4-firmware-ebb111dafa4929ddc3cab75a6202655c39756db0.tar.gz px4-firmware-ebb111dafa4929ddc3cab75a6202655c39756db0.tar.bz2 px4-firmware-ebb111dafa4929ddc3cab75a6202655c39756db0.zip | |
AttPosEKF: Replace sqrt with sqrtf
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5e4377f8a..cb0f0e7cb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1267,7 +1267,7 @@ void AttPosEKF::FuseVelposNED() { states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) // divide by 0 protection { for (uint8_t i = 0; i<=3; i++) @@ -1594,7 +1594,7 @@ void AttPosEKF::FuseMagnetometer() states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) @@ -1687,7 +1687,7 @@ void AttPosEKF::FuseAirspeed() if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[0] = 1.0f / (sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; @@ -1758,7 +1758,7 @@ void AttPosEKF::FuseAirspeed() states[j] = states[j] - Kfusion[j] * innovVtas; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j <= 3; j++) @@ -2037,7 +2037,7 @@ void AttPosEKF::FuseOptFlow() states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) |