Ported over MEMS demo into mems_demo.cpp. Stripped out all USB stuff from demo. All sensors are working.