Small fixes with the reads and added MadgwickAHRS