Madgwick

[celery6]

Soooo the madgwick thing is obviously uncalibrated but there are non-zero numbers at least 💀

Notable changes in this:

• increase freertos heap size to 131072 (2^17) because task stacks r fat
• refactor ICM_init() into ICM_init() which creates mutex and stuff, and ICM_setup which uses i2c to setup registers. This ensures ICM is actually ready on time
• Add mutex for ICM functions so that ICM otits and stateEst ICM reads can happen togheter
• stateEst task courtesy of @Lucifersan 's great work.
  • I added the send to angleQueue
• enable stateEst task

[Joe-Joe-Joe-Joe]

but this looks very promising, thank you!

[celery6]

I need to look at the meat of the ICM changes when I am more awake

I think it looks more complicated than it actually is lol. It's just adding a mutex take and give for each ICM method that involves my2c.

Ensures each sequence of my2c calls can finish without getting interrupted.

And then splitting up the init method into one that requires rtos and one that doesn't