Implement Mahony filter for attitude estimation

[bjornhbengtsson]

Description

Implements the Mahony attitude filter for quaternion based attitude estimation.

The filter maintains a body to world attitude quaternion using body frame gyroscope propagation with proportional and integral accelerometer feedback.

Changes include:

• Added a standalone Mahony attitude filter module
• Added gyroscope based quaternion propagation
• Added proportional accelerometer correction
• Added integral gyro bias correction
• Added per axis integral anti windup limits
• Added accelerometer magnitude validity gating
• Added support for externally enabling or disabling accelerometer correction
• Added gyro only fallback when accelerometer samples are disabled or invalid
• Added input validation and quaternion normalization
• Integrated the Mahony attitude estimate into sensor state estimation
• Added gain selection rationale and implementation comments
• Added diagnostic Euler angle and quaternion output tests
• Added comprehensive Mahony unit tests

Issue Link

Depends on #138
#138

Closes #141
#141

Testing

• Unit tests added

Mahony unit test results:

-------------Test Complete--------------

Passes: 20141
Fails: 0
Result: PASS

note: there are about 23 test cases which is substantial enough, but what really causes the 20141 (which is huge) is the test framework counting every assertion inside every loop.

Math test results:

-------------Test Complete--------------

Passes: 26
Fails: 0
Result: PASS

Other

Accelerometer correction is only applied when:

• the caller enables accelerometer feedback
• the sample contains finite values
• the measured acceleration magnitude lies within the configured near 1 g validity range

When accelerometer feedback is disabled or rejected, the filter continues using gyroscope only propagation.

The current accelerometer thresholds, proportional gain, integral gain, and integral limit are initial software values. Final tuning should be performed using hardware characterization, vibration testing, and flight data.

Deferred work includes:

• Final flight state accelerometer correction policy
• Hardware and flight data gain tuning
• Additional correction validity telemetry
• Calibrated magnetometer feedback for yaw correction

Reviewer Checklist

Standards

• Follows FCF Architectural Standards
• Follows SDR Coding Standards
• Code complexity/function Size is minimized
• Code is testable
• Code is readable and commented properly
• License terms are respected

Error Handling

• Potentially unsafe functions return a status code
• Error returns properly handled

Memory

• Stack allocated memory is scoped correctly
• Heap allocated memory is avoided
• Globally allocated memory is minimized except when necessary
• Pointers are used correctly
• Concurrency has been considered

Performance

• Rate limiters are respected
• Busy waiting is avoided
• "Delay" calls are not used in performance sensitive code

[NArmistead]

Yeeeeeees 🔥🔥🔥🔥🔥🔥will review soon

[NArmistead]

2600 line test 💀 wdym

[bjornhbengtsson]

Yeah I think its nuts too 🐿️