...
The sensor fusion frequency constant could be defined in a better location for organization.
Safety:
It’s just weird. Bad function names, bad comments, redundant functions… There’s not that much of it, but it’d be realllly nice to go thru the whole thing and clean up.