forked from Archive/PX4-Autopilot
AttPosEKF: Replace sqrt with sqrtf
This commit is contained in:
parent
ce07e0de2d
commit
ebb111dafa
|
@ -1267,7 +1267,7 @@ void AttPosEKF::FuseVelposNED()
|
||||||
{
|
{
|
||||||
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
|
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
|
if (quatMag > 1e-12f) // divide by 0 protection
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i<=3; i++)
|
for (uint8_t i = 0; i<=3; i++)
|
||||||
|
@ -1594,7 +1594,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||||
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
|
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
|
||||||
}
|
}
|
||||||
// normalise the quaternion states
|
// 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)
|
if (quatMag > 1e-12f)
|
||||||
{
|
{
|
||||||
for (uint8_t j= 0; j<=3; j++)
|
for (uint8_t j= 0; j<=3; j++)
|
||||||
|
@ -1687,7 +1687,7 @@ void AttPosEKF::FuseAirspeed()
|
||||||
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
|
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
|
||||||
{
|
{
|
||||||
// Calculate observation jacobians
|
// 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[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
|
||||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/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;
|
states[j] = states[j] - Kfusion[j] * innovVtas;
|
||||||
}
|
}
|
||||||
// normalise the quaternion states
|
// 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)
|
if (quatMag > 1e-12f)
|
||||||
{
|
{
|
||||||
for (uint8_t j= 0; j <= 3; j++)
|
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];
|
states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex];
|
||||||
}
|
}
|
||||||
// normalise the quaternion states
|
// 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)
|
if (quatMag > 1e-12f)
|
||||||
{
|
{
|
||||||
for (uint8_t j= 0; j<=3; j++)
|
for (uint8_t j= 0; j<=3; j++)
|
||||||
|
|
Loading…
Reference in New Issue