Add rocket orientation estimator - [merged]

[mcm001]

In GitLab by @SamHaggans on May 11, 2022, 20:06

Merges orientation-estimation -> master

[mcm001]

In GitLab by @SamHaggans on May 11, 2022, 20:39

added 1 commit

• 8a62810e - Move orientation reset and fix quaternion copying to filter values

Compare with previous version

[mcm001]

In GitLab by @matthew.morley.ca on May 11, 2022, 20:42

Commented on System/Filtering/filters.cpp line 275

I think we should check here if the norm([ax, ay, az]) ~ 1? The Z component might be 0.5, but the norm should be 10m/s/s (so maybe check if the norm is between 8 and 12 or something? IDK)

[mcm001]

In GitLab by @matthew.morley.ca on May 11, 2022, 20:44

Commented on System/Filtering/filters.cpp line 44

Nit: OrientationEstimation could be congugated OrientationEstimator I think? and orientation remanednamed orientationEstimator? That reads a bit better to me at least

[mcm001]

In GitLab by @SamHaggans on May 18, 2022, 18:32

Commented on System/Filtering/filters.cpp line 275

changed this line in version 3 of the diff

[mcm001]

In GitLab by @SamHaggans on May 18, 2022, 18:32

Commented on System/Filtering/filters.cpp line 44

changed this line in version 3 of the diff

[mcm001]

In GitLab by @SamHaggans on May 18, 2022, 18:32

added 1 commit

• 83076204 - Work in Progress orientation changes

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 6, 2022, 17:49

added 2 commits

• 43017f16 - Add gyro biasing to correct for offset
• 0dff04d0 - Change FCB coordinate frame to be x axis in direction of rocket travel

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 8, 2022, 21:36

added 2 commits

• 4218ba6d - WIP
• 21e975e7 - Re-update gravity reference interval rate

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 8, 2022, 21:38

Commented on Devices/LSM9DS1.c line 33

Is this change something we want right now? Or do we want to leave this until we test other gyro tweaks?

[mcm001]

In GitLab by @SamHaggans on Jun 8, 2022, 21:52

added 2 commits

• a16c0f76 - Misc. fixes and removals of testing code
• 66ca4da9 - Fix always sending invalid ack for pyrofire command

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 8, 2022, 22:06

added 30 commits

• 66ca4da9...914f98ab - 3 commits from branch master
• d829ecd9 - Add kalman filtering
• a12aec90 - Address code review
• 04db47ce - Address code review, update to use new structure
• 47e0c981 - Add arm matrix memery
• c5b2300b - Fix include path for filtering
• 2b86bee4 - Remove duplicate CMSIS headers
• 62c0f94d - Add basic operators
• 529aab78 - Add matrix ctor
• 7015669a - Enforce matrix size
• 91387112 - Change libs to relative include path
• 6997fbda - Add additional matrix operator overloads and methods
• 5711c6da - WIP orientation estimation
• 8c0ab328 - Rebase onto master
• 8390f686 - Fix include paths
• dc7a9c23 - Fix cproject issues on v1
• bad78182 - Add orientation estimation
• e622c66c - Remove unneeded include paths
• f98beba5 - Move orientation reset and fix quaternion copying to filter values
• beff62f6 - Work in Progress orientation changes
• 3b9a536e - Add gyro biasing to correct for offset
• f423f2f9 - Change FCB coordinate frame to be x axis in direction of rocket travel
• 3bde3959 - WIP
• 1bb0f56a - Re-update gravity reference interval rate
• d05b8003 - Misc. fixes and removals of testing code
• 2fd35331 - Fix always sending invalid ack for pyrofire command
• 78724944 - Fix cproject issues
• 76a2fe77 - Merge remote

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 8, 2022, 22:07

added 1 commit

• c0788665 - Fix merge conflict

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 8, 2022, 22:09

marked this merge request as ready

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 16:18

added 1 commit

• b8244a40 - Fix incorrect dequeue in filters

Compare with previous version

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on H743ZIT6-flight-board-v1/.project line 54

(nit) Can we keep libs capital to stay consistent across our folders?

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on H750VBT6-flight-board-v0/.cproject line 71

I don't think all of these were supposed to make it in here? A bunch are duplicated above

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/filters.cpp line 236

gyroYRefBuffer

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/filters.cpp line 259

There are three sets of essentially the same code here. Maybe a helper function called three times would help make this more maintainable

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/filters.h line 32

Your choice, but I think it would make more sense to rename everything currently in filters that has to do with gyros to rotVel or angVel. At the end of filters, what we care about having is angular/linear position & its derivates along each axis. So it would make more sense for higher level code to check the angular velocity, where it won't necessarily care if it came from gyros or some other source.

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/orientation_estimator.cpp line 24

