From d3d63a30a4e79ed3d0ff5b4ba66e03f80150c7e9 Mon Sep 17 00:00:00 2001 From: TheJackiMonster Date: Wed, 5 Apr 2023 02:06:03 +0200 Subject: [PATCH] Adjust magnometer and flip gravity acceleration Signed-off-by: TheJackiMonster --- .gitmodules | 2 +- interface_lib/include/device3.h | 6 +++--- interface_lib/src/device3.c | 27 +++++++++++++++++++------- modules/Fusion | 2 +- src/driver.c | 34 ++++++++++++++++++++++++++++++++- 5 files changed, 58 insertions(+), 13 deletions(-) diff --git a/.gitmodules b/.gitmodules index ddbb0a1..5192490 100644 --- a/.gitmodules +++ b/.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 diff --git a/interface_lib/include/device3.h b/interface_lib/include/device3.h index b17551c..e0602a8 100644 --- a/interface_lib/include/device3.h +++ b/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; - int16_t magnetic_y; - int16_t magnetic_z; + uint8_t magnetic_x [2]; + uint8_t magnetic_y [2]; + uint8_t magnetic_z [2]; uint32_t checksum; uint8_t _padding4 [6]; }; diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index 04fb736..13c70b1 100644 --- a/interface_lib/src/device3.c +++ b/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_y = packet->magnetic_y; - int16_t magnet_z = packet->magnetic_z; + int16_t magnet_x = pack16bit_signed(packet->magnetic_x); + int16_t magnet_y = pack16bit_signed(packet->magnetic_y); + 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; diff --git a/modules/Fusion b/modules/Fusion index 6c0de47..55cbd59 160000 --- a/modules/Fusion +++ b/modules/Fusion @@ -1 +1 @@ -Subproject commit 6c0de47eb6da355985c86030e78e552ab588e049 +Subproject commit 55cbd59d56fbf3b83251806efb5e852fb146f4d2 diff --git a/src/driver.c b/src/driver.c index 3fb39e8..8fc8524 100644 --- a/src/driver.c +++ b/src/driver.c @@ -29,17 +29,49 @@ #include #include +#include + 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,