diff --git a/examples/debug_d3/src/debug.c b/examples/debug_d3/src/debug.c index c598c0a..394f3a9 100644 --- a/examples/debug_d3/src/debug.c +++ b/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; diff --git a/examples/debug_d4/src/debug.c b/examples/debug_d4/src/debug.c index ea2bb59..8446d41 100644 --- a/examples/debug_d4/src/debug.c +++ b/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; diff --git a/interface_lib/include/device3.h b/interface_lib/include/device3.h index f3ba441..fd7738b 100644 --- a/interface_lib/include/device3.h +++ b/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); diff --git a/interface_lib/include/device4.h b/interface_lib/include/device4.h index 623ca07..bf79b91 100644 --- a/interface_lib/include/device4.h +++ b/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); diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index 7ed4868..a384b64 100644 --- a/interface_lib/src/device3.c +++ b/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_y = pack16bit_signed_swap(packet->magnetic_y); - int16_t magnet_z = pack16bit_signed_swap(packet->magnetic_z); + int16_t magnet_x = pack16bit_signed_bizarre(packet->magnetic_x); + int16_t magnet_y = pack16bit_signed_bizarre(packet->magnetic_y); + 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) { diff --git a/interface_lib/src/device4.c b/interface_lib/src/device4.c index 5df1ff8..89f8751 100644 --- a/interface_lib/src/device4.c +++ b/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; diff --git a/src/driver.c b/src/driver.c index 79366ba..82f90d9 100644 --- a/src/driver.c +++ b/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) {