From a0cab5ce5002e155c59675166592a3b2cd6292e3 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 28 Sep 2025 22:54:49 +0900 Subject: [PATCH 1/5] in dev of acc based yaw estimate --- .vscode/settings.json | 3 +- src/main/fc/settings.yaml | 2 +- src/main/flight/imu.c | 112 +++++++++++++++++++++++++------------- src/main/flight/imu.h | 1 + 4 files changed, 78 insertions(+), 40 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5de759a0db4..76df0735b4c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -7,7 +7,8 @@ "rth_trackback.h": "c", "platform.h": "c", "timer.h": "c", - "bus.h": "c" + "bus.h": "c", + "maths.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5fe29261630..f46f91444f6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1512,7 +1512,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: 15 + default_value: 20 field: acc_ignore_rate min: 0 max: 30 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d3f9d90d7db..017a467c102 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -103,6 +103,11 @@ STATIC_FASTRAM pt1Filter_t rotRateFilterY; STATIC_FASTRAM pt1Filter_t rotRateFilterZ; FASTRAM fpVector3_t imuMeasuredRotationBFFiltered = {.v = {0.0f, 0.0f, 0.0f}}; +STATIC_FASTRAM pt1Filter_t accelFilterX; +STATIC_FASTRAM pt1Filter_t accelFilterY; +STATIC_FASTRAM pt1Filter_t accelFilterZ; +FASTRAM fpVector3_t imuMeasuredAccelBFFiltered = {.v = {0.0f, 0.0f, 0.0f}}; + STATIC_FASTRAM pt1Filter_t HeadVecEFFilterX; STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY; STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ; @@ -196,6 +201,10 @@ void imuInit(void) pt1FilterReset(&rotRateFilterX, 0); pt1FilterReset(&rotRateFilterY, 0); pt1FilterReset(&rotRateFilterZ, 0); + // Initialize accel filter + pt1FilterReset(&accelFilterX, 0); + pt1FilterReset(&accelFilterY, 0); + pt1FilterReset(&accelFilterZ, 0); // Initialize Heading vector filter pt1FilterReset(&HeadVecEFFilterX, 0); pt1FilterReset(&HeadVecEFFilterY, 0); @@ -339,14 +348,21 @@ bool isGPSTrustworthy(void) static float imuCalculateMcCogWeight(void) { - float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF); + float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBFFiltered); float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f; wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } +static float imuCalculateMcCogAccWeight(void) +{ + fpVector3_t accBFNorm; + vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); + float wCoGAcc = constrainf(accBFNorm.z - 1.0f, 0.0f, 1.0f); + return wCoGAcc; +} -static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) +static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; @@ -358,7 +374,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 1: Yaw correction */ // Use measured magnetic field vector - if (magBF || useCOG) { + if (magBF || vCOG) { float wMag = 1.0f; float wCoG = 1.0f; if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;} @@ -399,8 +415,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe quaternionRotateVector(&vMagErr, &vMagErr, &orientation); } } - if (useCOG) { + if (vCOG) { fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } }; + fpVector3_t vCoG = *vCOG; //vForward as trust vector if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){ vForward.z = 1.0f; @@ -409,17 +426,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe } fpVector3_t vHeadingEF; - // Use raw heading error (from GPS or whatever else) - while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); - while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf); - - // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23 - // (Rxx; Ryx) - measured (estimated) heading vector (EF) - // (-cos(COG), sin(COG)) - reference heading vector (EF) - float airSpeed = gpsSol.groundSpeed; - // Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards. - fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } }; #if defined(USE_WIND_ESTIMATOR) // remove wind elements in vCoG for better heading estimation if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) @@ -438,9 +445,22 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe if (STATE(MULTIROTOR)){ //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); - //scale accroading to multirotor`s tilt angle + //handle acc based vector + if(vCOGAcc){ + float wCoGAcc = imuCalculateMcCogAccWeight(); + LOG_DEBUG(IMU, "accFiltZ=%.3f", (double)imuMeasuredAccelBFFiltered.z); + if (wCoGAcc > wCoG){ + //when copter is accelerating use acc based vector instead of gps based vector + wCoG = wCoGAcc; + vCoG = *vCOGAcc; + } + LOG_DEBUG(IMU, "wCoGAcc=%.3f wCoG=%.3f", (double)wCoGAcc, (double)wCoG); + } + + //scale according to multirotor`s tilt angle wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f); - //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed + // Inverted flight relies on the existing tilt scaling(scaleRangef); there is no extra handling here + } vHeadingEF.z = 0.0f; @@ -637,6 +657,11 @@ static void imuCalculateFilters(float dT) imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT); imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT); imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT); + + imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT); + imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT); + imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT); + HeadVecEFFiltered.x = pt1FilterApply4(&HeadVecEFFilterX, rMat[0][0], IMU_ROTATION_LPF, dT); HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT); HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT); @@ -646,7 +671,7 @@ static void imuCalculateFilters(float dT) GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT); } -static void imuCalculateGPSacceleration(fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler) +static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler) { static rtcTime_t lastGPSNewDataTime = 0; static bool lastGPSHeartbeat; @@ -660,17 +685,16 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstcentrifugalAccelBF, flo if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) { // on new gps frame, update accEF and estimate centrifugal accleration - fpVector3_t vGPSacc = {.v = {0.0f, 0.0f, 0.0f}}; - vGPSacc.x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward - vGPSacc.y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms)); - vGPSacc.z = (currentGPSvel.z - lastGPSvel.z) / (MS2S(time_delta_ms)); + vEstAccelEF->x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward + vEstAccelEF->y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms)); + vEstAccelEF->z = (currentGPSvel.z - lastGPSvel.z) / (MS2S(time_delta_ms)); // Calculate estimated centrifugal accleration vector in body frame - quaternionRotateVector(vEstcentrifugalAccelBF, &vGPSacc, &orientation); // EF -> BF + quaternionRotateVector(vEstcentrifugalAccelBF, vEstAccelEF, &orientation); // EF -> BF lastGPSNewDataTime = currenttime; lastGPSvel = currentGPSvel; } lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat; - *acc_ignore_slope_multipiler = 4; + *acc_ignore_slope_multipiler = 4.0f; } static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF, float dT, float *acc_ignore_slope_multipiler) @@ -684,14 +708,14 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF *acc_ignore_slope_multipiler = 4.0f; } #ifdef USE_PITOT - if (sensors(SENSOR_PITOT) && pitotIsHealthy() && currentspeed < 0) + else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { // second choice is pitot currentspeed = getAirspeedEstimate(); *acc_ignore_slope_multipiler = 2.0f; } #endif - if (currentspeed < 0) + else { //third choice is fixedWingReferenceAirspeed currentspeed = pidProfile()->fixedWingReferenceAirspeed; @@ -739,9 +763,14 @@ static void imuCalculateEstimatedAttitude(float dT) const bool canUseMAG = false; #endif - float courseOverGround = 0; + // centrifugal force compensation + static fpVector3_t vEstAccelBF_velned; + static fpVector3_t vEstAccelBF_turnrate; + static fpVector3_t vCOG; + static fpVector3_t vCOGAcc; bool useMag = false; bool useCOG = false; + bool useCOGAcc = false; #if defined(USE_GPS) bool canUseCOG = isGPSHeadingValid(); @@ -753,8 +782,15 @@ static void imuCalculateEstimatedAttitude(float dT) // Use GPS (if available) if (canUseCOG) { if (gpsHeadingInitialized) { - courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); - useCOG = true; + float courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); + while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); + while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf); + // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23 + // (Rxx; Ryx) - measured (estimated) heading vector (EF) + // (-cos(COG), sin(COG)) - reference heading vector (EF) + vCOG.x = -cos_approx(courseOverGround); // the x axis of accerometer is pointing tail + vCOG.y = sin_approx(courseOverGround); + vCOG.z = 0; } else if (!canUseMAG) { // Re-initialize quaternion from known Roll, Pitch and GPS heading @@ -769,27 +805,26 @@ static void imuCalculateEstimatedAttitude(float dT) } imuCalculateFilters(dT); - // centrifugal force compensation - static fpVector3_t vEstcentrifugalAccelBF_velned; - static fpVector3_t vEstcentrifugalAccelBF_turnrate; + float acc_ignore_slope_multipiler = 1.0f; // when using gps centrifugal_force_compensation, AccelerometerWeightRateIgnore slope will be multiplied by this value if (isGPSTrustworthy()) { - imuCalculateGPSacceleration(&vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); + imuCalculateGPSacceleration(&vCOGAcc, &vEstAccelBF_velned, &acc_ignore_slope_multipiler); + useCOGAcc = true; //currently only for multicopter } if (STATE(AIRPLANE)) { - imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); + imuCalculateTurnRateacceleration(&vEstAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); } if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { //pick the best centrifugal acceleration between velned and turnrate fpVector3_t compansatedGravityBF_velned; - vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); + vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstAccelBF_velned); float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); fpVector3_t compansatedGravityBF_turnrate; - vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); + vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstAccelBF_turnrate); float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned; @@ -797,12 +832,12 @@ static void imuCalculateEstimatedAttitude(float dT) else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { //velned centrifugal force compensation, quad will use this method - vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); + vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstAccelBF_velned); } else if (STATE(AIRPLANE)) { //turnrate centrifugal force compensation - vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); + vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstAccelBF_turnrate); } else { @@ -824,7 +859,8 @@ static void imuCalculateEstimatedAttitude(float dT) imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF, useAcc ? &compansatedGravityBF : NULL, useMag ? &measuredMagBF : NULL, - useCOG, courseOverGround, + useCOG ? &vCOG : NULL, + useCOGAcc ? &vCOGAcc : NULL, accWeight, magWeight); imuUpdateTailSitter(); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 8afcc50e579..6d285d0f794 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -25,6 +25,7 @@ #include "config/parameter_group.h" extern fpVector3_t imuMeasuredAccelBF; // cm/s/s +extern fpVector3_t imuMeasuredAccelBFFiltered; // cm/s/s extern fpVector3_t imuMeasuredRotationBF; // rad/s extern fpVector3_t imuMeasuredRotationBFFiltered; // rad/s extern fpVector3_t compansatedGravityBF; // cm/s/s From ee24baf6b00e5b20cab536d22754c4374acb8abc Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 29 Sep 2025 02:58:39 +0900 Subject: [PATCH 2/5] acc based yaw estimation --- src/main/flight/imu.c | 64 +++++++++++++++------------ src/main/target/SITL/sim/realFlight.c | 6 +-- 2 files changed, 39 insertions(+), 31 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 017a467c102..f48dea3e429 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -358,14 +358,13 @@ static float imuCalculateMcCogAccWeight(void) { fpVector3_t accBFNorm; vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); - float wCoGAcc = constrainf(accBFNorm.z - 1.0f, 0.0f, 1.0f); + float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); return wCoGAcc; } static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; - fpQuaternion_t prevOrientation = orientation; fpVector3_t vRotation = *gyroBF; @@ -374,10 +373,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 1: Yaw correction */ // Use measured magnetic field vector - if (magBF || vCOG) { + if (magBF || vCOG || vCOGAcc) { float wMag = 1.0f; float wCoG = 1.0f; - if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;} + if (magBF) { wCoG *= imuConfig()->gps_yaw_weight / 100.0f; } fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; @@ -415,9 +414,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe quaternionRotateVector(&vMagErr, &vMagErr, &orientation); } } - if (vCOG) { + if (vCOG || vCOGAcc) { + fpVector3_t vCoGlocal = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } }; - fpVector3_t vCoG = *vCOG; //vForward as trust vector if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){ vForward.z = 1.0f; @@ -425,52 +424,58 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vForward.x = 1.0f; } fpVector3_t vHeadingEF; - - float airSpeed = gpsSol.groundSpeed; -#if defined(USE_WIND_ESTIMATOR) - // remove wind elements in vCoG for better heading estimation - if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) - { - vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed); - vCoG.x += getEstimatedWindSpeed(X); - vCoG.y -= getEstimatedWindSpeed(Y); - airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG)); - vectorNormalize(&vCoG, &vCoG); - } -#endif - wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); - + if (vCOG) { //capital O in COG + 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) + // remove wind elements in vCoGlocal for better heading estimation + if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) + { + vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed); + vCoGlocal.x += getEstimatedWindSpeed(X); + vCoGlocal.y -= getEstimatedWindSpeed(Y); + airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal)); + } + #endif + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); + } else { //then vCOGAcc is not null + wCoG = 0.0f; + } if (STATE(MULTIROTOR)){ //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); //handle acc based vector if(vCOGAcc){ float wCoGAcc = imuCalculateMcCogAccWeight(); - LOG_DEBUG(IMU, "accFiltZ=%.3f", (double)imuMeasuredAccelBFFiltered.z); + 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 acc based vector instead of gps based vector + //when copter is accelerating use gps acc vector instead of gps speed vector wCoG = wCoGAcc; - vCoG = *vCOGAcc; + vCoGlocal = *vCOGAcc; } - LOG_DEBUG(IMU, "wCoGAcc=%.3f wCoG=%.3f", (double)wCoGAcc, (double)wCoG); } - //scale according to multirotor`s tilt angle 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 if (vectorNormSquared(&vHeadingEF) > 0.01f) { // Normalize to unit vector vectorNormalize(&vHeadingEF, &vHeadingEF); + vectorNormalize(&vCoGlocal, &vCoGlocal); // error is cross product between reference heading and estimated heading (calculated in EF) - vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF); + vectorCrossProduct(&vCoGErr, &vCoGlocal, &vHeadingEF); // Rotate error back into body frame quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation); @@ -791,6 +796,8 @@ 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) { // Re-initialize quaternion from known Roll, Pitch and GPS heading @@ -809,6 +816,7 @@ 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, &vEstAccelBF_velned, &acc_ignore_slope_multipiler); useCOGAcc = true; //currently only for multicopter } diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index a8d241425d1..a88d20a612d 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -344,9 +344,9 @@ static void exchangeData(void) altitude, (int16_t)roundf(rfValues.m_groundspeed_MPS * 100), course, - 0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction - 0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100), - 0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100), + (int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //direction seems ok + (int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),//direction seems ok + (int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),//direction not sure 0 ); From 3c4184958cbba30f042343812fdcd56673a9dc32 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 29 Sep 2025 03:32:46 +0900 Subject: [PATCH 3/5] revert some changes --- .vscode/settings.json | 3 +-- src/main/flight/imu.c | 24 +++++++++++------------- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 76df0735b4c..5de759a0db4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -7,8 +7,7 @@ "rth_trackback.h": "c", "platform.h": "c", "timer.h": "c", - "bus.h": "c", - "maths.h": "c" + "bus.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f48dea3e429..15042248a2e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -426,7 +426,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe fpVector3_t vHeadingEF; // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); - if (vCOG) { //capital O in COG + if (vCOG) { LOG_DEBUG(IMU, "vCOG=(%f,%f,%f)", (double)vCOG->x, (double)vCOG->y, (double)vCOG->z); vCoGlocal = *vCOG; float airSpeed = gpsSol.groundSpeed; @@ -449,7 +449,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe wCoG *= imuCalculateMcCogWeight(); //handle acc based vector if(vCOGAcc){ - float wCoGAcc = imuCalculateMcCogAccWeight(); + 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); @@ -767,10 +767,6 @@ static void imuCalculateEstimatedAttitude(float dT) #else const bool canUseMAG = false; #endif - - // centrifugal force compensation - static fpVector3_t vEstAccelBF_velned; - static fpVector3_t vEstAccelBF_turnrate; static fpVector3_t vCOG; static fpVector3_t vCOGAcc; bool useMag = false; @@ -812,27 +808,29 @@ static void imuCalculateEstimatedAttitude(float dT) } imuCalculateFilters(dT); - + // centrifugal force compensation + static fpVector3_t vEstcentrifugalAccelBF_velned; + static fpVector3_t vEstcentrifugalAccelBF_turnrate; 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, &vEstAccelBF_velned, &acc_ignore_slope_multipiler); + imuCalculateGPSacceleration(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); useCOGAcc = true; //currently only for multicopter } if (STATE(AIRPLANE)) { - imuCalculateTurnRateacceleration(&vEstAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); + imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); } if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { //pick the best centrifugal acceleration between velned and turnrate fpVector3_t compansatedGravityBF_velned; - vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstAccelBF_velned); + vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); fpVector3_t compansatedGravityBF_turnrate; - vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstAccelBF_turnrate); + vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned; @@ -840,12 +838,12 @@ static void imuCalculateEstimatedAttitude(float dT) else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { //velned centrifugal force compensation, quad will use this method - vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstAccelBF_velned); + vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); } else if (STATE(AIRPLANE)) { //turnrate centrifugal force compensation - vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstAccelBF_turnrate); + vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); } else { From f69a3dab83c369c13696acaad2f221a885b4cf63 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 29 Sep 2025 03:35:22 +0900 Subject: [PATCH 4/5] cli docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 256dd05a005..5fb839148ad 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -188,7 +188,7 @@ Total gyro rotation rate threshold [deg/s] before scaling to consider accelerome | Default | Min | Max | | --- | --- | --- | -| 15 | 0 | 30 | +| 20 | 0 | 30 | --- From 209e8a0716f80696b44e40be40901baf8aa765db Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 12 Oct 2025 22:41:24 +0900 Subject: [PATCH 5/5] updates --- src/main/flight/imu.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 15042248a2e..c7de6e87f0f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -358,7 +358,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); + float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL return wCoGAcc; } @@ -441,7 +441,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe } #endif wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); - } else { //then vCOGAcc is not null + } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero wCoG = 0.0f; } if (STATE(MULTIROTOR)){ @@ -469,7 +469,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vHeadingEF.z = 0.0f; // We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero - if (vectorNormSquared(&vHeadingEF) > 0.01f) { + if (vectorNormSquared(&vHeadingEF) > 0.01f && vectorNormSquared(&vCoGlocal) > 0.01f) { // Normalize to unit vector vectorNormalize(&vHeadingEF, &vHeadingEF); vectorNormalize(&vCoGlocal, &vCoGlocal);