From 71a0a563f1a2be17666d69319cb553283d79f383 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 26 Dec 2015 14:26:36 +0100 Subject: [PATCH] LP-204 Replace fast_invsqrtf() with invsqrtf() : 1.0f / sqrtf() --- flight/libraries/insgps13state.c | 14 +++++++------- flight/libraries/math/mathmisc.h | 22 +++------------------- flight/modules/Attitude/attitude.c | 8 ++++---- flight/modules/StateEstimation/filtercf.c | 14 +++++++------- 4 files changed, 21 insertions(+), 37 deletions(-) diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index cb41469c8..c311f05d3 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -280,11 +280,11 @@ void INSSetBaroVar(float baro_var) void INSSetMagNorth(float B[3]) { - float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); + float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); - ekf.Be[0] = B[0] / mag; - ekf.Be[1] = B[1] / mag; - ekf.Be[2] = B[2] / mag; + ekf.Be[0] = B[0] * invmag; + ekf.Be[1] = B[1] * invmag; + ekf.Be[2] = B[2] * invmag; } void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) @@ -305,7 +305,7 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) // EKF prediction step LinearizeFG(ekf.X, U, ekf.F, ekf.G); RungeKutta(ekf.X, U, dT); - invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); ekf.X[6] *= invqmag; ekf.X[7] *= invqmag; ekf.X[8] *= invqmag; @@ -390,7 +390,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], if (SensorsUsed & MAG_SENSORS) { - float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); + float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); Z[6] = mag_data[0] * invBmag; Z[7] = mag_data[1] * invBmag; Z[8] = mag_data[2] * invBmag; @@ -404,7 +404,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], MeasurementEq(ekf.X, ekf.Be, Y); SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); - float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + float invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); ekf.X[6] *= invqmag; ekf.X[7] *= invqmag; ekf.X[8] *= invqmag; diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index 68026f682..859375258 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -119,28 +119,12 @@ static inline float y_on_curve(float x, const pointf points[], int num_points) // Find the y value on the selected line. return y_on_line(x, &points[end_point - 1], &points[end_point]); } -// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c" -// http://en.wikipedia.org/wiki/Fast_inverse_square_root -static inline float fast_invsqrtf(float number) +static inline float invsqrtf(float number) { - float x2, y; - const float threehalfs = 1.5F; - - union { - float f; - uint32_t u; - } i; - - x2 = number * 0.5F; - y = number; - - i.f = y; // evil floating point bit level hacking - i.u = 0x5f3759df - (i.u >> 1); // what the fxck? - y = i.f; - y = y * (threehalfs - (x2 * y * y)); // 1st iteration -// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed + float y; + y = 1.0f / sqrtf(number); return y; } diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index c9d5fd5f8..f2d024d24 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -628,7 +628,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); // Account for accel magnitude - float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); + float inv_accel_mag = invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); if (inv_accel_mag > 1e3f) { return; } @@ -637,7 +637,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel float inv_grot_mag; if (accel_filter_enabled) { - inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); + inv_grot_mag = invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); } else { inv_grot_mag = 1.0f; } @@ -685,8 +685,8 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel } } - // Renomalize - float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + // Renormalize + float inv_qmag = invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 9d5ad1374..b1c44bd9a 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -438,16 +438,16 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float attitude[3] = -attitude[3]; } - // Renomalize - float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]); - attitude[0] = attitude[0] / qmag; - attitude[1] = attitude[1] / qmag; - attitude[2] = attitude[2] / qmag; - attitude[3] = attitude[3] / qmag; + // Renormalize + float inv_qmag = invsqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]); + attitude[0] = attitude[0] * inv_qmag; + attitude[1] = attitude[1] * inv_qmag; + attitude[2] = attitude[2] * inv_qmag; + attitude[3] = attitude[3] * inv_qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { + if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) { this->first_run = 1; return FILTERRESULT_WARNING; }