Correct reading values from magnetometer and apply z axis flip properly

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

examples/debug_d3/src/debug.c

@@ -53,6 +53,8 @@ int main(int argc, const char** argv) {
 		return 1;
 	}
 	
+	device3_clear(dev3);
+	
 	while (dev3) {
 		if (device3_read(dev3, 0) < 0) {
 			break;

examples/debug_d4/src/debug.c

@@ -55,6 +55,8 @@ int main(int argc, const char** argv) {
 		return 1;
 	}
 	
+	device4_clear(dev4);
+	
 	while (dev4) {
 		if (device4_read(dev4, 0) < 0) {
 			break;

interface_lib/include/device3.h

@@ -137,6 +137,8 @@ int device3_load_calibration(device3_type* device, const char* path);
 
 int device3_save_calibration(device3_type* device, const char* path);
 
+void device3_clear(device3_type* device);
+
 int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet);
 
 int device3_read(device3_type* device, int timeout);

interface_lib/include/device4.h

@@ -108,6 +108,8 @@ typedef struct device4_t device4_type;
 
 device4_type* device4_open(device4_event_callback callback);
 
+void device4_clear(device4_type* device);
+
 int device4_read(device4_type* device, int timeout);
 
 void device4_close(device4_type* device);

interface_lib/src/device3.c

@@ -423,8 +423,6 @@ 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->magnetometerMisalignment = FUSION_IDENTITY_MATRIX;
 	device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES;
 	device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO;
@@ -522,7 +520,7 @@ static int32_t pack24bit_signed(const uint8_t* data) {
 }
 
 static int16_t pack16bit_signed(const uint8_t* data) {
-	uint16_t unsigned_value = (data[0]) | (data[1] << 8);
+	uint16_t unsigned_value = (data[1] << 8) | (data[0]);
 	return (int16_t) unsigned_value;
 }
 
@@ -536,6 +534,11 @@ static int16_t pack16bit_signed_swap(const uint8_t* data) {
 	return (int16_t) unsigned_value;
 }
 
+static int16_t pack16bit_signed_bizarre(const uint8_t* data) {
+	uint16_t unsigned_value = (data[0]) | ((data[1] ^ 0x80) << 8);
+	return (int16_t) unsigned_value;
+}
+
 static void readIMU_from_packet(const device3_packet_type* packet,
 								FusionVector* gyroscope,
 								FusionVector* accelerometer,
@@ -565,9 +568,9 @@ static void readIMU_from_packet(const device3_packet_type* packet,
 	int32_t magnet_m = pack16bit_signed_swap(packet->magnetic_multiplier);
 	int32_t magnet_d = pack32bit_signed_swap(packet->magnetic_divisor);
 	
-	int16_t magnet_x = pack16bit_signed_swap(packet->magnetic_x);
+	int16_t magnet_x = pack16bit_signed_bizarre(packet->magnetic_x);
-	int16_t magnet_y = pack16bit_signed_swap(packet->magnetic_y);
+	int16_t magnet_y = pack16bit_signed_bizarre(packet->magnetic_y);
-	int16_t magnet_z = pack16bit_signed_swap(packet->magnetic_z);
+	int16_t magnet_z = pack16bit_signed_bizarre(packet->magnetic_z);
 	
 	magnetometer->axis.x = (float) magnet_x * (float) magnet_m / (float) magnet_d;
 	magnetometer->axis.y = (float) magnet_y * (float) magnet_m / (float) magnet_d;
@@ -620,8 +623,6 @@ static void apply_calibration(const device3_type* device,
 		accelerometerSensitivity = FUSION_VECTOR_ONES;
 		accelerometerOffset = FUSION_VECTOR_ZERO;
 		
-		accelerometerMisalignment.array[2][2] = -1.0f;
-		
 		magnetometerMisalignment = FUSION_IDENTITY_MATRIX;
 		magnetometerSensitivity = FUSION_VECTOR_ONES;
 		magnetometerOffset = FUSION_VECTOR_ZERO;
@@ -656,6 +657,14 @@ static void apply_calibration(const device3_type* device,
 			softIronMatrix,
 			hardIronOffset
 	);
+	
+	gyroscope->axis.z *= -1.0f;
+	accelerometer->axis.z *= -1.0f;
+	magnetometer->axis.z *= -1.0f;
+}
+
+void device3_clear(device3_type* device) {
+	device3_read(device, 0);
 }
 
 int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) {

interface_lib/src/device4.c

@@ -170,6 +170,10 @@ static void device4_callback(device4_type* device,
 	device->callback(timestamp, event, brightness, msg);
 }
 
+void device4_clear(device4_type* device) {
+	device4_read(device, 0);
+}
+
 int device4_read(device4_type* device, int timeout) {
 	if (!device) {
 		return -1;

src/driver.c

@@ -106,6 +106,7 @@ int main(int argc, const char** argv) {
 	
 	if (pid == 0) {
 		device3_type* dev3 = device3_open(test3);
+		device3_clear(dev3);
 		
 		while (dev3) {
 			if (device3_read(dev3, 0) < 0) {
@@ -117,6 +118,7 @@ int main(int argc, const char** argv) {
 		return 0;
 	} else {
 		device4_type* dev4 = device4_open(test4);
+		device4_clear(dev4);
 		
 		while (dev4) {
 			if (device4_read(dev4, 0) < 0) {