rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

LP-204 Replace fast_invsqrtf() with invsqrtf() : 1.0f / sqrtf()

Browse Source
This commit is contained in:
Laurent Lalanne 2015-12-26 14:26:36 +01:00
parent cefcacbbcf
commit 71a0a563f1
4 changed files with 21 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/libraries/insgps13state.c
View File

@ -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;

22
flight/libraries/math/mathmisc.h
View File

@ -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;
}

8
flight/modules/Attitude/attitude.c
View File

@ -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

14
flight/modules/StateEstimation/filtercf.c
View File

@ -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;
}