Adjust magnometer and flip gravity acceleration

Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "modules/Fusion"]
 	path = modules/Fusion
-	url = https://github.com/xioTechnologies/Fusion.git
+	url = https://github.com/TheJackiMonster/Fusion.git

interface_lib/include/device3.h

@@ -39,9 +39,9 @@ struct __attribute__((__packed__)) device3_packet_t {
 	uint8_t acceleration_y [3];
 	uint8_t acceleration_z [3];
 	uint8_t _padding3 [6];
-	int16_t magnetic_x;
+	uint8_t magnetic_x [2];
-	int16_t magnetic_y;
+	uint8_t magnetic_y [2];
-	int16_t magnetic_z;
+	uint8_t magnetic_z [2];
 	uint32_t checksum;
 	uint8_t _padding4 [6];
 };

interface_lib/src/device3.c

@@ -210,6 +210,8 @@ void device3_reset_calibration(device3_type* device) {
 	device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES;
 	device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO;
 	
+	device->calibration->accelerometerMisalignment.array[2][2] = -1.0f;
+	
 	device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX;
 	device->calibration->hardIronOffset = FUSION_VECTOR_ZERO;
 }
@@ -262,7 +264,6 @@ int device3_save_calibration(device3_type* device, const char* path) {
 	}
 	
 	size_t count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file);
-	printf("count: %lu : %lu\n", count, sizeof(device3_calibration_type));
 	
 	if (sizeof(device3_calibration_type) != count) {
 		perror("Not fully saved!\n");
@@ -293,6 +294,11 @@ static int32_t pack24bit_signed(const uint8_t* data) {
 	return (int32_t) unsigned_value;
 }
 
+static int16_t pack16bit_signed(const uint8_t* data) {
+	uint16_t unsigned_value = (data[0] << 8) | (data[1]);
+	return (int16_t) unsigned_value;
+}
+
 // based on 24bit signed int w/ FSR = +/-2000 dps, datasheet option
 #define GYRO_SCALAR (1.0f / 8388608.0f * 2000.0f)
 
@@ -322,9 +328,9 @@ static void readIMU_from_packet(const device3_packet_type* packet,
 	accelerometer->axis.y = (float) accel_y * ACCEL_SCALAR;
 	accelerometer->axis.z = (float) accel_z * ACCEL_SCALAR;
 	
-	int16_t magnet_x = packet->magnetic_x;
+	int16_t magnet_x = pack16bit_signed(packet->magnetic_x);
-	int16_t magnet_y = packet->magnetic_y;
+	int16_t magnet_y = pack16bit_signed(packet->magnetic_y);
-	int16_t magnet_z = packet->magnetic_z;
+	int16_t magnet_z = pack16bit_signed(packet->magnetic_z);
 	
 	magnetometer->axis.x = (float) magnet_x * MAGNO_SCALAR;
 	magnetometer->axis.y = (float) magnet_y * MAGNO_SCALAR;
@@ -369,6 +375,8 @@ static void apply_calibration(const device3_type* device,
 		accelerometerSensitivity = FUSION_VECTOR_ONES;
 		accelerometerOffset = FUSION_VECTOR_ZERO;
 		
+		accelerometerMisalignment.array[2][2] = -1.0f;
+		
 		softIronMatrix = FUSION_IDENTITY_MATRIX;
 		hardIronOffset = FUSION_VECTOR_ZERO;
 	}
@@ -452,11 +460,18 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
 		FusionVector magnetometer;
 		
 		readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer);
-		apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
 		
 		if (initialized) {
 			cal_gyroscope = FusionVectorAdd(cal_gyroscope, gyroscope);
 			cal_accelerometer = FusionVectorAdd(cal_accelerometer, accelerometer);
+		} else {
+			cal_gyroscope = gyroscope;
+			cal_accelerometer = accelerometer;
+		}
+		
+		apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
+		
+		if (initialized) {
 			cal_magnetometer[0].axis.x = min(cal_magnetometer[0].axis.x, magnetometer.axis.x);
 			cal_magnetometer[0].axis.y = min(cal_magnetometer[0].axis.y, magnetometer.axis.y);
 			cal_magnetometer[0].axis.z = min(cal_magnetometer[0].axis.z, magnetometer.axis.z);
@@ -464,8 +479,6 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
 			cal_magnetometer[1].axis.y = max(cal_magnetometer[1].axis.y, magnetometer.axis.y);
 			cal_magnetometer[1].axis.z = max(cal_magnetometer[1].axis.z, magnetometer.axis.z);
 		} else {
-			cal_gyroscope = gyroscope;
-			cal_accelerometer = accelerometer;
 			cal_magnetometer[0] = magnetometer;
 			cal_magnetometer[1] = magnetometer;
 			initialized = true;

modules/Fusion

@@ -1 +1 @@
-Subproject commit 6c0de47eb6da355985c86030e78e552ab588e049
+Subproject commit 55cbd59d56fbf3b83251806efb5e852fb146f4d2

src/driver.c

@@ -29,17 +29,49 @@
 #include <sys/wait.h>
 #include <unistd.h>
 
+#include <math.h>
+
 void test3(uint64_t timestamp,
 		   device3_event_type event,
 		   const device3_ahrs_type* ahrs) {
+	static device3_quat_type old;
+	static float dmax = -1.0f;
+	
 	if (event != DEVICE3_EVENT_UPDATE) {
 		return;
 	}
 	
 	device3_quat_type q = device3_get_orientation(ahrs);
+	
+	const float dx = (old.x - q.x) * (old.x - q.x);
+	const float dy = (old.y - q.y) * (old.y - q.y);
+	const float dz = (old.z - q.z) * (old.z - q.z);
+	const float dw = (old.w - q.w) * (old.w - q.w);
+	
+	const float d = sqrtf(dx*dx + dy*dy + dz*dz + dw*dw);
+	
+	if (dmax < 0.0f) {
+		dmax = 0.0f;
+	} else {
+		dmax = (d > dmax? d : dmax);
+	}
+	
 	device3_vec3_type e = device3_get_euler(q);
 	
-	printf("Pitch: %f; Roll: %f; Yaw: %f\n", e.x, e.y, e.z);
+	if (d >= 0.00005f) {
+		device3_vec3_type e0 = device3_get_euler(old);
+		
+		printf("Pitch: %f; Roll: %f; Yaw: %f\n", e0.x, e0.y, e0.z);
+		printf("Pitch: %f; Roll: %f; Yaw: %f\n", e.x, e.y, e.z);
+		printf("D = %f; ( %f )\n", d, dmax);
+		
+		printf("X: %f; Y: %f; Z: %f; W: %f;\n", old.x, old.y, old.z, old.w);
+		printf("X: %f; Y: %f; Z: %f; W: %f;\n", q.x, q.y, q.z, q.w);
+	} else {
+		printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", e.x, e.y, e.z);
+	}
+	
+	old = q;
 }
 
 void test4(uint32_t timestamp,