2. Switched to level 5 IMU filter
3. logging setpoints
4. Logging for up to 5 minutes
5. Also accounting for time spent during logging when calculating LOOP_TIME
6. Logging during manual mode now, too.

char arrays are still statically allocated, but will be truncated if trying to print more to them than space available, rather than just writing to random memory.

Shuffled around #include dependencies to allow the control algorithm diagram to be generated without copying code out of control_algorithm.c