dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AttPosEKF: Replace sqrt with sqrtf

This commit is contained in:
Johan Jansen 2015-02-05 13:35:13 +01:00
parent ce07e0de2d
commit ebb111dafa
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -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++)