From 32e1539c2201b1c46856919543b716f5be1affa9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Florian=20M=C3=A4rkl?= Date: Wed, 6 Jan 2021 15:47:42 +0100 Subject: [PATCH] Add Orientation Tracker --- lib/CMakeLists.txt | 6 +- lib/include/chiaki/orientation.h | 43 +++++++++++ lib/src/orientation.c | 126 +++++++++++++++++++++++++++++++ setsu/demo/motion.c | 4 +- setsu/include/setsu.h | 2 +- setsu/src/setsu.c | 9 ++- 6 files changed, 182 insertions(+), 8 deletions(-) create mode 100644 lib/include/chiaki/orientation.h create mode 100644 lib/src/orientation.c diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index e2a79d3..36ac38c 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -35,7 +35,8 @@ set(HEADER_FILES include/chiaki/time.h include/chiaki/fec.h include/chiaki/regist.h - include/chiaki/opusdecoder.h) + include/chiaki/opusdecoder.h + include/chiaki/orientation.h) set(SOURCE_FILES src/common.c @@ -73,7 +74,8 @@ set(SOURCE_FILES src/time.c src/fec src/regist.c - src/opusdecoder.c) + src/opusdecoder.c + src/orientation.c) if(CHIAKI_ENABLE_FFMPEG_DECODER) list(APPEND HEADER_FILES include/chiaki/ffmpegdecoder.h) diff --git a/lib/include/chiaki/orientation.h b/lib/include/chiaki/orientation.h new file mode 100644 index 0000000..4a29c2e --- /dev/null +++ b/lib/include/chiaki/orientation.h @@ -0,0 +1,43 @@ +// SPDX-License-Identifier: LicenseRef-AGPL-3.0-only-OpenSSL + +#ifndef CHIAKI_ORIENTATION_H +#define CHIAKI_ORIENTATION_H + +#include "common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Quaternion orientation from accelerometer and gyroscope + * using Madgwick's algorithm. + * See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + */ +typedef struct chiaki_orientation_t +{ + float x, y, z, w; +} ChiakiOrientation; + +CHIAKI_EXPORT void chiaki_orientation_init(ChiakiOrientation *orient); +CHIAKI_EXPORT void chiaki_orientation_update(ChiakiOrientation *orient, float gx, float gy, float gz, float ax, float ay, float az, float time_step_sec); + +/** + * Extension of ChiakiOrientation, also tracking an absolute timestamp + */ +typedef struct chiaki_orientation_tracker_t +{ + ChiakiOrientation orient; + uint32_t timestamp; + bool first_sample; +} ChiakiOrientationTracker; + +CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tracker); +CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *tracker, + float gx, float gy, float gz, float ax, float ay, float az, uint32_t timestamp_us); + +#ifdef __cplusplus +} +#endif + +#endif // CHIAKI_ORIENTATION_H diff --git a/lib/src/orientation.c b/lib/src/orientation.c new file mode 100644 index 0000000..90e991f --- /dev/null +++ b/lib/src/orientation.c @@ -0,0 +1,126 @@ +// SPDX-License-Identifier: LicenseRef-AGPL-3.0-only-OpenSSL + +#include + +CHIAKI_EXPORT void chiaki_orientation_init(ChiakiOrientation *orient) +{ + orient->x = orient->y = orient->z = 0.0f; + orient->w = 1.0f; +} + +#define BETA 0.1f // 2 * proportional gain +static float inv_sqrt(float x); + +CHIAKI_EXPORT void chiaki_orientation_update(ChiakiOrientation *orient, float gx, float gy, float gz, float ax, float ay, float az, float time_step_sec) +{ + float q0 = orient->w, q1 = orient->x, q2 = orient->y, q3 = orient->z; + // Madgwick's IMU algorithm. + // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + q_dot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + q_dot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + q_dot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + q_dot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) + { + // Normalise accelerometer measurement + recip_norm = inv_sqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recip_norm = inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + // Apply feedback step + q_dot1 -= BETA * s0; + q_dot2 -= BETA * s1; + q_dot3 -= BETA * s2; + q_dot4 -= BETA * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += q_dot1 * time_step_sec; + q1 += q_dot2 * time_step_sec; + q2 += q_dot3 * time_step_sec; + q3 += q_dot4 * time_step_sec; + + // Normalise quaternion + recip_norm = inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recip_norm; + q1 *= recip_norm; + q2 *= recip_norm; + q3 *= recip_norm; + + orient->x = q1; + orient->y = q2; + orient->z = q3; + orient->w = q0; +} + +static float inv_sqrt(float x) +{ + // Fast inverse square-root + // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} + +CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tracker) +{ + chiaki_orientation_init(&tracker->orient); + tracker->timestamp = 0; + tracker->first_sample = true; +} + +CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *tracker, + float gx, float gy, float gz, float ax, float ay, float az, uint32_t timestamp_us) +{ + if(tracker->first_sample) + { + tracker->first_sample = false; + tracker->timestamp = timestamp_us; + return; + } + uint64_t delta_us = timestamp_us; + if(delta_us < tracker->timestamp) + delta_us += (1ULL << 32); + delta_us -= tracker->timestamp; + tracker->timestamp = timestamp_us; + chiaki_orientation_update(&tracker->orient, gx, gy, gz, ax, ay, az, (float)delta_us / 1000000.0f); +} diff --git a/setsu/demo/motion.c b/setsu/demo/motion.c index 0b0f88b..ebb5fdd 100644 --- a/setsu/demo/motion.c +++ b/setsu/demo/motion.c @@ -44,7 +44,7 @@ void sigint(int s) #define BAR_LENGTH 100 #define BAR_MAX 2.0f -#define BAR_MAX_GYRO 180.0f +#define BAR_MAX_GYRO M_PI void print_state() { @@ -69,7 +69,7 @@ void print_state() } bool cov = ((vals.v[b] < 0.0f) == (cur < 0.0f)) && fabsf(vals.v[b]) > fabsf(cur); float next = BAR_VAL(bi + 1); -#define MARK_VAL (b > 2 ? 90.0f : 1.0f) +#define MARK_VAL (b > 2 ? 0.5f * M_PI : 1.0f) bool mark = cur < -MARK_VAL && next >= -MARK_VAL || prev < MARK_VAL && cur >= MARK_VAL; buf[i++] = cov ? (mark ? '#' : '=') : (mark ? '.' : ' '); #undef BAR_VAL diff --git a/setsu/include/setsu.h b/setsu/include/setsu.h index 36ecf03..a3ed0e3 100644 --- a/setsu/include/setsu.h +++ b/setsu/include/setsu.h @@ -80,7 +80,7 @@ typedef struct setsu_event_t struct { float accel_x, accel_y, accel_z; // unit is 1G - float gyro_x, gyro_y, gyro_z; // unit is deg/sec + float gyro_x, gyro_y, gyro_z; // unit is rad/sec uint32_t timestamp; // microseconds } motion; }; diff --git a/setsu/src/setsu.c b/setsu/src/setsu.c index 6395313..c31fe83 100644 --- a/setsu/src/setsu.c +++ b/setsu/src/setsu.c @@ -11,6 +11,7 @@ #include #include #include +#include #include @@ -22,6 +23,8 @@ #define SETSU_LOG(...) do {} while(0) #endif +#define DEG2RAD (2.0f * M_PI / 360.0f) + typedef struct setsu_avail_device_t { struct setsu_avail_device_t *next; @@ -686,9 +689,9 @@ static void device_drain(Setsu *setsu, SetsuDevice *dev, SetsuEventCb cb, void * event.motion.accel_x = (float)dev->motion.accel_x / (float)dev->motion.accel_res_x; event.motion.accel_y = (float)dev->motion.accel_y / (float)dev->motion.accel_res_y; event.motion.accel_z = (float)dev->motion.accel_z / (float)dev->motion.accel_res_z; - event.motion.gyro_x = (float)dev->motion.gyro_x / (float)dev->motion.gyro_res_x; - event.motion.gyro_y = (float)dev->motion.gyro_y / (float)dev->motion.gyro_res_y; - event.motion.gyro_z = (float)dev->motion.gyro_z / (float)dev->motion.gyro_res_z; + event.motion.gyro_x = DEG2RAD * (float)dev->motion.gyro_x / (float)dev->motion.gyro_res_x; + event.motion.gyro_y = DEG2RAD * (float)dev->motion.gyro_y / (float)dev->motion.gyro_res_y; + event.motion.gyro_z = DEG2RAD * (float)dev->motion.gyro_z / (float)dev->motion.gyro_res_z; event.motion.timestamp = dev->motion.timestamp; SEND_EVENT(); dev->motion.dirty = false;