Better yaw estimation for mulitirotor(magless operation)

[shota3527]

User description

Follow-up PR for #11046
cleaned up the code.
some twicks
realflight testes, shows its ablity to recover from toliet blow
https://youtu.be/46TANQQ4KBc

---

PR Type

Enhancement

---

Description

• Add new function to calculate accelerometer weight based on angular rate

• Integrate rate-based weight reduction in GPS acceleration heading estimation

• Remove debug logging statements for cleaner production code

• Lower default acc_ignore_rate threshold from 20 to 15 deg/s

---

Diagram Walkthrough

flowchart LR
  A["New Rate Weight Function"] --> B["GPS Acc Weight Calculation"]
  B --> C["Reduced Debug Logging"]
  D["Lower acc_ignore_rate Default"] --> E["Configuration Update"]
  B --> F["Better Yaw Estimation"]

Loading

File Walkthrough

	Relevant files
Enhancement	imu.c Enhance yaw estimation with angular rate weighting              src/main/flight/imu.c Added new function declaration imuCalculateAccelerometerWeightRateIgnore() to calculate weight reduction based on angular rate Integrated rate-based weight multiplier in imuCalculateMcCogAccWeight() to reduce GPS acceleration vector weight during high angular rates Removed multiple debug log statements from imuMahonyAHRSupdate() and imuCalculateEstimatedAttitude() functions Added clarifying comment about 1G acceleration usage in straight flight scenarios +3/-9
Documentation	Settings.md Lower accelerometer ignore rate threshold default                docs/Settings.md Updated default value for acc_ignore_rate parameter from 20 to 15 deg/s Maintains min/max range of 0-30 deg/s +1/-1
Configuration changes	settings.yaml Update default accelerometer ignore rate setting                  src/main/fc/settings.yaml Changed ahrs_acc_ignore_rate default value from 20 to 15 deg/s Aligns configuration with documentation update +1/-1

---

[qodo-code-review]

PR Compliance Guide 🔍

(Compliance updated until commit 3110059)

Below is a summary of compliance checks for this PR:

Security Compliance
🟢	No security concerns identified No security vulnerabilities detected by AI analysis. Human verification advised for critical code.
Ticket Compliance
⚪	🎫 No ticket provided Create ticket/issue
Codebase Duplication Compliance
⚪	Codebase context is not defined Follow the guide to enable codebase context checks.
Custom Compliance
🟢	Generic: Secure Error Handling Objective: To prevent the leakage of sensitive system information through error messages while providing sufficient detail for internal debugging. Status: Passed Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Secure Logging Practices Objective: To ensure logs are useful for debugging and auditing without exposing sensitive information like PII, PHI, or cardholder data. Status: Passed Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Security-First Input Validation and Data Handling Objective: Ensure all data inputs are validated, sanitized, and handled securely to prevent vulnerabilities Status: Passed Learn more about managing compliance generic rules or creating your own custom rules
⚪	Generic: Comprehensive Audit Trails Objective: To create a detailed and reliable record of critical system actions for security analysis and compliance. Status: No audit logs: The new logic adjusting GPS/ACC weighting and configuration changes introduce critical flight-behavior decisions without corresponding audit/event logging of key outcomes or thresholds used. Referred Code 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; Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Meaningful Naming and Self-Documenting Code Objective: Ensure all identifiers clearly express their purpose and intent, making code self-documenting Status: Ambiguous naming: The parameter and variable name acc_ignore_slope_multipiler appears misspelled and unclear, which can hinder readability and intent comprehension. Referred Code 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(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); useCOGAcc = true; //currently only for multicopter } if (STATE(AIRPLANE)) { imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Robust Error Handling and Edge Case Management Objective: Ensure comprehensive error handling that provides meaningful context and graceful degradation Status: Missing edge checks: Newly added weighting paths depend on GPS trust and vectors but add no explicit handling for cases like null/invalid vectors or extreme rates beyond expected bounds in the new rate-ignored weighting. Referred Code 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; Learn more about managing compliance generic rules or creating your own custom rules

Compliance status legend

🟢 - Fully Compliant
🟡 - Partial Compliant
🔴 - Not Compliant
⚪ - Requires Further Human Verification
🏷️ - Compliance label

---

Previous compliance checks

Compliance check up to commit 3110059

Security Compliance
🟢	No security concerns identified No security vulnerabilities detected by AI analysis. Human verification advised for critical code.
Ticket Compliance
⚪	🎫 No ticket provided Create ticket/issue
Codebase Duplication Compliance
⚪	Codebase context is not defined Follow the guide to enable codebase context checks.
Custom Compliance
🟢	Generic: Meaningful Naming and Self-Documenting Code Objective: Ensure all identifiers clearly express their purpose and intent, making code self-documenting Status: Passed Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Secure Error Handling Objective: To prevent the leakage of sensitive system information through error messages while providing sufficient detail for internal debugging. Status: Passed Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Secure Logging Practices Objective: To ensure logs are useful for debugging and auditing without exposing sensitive information like PII, PHI, or cardholder data. Status: Passed Learn more about managing compliance generic rules or creating your own custom rules
⚪	Generic: Comprehensive Audit Trails Objective: To create a detailed and reliable record of critical system actions for security analysis and compliance. Status: No audit logs: New logic changes IMU weighting and GPS usage without adding any critical action logging, but it's unclear whether this subsystem is expected to produce audit trails. Referred Code 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; 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)* 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; } 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 }; ... (clipped 96 lines) Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Robust Error Handling and Edge Case Management Objective: Ensure comprehensive error handling that provides meaningful context and graceful degradation Status: No error paths: The new computations (e.g., weighting and vector operations) add no checks for invalid inputs (null vectors, NaNs) or edge cases, though this may be handled elsewhere in the flight stack. Referred Code 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; } 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; /* Calculate general spin rate (rad/s) */ const float spin_rate_sq = vectorNormSquared(&vRotation); /* Step 1: Yaw correction */ // Use measured magnetic field vector if (magBF || vCOG || vCOGAcc) { ... (clipped 90 lines) Learn more about managing compliance generic rules or creating your own custom rules
	Generic: Security-First Input Validation and Data Handling Objective: Ensure all data inputs are validated, sanitized, and handled securely to prevent vulnerabilities Status: Input validation: New use of GPS-derived vectors and rates lacks explicit validation in the diff (e.g., bounds, NaN checks), but these may be guaranteed upstream in sensor fusion code. Referred Code 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(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); useCOGAcc = true; //currently only for multicopter } if (STATE(AIRPLANE)) { imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler); Learn more about managing compliance generic rules or creating your own custom rules

[qodo-code-review]

PR Code Suggestions ✨

Explore these optional code suggestions:

Category	Suggestion	Impact
Learned best practice	Correct and clarify comments Fix typos and clarify comment phrasing to accurately reflect intent (e.g., "straight", "GPS acceleration"). src/main/flight/imu.c [352-364] -//used when flying stright. 1G acceleration +// Used when flying straight: 1 g acceleration ... -wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); //reduce weight when fast angular rate, gps acc is considered lagging +wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); // Reduce weight at high angular rate; GPS acceleration may lag [To ensure code accuracy, apply this suggestion manually] Suggestion importance[1-10]: 5 __ Why: Relevant best practice - Use clear, correctly spelled comments to improve maintainability and avoid ambiguity.	Low
More

[shota3527]

@sensei-hacker
Follow-up PR for #11046 here