AP_Airspeed: use pythagorous3()

2014-04-01 22:15:12 +11:00 · 2014-04-01 22:15:12 +11:00 · b8e4e35a0d
commit b8e4e35a0d
parent a7f213a5c5
1 changed files with 1 additions and 1 deletions
--- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp
+++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
@ -54,7 +54,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
    // No state prediction required because states are assumed to be time
    // invariant plus process noise
    // Ignore vertical wind component
-    float TAS_pred = state.z * sqrtf(sq(vg.x - state.x) + sq(vg.y - state.y) + sq(vg.z));
+    float TAS_pred = state.z * pythagorous3(vg.x - state.x, vg.y - state.y, vg.z);
    float TAS_mea  = airspeed;
    
    // Calculate the observation Jacobian H_TAS