Can numbers 9 and 11 be private constants in the header file or defines in this file so we know what they represent and can change them easily?

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/orientation_estimator.h line 34

Destructor (shouldn't really matter in our use case, but still good practice)

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/orientation_estimator.h line 24

Can you add a doc either at the top of the file or class explaining the purpose and general outline of what this does? There's a lot going on underneath, so this would be a good thing to have for anyone new looking at the code

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/orientation_estimator.h line 23

Can you add a doc at least for update? This one specifically isn't self-explanatory that the array passed in needs to be angVel along the X/Y/Z axes.

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/Filtering/filters.cpp line 267

Isn't everything in filters now in the global frame of reference? Maybe I'm wrong on that, but that will affect a lot of other things throughout the PR, so I'll wait on an answer on that before going too much more deeply into the usage of the filter values in this PR

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on System/radio_manager.c line 56

Is this supposed to be in here or did it just find its way in? If it's just for debug, the stuff in the else should probably just be commented out because it doesn't make sense to check if (true) every time you want to send something.

[mcm001]

In GitLab by @bf2799 on Jun 9, 2022, 16:27

Commented on Utils/matrix.h line 13

I think you can just include "board_config" here and remove FCB_VERSION dependency since board_config includes "stm32h7xx.h" which includes the right board header

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 17:17

Commented on H743ZIT6-flight-board-v1/.project line 54

changed this line in version 10 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 17:17

added 1 commit

• bdfa8b2d - Remove duplicate include paths and capitalize Libs

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 17:19

added 1 commit

• 50d4ce46 - Re-add library

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 17:21

Commented on H750VBT6-flight-board-v0/.cproject line 71

Should be fixed

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 17:44

Commented on System/Filtering/filters.cpp line 267

Right now, the acceleration in filters is just using the rocket's frame of reference. Once we have enough trust in the orientation estimate, the plan was to then convert all of the values into the global reference frame. I changed this here because it's still currently the rocket reference frame which isn't the same as the global frame on the ground anymore.

That being said, in hindsight it's probably better to do what you said and report everything from filters as global reference, even if it isn't technically that currently. I can change it to do that if you think that makes more sense.

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 17:51

Commented on System/Filtering/orientation_estimator.h line 24

Yeah sorry, definitely should have had this originally. Added a general overview for the class and docstrings for the methods

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:42

Commented on System/Filtering/filters.cpp line 259

changed this line in version 12 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:42

Commented on System/Filtering/orientation_estimator.cpp line 24

changed this line in version 12 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:42

Commented on System/Filtering/orientation_estimator.h line 23

changed this line in version 12 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:42

added 1 commit

• a10d8f59 - Add docstrings for matrix and orientation estimator and other improvements

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:44

Commented on System/Filtering/filters.cpp line 259

Definitely poor design on my part, fixed now

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:53

Commented on System/radio_manager.c line 56

Yep, just debug things, commented out now

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 18:53

added 1 commit

• 35a4eb9b - Remove debug radio send

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 19:26

Commented on Utils/matrix.h line 13

changed this line in version 14 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 9, 2022, 19:26

added 1 commit

• 542453eb - Include board config in matrix

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 10, 2022, 12:17

Commented on System/Filtering/filters.cpp line 267

changed this line in version 15 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 10, 2022, 12:17

added 1 commit

• 7ea2a57c - Add separate acceleration and velocity for world and rocket reference frames

Compare with previous version

[mcm001]

In GitLab by @SamHaggans on Jun 10, 2022, 12:28

Commented on System/Filtering/filters.h line 32

changed this line in version 16 of the diff

[mcm001]

In GitLab by @SamHaggans on Jun 10, 2022, 12:28

added 1 commit

• aaa25ecf - Rename filter gyros to angular velocity and send world accelerations

Compare with previous version

[mcm001]

In GitLab by @matthew.morley.ca on Jun 10, 2022, 16:20

Commented on Devices/LSM9DS1.c line 33

It shouldn't cause issues to do it here I don't think?

[mcm001]

In GitLab by @SamHaggans on Jun 10, 2022, 16:24

Commented on Devices/LSM9DS1.c line 33

Sounds good to me, I didn't notice any issues from it

[mcm001]

In GitLab by @bf2799 on Jun 10, 2022, 19:50

Commented on System/Filtering/orientation_estimator.h line 57

Can you make clear here for parameters of setAccelVector and update that they're in the rocket's frame?

[mcm001]

In GitLab by @bf2799 on Jun 10, 2022, 19:50

Commented on System/Filtering/filters.cpp line 401

Any way to easily make this based on orientations in board config? Ik deadline tomorrow, so ok to keep for now if needed

[mcm001]

In GitLab by @bf2799 on Jun 10, 2022, 19:50

Commented on System/Filtering/filters.h line 32

Can we prefix ang_vel with rocket to make that clear too?