diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 65158fbb2..9c5ffd789 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -43,7 +43,7 @@ // Private constants #define TWO_PI 6.283185308f #define SQRT2 1.414213562f -#define EPS_REORIENTATION 1e-8f +#define EPS_REORIENTATION 1e-10f #define EPS_VELOCITY 1.f // Private types @@ -96,13 +96,16 @@ static void Quaternion2PY(const float q0, const float q1, const float q2, const *yPtr = y_; } else { // calculate needed mutliples of 2pi to avoid jumps - // take slightly less than 2pi, because the jump will never be exactly 2pi - const int mod = (int)((y_ - *yPtr) / (TWO_PI * 0.9f)); - *yPtr = y_ - TWO_PI * mod; + // number of cycles accumulated in old yaw + const int cycles = (int)(*yPtr / TWO_PI); + // look for a jump by substracting the modulus, i.e. there is maximally one jump. + // take slightly less than 2pi, because the jump will always be lower than 2pi + const int mod = (int)((y_ - (*yPtr - cycles * TWO_PI)) / (TWO_PI * 0.9f)); + *yPtr = y_ + TWO_PI * (cycles - mod); } } -static void PY2xB(float p, float y, float x[3]) +static void PY2xB(const float p, const float y, float x[3]) { const float cosp = cosf(p); @@ -112,6 +115,16 @@ static void PY2xB(float p, float y, float x[3]) } +static void PY2DeltaxB(const float p, const float y, float x[3]) +{ + const float cosp = cosf(p); + + x[0] -= cosp * cosf(y); + x[1] -= cosp * sinf(y); + x[2] -= -sinf(p); +} + + // second order Butterworth filter with cut-off frequency ratio ff // Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored // function takes care of updating the values wn1 and wn2 @@ -196,7 +209,7 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air VelocityStateData velData; VelocityStateGet(&velData); float p = imu->pOld, y = imu->yOld; - float dxB[3], xBOld[3]; + float dxB[3]; // get pitch and roll Euler angles from quaternion // do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw @@ -206,13 +219,10 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air p = FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); y = FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); // transform pitch and yaw into fuselage vector xB and xBold - PY2xB(imu->pOld, imu->yOld, xBOld); PY2xB(p, y, dxB); - // calculate change in fuselage vector - dxB[0] -= xBOld[0]; - dxB[1] -= xBOld[1]; - dxB[2] -= xBOld[2]; - + // calculate change in fuselage vector by substraction of old value + PY2DeltaxB(imu->pOld, imu->yOld, dxB); + // filter ground speed from VelocityState const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2));