diff --git a/docs/Settings.md b/docs/Settings.md index ea35813fbdd..46037ecae2b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -198,7 +198,7 @@ Total gyro rotation rate threshold [deg/s] before scaling to consider accelerome | Default | Min | Max | | --- | --- | --- | -| 20 | 0 | 30 | +| 15 | 0 | 30 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 73077a2ec85..bd7bf2e24c6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1544,7 +1544,7 @@ groups: max: 180 - name: ahrs_acc_ignore_rate description: "Total gyro rotation rate threshold [deg/s] before scaling to consider accelerometer trustworthy" - default_value: 20 + default_value: 15 field: acc_ignore_rate min: 0 max: 30 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c7de6e87f0f..b012c040d27 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -121,6 +121,7 @@ FASTRAM bool gpsHeadingInitialized; FASTRAM bool imuUpdated = false; static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF); +static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_slope_multipiler); PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2); @@ -348,6 +349,7 @@ bool isGPSTrustworthy(void) static float imuCalculateMcCogWeight(void) { + //used when flying stright. 1G acceleration float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBFFiltered); float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f; @@ -359,6 +361,7 @@ static float imuCalculateMcCogAccWeight(void) fpVector3_t accBFNorm; vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL + wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); //reduce weight when fast angular rate, gps acc is considered lagging return wCoGAcc; } @@ -427,7 +430,6 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); if (vCOG) { - LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG->x, (double)vCOG->y, (double)vCOG->z); vCoGlocal = *vCOG; float airSpeed = gpsSol.groundSpeed; #if defined(USE_WIND_ESTIMATOR) @@ -450,10 +452,6 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe //handle acc based vector if(vCOGAcc){ float wCoGAcc = imuCalculateMcCogAccWeight();//stronger weight on acc if body frame z axis greate than 1G - LOG_DEBUG(IMU, "accFiltZ=%f", (double)imuMeasuredAccelBFFiltered.z); - LOG_DEBUG(IMU, "wCoGAcc=%f wCoG=%f", (double)wCoGAcc, (double)wCoG); - LOG_DEBUG(IMU, "vHeadingEF=(%f,%f,%f)", (double)vHeadingEF.x, (double)vHeadingEF.y, (double)vHeadingEF.z); - LOG_DEBUG(IMU, "vCOGAcc=(%f,%f,%f)", (double)vCOGAcc->x, (double)vCOGAcc->y, (double)vCOGAcc->z); if (wCoGAcc > wCoG){ //when copter is accelerating use gps acc vector instead of gps speed vector wCoG = wCoGAcc; @@ -464,8 +462,6 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f); // Inverted flight relies on the existing tilt scaling(scaleRangef); there is no extra handling here } - LOG_DEBUG(IMU, " wCoG=%f", (double)wCoG); - LOG_DEBUG(IMU, "vCoGlocal=(%f,%f,%f)", (double)vCoGlocal.x, (double)vCoGlocal.y, (double)vCoGlocal.z); vHeadingEF.z = 0.0f; // We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero @@ -792,7 +788,6 @@ static void imuCalculateEstimatedAttitude(float dT) vCOG.x = -cos_approx(courseOverGround); // the x axis of accerometer is pointing tail vCOG.y = sin_approx(courseOverGround); vCOG.z = 0; - // LOG_DEBUG(IMU, "currentGPSvel=(%f,%f,%f)", -(double)gpsSol.velNED[X], (double)gpsSol.velNED[Y], (double)gpsSol.velNED[Z]); useCOG = true; } else if (!canUseMAG) { @@ -814,7 +809,6 @@ static void imuCalculateEstimatedAttitude(float dT) float acc_ignore_slope_multipiler = 1.0f; // when using gps centrifugal_force_compensation, AccelerometerWeightRateIgnore slope will be multiplied by this value if (isGPSTrustworthy()) { - LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG.x, (double)vCOG.y, (double)vCOG.z); imuCalculateGPSacceleration(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); useCOGAcc = true; //currently only for multicopter }