From e2523135a962fe884088787c6d4d6769a347c864 Mon Sep 17 00:00:00 2001 From: TheJackiMonster Date: Thu, 6 Apr 2023 01:39:35 +0200 Subject: [PATCH] Add correction to workaround rejection Signed-off-by: TheJackiMonster --- examples/debug_d3/src/debug.c | 5 +- interface_lib/include/device3.h | 10 ++- interface_lib/src/device3.c | 133 ++++++++++++++++++++++++++------ modules/Fusion | 2 +- src/driver.c | 5 +- 5 files changed, 123 insertions(+), 32 deletions(-) diff --git a/examples/debug_d3/src/debug.c b/examples/debug_d3/src/debug.c index c598c0a..ebd5704 100644 --- a/examples/debug_d3/src/debug.c +++ b/examples/debug_d3/src/debug.c @@ -28,7 +28,8 @@ void test3(uint64_t timestamp, device3_event_type event, - const device3_ahrs_type* ahrs) { + const device3_ahrs_type* ahrs, + const device3_correction_type* correction) { device3_quat_type orientation; device3_vec3_type euler; @@ -37,7 +38,7 @@ void test3(uint64_t timestamp, printf("Initialized\n"); break; case DEVICE3_EVENT_UPDATE: - orientation = device3_get_orientation(ahrs); + orientation = device3_get_orientation(ahrs, correction); euler = device3_get_euler(orientation); printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", euler.x, euler.y, euler.z); break; diff --git a/interface_lib/include/device3.h b/interface_lib/include/device3.h index 83cc00a..2b5b888 100644 --- a/interface_lib/include/device3.h +++ b/interface_lib/include/device3.h @@ -72,6 +72,8 @@ struct device3_filters_t { float magnetic [3 * 4]; }; +struct device3_correction_t; + struct device3_vec3_t { float x; float y; @@ -91,6 +93,7 @@ typedef enum device3_event_t device3_event_type; typedef struct device3_ahrs_t device3_ahrs_type; typedef struct device3_calibration_t device3_calibration_type; typedef struct device3_filters_t device3_filters_type; +typedef struct device3_correction_t device3_correction_type; typedef struct device3_vec3_t device3_vec3_type; typedef struct device3_quat_t device3_quat_type; @@ -98,7 +101,8 @@ typedef struct device3_quat_t device3_quat_type; typedef void (*device3_event_callback)( uint64_t timestamp, device3_event_type event, - const device3_ahrs_type* ahrs + const device3_ahrs_type* ahrs, + const device3_correction_type* correction ); struct device3_t { @@ -127,6 +131,7 @@ struct device3_t { device3_event_callback callback; device3_calibration_type* calibration; device3_filters_type filters; + device3_correction_type* correction; }; typedef struct device3_t device3_type; @@ -147,7 +152,8 @@ device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs); device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs); -device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs); +device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs, + const device3_correction_type* correction); device3_vec3_type device3_get_euler(device3_quat_type quat); diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index aa0e392..5085cd8 100644 --- a/interface_lib/src/device3.c +++ b/interface_lib/src/device3.c @@ -45,6 +45,11 @@ struct device3_calibration_t { FusionVector hardIronOffset; }; +struct device3_correction_t { + FusionQuaternion inverseDrift; + FusionQuaternion stable; +}; + static void reset_filter_v3(float* filter) { for (uint8_t i = 0; i < 3; i++) { const uint8_t i4 = i * 4; @@ -195,6 +200,10 @@ device3_type* device3_open(device3_event_callback callback) { reset_filter_v3(device->filters.acceleration); reset_filter_v3(device->filters.magnetic); + device->correction = malloc(sizeof(device3_correction_type)); + device->correction->inverseDrift = FUSION_IDENTITY_QUATERNION; + device->correction->stable = FUSION_IDENTITY_QUATERNION; + const FusionAhrsSettings settings = { .convention = FusionConventionNwu, .gain = 0.5f, @@ -249,11 +258,19 @@ int device3_load_calibration(device3_type* device, const char* path) { return -3; } - size_t count = fread(device->calibration, 1, sizeof(device3_calibration_type), file); + size_t count; + count = fread(device->calibration, 1, sizeof(device3_calibration_type), file); + if (sizeof(device3_calibration_type) != count) { perror("Not fully loaded!\n"); } + count = fread(&(device->filters), 1, sizeof(device3_filters_type), file); + + if (sizeof(device3_filters_type) != count) { + perror("Not fully loaded!\n"); + } + if (0 != fclose(file)) { perror("No file closed!\n"); return -4; @@ -279,12 +296,19 @@ int device3_save_calibration(device3_type* device, const char* path) { return -3; } - size_t count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file); + size_t count; + count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file); if (sizeof(device3_calibration_type) != count) { perror("Not fully saved!\n"); } + count = fwrite(&(device->filters), 1, sizeof(device3_filters_type), file); + + if (sizeof(device3_filters_type) != count) { + perror("Not fully saved!\n"); + } + if (0 != fclose(file)) { perror("No file closed!\n"); return -4; @@ -295,13 +319,12 @@ int device3_save_calibration(device3_type* device, const char* path) { static void device3_callback(device3_type* device, uint64_t timestamp, - device3_event_type event, - const device3_ahrs_type* ahrs) { + device3_event_type event) { if (!device->callback) { return; } - device->callback(timestamp, event, ahrs); + device->callback(timestamp, event, device->ahrs, device->correction); } static int32_t pack24bit_signed(const uint8_t* data) { @@ -418,6 +441,23 @@ static void apply_calibration(const device3_type* device, ); } +static void update_filter_v3(float* filter, FusionVector v3) { + for (uint8_t i = 0; i < 3; i++) { + const float v = v3.array[i]; + const uint8_t i4 = i * 4; + + if (v <= -0.0f) { + filter[i4 + 0] = max(v, filter[i4 + 0]); + filter[i4 + 1] = min(v, filter[i4 + 1]); + } + + if (v >= +0.0f) { + filter[i4 + 2] = min(v, filter[i4 + 2]); + filter[i4 + 3] = max(v, filter[i4 + 3]); + } + } +} + int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { if (!device) { perror("No device!\n"); @@ -487,6 +527,10 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); + update_filter_v3(device->filters.angular_velocity, gyroscope); + update_filter_v3(device->filters.acceleration, accelerometer); + update_filter_v3(device->filters.magnetic, 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); @@ -538,23 +582,6 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool return 0; } -static void update_filter_v3(float* filter, FusionVector v3) { - for (uint8_t i = 0; i < 3; i++) { - const float v = v3.array[i]; - const uint8_t i4 = i * 4; - - if (v <= -0.0f) { - filter[i4 + 0] = max(v, filter[i4 + 0]); - filter[i4 + 1] = min(v, filter[i4 + 1]); - } - - if (v >= +0.0f) { - filter[i4 + 2] = min(v, filter[i4 + 2]); - filter[i4 + 3] = max(v, filter[i4 + 3]); - } - } -} - static void apply_filter_v3(const float* filter, FusionVector* v3) { for (uint8_t i = 0; i < 3; i++) { float v = v3->array[i]; @@ -572,6 +599,47 @@ static void apply_filter_v3(const float* filter, FusionVector* v3) { } } +static void apply_filter_drop_v3(const float* filter, FusionVector* v3) { + for (uint8_t i = 0; i < 3; i++) { + const float v = v3->array[i] * 1e-5f; + const uint8_t i4 = i * 4; + + if ((v >= filter[i4 + 0]) && (v <= filter[i4 + 2])) { + v3->array[i] = 0.0f; + } else { + v3->array[i] -= (filter[i4 + 0] + filter[i4 + 2]) * 0.5f; + } + } +} + +static void update_correction_with_ahrs(device3_correction_type* correction, + const FusionAhrs* ahrs) { + FusionAhrsFlags flags = FusionAhrsGetFlags(ahrs); + + if (flags.magneticRejectionTimeout || flags.accelerationRejectionTimeout) { + FusionQuaternion unstable = FusionAhrsGetQuaternion(ahrs); + + const float u2 = ( + unstable.element.w * unstable.element.w + + unstable.element.x * unstable.element.x + + unstable.element.y * unstable.element.y + + unstable.element.z * unstable.element.z + ); + + unstable.element.w *= +1.0f / u2; + unstable.element.x *= -1.0f / u2; + unstable.element.y *= -1.0f / u2; + unstable.element.z *= -1.0f / u2; + + correction->inverseDrift = FusionQuaternionMultiply(unstable, correction->stable); + } else { + correction->stable = FusionQuaternionMultiply( + FusionAhrsGetQuaternion(ahrs), + correction->inverseDrift + ); + } +} + int device3_read(device3_type* device, int timeout) { if (!device) { perror("No device!\n"); @@ -613,7 +681,7 @@ int device3_read(device3_type* device, int timeout) { const uint64_t timestamp = packet.timestamp; if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { - device3_callback(device, timestamp, DEVICE3_EVENT_INIT, device->ahrs); + device3_callback(device, timestamp, DEVICE3_EVENT_INIT); return 0; } @@ -644,9 +712,15 @@ int device3_read(device3_type* device, int timeout) { apply_filter_v3(device->filters.acceleration, &accelerometer); apply_filter_v3(device->filters.magnetic, &magnetometer); + apply_filter_drop_v3(device->filters.angular_velocity, &gyroscope); + FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); - device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE, device->ahrs); + if (device->correction) { + update_correction_with_ahrs(device->correction, (const FusionAhrs*) device->ahrs); + } + + device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE); return 0; } @@ -668,8 +742,17 @@ device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs) return a; } -device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) { +device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs, + const device3_correction_type* correction) { FusionQuaternion quaternion = FusionAhrsGetQuaternion((const FusionAhrs*) ahrs); + + if (correction) { + quaternion = FusionQuaternionMultiply( + quaternion, + correction->inverseDrift + ); + } + device3_quat_type q; q.x = quaternion.element.x; q.y = quaternion.element.y; diff --git a/modules/Fusion b/modules/Fusion index 55cbd59..01a6f8e 160000 --- a/modules/Fusion +++ b/modules/Fusion @@ -1 +1 @@ -Subproject commit 55cbd59d56fbf3b83251806efb5e852fb146f4d2 +Subproject commit 01a6f8e36c4d79a837c06b3047f47e2ad27e3be3 diff --git a/src/driver.c b/src/driver.c index 8fc8524..33088a9 100644 --- a/src/driver.c +++ b/src/driver.c @@ -33,7 +33,8 @@ void test3(uint64_t timestamp, device3_event_type event, - const device3_ahrs_type* ahrs) { + const device3_ahrs_type* ahrs, + const device3_correction_type* correction) { static device3_quat_type old; static float dmax = -1.0f; @@ -41,7 +42,7 @@ void test3(uint64_t timestamp, return; } - device3_quat_type q = device3_get_orientation(ahrs); + device3_quat_type q = device3_get_orientation(ahrs, correction); const float dx = (old.x - q.x) * (old.x - q.x); const float dy = (old.y - q.y) * (old.y - q.y);