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.