Working on mathlib unit testing.
Merge branch 'sensor_hil_fixedwing' of github.com:jgoppert/Firmware into sensor_hil_fixedwing