Browse Source

AP_NavEKF: use pythagorous2()

master
Andrew Tridgell 11 years ago
parent
commit
a7f213a5c5
  1. 2
      libraries/AP_NavEKF/AP_NavEKF.cpp

2
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -3253,7 +3253,7 @@ void NavEKF::decayGpsOffset() @@ -3253,7 +3253,7 @@ void NavEKF::decayGpsOffset()
{
float lapsedTime = 0.001f*float(hal.scheduler->millis() - lastDecayTime_ms);
lastDecayTime_ms = hal.scheduler->millis();
float offsetRadius = sqrt(sq(posnOffsetNorth) + sq(posnOffsetEast));
float offsetRadius = pythagorous2(posnOffsetNorth, posnOffsetEast);
// decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero)
if (offsetRadius > (lapsedTime + 0.1f)) {
// calculate scale factor to be applied to both offset components

Loading…
Cancel
Save