aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Changed sqrt method to sqrtf method.

Browse Source 
AP_NavEKF2: Changed sqrt method to sqrtf method.
This commit is contained in:
murata 2016-12-05 20:23:07 +09:00 committed by Francisco Ferreira
parent 05d9455f87
commit 25e47cce6b
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
View File

@ -83,7 +83,7 @@ void NavEKF2_core::FuseRngBcn()
float t6 = t3*t3;
float t7 = t4*t4;
float t8 = t5+t6+t7;
float t9 = 1.0f/sqrt(t8);
float t9 = 1.0f/sqrtf(t8);
H_BCN[6] = -t4*t9;
H_BCN[7] = -t3*t9;
H_BCN[8] = -t2*t9;
@ -369,7 +369,7 @@ void NavEKF2_core::FuseRngBcnStatic()
// calculation will be badly conditioned
return;
}
float t9 = 1.0f/sqrt(t8);
float t9 = 1.0f/sqrtf(t8);
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
float t15 = receiverPos.x*2.0f;
float t11 = t10-t15;
@ -498,11 +498,11 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
float t8 = t5+t6+t7;
float t9;
if (t8 > 0.1f) {
t9 = 1.0f/sqrt(t8);
t9 = 1.0f/sqrtf(t8);
obsDeriv = t2*t9;
// Calculate innovation
innov = sqrt(t8) - rngBcnDataDelayed.rng;
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
// calculate a filtered innovation magnitude to be used to select between the high or low offset
OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov);
@ -535,11 +535,11 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
t5 = t2*t2;
t8 = t5+t6+t7;
if (t8 > 0.1f) {
t9 = 1.0f/sqrt(t8);
t9 = 1.0f/sqrtf(t8);
obsDeriv = t2*t9;
// Calculate innovation
innov = sqrt(t8) - rngBcnDataDelayed.rng;
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
// calculate a filtered innovation magnitude to be used to select between the high or low offset
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov);