Apply gravity compensation to accelerometer readings by bjornhbengtsson · Pull Request #138 · SunDevilRocketry/mod

bjornhbengtsson · 2026-07-21T20:37:12Z

Description

Adds quaternion vector rotation helpers and uses the current body to world attitude estimate to remove gravity from accelerometer measurements before velocity integration.

Changes

Document the attitude quaternion as a body to world rotation.
Add world to body vector rotation:
- v_body = conjugate(q) * v_world * q
Add body-to-world vector rotation:
- v_world = q * v_body * conjugate(q)
Define gravity in the world frame along +Z.
Rotate world gravity into the body frame.
Subtract body-frame gravity from body-frame accelerometer measurements.
Rotate the resulting linear acceleration back into the world frame.
Integrate world-frame linear acceleration into world-frame velocity.
Correct the microseconds-to-seconds conversion so it uses floating-point division, so values smaller than one second are preserved instead of being truncated to zero by integer division.
Replace powf(x, 2) calls with direct multiplication, because it's computationally zoomier.

Frame convention

state_estimate->attitude represents the body to world rotation.

v_world = q * v_body * conjugate(q)
v_body  = conjugate(q) * v_world * q

The gravity-compensation path is now:

g_world
-> world to body rotation
g_body

accel_measured_body - g_body
= accel_linear_body

accel_linear_body
-> body-to-world rotation
accel_linear_world

accel_linear_world * dt
-> world-frame velocity

Issue Link

github.com//issues/139

child: SunDevilRocketry/Flight-Computer-Firmware#304

Testing

[] Passes existing unit tests
Unit tests modified (link the test changes as a child PR)
Integration test performed

The corresponding quaternion and gravity-compensation tests cover:

Identity world-to-body rotation
Identity body-to-world rotation
Body-to-world-to-body round trip
Positive 90-degree yaw rotation
Stationary identity attitude producing zero linear acceleration after gravity subtraction

Local test result:

Passes: 26
Fails: 0
Result: PASS

Other

Leave any additional notes here

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

ETSells · 2026-07-22T00:50:54Z

Linked child PR. Please remember to do that going forward so we can navigate between dependent code easier!

ETSells

@NArmistead Gonna want you to spot check the math here, but I'm happy from a software design perspective.

The one change request on the child PR is to move the tests into this repo.

ETSells · 2026-07-22T00:51:51Z

+    QUAT vector_world
+    )
+{
+    QUAT attitude_conj = quat_conj(attitude);


nit: format

bjornhbengtsson · 2026-07-23T19:25:49Z

Video resource if anyone wants extra study material on the subject of frame rotations: https://youtu.be/4WKghhPzonE?is=AnQzI4YVkjorqC7R

NArmistead · 2026-07-25T17:33:45Z

Video resource if anyone wants extra study material on the subject of frame rotations: https://youtu.be/4WKghhPzonE?is=AnQzI4YVkjorqC7R

dr shane ross!!! 🗣️🗣️🗣️🗣️

NArmistead

Math looks good too but I want to wait until we try it on hardware before we merge. Unless you already did that then go ahead and send it. Also thanks for writing unit tests for this and getting that set up for mod although I have one non-critical request for remaning the test folder

NArmistead · 2026-07-21T21:14:30Z

+    velo_x * velo_x +
+    velo_y * velo_y +
+    velo_z * velo_z


yeah thanks

NArmistead · 2026-07-21T21:16:00Z

+        (
+        quat_mult(attitude, vector_body),
+        attitude_conj
+        );


rare case where i wish we had operator overloading

NArmistead · 2026-07-21T21:19:14Z

-float accel_x = imu_converted->accel_x;
-float accel_y = imu_converted->accel_y;
-float accel_z = imu_converted->accel_z;
+/* The world frame defines gravity in the +Z direction. */


we should probably mention somewhere that we use NED

NArmistead · 2026-07-25T17:37:59Z

+return quat_mult
+    (
+    quat_mult(attitude, vector_body),
+    attitude_conj
+    );


this is hard to read but I think the comment explains it
one of the rare cases where I kinda wish we had operator overloading lol

NArmistead · 2026-07-25T17:52:46Z

only thing I would maybe like you to change is to rename the "test" folder to "_test" to be consistent with driver

+1 tbh, I did that in driver to make it not look like we had a driver named test. would be good here since the test framework submodule doesnt live in mod and that might be ambiguous

bjornhbengtsson · 2026-07-25T18:02:17Z

Sounds good. Haven't tested it on hardware

NArmistead · 2026-07-31T02:49:11Z

@bjornhbengtsson actually can we wait to merge this and #142 until #134 goes in? most of that PR is reviewed and ready so we don't want to creep it too much

bjornhbengtsson · 2026-07-31T03:19:57Z

Yup we can wait. And if you want to wait on reviewing Mahony for Kalman that's cool. I'm deep into Kalman by now

bjornhbengtsson requested a review from NArmistead July 21, 2026 20:37

bjornhbengtsson mentioned this pull request Jul 21, 2026

Add gravity compensated acceleration and quaternion tests SunDevilRocketry/Flight-Computer-Firmware#304

Open

23 tasks

bjornhbengtsson added 3 commits July 21, 2026 14:32

Document attitude quaternion frame convention

c4dbc33

Add world-to-body quaternion vector rotation

4c64bb7

Apply gravity compensation to velocity integration

654782b

ETSells approved these changes Jul 22, 2026

View reviewed changes

Fix quaternion rotation function formatting

15ecd72

Add math_sdr unit tests

2243a5d

NArmistead approved these changes Jul 25, 2026

View reviewed changes

Rename test directory and document NED frame

e841587

bjornhbengtsson mentioned this pull request Jul 30, 2026

Implement Mahony filter for attitude estimation #142

Open

17 tasks

Uh oh!

Conversation

bjornhbengtsson commented Jul 21, 2026 • edited by ETSells Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Changes

Frame convention

Issue Link

Testing

Other

Reviewer Checklist

Standards

Error Handling

Memory

Performance

Uh oh!

ETSells commented Jul 22, 2026

Uh oh!

ETSells left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bjornhbengtsson commented Jul 23, 2026

Uh oh!

NArmistead commented Jul 25, 2026

Uh oh!

NArmistead left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bjornhbengtsson commented Jul 25, 2026

Uh oh!

NArmistead commented Jul 31, 2026

Uh oh!

bjornhbengtsson commented Jul 31, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

bjornhbengtsson commented Jul 21, 2026 •

edited by ETSells

Loading

ETSells left a comment •

edited

Loading