Get hold heading working with imu, and a few macro functions to make life easier