- Fixed IMU calibration function bugs

- Fixed IMU data processing bugs
- Fixed madgwick init bug
- Inverted magn axis relative to IMU (180 degree rotation) in madgwick
- Acc bias currently set to 0 and 1 for gravity
- Gyro values now rad/s instead of deg/s as madgwick requires

Core/Src/trinity_helperlib.c

@@ -1,4 +1,12 @@
 #include "trinity_helperlib.h"
 
+void convert_rad_to_deg (float data[]) {
+	float factor = 0.01745329252f;
+
+	for (int i = 0; i <= sizeof(data)/sizeof(data[0]); i++) {
+		data[i] *= factor;
+	}
+}
+