Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Better yaw estimation for mulitirotor(magless operation)#11114

Merged
sensei-hacker merged 1 commit into
iNavFlight:masterfrom
shota3527:sh_mc_yaw
Nov 14, 2025
Merged

Better yaw estimation for mulitirotor(magless operation)#11114
sensei-hacker merged 1 commit into
iNavFlight:masterfrom
shota3527:sh_mc_yaw

Conversation

@shota3527
Copy link
Copy Markdown
Contributor

@shota3527 shota3527 commented Nov 13, 2025 •
edited
Loading

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 qodo-code-review Bot changed the title Better yaw estimation for mulitirotor(magless operation) Better yaw estimation for mulitirotor(magless operation) Nov 13, 2025
@qodo-code-review
Copy link
Copy Markdown
Contributor

qodo-code-review Bot commented Nov 13, 2025 •
edited
Loading

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
Copy link
Copy Markdown
Contributor

qodo-code-review Bot commented Nov 13, 2025

PR Code Suggestions ✨

Explore these optional code suggestions:

CategorySuggestion                                                                                                                                    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
Copy link
Copy Markdown
Contributor Author

shota3527 commented Nov 14, 2025

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

@sensei-hacker sensei-hacker merged commit 8870a0e into iNavFlight:master Nov 14, 2025
22 checks passed
@sensei-hacker sensei-hacker added this to the 9.0 milestone Nov 14, 2025
@Jetrell Jetrell mentioned this pull request Apr 20, 2026
Open
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

Review effort 2/5

Projects

None yet

Milestone

9.0

Development

Successfully merging this pull request may close these issues.

2 participants

@shota3527 @sensei-hacker