From ad3045fc290fb6762ce5980f3757cf5012208c10 Mon Sep 17 00:00:00 2001 From: imterah Date: Mon, 23 Jun 2025 13:49:53 -0400 Subject: [PATCH] chore: Add basic skeleton of the Xreal driver --- Makefile | 4 +- ardriver/commons/interface.go | 22 + ardriver/xreal/Fusion.h | 32 + ardriver/xreal/FusionAhrs.c | 510 +++++++++++ ardriver/xreal/FusionAhrs.h | 111 +++ ardriver/xreal/FusionAxes.h | 187 ++++ ardriver/xreal/FusionCalibration.h | 44 + ardriver/xreal/FusionCompass.c | 49 ++ ardriver/xreal/FusionCompass.h | 25 + ardriver/xreal/FusionConvention.h | 25 + ardriver/xreal/FusionMath.h | 481 +++++++++++ ardriver/xreal/FusionOffset.c | 77 ++ ardriver/xreal/FusionOffset.h | 40 + ardriver/xreal/README.md | 5 + ardriver/xreal/crc32.c | 102 +++ ardriver/xreal/crc32.h | 40 + ardriver/xreal/device.c | 50 ++ ardriver/xreal/device.h | 46 + ardriver/xreal/device_imu.c | 1272 ++++++++++++++++++++++++++++ ardriver/xreal/device_imu.h | 227 +++++ ardriver/xreal/device_mcu.c | 731 ++++++++++++++++ ardriver/xreal/device_mcu.h | 212 +++++ ardriver/xreal/hid_ids.c | 108 +++ ardriver/xreal/hid_ids.h | 54 ++ ardriver/xreal/xreal.go | 12 + ardriver/xreal/xreal_disabled.go | 6 + 26 files changed, 4470 insertions(+), 2 deletions(-) create mode 100644 ardriver/commons/interface.go create mode 100644 ardriver/xreal/Fusion.h create mode 100644 ardriver/xreal/FusionAhrs.c create mode 100644 ardriver/xreal/FusionAhrs.h create mode 100644 ardriver/xreal/FusionAxes.h create mode 100644 ardriver/xreal/FusionCalibration.h create mode 100644 ardriver/xreal/FusionCompass.c create mode 100644 ardriver/xreal/FusionCompass.h create mode 100644 ardriver/xreal/FusionConvention.h create mode 100644 ardriver/xreal/FusionMath.h create mode 100644 ardriver/xreal/FusionOffset.c create mode 100644 ardriver/xreal/FusionOffset.h create mode 100644 ardriver/xreal/README.md create mode 100644 ardriver/xreal/crc32.c create mode 100644 ardriver/xreal/crc32.h create mode 100644 ardriver/xreal/device.c create mode 100644 ardriver/xreal/device.h create mode 100644 ardriver/xreal/device_imu.c create mode 100644 ardriver/xreal/device_imu.h create mode 100644 ardriver/xreal/device_mcu.c create mode 100644 ardriver/xreal/device_mcu.h create mode 100644 ardriver/xreal/hid_ids.c create mode 100644 ardriver/xreal/hid_ids.h create mode 100644 ardriver/xreal/xreal.go create mode 100644 ardriver/xreal/xreal_disabled.go diff --git a/Makefile b/Makefile index b5d2de6..822f8a8 100644 --- a/Makefile +++ b/Makefile @@ -1,13 +1,13 @@ APP_DIR := ./app OUTPUT := uxr -TAGS := drm drm_leasing drm_disable_input +TAGS := xreal drm drm_leasing drm_disable_input .PHONY: all build clean all: build build: - go build -tags '$(TAGS)' -o $(OUTPUT) $(APP_DIR) + go build -v -tags '$(TAGS)' -o $(OUTPUT) $(APP_DIR) clean: rm -f $(OUTPUT) diff --git a/ardriver/commons/interface.go b/ardriver/commons/interface.go new file mode 100644 index 0000000..4a778b6 --- /dev/null +++ b/ardriver/commons/interface.go @@ -0,0 +1,22 @@ +package commons + +type AREventListener struct { + PitchCallback func(float32) + YawCallback func(float32) + RollCallback func(float32) +} + +type ARDevice interface { + // Initializes the AR device's sensors. + Initialize() error + // Ends the AR device's sensors. + End() error + // Polls the AR device's sensors. + Poll() error + // Checks if the underlying AR library is polling-based. + IsPollingLibrary() bool + // Checks if the underlying AR library is event-based. + IsEventBasedLibrary() bool + // Registers event listeners for the AR device. + RegisterEventListeners(eventListener *AREventListener) +} diff --git a/ardriver/xreal/Fusion.h b/ardriver/xreal/Fusion.h new file mode 100644 index 0000000..8d38b8b --- /dev/null +++ b/ardriver/xreal/Fusion.h @@ -0,0 +1,32 @@ +/** + * @file Fusion.h + * @author Seb Madgwick + * @brief Main header file for the Fusion library. This is the only file that + * needs to be included when using the library. + */ + +#ifndef FUSION_H +#define FUSION_H + +//------------------------------------------------------------------------------ +// Includes + +#ifdef __cplusplus +extern "C" { +#endif + +#include "FusionAhrs.h" +#include "FusionAxes.h" +#include "FusionCalibration.h" +#include "FusionCompass.h" +#include "FusionConvention.h" +#include "FusionMath.h" +#include "FusionOffset.h" + +#ifdef __cplusplus +} +#endif + +#endif +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionAhrs.c b/ardriver/xreal/FusionAhrs.c new file mode 100644 index 0000000..a86f629 --- /dev/null +++ b/ardriver/xreal/FusionAhrs.c @@ -0,0 +1,510 @@ +/** + * @file FusionAhrs.c + * @author Seb Madgwick + * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer + * measurements into a single measurement of orientation relative to the Earth. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include +#include "FusionAhrs.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Initial gain used during the initialisation. + */ +#define INITIAL_GAIN (10.0f) + +/** + * @brief Initialisation period in seconds. + */ +#define INITIALISATION_PERIOD (3.0f) + +//------------------------------------------------------------------------------ +// Function declarations + +static inline FusionVector HalfGravity(const FusionAhrs *const ahrs); + +static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs); + +static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference); + +static inline int Clamp(const int value, const int min, const int max); + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Initialises the AHRS algorithm structure. + * @param ahrs AHRS algorithm structure. + */ +void FusionAhrsInitialise(FusionAhrs *const ahrs) { + const FusionAhrsSettings settings = { + .convention = FusionConventionNwu, + .gain = 0.5f, + .gyroscopeRange = 0.0f, + .accelerationRejection = 90.0f, + .magneticRejection = 90.0f, + .recoveryTriggerPeriod = 0, + }; + FusionAhrsSetSettings(ahrs, &settings); + FusionAhrsReset(ahrs); +} + +/** + * @brief Resets the AHRS algorithm. This is equivalent to reinitialising the + * algorithm while maintaining the current settings. + * @param ahrs AHRS algorithm structure. + */ +void FusionAhrsReset(FusionAhrs *const ahrs) { + ahrs->quaternion = FUSION_IDENTITY_QUATERNION; + ahrs->accelerometer = FUSION_VECTOR_ZERO; + ahrs->initialising = true; + ahrs->rampedGain = INITIAL_GAIN; + ahrs->angularRateRecovery = false; + ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger = 0; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magnetometerIgnored = false; + ahrs->magneticRecoveryTrigger = 0; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; +} + +/** + * @brief Sets the AHRS algorithm settings. + * @param ahrs AHRS algorithm structure. + * @param settings Settings. + */ +void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) { + ahrs->settings.convention = settings->convention; + ahrs->settings.gain = settings->gain; + ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange; + ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); + ahrs->settings.magneticRejection = settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); + ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero + ahrs->settings.accelerationRejection = FLT_MAX; + ahrs->settings.magneticRejection = FLT_MAX; + } + if (ahrs->initialising == false) { + ahrs->rampedGain = ahrs->settings.gain; + } + ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and + * magnetometer measurements. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param magnetometer Magnetometer measurement in arbitrary units. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) { +#define Q ahrs->quaternion.element + + // Store accelerometer + ahrs->accelerometer = accelerometer; + + // Reinitialise if gyroscope range exceeded + if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) { + const FusionQuaternion quaternion = ahrs->quaternion; + FusionAhrsReset(ahrs); + ahrs->quaternion = quaternion; + ahrs->angularRateRecovery = true; + } + + // Ramp down gain during initialisation + if (ahrs->initialising) { + ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; + if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) { + ahrs->rampedGain = ahrs->settings.gain; + ahrs->initialising = false; + ahrs->angularRateRecovery = false; + } + } + + // Calculate direction of gravity indicated by algorithm + const FusionVector halfGravity = HalfGravity(ahrs); + + // Calculate accelerometer feedback + FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = true; + if (FusionVectorIsZero(accelerometer) == false) { + + // Calculate accelerometer feedback scaled by 0.5 + ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity); + + // Don't ignore accelerometer if acceleration error below threshold + if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) { + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger -= 9; + } else { + ahrs->accelerationRecoveryTrigger += 1; + } + + // Don't ignore accelerometer during acceleration recovery + if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) { + ahrs->accelerationRecoveryTimeout = 0; + ahrs->accelerometerIgnored = false; + } else { + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); + + // Apply accelerometer feedback + if (ahrs->accelerometerIgnored == false) { + halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; + } + } + + // Calculate magnetometer feedback + FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->magnetometerIgnored = true; + if (FusionVectorIsZero(magnetometer) == false) { + + // Calculate direction of magnetic field indicated by algorithm + const FusionVector halfMagnetic = HalfMagnetic(ahrs); + + // Calculate magnetometer feedback scaled by 0.5 + ahrs->halfMagnetometerFeedback = Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic); + + // Don't ignore magnetometer if magnetic error below threshold + if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) { + ahrs->magnetometerIgnored = false; + ahrs->magneticRecoveryTrigger -= 9; + } else { + ahrs->magneticRecoveryTrigger += 1; + } + + // Don't ignore magnetometer during magnetic recovery + if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) { + ahrs->magneticRecoveryTimeout = 0; + ahrs->magnetometerIgnored = false; + } else { + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); + + // Apply magnetometer feedback + if (ahrs->magnetometerIgnored == false) { + halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; + } + } + + // Convert gyroscope to radians per second scaled by 0.5 + const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); + + // Apply feedback to gyroscope + const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain)); + + // Integrate rate of change of quaternion + ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime))); + + // Normalise quaternion + ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); +#undef Q +} + +/** + * @brief Returns the direction of gravity scaled by 0.5. + * @param ahrs AHRS algorithm structure. + * @return Direction of gravity scaled by 0.5. + */ +static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: { + const FusionVector halfGravity = {.axis = { + .x = Q.x * Q.z - Q.w * Q.y, + .y = Q.y * Q.z + Q.w * Q.x, + .z = Q.w * Q.w - 0.5f + Q.z * Q.z, + }}; // third column of transposed rotation matrix scaled by 0.5 + return halfGravity; + } + case FusionConventionNed: { + const FusionVector halfGravity = {.axis = { + .x = Q.w * Q.y - Q.x * Q.z, + .y = -1.0f * (Q.y * Q.z + Q.w * Q.x), + .z = 0.5f - Q.w * Q.w - Q.z * Q.z, + }}; // third column of transposed rotation matrix scaled by -0.5 + return halfGravity; + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning +#undef Q +} + +/** + * @brief Returns the direction of the magnetic field scaled by 0.5. + * @param ahrs AHRS algorithm structure. + * @return Direction of the magnetic field scaled by 0.5. + */ +static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + switch (ahrs->settings.convention) { + case FusionConventionNwu: { + const FusionVector halfMagnetic = {.axis = { + .x = Q.x * Q.y + Q.w * Q.z, + .y = Q.w * Q.w - 0.5f + Q.y * Q.y, + .z = Q.y * Q.z - Q.w * Q.x, + }}; // second column of transposed rotation matrix scaled by 0.5 + return halfMagnetic; + } + case FusionConventionEnu: { + const FusionVector halfMagnetic = {.axis = { + .x = 0.5f - Q.w * Q.w - Q.x * Q.x, + .y = Q.w * Q.z - Q.x * Q.y, + .z = -1.0f * (Q.x * Q.z + Q.w * Q.y), + }}; // first column of transposed rotation matrix scaled by -0.5 + return halfMagnetic; + } + case FusionConventionNed: { + const FusionVector halfMagnetic = {.axis = { + .x = -1.0f * (Q.x * Q.y + Q.w * Q.z), + .y = 0.5f - Q.w * Q.w - Q.y * Q.y, + .z = Q.w * Q.x - Q.y * Q.z, + }}; // second column of transposed rotation matrix scaled by -0.5 + return halfMagnetic; + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning +#undef Q +} + +/** + * @brief Returns the feedback. + * @param sensor Sensor. + * @param reference Reference. + * @return Feedback. + */ +static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) { + if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees + return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference)); + } + return FusionVectorCrossProduct(sensor, reference); +} + +/** + * @brief Returns a value limited to maximum and minimum. + * @param value Value. + * @param min Minimum value. + * @param max Maximum value. + * @return Value limited to maximum and minimum. + */ +static inline int Clamp(const int value, const int min, const int max) { + if (value < min) { + return min; + } + if (value > max) { + return max; + } + return value; +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope and accelerometer + * measurements only. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) { + + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); + + // Zero heading during initialisation + if (ahrs->initialising) { + FusionAhrsSetHeading(ahrs, 0.0f); + } +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and + * heading measurements. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param heading Heading measurement in degrees. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime) { +#define Q ahrs->quaternion.element + + // Calculate roll + const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); + + // Calculate magnetometer + const float headingRadians = FusionDegreesToRadians(heading); + const float sinHeadingRadians = sinf(headingRadians); + const FusionVector magnetometer = {.axis = { + .x = cosf(headingRadians), + .y = -1.0f * cosf(roll) * sinHeadingRadians, + .z = sinHeadingRadians * sinf(roll), + }}; + + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); +#undef Q +} + +/** + * @brief Returns the quaternion describing the sensor relative to the Earth. + * @param ahrs AHRS algorithm structure. + * @return Quaternion describing the sensor relative to the Earth. + */ +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { + return ahrs->quaternion; +} + +/** + * @brief Sets the quaternion describing the sensor relative to the Earth. + * @param ahrs AHRS algorithm structure. + * @param quaternion Quaternion describing the sensor relative to the Earth. + */ +void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) { + ahrs->quaternion = quaternion; +} + +/** + * @brief Returns the direction of gravity in the sensor coordinate frame. + * @param ahrs AHRS algorithm structure. + * @return Direction of gravity in the sensor coordinate frame. + */ +FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + const FusionVector gravity = {.axis = { + .x = 2.0f * (Q.x * Q.z - Q.w * Q.y), + .y = 2.0f * (Q.y * Q.z + Q.w * Q.x), + .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), + }}; // third column of transposed rotation matrix + return gravity; +#undef Q +} + +/** + * @brief Returns the linear acceleration measurement equal to the accelerometer + * measurement with gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Linear acceleration measurement in g. + */ +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: { + return FusionVectorSubtract(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); + } + case FusionConventionNed: { + return FusionVectorAdd(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning +} + +/** + * @brief Returns the Earth acceleration measurement equal to accelerometer + * measurement in the Earth coordinate frame with gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Earth acceleration measurement in g. + */ +FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element +#define A ahrs->accelerometer.axis + + // Calculate accelerometer measurement in the Earth coordinate frame + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + FusionVector accelerometer = {.axis = { + .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), + .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), + .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z), + }}; // rotation matrix multiplied with the accelerometer + + // Remove gravity from accelerometer measurement + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: + accelerometer.axis.z -= 1.0f; + break; + case FusionConventionNed: + accelerometer.axis.z += 1.0f; + break; + } + return accelerometer; +#undef Q +#undef A +} + +/** + * @brief Returns the AHRS algorithm internal states. + * @param ahrs AHRS algorithm structure. + * @return AHRS algorithm internal states. + */ +FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) { + const FusionAhrsInternalStates internalStates = { + .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), + .accelerometerIgnored = ahrs->accelerometerIgnored, + .accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->accelerationRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, + .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), + .magnetometerIgnored = ahrs->magnetometerIgnored, + .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->magneticRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, + }; + return internalStates; +} + +/** + * @brief Returns the AHRS algorithm flags. + * @param ahrs AHRS algorithm structure. + * @return AHRS algorithm flags. + */ +FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) { + const FusionAhrsFlags flags = { + .initialising = ahrs->initialising, + .angularRateRecovery = ahrs->angularRateRecovery, + .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout, + .magneticRecovery= ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout, + }; + return flags; +} + +/** + * @brief Sets the heading of the orientation measurement provided by the AHRS + * algorithm. This function can be used to reset drift in heading when the AHRS + * algorithm is being used without a magnetometer. + * @param ahrs AHRS algorithm structure. + * @param heading Heading angle in degrees. + */ +void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) { +#define Q ahrs->quaternion.element + const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); + const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); + const FusionQuaternion rotation = {.element = { + .w = cosf(halfYawMinusHeading), + .x = 0.0f, + .y = 0.0f, + .z = -1.0f * sinf(halfYawMinusHeading), + }}; + ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); +#undef Q +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionAhrs.h b/ardriver/xreal/FusionAhrs.h new file mode 100644 index 0000000..fca2476 --- /dev/null +++ b/ardriver/xreal/FusionAhrs.h @@ -0,0 +1,111 @@ +/** + * @file FusionAhrs.h + * @author Seb Madgwick + * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer + * measurements into a single measurement of orientation relative to the Earth. + */ + +#ifndef FUSION_AHRS_H +#define FUSION_AHRS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionConvention.h" +#include "FusionMath.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief AHRS algorithm settings. + */ +typedef struct { + FusionConvention convention; + float gain; + float gyroscopeRange; + float accelerationRejection; + float magneticRejection; + unsigned int recoveryTriggerPeriod; +} FusionAhrsSettings; + +/** + * @brief AHRS algorithm structure. Structure members are used internally and + * must not be accessed by the application. + */ +typedef struct { + FusionAhrsSettings settings; + FusionQuaternion quaternion; + FusionVector accelerometer; + bool initialising; + float rampedGain; + float rampedGainStep; + bool angularRateRecovery; + FusionVector halfAccelerometerFeedback; + FusionVector halfMagnetometerFeedback; + bool accelerometerIgnored; + int accelerationRecoveryTrigger; + int accelerationRecoveryTimeout; + bool magnetometerIgnored; + int magneticRecoveryTrigger; + int magneticRecoveryTimeout; +} FusionAhrs; + +/** + * @brief AHRS algorithm internal states. + */ +typedef struct { + float accelerationError; + bool accelerometerIgnored; + float accelerationRecoveryTrigger; + float magneticError; + bool magnetometerIgnored; + float magneticRecoveryTrigger; +} FusionAhrsInternalStates; + +/** + * @brief AHRS algorithm flags. + */ +typedef struct { + bool initialising; + bool angularRateRecovery; + bool accelerationRecovery; + bool magneticRecovery; +} FusionAhrsFlags; + +//------------------------------------------------------------------------------ +// Function declarations + +void FusionAhrsInitialise(FusionAhrs *const ahrs); + +void FusionAhrsReset(FusionAhrs *const ahrs); + +void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); + +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); + +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); + +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); + +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); + +void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion); + +FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs); + +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); + +FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); + +FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs); + +FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs); + +void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionAxes.h b/ardriver/xreal/FusionAxes.h new file mode 100644 index 0000000..32c07db --- /dev/null +++ b/ardriver/xreal/FusionAxes.h @@ -0,0 +1,187 @@ +/** + * @file FusionAxes.h + * @author Seb Madgwick + * @brief Swaps sensor axes for alignment with the body axes. + */ + +#ifndef FUSION_AXES_H +#define FUSION_AXES_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Axes alignment describing the sensor axes relative to the body axes. + * For example, if the body X axis is aligned with the sensor Y axis and the + * body Y axis is aligned with sensor X axis but pointing the opposite direction + * then alignment is +Y-X+Z. + */ +typedef enum { + FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ + FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ + FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ + FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ + FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ + FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ + FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ + FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ + FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ + FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ + FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ + FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ + FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ + FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ + FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ + FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ + FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ + FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ + FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ + FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ + FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ + FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ + FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ + FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ +} FusionAxesAlignment; + +//------------------------------------------------------------------------------ +// Inline functions + +/** + * @brief Swaps sensor axes for alignment with the body axes. + * @param sensor Sensor axes. + * @param alignment Axes alignment. + * @return Sensor axes aligned with the body axes. + */ +static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { + FusionVector result; + switch (alignment) { + case FusionAxesAlignmentPXPYPZ: + break; + case FusionAxesAlignmentPXNZPY: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPXNYNZ: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPXPZNY: + result.axis.x = +sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNXPYNZ: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNXPZPY: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNXNYPZ: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNXNZNY: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentPYNXPZ: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentPYNZNX: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPYPXNZ: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPYPZPX: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYPXPZ: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNYNZPX: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYNXNZ: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNYPZNX: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPYNX: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPXPY: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPZNYPX: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentPZNXNY: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNZPYPX: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNZNXPY: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNZNYNX: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentNZPXNY: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + } + return sensor; // avoid compiler warning +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionCalibration.h b/ardriver/xreal/FusionCalibration.h new file mode 100644 index 0000000..733f572 --- /dev/null +++ b/ardriver/xreal/FusionCalibration.h @@ -0,0 +1,44 @@ +/** + * @file FusionCalibration.h + * @author Seb Madgwick + * @brief Gyroscope, accelerometer, and magnetometer calibration models. + */ + +#ifndef FUSION_CALIBRATION_H +#define FUSION_CALIBRATION_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Inline functions + +/** + * @brief Gyroscope and accelerometer calibration model. + * @param uncalibrated Uncalibrated measurement. + * @param misalignment Misalignment matrix. + * @param sensitivity Sensitivity. + * @param offset Offset. + * @return Calibrated measurement. + */ +static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { + return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); +} + +/** + * @brief Magnetometer calibration model. + * @param uncalibrated Uncalibrated measurement. + * @param softIronMatrix Soft-iron matrix. + * @param hardIronOffset Hard-iron offset. + * @return Calibrated measurement. + */ +static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { + return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset)); +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionCompass.c b/ardriver/xreal/FusionCompass.c new file mode 100644 index 0000000..e34617c --- /dev/null +++ b/ardriver/xreal/FusionCompass.c @@ -0,0 +1,49 @@ +/** + * @file FusionCompass.c + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate the magnetic heading using + * accelerometer and magnetometer measurements. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionAxes.h" +#include "FusionCompass.h" +#include + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Calculates the magnetic heading. + * @param convention Earth axes convention. + * @param accelerometer Accelerometer measurement in any calibrated units. + * @param magnetometer Magnetometer measurement in any calibrated units. + * @return Heading angle in degrees. + */ +float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer) { + switch (convention) { + case FusionConventionNwu: { + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); + return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); + } + case FusionConventionEnu: { + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); + const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f); + return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x)); + } + case FusionConventionNed: { + const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f); + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up)); + return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); + } + } + return 0; // avoid compiler warning +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionCompass.h b/ardriver/xreal/FusionCompass.h new file mode 100644 index 0000000..78326c0 --- /dev/null +++ b/ardriver/xreal/FusionCompass.h @@ -0,0 +1,25 @@ +/** + * @file FusionCompass.h + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate the magnetic heading using + * accelerometer and magnetometer measurements. + */ + +#ifndef FUSION_COMPASS_H +#define FUSION_COMPASS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionConvention.h" +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Function declarations + +float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionConvention.h b/ardriver/xreal/FusionConvention.h new file mode 100644 index 0000000..0b0d43a --- /dev/null +++ b/ardriver/xreal/FusionConvention.h @@ -0,0 +1,25 @@ +/** + * @file FusionConvention.h + * @author Seb Madgwick + * @brief Earth axes convention. + */ + +#ifndef FUSION_CONVENTION_H +#define FUSION_CONVENTION_H + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Earth axes convention. + */ +typedef enum { + FusionConventionNwu, /* North-West-Up */ + FusionConventionEnu, /* East-North-Up */ + FusionConventionNed, /* North-East-Down */ +} FusionConvention; + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionMath.h b/ardriver/xreal/FusionMath.h new file mode 100644 index 0000000..5ee92b0 --- /dev/null +++ b/ardriver/xreal/FusionMath.h @@ -0,0 +1,481 @@ +/** + * @file FusionMath.h + * @author Seb Madgwick + * @brief Math library. + */ + +#ifndef FUSION_MATH_H +#define FUSION_MATH_H + +//------------------------------------------------------------------------------ +// Includes + +#include +#include +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief 3D vector. + */ +typedef union { + float array[3]; + + struct { + float x; + float y; + float z; + } axis; +} FusionVector; + +/** + * @brief Quaternion. + */ +typedef union { + float array[4]; + + struct { + float w; + float x; + float y; + float z; + } element; +} FusionQuaternion; + +/** + * @brief 3x3 matrix in row-major order. + * See http://en.wikipedia.org/wiki/Row-major_order + */ +typedef union { + float array[3][3]; + + struct { + float xx; + float xy; + float xz; + float yx; + float yy; + float yz; + float zx; + float zy; + float zz; + } element; +} FusionMatrix; + +/** + * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around + * X, Y, and Z respectively. + */ +typedef union { + float array[3]; + + struct { + float roll; + float pitch; + float yaw; + } angle; +} FusionEuler; + +/** + * @brief Vector of zeros. + */ +#define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) + +/** + * @brief Vector of ones. + */ +#define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) + +/** + * @brief Identity quaternion. + */ +#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) + +/** + * @brief Identity matrix. + */ +#define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) + +/** + * @brief Euler angles of zero. + */ +#define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) + +/** + * @brief Pi. May not be defined in math.h. + */ +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +/** + * @brief Include this definition or add as a preprocessor definition to use + * normal square root operations. + */ +//#define FUSION_USE_NORMAL_SQRT + +//------------------------------------------------------------------------------ +// Inline functions - Degrees and radians conversion + +/** + * @brief Converts degrees to radians. + * @param degrees Degrees. + * @return Radians. + */ +static inline float FusionDegreesToRadians(const float degrees) { + return degrees * ((float) M_PI / 180.0f); +} + +/** + * @brief Converts radians to degrees. + * @param radians Radians. + * @return Degrees. + */ +static inline float FusionRadiansToDegrees(const float radians) { + return radians * (180.0f / (float) M_PI); +} + +//------------------------------------------------------------------------------ +// Inline functions - Arc sine + +/** + * @brief Returns the arc sine of the value. + * @param value Value. + * @return Arc sine of the value. + */ +static inline float FusionAsin(const float value) { + if (value <= -1.0f) { + return (float) M_PI / -2.0f; + } + if (value >= 1.0f) { + return (float) M_PI / 2.0f; + } + return asinf(value); +} + +//------------------------------------------------------------------------------ +// Inline functions - Fast inverse square root + +#ifndef FUSION_USE_NORMAL_SQRT + +/** + * @brief Calculates the reciprocal of the square root. + * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ + * @param x Operand. + * @return Reciprocal of the square root of x. + */ +static inline float FusionFastInverseSqrt(const float x) { + + typedef union { + float f; + int32_t i; + } Union32; + + Union32 union32 = {.f = x}; + union32.i = 0x5F1F1412 - (union32.i >> 1); + return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); +} + +#endif + +//------------------------------------------------------------------------------ +// Inline functions - Vector operations + +/** + * @brief Returns true if the vector is zero. + * @param vector Vector. + * @return True if the vector is zero. + */ +static inline bool FusionVectorIsZero(const FusionVector vector) { + return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); +} + +/** + * @brief Returns the sum of two vectors. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Sum of two vectors. + */ +static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x + vectorB.axis.x, + .y = vectorA.axis.y + vectorB.axis.y, + .z = vectorA.axis.z + vectorB.axis.z, + }}; + return result; +} + +/** + * @brief Returns vector B subtracted from vector A. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Vector B subtracted from vector A. + */ +static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x - vectorB.axis.x, + .y = vectorA.axis.y - vectorB.axis.y, + .z = vectorA.axis.z - vectorB.axis.z, + }}; + return result; +} + +/** + * @brief Returns the sum of the elements. + * @param vector Vector. + * @return Sum of the elements. + */ +static inline float FusionVectorSum(const FusionVector vector) { + return vector.axis.x + vector.axis.y + vector.axis.z; +} + +/** + * @brief Returns the multiplication of a vector by a scalar. + * @param vector Vector. + * @param scalar Scalar. + * @return Multiplication of a vector by a scalar. + */ +static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { + const FusionVector result = {.axis = { + .x = vector.axis.x * scalar, + .y = vector.axis.y * scalar, + .z = vector.axis.z * scalar, + }}; + return result; +} + +/** + * @brief Calculates the Hadamard product (element-wise multiplication). + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Hadamard product. + */ +static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x * vectorB.axis.x, + .y = vectorA.axis.y * vectorB.axis.y, + .z = vectorA.axis.z * vectorB.axis.z, + }}; + return result; +} + +/** + * @brief Returns the cross product. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Cross product. + */ +static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { +#define A vectorA.axis +#define B vectorB.axis + const FusionVector result = {.axis = { + .x = A.y * B.z - A.z * B.y, + .y = A.z * B.x - A.x * B.z, + .z = A.x * B.y - A.y * B.x, + }}; + return result; +#undef A +#undef B +} + +/** + * @brief Returns the dot product. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Dot product. + */ +static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) { + return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB)); +} + +/** + * @brief Returns the vector magnitude squared. + * @param vector Vector. + * @return Vector magnitude squared. + */ +static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { + return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); +} + +/** + * @brief Returns the vector magnitude. + * @param vector Vector. + * @return Vector magnitude. + */ +static inline float FusionVectorMagnitude(const FusionVector vector) { + return sqrtf(FusionVectorMagnitudeSquared(vector)); +} + +/** + * @brief Returns the normalised vector. + * @param vector Vector. + * @return Normalised vector. + */ +static inline FusionVector FusionVectorNormalise(const FusionVector vector) { +#ifdef FUSION_USE_NORMAL_SQRT + const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); +#else + const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); +#endif + return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); +} + +//------------------------------------------------------------------------------ +// Inline functions - Quaternion operations + +/** + * @brief Returns the sum of two quaternions. + * @param quaternionA Quaternion A. + * @param quaternionB Quaternion B. + * @return Sum of two quaternions. + */ +static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { + const FusionQuaternion result = {.element = { + .w = quaternionA.element.w + quaternionB.element.w, + .x = quaternionA.element.x + quaternionB.element.x, + .y = quaternionA.element.y + quaternionB.element.y, + .z = quaternionA.element.z + quaternionB.element.z, + }}; + return result; +} + +/** + * @brief Returns the multiplication of two quaternions. + * @param quaternionA Quaternion A (to be post-multiplied). + * @param quaternionB Quaternion B (to be pre-multiplied). + * @return Multiplication of two quaternions. + */ +static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { +#define A quaternionA.element +#define B quaternionB.element + const FusionQuaternion result = {.element = { + .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, + .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, + .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, + .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, + }}; + return result; +#undef A +#undef B +} + +/** + * @brief Returns the multiplication of a quaternion with a vector. This is a + * normal quaternion multiplication where the vector is treated a + * quaternion with a W element value of zero. The quaternion is post- + * multiplied by the vector. + * @param quaternion Quaternion. + * @param vector Vector. + * @return Multiplication of a quaternion with a vector. + */ +static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { +#define Q quaternion.element +#define V vector.axis + const FusionQuaternion result = {.element = { + .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, + .x = Q.w * V.x + Q.y * V.z - Q.z * V.y, + .y = Q.w * V.y - Q.x * V.z + Q.z * V.x, + .z = Q.w * V.z + Q.x * V.y - Q.y * V.x, + }}; + return result; +#undef Q +#undef V +} + +/** + * @brief Returns the normalised quaternion. + * @param quaternion Quaternion. + * @return Normalised quaternion. + */ +static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { +#define Q quaternion.element +#ifdef FUSION_USE_NORMAL_SQRT + const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); +#else + const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); +#endif + const FusionQuaternion result = {.element = { + .w = Q.w * magnitudeReciprocal, + .x = Q.x * magnitudeReciprocal, + .y = Q.y * magnitudeReciprocal, + .z = Q.z * magnitudeReciprocal, + }}; + return result; +#undef Q +} + +//------------------------------------------------------------------------------ +// Inline functions - Matrix operations + +/** + * @brief Returns the multiplication of a matrix with a vector. + * @param matrix Matrix. + * @param vector Vector. + * @return Multiplication of a matrix with a vector. + */ +static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { +#define R matrix.element + const FusionVector result = {.axis = { + .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, + .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, + .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, + }}; + return result; +#undef R +} + +//------------------------------------------------------------------------------ +// Inline functions - Conversion operations + +/** + * @brief Converts a quaternion to a rotation matrix. + * @param quaternion Quaternion. + * @return Rotation matrix. + */ +static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { +#define Q quaternion.element + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + const FusionMatrix matrix = {.element = { + .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), + .xy = 2.0f * (qxqy - qwqz), + .xz = 2.0f * (qxqz + qwqy), + .yx = 2.0f * (qxqy + qwqz), + .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), + .yz = 2.0f * (qyqz - qwqx), + .zx = 2.0f * (qxqz - qwqy), + .zy = 2.0f * (qyqz + qwqx), + .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), + }}; + return matrix; +#undef Q +} + +/** + * @brief Converts a quaternion to ZYX Euler angles in degrees. + * @param quaternion Quaternion. + * @return Euler angles in degrees. + */ +static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { +#define Q quaternion.element + const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations + const FusionEuler euler = {.angle = { + .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), + .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), + .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), + }}; + return euler; +#undef Q +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionOffset.c b/ardriver/xreal/FusionOffset.c new file mode 100644 index 0000000..1359c3d --- /dev/null +++ b/ardriver/xreal/FusionOffset.c @@ -0,0 +1,77 @@ +/** + * @file FusionOffset.c + * @author Seb Madgwick + * @brief Gyroscope offset correction algorithm for run-time calibration of the + * gyroscope offset. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionOffset.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Cutoff frequency in Hz. + */ +#define CUTOFF_FREQUENCY (0.02f) + +/** + * @brief Timeout in seconds. + */ +#define TIMEOUT (5) + +/** + * @brief Threshold in degrees per second. + */ +#define THRESHOLD (3.0f) + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Initialises the gyroscope offset algorithm. + * @param offset Gyroscope offset algorithm structure. + * @param sampleRate Sample rate in Hz. + */ +void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) { + offset->filterCoefficient = 2.0f * (float) M_PI * CUTOFF_FREQUENCY * (1.0f / (float) sampleRate); + offset->timeout = TIMEOUT * sampleRate; + offset->timer = 0; + offset->gyroscopeOffset = FUSION_VECTOR_ZERO; +} + +/** + * @brief Updates the gyroscope offset algorithm and returns the corrected + * gyroscope measurement. + * @param offset Gyroscope offset algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @return Corrected gyroscope measurement in degrees per second. + */ +FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) { + + // Subtract offset from gyroscope measurement + gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); + + // Reset timer if gyroscope not stationary + if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) { + offset->timer = 0; + return gyroscope; + } + + // Increment timer while gyroscope stationary + if (offset->timer < offset->timeout) { + offset->timer++; + return gyroscope; + } + + // Adjust offset if timer has elapsed + offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient)); + return gyroscope; +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionOffset.h b/ardriver/xreal/FusionOffset.h new file mode 100644 index 0000000..4782356 --- /dev/null +++ b/ardriver/xreal/FusionOffset.h @@ -0,0 +1,40 @@ +/** + * @file FusionOffset.h + * @author Seb Madgwick + * @brief Gyroscope offset correction algorithm for run-time calibration of the + * gyroscope offset. + */ + +#ifndef FUSION_OFFSET_H +#define FUSION_OFFSET_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Gyroscope offset algorithm structure. Structure members are used + * internally and must not be accessed by the application. + */ +typedef struct { + float filterCoefficient; + unsigned int timeout; + unsigned int timer; + FusionVector gyroscopeOffset; +} FusionOffset; + +//------------------------------------------------------------------------------ +// Function declarations + +void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate); + +FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/README.md b/ardriver/xreal/README.md new file mode 100644 index 0000000..48e024c --- /dev/null +++ b/ardriver/xreal/README.md @@ -0,0 +1,5 @@ +# Xreal driver +## Attribution + +- [Xreal driver](https://gitlab.com/TheJackiMonster/nrealAirLinuxDriver) +- [Fusion](https://github.com/xioTechnologies/Fusion) diff --git a/ardriver/xreal/crc32.c b/ardriver/xreal/crc32.c new file mode 100644 index 0000000..2e2d955 --- /dev/null +++ b/ardriver/xreal/crc32.c @@ -0,0 +1,102 @@ +// +// Created by thejackimonster on 21.04.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "crc32.h" + +const uint32_t crc32_table [256] = { + 0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, + 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3, + 0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, + 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91, + 0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, + 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7, + 0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, + 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5, + 0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, + 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B, + 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, + 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59, + 0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, + 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F, + 0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, + 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D, + 0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, + 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433, + 0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, + 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01, + 0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, + 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457, + 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, + 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65, + 0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, + 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB, + 0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, + 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9, + 0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, + 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F, + 0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, + 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD, + 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, + 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683, + 0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, + 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1, + 0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, + 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7, + 0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, + 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5, + 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, + 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B, + 0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, + 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79, + 0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, + 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F, + 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, + 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D, + 0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, + 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713, + 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, + 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21, + 0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, + 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777, + 0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, + 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45, + 0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, + 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB, + 0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, + 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9, + 0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, + 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF, + 0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, + 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D +}; + +uint32_t crc32_checksum(const uint8_t* buf, uint32_t len) { + uint32_t CRC32_data = 0xFFFFFFFF; + for (uint32_t i = 0; i < len; i++) { + const uint8_t t = (CRC32_data ^ buf[i]) & 0xFF; + CRC32_data = ((CRC32_data >> 8) & 0xFFFFFF) ^ crc32_table[t]; + } + + return ~CRC32_data; +} diff --git a/ardriver/xreal/crc32.h b/ardriver/xreal/crc32.h new file mode 100644 index 0000000..50be774 --- /dev/null +++ b/ardriver/xreal/crc32.h @@ -0,0 +1,40 @@ +#pragma once +// +// Created by thejackimonster on 21.04.23. +// +// Copyright (c) 2023 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +uint32_t crc32_checksum(const uint8_t* buf, uint32_t len); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/device.c b/ardriver/xreal/device.c new file mode 100644 index 0000000..ab377f3 --- /dev/null +++ b/ardriver/xreal/device.c @@ -0,0 +1,50 @@ +// +// Created by thejackimonster on 19.12.23. +// +// Copyright (c) 2023 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "device.h" + +#include + +static size_t hid_device_counter = 0; + +bool device_init() { + if ((!hid_device_counter) && (0 != hid_init())) { + return false; + } + + hid_device_counter++; + return true; +} + +void device_exit() { + if (!hid_device_counter) { + return; + } + + hid_device_counter--; + + if (0 == hid_device_counter) { + hid_exit(); + } +} diff --git a/ardriver/xreal/device.h b/ardriver/xreal/device.h new file mode 100644 index 0000000..7f2c781 --- /dev/null +++ b/ardriver/xreal/device.h @@ -0,0 +1,46 @@ +#pragma once +// +// Created by thejackimonster on 19.12.23. +// +// Copyright (c) 2023 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +bool device_init(); + +void device_exit(); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/device_imu.c b/ardriver/xreal/device_imu.c new file mode 100644 index 0000000..add1338 --- /dev/null +++ b/ardriver/xreal/device_imu.c @@ -0,0 +1,1272 @@ +// +// Created by thejackimonster on 30.03.23. +// +// Copyright (c) 2023-2025 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "device_imu.h" +#include "device.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "crc32.h" +#include "hid_ids.h" + +#define GRAVITY_G (9.806f) + +#ifndef NDEBUG +#define device_imu_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#else +#define device_imu_error(msg) (0) +#endif + +struct device_imu_camera_sensor_t { + FusionMatrix cameraMisalignment; + FusionVector cameraOffset; + + uint16_t resolution [2]; + + float cc [2]; + float fc [2]; + + uint32_t num_kc; + float* kc; +}; + +struct device_imu_camera_t { + uint32_t num_sensors; + device_imu_camera_sensor_type* sensors; +}; + +struct device_imu_camera_calibration_t { + uint32_t num_cameras; + device_imu_camera_type *cameras; +}; + +struct device_imu_calibration_t { + FusionMatrix gyroscopeMisalignment; + FusionVector gyroscopeSensitivity; + FusionVector gyroscopeOffset; + + FusionMatrix accelerometerMisalignment; + FusionVector accelerometerSensitivity; + FusionVector accelerometerOffset; + + FusionMatrix magnetometerMisalignment; + FusionVector magnetometerSensitivity; + FusionVector magnetometerOffset; + + FusionMatrix softIronMatrix; + FusionVector hardIronOffset; + + FusionQuaternion noises; + + device_imu_camera_calibration_type cam; +}; + +static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t* payload) { + int payload_size = size; + if (payload_size > device->max_payload_size) { + payload_size = device->max_payload_size; + } + + int transferred = hid_write(device->handle, payload, payload_size); + + if (transferred != payload_size) { + device_imu_error("Sending payload failed"); + return false; + } + + return (transferred == size); +} + +static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payload) { + int payload_size = size; + if (payload_size > device->max_payload_size) { + payload_size = device->max_payload_size; + } + + int transferred = hid_read(device->handle, payload, payload_size); + + if (transferred >= payload_size) { + transferred = payload_size; + } + + if (transferred == 0) { + return false; + } + + if (transferred != payload_size) { + device_imu_error("Receiving payload failed"); + return false; + } + + return (transferred == size); +} + +struct __attribute__((__packed__)) device_imu_payload_packet_t { + uint8_t head; + uint32_t checksum; + uint16_t length; + uint8_t msgid; + uint8_t data [512 - 8]; +}; + +typedef struct device_imu_payload_packet_t device_imu_payload_packet_type; + +static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, const uint8_t* data) { + static device_imu_payload_packet_type packet; + + const uint16_t packet_len = 3 + len; + const uint16_t payload_len = 5 + packet_len; + + packet.head = 0xAA; + packet.length = htole16(packet_len); + packet.msgid = msgid; + + memcpy(packet.data, data, len); + packet.checksum = htole32( + crc32_checksum( + (const uint8_t*) (&packet.length), + packet.length + ) + ); + + return send_payload(device, payload_len, (uint8_t*) (&packet)); +} + +static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint8_t signal) { + return send_payload_msg(device, msgid, 1, &signal); +} + +static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) { + static device_imu_payload_packet_type packet; + + packet.head = 0; + packet.length = 0; + packet.msgid = 0; + + const uint16_t packet_len = 3 + len; + const uint16_t payload_len = 5 + packet_len; + + do { + if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { + return false; + } + } while (packet.msgid != msgid); + + memcpy(data, packet.data, len); + return true; +} + +static FusionVector json_object_get_vector(struct json_object* obj) { + if ((!json_object_is_type(obj, json_type_array)) || + (json_object_array_length(obj) != 3)) { + return FUSION_VECTOR_ZERO; + } + + FusionVector vector; + vector.axis.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); + vector.axis.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); + vector.axis.z = (float) json_object_get_double(json_object_array_get_idx(obj, 2)); + return vector; +} + +static FusionQuaternion json_object_get_quaternion(struct json_object* obj) { + if ((!json_object_is_type(obj, json_type_array)) || + (json_object_array_length(obj) != 4)) { + return FUSION_IDENTITY_QUATERNION; + } + + FusionQuaternion quaternion; + quaternion.element.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); + quaternion.element.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); + quaternion.element.z = (float) json_object_get_double(json_object_array_get_idx(obj, 2)); + quaternion.element.w = (float) json_object_get_double(json_object_array_get_idx(obj, 3)); + return quaternion; +} + +static uint32_t json_object_get_array_f32(struct json_object* obj, float** array, uint32_t n) { + if ((!json_object_is_type(obj, json_type_array)) || + ((n > 0) && (json_object_array_length(obj) != n))) { + return 0; + } + + if (n == 0) { + n = json_object_array_length(obj); + *array = malloc(sizeof(float) * n); + } + + for (uint32_t i = 0; i < n; i++) { + (*array)[i] = (float) json_object_get_double(json_object_array_get_idx(obj, i)); + } + + return n; +} + +static uint32_t json_object_get_array_u16(struct json_object* obj, uint16_t** array, uint32_t n) { + if ((!json_object_is_type(obj, json_type_array)) || + ((n > 0) && (json_object_array_length(obj) != n))) { + return 0; + } + + if (n == 0) { + n = json_object_array_length(obj); + *array = malloc(sizeof(uint16_t) * n); + } + + for (uint32_t i = 0; i < n; i++) { + (*array)[i] = (uint16_t) json_object_get_int(json_object_array_get_idx(obj, i)); + } + + return n; +} + +static void init_device_imu_camera(device_imu_camera_type *camera, json_object *cam) { + uint32_t num_sensors = json_object_get_int(json_object_object_get(cam, "num_of_cameras")); + + device_imu_camera_sensor_type *sensors = NULL; + + if (num_sensors > 0) { + sensors = malloc(sizeof(device_imu_camera_sensor_type) * num_sensors); + } + + if (!sensors) { + num_sensors = 0; + } + + for (uint32_t n = 0; n < num_sensors; n++) { + device_imu_camera_sensor_type *sensor = &(sensors[n]); + + char device_name [64]; + snprintf(device_name, 64, "device_%u", (n + 1)); + + struct json_object* dev = json_object_object_get(cam, device_name); + + FusionQuaternion imu_q_cam = json_object_get_quaternion(json_object_object_get(dev, "imu_q_cam")); + FusionVector cam_offset = json_object_get_vector(json_object_object_get(dev, "imu_p_cam")); + + sensor->cameraMisalignment = FusionQuaternionToMatrix(imu_q_cam); + sensor->cameraOffset = cam_offset; + + uint16_t* resolution = sensor->resolution; + float* cc = sensor->cc; + float* fc = sensor->fc; + float* kc = sensor->kc; + + json_object_get_array_u16(json_object_object_get(dev, "resolution"), &resolution, 2); + + json_object_get_array_f32(json_object_object_get(dev, "cc"), &cc, 2); + json_object_get_array_f32(json_object_object_get(dev, "fc"), &fc, 2); + + sensor->num_kc = json_object_get_array_f32(json_object_object_get(dev, "kc"), &kc, 0); + sensor->kc = kc; + } + + camera->num_sensors = num_sensors; + camera->sensors = sensors; +} + +device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_callback callback) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + memset(device, 0, sizeof(device_imu_type)); + device->vendor_id = xreal_vendor_id; + device->product_id = 0; + device->callback = callback; + + if (!device_init()) { + device_imu_error("Not initialized"); + return DEVICE_IMU_ERROR_NOT_INITIALIZED; + } + + struct hid_device_info* info = hid_enumerate( + device->vendor_id, + device->product_id + ); + + struct hid_device_info* it = info; + while (it) { + int interface_id = xreal_imu_interface_id(it->product_id); + if (interface_id != -1 && it->interface_number == interface_id) { +#ifndef NDEBUG + printf("Found IMU device with product_id 0x%x on interface %d\n", it->product_id, interface_id); +#endif + device->product_id = it->product_id; + device->handle = hid_open_path(it->path); + device->max_payload_size = xreal_imu_max_payload_size(device->product_id); + break; + } + + it = it->next; + } + + hid_free_enumeration(info); + + if (!device->handle) { + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; + } + + if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || + (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + device_imu_error("Failed sending payload to stop imu data stream"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + device_imu_clear(device); + + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) { + device_imu_error("Failed sending payload to get static id"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + uint32_t static_id = 0; + if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { + device->static_id = static_id; + } else { + device->static_id = 0x20220101; + } + + device->calibration = malloc(sizeof(device_imu_calibration_type)); + memset(device->calibration, 0, sizeof(device_imu_calibration_type)); + + device_imu_reset_calibration(device); + + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { + device_imu_error("Failed sending payload to get calibration data length"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + uint32_t calibration_len = 0; + if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) { + const uint16_t max_packet_size = (device->max_payload_size - 8); + char* calibration_data = malloc(calibration_len + 1); + + uint32_t position = 0; + while (position < calibration_len) { + const uint32_t remaining = (calibration_len - position); + + if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { + break; + } + + const uint16_t next = (remaining > max_packet_size? max_packet_size : remaining); + + if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) { + break; + } + + position += next; + } + + calibration_data[calibration_len] = '\0'; + + struct json_tokener* tokener = json_tokener_new(); + struct json_object* root = json_tokener_parse_ex(tokener, calibration_data, calibration_len); + struct json_object* imu = json_object_object_get(root, "IMU"); + struct json_object* dev1 = json_object_object_get(imu, "device_1"); + + FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias")); + FusionQuaternion accel_q_gyro = json_object_get_quaternion(json_object_object_get(dev1, "accel_q_gyro")); + FusionVector gyro_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias")); + FusionQuaternion gyro_q_mag = json_object_get_quaternion(json_object_object_get(dev1, "gyro_q_mag")); + FusionVector mag_bias = json_object_get_vector(json_object_object_get(dev1, "mag_bias")); + FusionQuaternion imu_noises = json_object_get_quaternion(json_object_object_get(dev1, "imu_noises")); + FusionVector scale_accel = json_object_get_vector(json_object_object_get(dev1, "scale_accel")); + FusionVector scale_gyro = json_object_get_vector(json_object_object_get(dev1, "scale_gyro")); + FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag")); + + const FusionQuaternion accel_q_mag = FusionQuaternionMultiply(accel_q_gyro, gyro_q_mag); + + device->calibration->gyroscopeMisalignment = FusionQuaternionToMatrix(accel_q_gyro); + device->calibration->gyroscopeSensitivity = scale_gyro; + device->calibration->gyroscopeOffset = gyro_bias; + + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->accelerometerSensitivity = scale_accel; + device->calibration->accelerometerOffset = accel_bias; + + device->calibration->magnetometerMisalignment = FusionQuaternionToMatrix(accel_q_mag); + device->calibration->magnetometerSensitivity = scale_mag; + device->calibration->magnetometerOffset = mag_bias; + + device->calibration->noises = imu_noises; + + struct json_object* rgb = json_object_object_get(root, "RGB_camera"); + struct json_object* slam = json_object_object_get(root, "SLAM_camera"); + + uint32_t num_cameras_rgb = json_object_get_int(json_object_object_get(rgb, "num_of_cameras")); + uint32_t num_cameras_slam = json_object_get_int(json_object_object_get(slam, "num_of_cameras")); + + const uint32_t num_cameras = (num_cameras_rgb > 0? 1 : 0) + (num_cameras_slam > 0? 1 : 0); + + device_imu_camera_type *cameras = NULL; + uint32_t camera_index = 0; + + if (num_cameras > 0) { + cameras = malloc(sizeof(device_imu_camera_type) * num_cameras); + } + + if ((cameras) && (num_cameras_rgb > 0)) { + init_device_imu_camera(&(cameras[camera_index++]), rgb); + } + + if ((cameras) && (num_cameras_slam > 0)) { + init_device_imu_camera(&(cameras[camera_index++]), slam); + } + + device->calibration->cam.num_cameras = cameras? num_cameras : 0; + device->calibration->cam.cameras = cameras; + + json_tokener_free(tokener); + free(calibration_data); + } + + if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x1)) || + (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + device_imu_error("Failed sending payload to start imu data stream"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + const uint32_t SAMPLE_RATE = 1000; + + device->offset = malloc(sizeof(FusionOffset)); + device->ahrs = malloc(sizeof(FusionAhrs)); + + if (device->offset) { + FusionOffsetInitialise((FusionOffset*) device->offset, SAMPLE_RATE); + } + + FusionAhrsInitialise((FusionAhrs*) device->ahrs); + + const FusionAhrsSettings settings = { + .convention = FusionConventionNed, + .gain = 0.5f, + .accelerationRejection = 10.0f, + .magneticRejection = 20.0f, + .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */ + }; + + FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings); + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_error_type device_imu_reset_calibration(device_imu_type* device) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->calibration) { + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->gyroscopeSensitivity = FUSION_VECTOR_ONES; + device->calibration->gyroscopeOffset = FUSION_VECTOR_ZERO; + + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES; + device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO; + + device->calibration->magnetometerMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES; + device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO; + + device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX; + device->calibration->hardIronOffset = FUSION_VECTOR_ZERO; + + device->calibration->noises = FUSION_IDENTITY_QUATERNION; + device->calibration->noises.element.w = 0.0f; + + if (device->calibration->cam.cameras) { + for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) { + if (!device->calibration->cam.cameras[i].sensors) { + continue; + } + + for (uint32_t j = 0; j < device->calibration->cam.cameras[i].num_sensors; j++) { + if (!device->calibration->cam.cameras[i].sensors[j].kc) { + continue; + } + + free(device->calibration->cam.cameras[i].sensors[j].kc); + + device->calibration->cam.cameras[i].sensors[j].num_kc = 0; + device->calibration->cam.cameras[i].sensors[j].kc = NULL; + } + + free(device->calibration->cam.cameras[i].sensors); + + device->calibration->cam.cameras[i].num_sensors = 0; + device->calibration->cam.cameras[i].sensors = NULL; + } + + free(device->calibration->cam.cameras); + } + + device->calibration->cam.num_cameras = 0; + device->calibration->cam.cameras = NULL; +} + +device_imu_error_type device_imu_load_calibration(device_imu_type* device, const char* path) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->calibration) { + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + FILE* file = fopen(path, "rb"); + if (!file) { + device_imu_error("No file opened"); + return DEVICE_IMU_ERROR_FILE_NOT_OPEN; + } + + device_imu_error_type result = DEVICE_IMU_ERROR_NO_ERROR; + const size_t calibration_size = ( + sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) + ); + + size_t count; + count = fread(device->calibration, 1, calibration_size, file); + + if (calibration_size != count) { + device_imu_error("Not fully loaded"); + result = DEVICE_IMU_ERROR_LOADING_FAILED; + } + + if (0 != fclose(file)) { + device_imu_error("No file closed"); + return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; + } + + return result; +} + +device_imu_error_type device_imu_save_calibration(device_imu_type* device, const char* path) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->calibration) { + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + FILE* file = fopen(path, "wb"); + if (!file) { + device_imu_error("No file opened"); + return DEVICE_IMU_ERROR_FILE_NOT_OPEN; + } + + device_imu_error_type result = DEVICE_IMU_ERROR_NO_ERROR; + const size_t calibration_size = ( + sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) + ); + + size_t count; + count = fwrite(device->calibration, 1, calibration_size, file); + + if (calibration_size != count) { + device_imu_error("Not fully saved"); + result = DEVICE_IMU_ERROR_SAVING_FAILED; + } + + if (0 != fclose(file)) { + device_imu_error("No file closed"); + return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; + } + + return result; +} + +static void device_imu_callback(device_imu_type* device, + uint64_t timestamp, + device_imu_event_type event) { + if (!device->callback) { + return; + } + + device->callback(timestamp, event, device->ahrs); +} + +static int32_t pack32bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16) | (data[3] << 24); + return ((int32_t) unsigned_value); +} + +static int32_t pack24bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16); + if ((data[2] & 0x80) != 0) unsigned_value |= (0xFF << 24); + return ((int32_t) unsigned_value); +} + +static int16_t pack16bit_signed(const uint8_t* data) { + uint16_t unsigned_value = (data[1] << 8) | (data[0]); + return (int16_t) unsigned_value; +} + +static int32_t pack32bit_signed_swap(const uint8_t* data) { + uint32_t unsigned_value = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | (data[3]); + return ((int32_t) unsigned_value); +} + +static int16_t pack16bit_signed_swap(const uint8_t* data) { + uint16_t unsigned_value = (data[0] << 8) | (data[1]); + 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 device_imu_packet_type* packet, + FusionVector* gyroscope, + FusionVector* accelerometer, + FusionVector* magnetometer) { + int32_t vel_m = pack16bit_signed(packet->angular_multiplier); + int32_t vel_d = pack32bit_signed(packet->angular_divisor); + + int32_t vel_x = pack24bit_signed(packet->angular_velocity_x); + int32_t vel_y = pack24bit_signed(packet->angular_velocity_y); + int32_t vel_z = pack24bit_signed(packet->angular_velocity_z); + + gyroscope->axis.x = (float) vel_x * (float) vel_m / (float) vel_d; + gyroscope->axis.y = (float) vel_y * (float) vel_m / (float) vel_d; + gyroscope->axis.z = (float) vel_z * (float) vel_m / (float) vel_d; + + int32_t accel_m = pack16bit_signed(packet->acceleration_multiplier); + int32_t accel_d = pack32bit_signed(packet->acceleration_divisor); + + int32_t accel_x = pack24bit_signed(packet->acceleration_x); + int32_t accel_y = pack24bit_signed(packet->acceleration_y); + int32_t accel_z = pack24bit_signed(packet->acceleration_z); + + accelerometer->axis.x = (float) accel_x * (float) accel_m / (float) accel_d; + accelerometer->axis.y = (float) accel_y * (float) accel_m / (float) accel_d; + accelerometer->axis.z = (float) accel_z * (float) accel_m / (float) accel_d; + + 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_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; + magnetometer->axis.z = (float) magnet_z * (float) magnet_m / (float) magnet_d; +} + +#define min(x, y) ((x) < (y)? (x) : (y)) +#define max(x, y) ((x) > (y)? (x) : (y)) + +static void pre_biased_coordinate_system(FusionVector* v) { + *v = FusionAxesSwap(*v, FusionAxesAlignmentNXNZNY); +} + +static void post_biased_coordinate_system(const FusionVector* v, FusionVector* res) { + *res = FusionAxesSwap(*v, FusionAxesAlignmentPZPXPY); +} + +static void iterate_iron_offset_estimation(const FusionVector* magnetometer, FusionMatrix* softIronMatrix, FusionVector* hardIronOffset) { + static FusionVector max = { FLT_MIN, FLT_MIN, FLT_MIN }; + static FusionVector min = { FLT_MAX, FLT_MAX, FLT_MAX }; + + for (int i = 0; i < 3; i++) { + max.array[i] = max(max.array[i], magnetometer->array[i]); + min.array[i] = min(min.array[i], magnetometer->array[i]); + } + + const float mx = (max.axis.x - min.axis.x) / 2.0f; + const float my = (max.axis.y - min.axis.y) / 2.0f; + const float mz = (max.axis.z - min.axis.z) / 2.0f; + + const float cx = (min.axis.x + max.axis.x) / 2.0f; + const float cy = (min.axis.y + max.axis.y) / 2.0f; + const float cz = (min.axis.z + max.axis.z) / 2.0f; + + memset(softIronMatrix, 0, sizeof(*softIronMatrix)); + + softIronMatrix->element.xx = 1.0f / mx; + softIronMatrix->element.yy = 1.0f / my; + softIronMatrix->element.zz = 1.0f / mz; + + hardIronOffset->axis.x = cx; + hardIronOffset->axis.y = cy; + hardIronOffset->axis.z = cz; +} + +static void apply_calibration(const device_imu_type* device, + FusionVector* gyroscope, + FusionVector* accelerometer, + FusionVector* magnetometer) { + FusionMatrix gyroscopeMisalignment; + FusionVector gyroscopeSensitivity; + FusionVector gyroscopeOffset; + + FusionMatrix accelerometerMisalignment; + FusionVector accelerometerSensitivity; + FusionVector accelerometerOffset; + + FusionMatrix magnetometerMisalignment; + FusionVector magnetometerSensitivity; + FusionVector magnetometerOffset; + + FusionMatrix softIronMatrix; + FusionVector hardIronOffset; + + if (device->calibration) { + gyroscopeMisalignment = device->calibration->gyroscopeMisalignment; + gyroscopeSensitivity = device->calibration->gyroscopeSensitivity; + gyroscopeOffset = device->calibration->gyroscopeOffset; + + accelerometerMisalignment = device->calibration->accelerometerMisalignment; + accelerometerSensitivity = device->calibration->accelerometerSensitivity; + accelerometerOffset = device->calibration->accelerometerOffset; + + magnetometerMisalignment = device->calibration->magnetometerMisalignment; + magnetometerSensitivity = device->calibration->magnetometerSensitivity; + magnetometerOffset = device->calibration->magnetometerOffset; + + softIronMatrix = device->calibration->softIronMatrix; + hardIronOffset = device->calibration->hardIronOffset; + } else { + gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; + gyroscopeSensitivity = FUSION_VECTOR_ONES; + gyroscopeOffset = FUSION_VECTOR_ZERO; + + accelerometerMisalignment = FUSION_IDENTITY_MATRIX; + accelerometerSensitivity = FUSION_VECTOR_ONES; + accelerometerOffset = FUSION_VECTOR_ZERO; + + magnetometerMisalignment = FUSION_IDENTITY_MATRIX; + magnetometerSensitivity = FUSION_VECTOR_ONES; + magnetometerOffset = FUSION_VECTOR_ZERO; + + softIronMatrix = FUSION_IDENTITY_MATRIX; + hardIronOffset = FUSION_VECTOR_ZERO; + } + + gyroscopeOffset = FusionVectorMultiplyScalar( + gyroscopeOffset, + FusionRadiansToDegrees(1.0f) + ); + + accelerometerOffset = FusionVectorMultiplyScalar( + accelerometerOffset, + 1.0f / GRAVITY_G + ); + + FusionVector g = *gyroscope; + FusionVector a = *accelerometer; + FusionVector m = *magnetometer; + + pre_biased_coordinate_system(&g); + pre_biased_coordinate_system(&a); + pre_biased_coordinate_system(&m); + + g = FusionCalibrationInertial( + g, + gyroscopeMisalignment, + gyroscopeSensitivity, + gyroscopeOffset + ); + + a = FusionCalibrationInertial( + a, + accelerometerMisalignment, + accelerometerSensitivity, + accelerometerOffset + ); + + m = FusionCalibrationInertial( + m, + magnetometerMisalignment, + magnetometerSensitivity, + magnetometerOffset + ); + + iterate_iron_offset_estimation( + &m, + &softIronMatrix, + &hardIronOffset + ); + + if (device->calibration) { + device->calibration->softIronMatrix = softIronMatrix; + device->calibration->hardIronOffset = hardIronOffset; + } + + m = FusionCalibrationMagnetic( + m, + softIronMatrix, + hardIronOffset + ); + + post_biased_coordinate_system(&g, gyroscope); + post_biased_coordinate_system(&a, accelerometer); + post_biased_coordinate_system(&m, magnetometer); +} + +device_imu_error_type device_imu_clear(device_imu_type* device) { + return device_imu_read(device, 10); +} + +device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; + } + + if (!device->calibration) { + device_imu_error("No calibration allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + if (sizeof(device_imu_packet_type) > device->max_payload_size) { + device_imu_error("Not proper size"); + return DEVICE_IMU_ERROR_WRONG_SIZE; + } + + device_imu_packet_type packet; + int transferred; + + bool initialized = false; + + FusionVector cal_gyroscope; + FusionVector cal_accelerometer; + + FusionMatrix softIronMatrix; + FusionVector hardIronOffset; + + const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f; + + FusionVector prev_accel; + while (iterations > 0) { + memset(&packet, 0, sizeof(device_imu_packet_type)); + + transferred = hid_read( + device->handle, + (uint8_t*) &packet, + sizeof(device_imu_packet_type) + ); + + if (transferred == -1) { + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; + } + + if (transferred == 0) { + continue; + } + + if (sizeof(device_imu_packet_type) != transferred) { + device_imu_error("Unexpected packet size"); + return DEVICE_IMU_ERROR_UNEXPECTED; + } + + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { + continue; + } + + FusionVector gyroscope; + FusionVector accelerometer; + FusionVector magnetometer; + + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); + + pre_biased_coordinate_system(&gyroscope); + pre_biased_coordinate_system(&accelerometer); + pre_biased_coordinate_system(&magnetometer); + + if (initialized) { + cal_gyroscope = FusionVectorAdd(cal_gyroscope, gyroscope); + cal_accelerometer = FusionVectorAdd(cal_accelerometer, FusionVectorSubtract(accelerometer, prev_accel)); + } else { + cal_gyroscope = gyroscope; + cal_accelerometer = FUSION_VECTOR_ZERO; + } + + prev_accel = accelerometer; + + iterate_iron_offset_estimation( + &magnetometer, + &softIronMatrix, + &hardIronOffset + ); + + iterations--; + } + + if (factor > 0.0f) { + if (gyro) { + device->calibration->gyroscopeOffset = FusionVectorAdd( + device->calibration->gyroscopeOffset, + FusionVectorMultiplyScalar( + cal_gyroscope, + FusionDegreesToRadians(factor) + ) + ); + } + + if (accel) { + device->calibration->accelerometerOffset = FusionVectorAdd( + device->calibration->accelerometerOffset, + FusionVectorMultiplyScalar( + cal_accelerometer, + factor * GRAVITY_G + ) + ); + } + + if (magnet) { + device->calibration->softIronMatrix = softIronMatrix; + device->calibration->hardIronOffset = hardIronOffset; + } + } + + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; + } + + if (sizeof(device_imu_packet_type) > device->max_payload_size) { + device_imu_error("Not proper size"); + return DEVICE_IMU_ERROR_WRONG_SIZE; + } + + device_imu_packet_type packet; + memset(&packet, 0, sizeof(device_imu_packet_type)); + + int transferred = hid_read_timeout( + device->handle, + (uint8_t*) &packet, + sizeof(device_imu_packet_type), + timeout + ); + + if (transferred == -1) { + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; + } + + if (transferred == 0) { + return DEVICE_IMU_ERROR_NO_ERROR; + } + + if (sizeof(device_imu_packet_type) != transferred) { + device_imu_error("Unexpected packet size"); + return DEVICE_IMU_ERROR_UNEXPECTED; + } + + const uint64_t timestamp = le64toh(packet.timestamp); + + if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT); + return DEVICE_IMU_ERROR_NO_ERROR; + } + + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { + device_imu_error("Not matching signature"); + return DEVICE_IMU_ERROR_WRONG_SIGNATURE; + } + + const uint64_t delta = timestamp - device->last_timestamp; + const float deltaTime = (float) ((double) delta / 1e9); + + device->last_timestamp = timestamp; + + int16_t temperature = pack16bit_signed(packet.temperature); + + // According to the ICM-42688-P datasheet: (offset: 25 °C, sensitivity: 132.48 LSB/°C) + device->temperature = ((float) temperature) / 132.48f + 25.0f; + + FusionVector gyroscope; + FusionVector accelerometer; + FusionVector magnetometer; + + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); + apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); + + if (device->offset) { + gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope); + } + +#ifndef NDEBUG + printf("G: %.2f %.2f %.2f\n", gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z); + printf("A: %.2f %.2f %.2f\n", accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z); + printf("M: %.2f %.2f %.2f\n", magnetometer.axis.x, magnetometer.axis.y, magnetometer.axis.z); +#endif + + if (device->ahrs) { + if (isnan(magnetometer.axis.x) || isnan(magnetometer.axis.x) || isnan(magnetometer.axis.x)) { + FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + } else { + /* The magnetometer seems to make results of sensor fusion generally worse. So it is not used currently. */ + // FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); + FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + } + + const device_imu_quat_type orientation = device_imu_get_orientation(device->ahrs); + + // TODO: fix detection of this case; quat.x as a nan value is only a side-effect of some issue with ahrs or + // the gyro/accel/magnet readings + if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) { + device_imu_error("Invalid orientation reading"); + return DEVICE_IMU_ERROR_INVALID_VALUE; + } + } + + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE); + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_vec3_type device_imu_get_earth_acceleration(const device_imu_ahrs_type* ahrs) { + FusionVector acceleration = ahrs? FusionAhrsGetEarthAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; + device_imu_vec3_type a; + a.x = acceleration.axis.x; + a.y = acceleration.axis.y; + a.z = acceleration.axis.z; + return a; +} + +device_imu_vec3_type device_imu_get_linear_acceleration(const device_imu_ahrs_type* ahrs) { + FusionVector acceleration = ahrs? FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; + device_imu_vec3_type a; + a.x = acceleration.axis.x; + a.y = acceleration.axis.y; + a.z = acceleration.axis.z; + return a; +} + +device_imu_quat_type device_imu_get_orientation(const device_imu_ahrs_type* ahrs) { + FusionQuaternion quaternion = ahrs? FusionAhrsGetQuaternion((const FusionAhrs*) ahrs) : FUSION_IDENTITY_QUATERNION; + device_imu_quat_type q; + q.x = quaternion.element.x; + q.y = quaternion.element.y; + q.z = quaternion.element.z; + q.w = quaternion.element.w; + return q; +} + +device_imu_euler_type device_imu_get_euler(device_imu_quat_type quat) { + FusionQuaternion quaternion; + quaternion.element.x = quat.x; + quaternion.element.y = quat.y; + quaternion.element.z = quat.z; + quaternion.element.w = quat.w; + FusionEuler euler = FusionQuaternionToEuler(quaternion); + device_imu_euler_type e; + e.roll = euler.angle.roll; + e.pitch = euler.angle.pitch; + e.yaw = euler.angle.yaw; + return e; +} + +uint32_t device_imu_get_num_of_cameras(device_imu_type *device) { + if (!device->calibration) { + return 0; + } + + return device->calibration->cam.num_cameras; +} + +const device_imu_camera_type* device_imu_get_camera(const device_imu_type *device, uint32_t index) { + if ((!device->calibration) || (!device->calibration->cam.cameras)) { + return NULL; + } + + return &(device->calibration->cam.cameras[index]); +} + +uint32_t device_imu_camera_get_num_of_sensors(const device_imu_camera_type *camera) { + if (!camera) { + return 0; + } + + return camera->num_sensors; +} + +const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_imu_camera_type *camera, uint32_t index) { + if (!camera->sensors) { + return NULL; + } + + return &(camera->sensors[index]); +} + +device_imu_mat3x3_type device_imu_sensor_get_rotation(const device_imu_camera_sensor_type *sensor) { + device_imu_mat3x3_type rotation; + rotation.m[0] = sensor->cameraMisalignment.array[0][0]; + rotation.m[1] = sensor->cameraMisalignment.array[0][1]; + rotation.m[2] = sensor->cameraMisalignment.array[0][2]; + rotation.m[3] = sensor->cameraMisalignment.array[1][0]; + rotation.m[4] = sensor->cameraMisalignment.array[1][1]; + rotation.m[5] = sensor->cameraMisalignment.array[1][2]; + rotation.m[6] = sensor->cameraMisalignment.array[2][0]; + rotation.m[7] = sensor->cameraMisalignment.array[2][1]; + rotation.m[8] = sensor->cameraMisalignment.array[2][2]; + return rotation; +} + +device_imu_vec3_type device_imu_sensor_get_position(const device_imu_camera_sensor_type *sensor) { + device_imu_vec3_type position; + position.x = sensor->cameraOffset.axis.x; + position.y = sensor->cameraOffset.axis.y; + position.z = sensor->cameraOffset.axis.z; + return position; +} + +device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor) { + device_imu_size_type resolution; + resolution.width = sensor->resolution[0]; + resolution.height = sensor->resolution[1]; + return resolution; +} + +device_imu_vec2_type device_imu_sensor_get_cc(const device_imu_camera_sensor_type *sensor) { + device_imu_vec2_type cc; + cc.x = sensor->cc[0]; + cc.y = sensor->cc[1]; + return cc; +} + +device_imu_vec2_type device_imu_sensor_get_fc(const device_imu_camera_sensor_type *sensor) { + device_imu_vec2_type fc; + fc.x = sensor->fc[0]; + fc.y = sensor->fc[1]; + return fc; +} + +device_imu_error_type device_imu_sensor_get_kc(const device_imu_camera_sensor_type *sensor, uint32_t *num_kc, float *kc) { + if ((!sensor) || (!num_kc)) { + device_imu_error("Wrong argument"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + if (!kc) { + *num_kc = sensor->num_kc; + } else { + uint32_t n = *num_kc; + + if (sensor->num_kc < n) { + n = sensor->num_kc; + } + + for (uint32_t i = 0; i < *num_kc; i++) { + kc[i] = sensor->kc[i]; + } + } + + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_error_type device_imu_close(device_imu_type* device) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (device->calibration) { + if (device->calibration->cam.cameras) { + for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) { + if (!device->calibration->cam.cameras[i].sensors) { + continue; + } + + for (uint32_t j = 0; j < device->calibration->cam.cameras[i].num_sensors; j++) { + if (!device->calibration->cam.cameras[i].sensors[j].kc) { + continue; + } + + free(device->calibration->cam.cameras[i].sensors[j].kc); + } + + free(device->calibration->cam.cameras[i].sensors); + } + + free(device->calibration->cam.cameras); + } + + free(device->calibration); + } + + if (device->ahrs) { + free(device->ahrs); + } + + if (device->offset) { + free(device->offset); + } + + if (device->handle) { + if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || + (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + device_imu_error("Failed sending payload to stop imu data stream"); + } + + hid_close(device->handle); + } + + memset(device, 0, sizeof(device_imu_type)); + device_exit(); + + return DEVICE_IMU_ERROR_NO_ERROR; +} diff --git a/ardriver/xreal/device_imu.h b/ardriver/xreal/device_imu.h new file mode 100644 index 0000000..85ceb6e --- /dev/null +++ b/ardriver/xreal/device_imu.h @@ -0,0 +1,227 @@ +#pragma once +// +// Created by thejackimonster on 30.03.23. +// +// Copyright (c) 2023-2025 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH 0x14 +#define DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT 0x15 +#define DEVICE_IMU_MSG_ALLOCATE_CAL_DATA_BUFFER 0x16 +#define DEVICE_IMU_MSG_WRITE_CAL_DATA_SEGMENT 0x17 +#define DEVICE_IMU_MSG_FREE_CAL_BUFFER 0x18 +#define DEVICE_IMU_MSG_START_IMU_DATA 0x19 +#define DEVICE_IMU_MSG_GET_STATIC_ID 0x1A +#define DEVICE_IMU_MSG_UNKNOWN 0x1D + +#ifdef __cplusplus +extern "C" { +#endif + +enum device_imu_error_t { + DEVICE_IMU_ERROR_NO_ERROR = 0, + DEVICE_IMU_ERROR_NO_DEVICE = 1, + DEVICE_IMU_ERROR_NO_HANDLE = 2, + DEVICE_IMU_ERROR_NO_ALLOCATION = 3, + DEVICE_IMU_ERROR_WRONG_SIZE = 4, + DEVICE_IMU_ERROR_FILE_NOT_OPEN = 5, + DEVICE_IMU_ERROR_FILE_NOT_CLOSED = 6, + DEVICE_IMU_ERROR_LOADING_FAILED = 7, + DEVICE_IMU_ERROR_SAVING_FAILED = 8, + DEVICE_IMU_ERROR_UNPLUGGED = 9, + DEVICE_IMU_ERROR_UNEXPECTED = 10, + DEVICE_IMU_ERROR_WRONG_SIGNATURE = 11, + DEVICE_IMU_ERROR_INVALID_VALUE = 12, + DEVICE_IMU_ERROR_NOT_INITIALIZED = 13, + DEVICE_IMU_ERROR_PAYLOAD_FAILED = 14, + DEVICE_IMU_ERROR_UNKNOWN = 15, +}; + +struct __attribute__((__packed__)) device_imu_packet_t { + uint8_t signature [2]; + uint8_t temperature [2]; + uint64_t timestamp; + uint8_t angular_multiplier [2]; + uint8_t angular_divisor [4]; + uint8_t angular_velocity_x [3]; + uint8_t angular_velocity_y [3]; + uint8_t angular_velocity_z [3]; + uint8_t acceleration_multiplier [2]; + uint8_t acceleration_divisor [4]; + uint8_t acceleration_x [3]; + uint8_t acceleration_y [3]; + uint8_t acceleration_z [3]; + uint8_t magnetic_multiplier [2]; + uint8_t magnetic_divisor [4]; + uint8_t magnetic_x [2]; + uint8_t magnetic_y [2]; + uint8_t magnetic_z [2]; + uint32_t checksum; + uint8_t _padding [6]; +}; + +enum device_imu_event_t { + DEVICE_IMU_EVENT_UNKNOWN = 0, + DEVICE_IMU_EVENT_INIT = 1, + DEVICE_IMU_EVENT_UPDATE = 2, +}; + +struct device_imu_ahrs_t; +struct device_imu_camera_sensor_t; +struct device_imu_camera_t; +struct device_imu_camera_calibration_t; +struct device_imu_calibration_t; + +struct device_imu_vec2_t { + float x; + float y; +}; + +struct device_imu_vec3_t { + float x; + float y; + float z; +}; + +struct device_imu_quat_t { + float x; + float y; + float z; + float w; +}; + +struct device_imu_euler_t { + float roll; + float pitch; + float yaw; +}; + +struct device_imu_mat3x3_t { + float m [9]; +}; + +struct device_imu_size_t { + uint16_t width; + uint16_t height; +}; + +typedef enum device_imu_error_t device_imu_error_type; +typedef struct device_imu_packet_t device_imu_packet_type; +typedef enum device_imu_event_t device_imu_event_type; + +typedef struct device_imu_ahrs_t device_imu_ahrs_type; + +typedef struct device_imu_camera_sensor_t device_imu_camera_sensor_type; +typedef struct device_imu_camera_t device_imu_camera_type; +typedef struct device_imu_camera_calibration_t device_imu_camera_calibration_type; +typedef struct device_imu_calibration_t device_imu_calibration_type; + +typedef struct device_imu_vec2_t device_imu_vec2_type; +typedef struct device_imu_vec3_t device_imu_vec3_type; +typedef struct device_imu_quat_t device_imu_quat_type; +typedef struct device_imu_euler_t device_imu_euler_type; +typedef struct device_imu_mat3x3_t device_imu_mat3x3_type; +typedef struct device_imu_size_t device_imu_size_type; + +typedef void (*device_imu_event_callback)( + uint64_t timestamp, + device_imu_event_type event, + const device_imu_ahrs_type* ahrs +); + +struct device_imu_t { + uint16_t vendor_id; + uint16_t product_id; + + void* handle; + uint16_t max_payload_size; + + uint32_t static_id; + + uint64_t last_timestamp; + float temperature; // (in °C) + + void* offset; + device_imu_ahrs_type* ahrs; + + device_imu_event_callback callback; + device_imu_calibration_type* calibration; +}; + +typedef struct device_imu_t device_imu_type; + +device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_callback callback); + +device_imu_error_type device_imu_reset_calibration(device_imu_type* device); + +device_imu_error_type device_imu_load_calibration(device_imu_type* device, const char* path); + +device_imu_error_type device_imu_save_calibration(device_imu_type* device, const char* path); + +device_imu_error_type device_imu_clear(device_imu_type* device); + +device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet); + +device_imu_error_type device_imu_read(device_imu_type* device, int timeout); + +device_imu_vec3_type device_imu_get_earth_acceleration(const device_imu_ahrs_type* ahrs); + +device_imu_vec3_type device_imu_get_linear_acceleration(const device_imu_ahrs_type* ahrs); + +device_imu_quat_type device_imu_get_orientation(const device_imu_ahrs_type* ahrs); + +device_imu_euler_type device_imu_get_euler(device_imu_quat_type quat); + +uint32_t device_imu_get_num_of_cameras(device_imu_type *device); + +const device_imu_camera_type* device_imu_get_camera(const device_imu_type *device, uint32_t index); + +uint32_t device_imu_camera_get_num_of_sensors(const device_imu_camera_type *camera); + +const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_imu_camera_type *camera, uint32_t index); + +device_imu_mat3x3_type device_imu_sensor_get_rotation(const device_imu_camera_sensor_type *sensor); + +device_imu_vec3_type device_imu_sensor_get_position(const device_imu_camera_sensor_type *sensor); + +device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor); + +device_imu_vec2_type device_imu_sensor_get_cc(const device_imu_camera_sensor_type *sensor); + +device_imu_vec2_type device_imu_sensor_get_fc(const device_imu_camera_sensor_type *sensor); + +device_imu_error_type device_imu_sensor_get_kc(const device_imu_camera_sensor_type *sensor, uint32_t *num_kc, float *kc); + +device_imu_error_type device_imu_close(device_imu_type* device); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/device_mcu.c b/ardriver/xreal/device_mcu.c new file mode 100644 index 0000000..551617d --- /dev/null +++ b/ardriver/xreal/device_mcu.c @@ -0,0 +1,731 @@ +// +// Created by thejackimonster on 29.03.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "device_mcu.h" +#include "device.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "crc32.h" +#include "hid_ids.h" + +#ifndef NDEBUG +#define device_mcu_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#else +#define device_mcu_error(msg) (0) +#endif + +#define device_mcu_warning(msg) device_mcu_error(msg) + +#define MAX_PACKET_SIZE 64 +#define PACKET_HEAD 0xFD + +static bool send_payload(device_mcu_type* device, uint8_t size, const uint8_t* payload) { + int payload_size = size; + if (payload_size > MAX_PACKET_SIZE) { + payload_size = MAX_PACKET_SIZE; + } + + int transferred = hid_write(device->handle, payload, payload_size); + + if (transferred != payload_size) { + device_mcu_error("Sending payload failed"); + return false; + } + + return (transferred == size); +} + +static bool recv_payload(device_mcu_type* device, uint8_t size, uint8_t* payload) { + int payload_size = size; + if (payload_size > MAX_PACKET_SIZE) { + payload_size = MAX_PACKET_SIZE; + } + + int transferred = hid_read(device->handle, payload, payload_size); + + if (transferred >= payload_size) { + transferred = payload_size; + } + + if (transferred == 0) { + return false; + } + + if (transferred != payload_size) { + device_mcu_error("Receiving payload failed"); + return false; + } + + return (transferred == size); +} + +static bool send_payload_action(device_mcu_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) { + static device_mcu_packet_type packet; + + const uint16_t packet_len = 17 + len; + const uint16_t payload_len = 5 + packet_len; + + packet.head = PACKET_HEAD; + packet.length = htole16(packet_len); + packet.timestamp = htole64(0); + packet.msgid = htole16(msgid); + memset(packet.reserved, 0, 5); + + memcpy(packet.data, data, len); + packet.checksum = htole32( + crc32_checksum( + (const uint8_t*) (&packet.length), + packet.length + ) + ); + + return send_payload(device, payload_len, (uint8_t*) (&packet)); +} + +static bool recv_payload_msg(device_mcu_type* device, uint16_t msgid, uint8_t len, uint8_t* data) { + static device_mcu_packet_type packet; + + packet.head = 0; + packet.length = 0; + packet.msgid = 0; + + const uint16_t packet_len = 18 + len; + const uint16_t payload_len = 5 + packet_len; + + if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { + return false; + } + + if (packet.head != PACKET_HEAD) { + device_mcu_error("Invalid payload received"); + return false; + } + + if (le16toh(packet.msgid) != msgid) { + device_mcu_error("Unexpected payload received"); + return false; + } + + const uint8_t status = packet.data[0]; + + if (status != 0) { + device_mcu_error("Payload status failed"); + return false; + } + + const uint16_t data_len = le16toh(packet.length) - 18; + + if (len <= data_len) { + memcpy(data, packet.data + 1, len); + } else { + memcpy(data, packet.data + 1, data_len); + memset(data + data_len, 0, len - data_len); + } + + return true; +} + +static bool do_payload_action(device_mcu_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) { + if (!send_payload_action(device, msgid, len, data)) { + return false; + } + + const uint16_t attempts_per_second = (device->active? 60 : 1); + + uint16_t attempts = attempts_per_second * 5; + + while (attempts > 0) { + if (recv_payload_msg(device, msgid, 0, NULL)) { + return true; + } + + attempts--; + } + + return false; +} + +device_mcu_error_type device_mcu_open(device_mcu_type* device, device_mcu_event_callback callback) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + memset(device, 0, sizeof(device_mcu_type)); + device->vendor_id = xreal_vendor_id; + device->product_id = 0; + device->callback = callback; + + if (!device_init()) { + device_mcu_error("Not initialized"); + return DEVICE_MCU_ERROR_NOT_INITIALIZED; + } + + struct hid_device_info* info = hid_enumerate( + device->vendor_id, + device->product_id + ); + + struct hid_device_info* it = info; + while (it) { + int interface_id = xreal_mcu_interface_id(it->product_id); + if (interface_id != -1 && it->interface_number == interface_id) { +#ifndef NDEBUG + printf("Found MCU device with product_id 0x%x on interface %d\n", it->product_id, interface_id); +#endif + device->product_id = it->product_id; + device->handle = hid_open_path(it->path); + break; + } + + it = it->next; + } + + hid_free_enumeration(info); + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + device_mcu_clear(device); + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 0, NULL)) { + device_mcu_error("Requesting activation time failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + uint8_t activated; + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 1, &activated)) { + device_mcu_error("Receiving activation time failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + device->activated = (activated != 0); + + if (!device->activated) { + device_mcu_warning("Device is not activated"); + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) { + device_mcu_error("Requesting current MCU app firmware version"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) { + device_mcu_error("Receiving current MCU app firmware version failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DP7911_FW_VERSION, 0, NULL)) { + device_mcu_error("Requesting current DP firmware version"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) { + device_mcu_error("Receiving current DP firmware version failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) { + device_mcu_error("Requesting current DSP app firmware version"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) { + device_mcu_error("Receiving current DSP app firmware version failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + +#ifndef NDEBUG + printf("MCU: %s\n", device->mcu_app_fw_version); + printf("DP: %s\n", device->dp_fw_version); + printf("DSP: %s\n", device->dsp_fw_version); +#endif + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_BRIGHTNESS, 0, NULL)) { + device_mcu_error("Requesting initial brightness failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_BRIGHTNESS, 1, &device->brightness)) { + device_mcu_error("Receiving initial brightness failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DISP_MODE, 0, NULL)) { + device_mcu_error("Requesting display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DISP_MODE, 1, &device->disp_mode)) { + device_mcu_error("Receiving display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + +#ifndef NDEBUG + printf("Brightness: %d\n", device->brightness); + printf("Disp-Mode: %d\n", device->disp_mode); +#endif + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +static void device_mcu_callback(device_mcu_type* device, + uint64_t timestamp, + device_mcu_event_type event, + uint8_t brightness, + const char* msg) { + if (!device->callback) { + return; + } + + device->callback(timestamp, event, brightness, msg); +} + +device_mcu_error_type device_mcu_clear(device_mcu_type* device) { + return device_mcu_read(device, 10); +} + +device_mcu_error_type device_mcu_read(device_mcu_type* device, int timeout) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (MAX_PACKET_SIZE != sizeof(device_mcu_packet_type)) { + device_mcu_error("Not proper size"); + return DEVICE_MCU_ERROR_WRONG_SIZE; + } + + device_mcu_packet_type packet; + memset(&packet, 0, sizeof(device_mcu_packet_type)); + + int transferred = hid_read_timeout( + device->handle, + (uint8_t*) &packet, + MAX_PACKET_SIZE, + timeout + ); + + if (transferred == -1) { + device_mcu_error("Device may be unplugged"); + return DEVICE_MCU_ERROR_UNPLUGGED; + } + + if (transferred == 0) { + return DEVICE_MCU_ERROR_NO_ERROR; + } + + if (MAX_PACKET_SIZE != transferred) { + device_mcu_error("Unexpected packet size"); + return DEVICE_MCU_ERROR_UNEXPECTED; + } + + if (packet.head != PACKET_HEAD) { + device_mcu_error("Wrong packet head"); + return DEVICE_MCU_ERROR_WRONG_HEAD; + } + + const uint32_t timestamp = le32toh(packet.timestamp); + const uint16_t msgid = le16toh(packet.msgid); + const uint16_t length = le16toh(packet.length); + + const size_t data_len = (size_t) &(packet.data) - (size_t) &(packet.length); + +#ifndef NDEBUG + printf("MSG: %d = %04x (%d)\n", msgid, msgid, length); + + if (length > 11) { + for (int i = 0; i < length - 11; i++) { + printf("%02x ", packet.data[i]); + } + + printf("\n"); + } +#endif + + switch (msgid) { + case DEVICE_MCU_MSG_P_START_HEARTBEAT: { + break; + } + case DEVICE_MCU_MSG_P_DISPLAY_TOGGLED: { + const uint8_t value = packet.data[0]; + + device->active = value; + + if (device->active) { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_ON, + device->brightness, + NULL + ); + } else { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_OFF, + device->brightness, + NULL + ); + } + break; + } + case DEVICE_MCU_MSG_P_BUTTON_PRESSED: { + const uint8_t phys_button = packet.data[0]; + const uint8_t virt_button = packet.data[4]; + const uint8_t value = packet.data[8]; + + switch (virt_button) { + case DEVICE_MCU_BUTTON_VIRT_DISPLAY_TOGGLE: + device->active = value; + + if (device->active) { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_ON, + device->brightness, + NULL + ); + } else { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_OFF, + device->brightness, + NULL + ); + } + break; + case DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_UP: + device->brightness = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_BRIGHTNESS_UP, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_DOWN: + device->brightness = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_BRIGHTNESS_DOWN, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_UP: + if (device->control_mode == DEVICE_MCU_CONTROL_MODE_VOLUME) + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_VOLUME_UP, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_DOWN: + if (device->control_mode == DEVICE_MCU_CONTROL_MODE_VOLUME) + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_VOLUME_DOWN, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_MODE_2D: + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_DISPLAY_MODE_2D, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_MODE_3D: + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_DISPLAY_MODE_3D, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_BLEND_CYCLE: + device->blend_state = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_BLEND_CYCLE, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_CONTROL_TOGGLE: + device->control_mode = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_CONTROL_TOGGLE, + device->brightness, + NULL + ); + break; + default: + break; + } + + break; + } + case DEVICE_MCU_MSG_P_ASYNC_TEXT_LOG: { + const char* text = packet.text; + const size_t text_len = strlen(text); + + device->active = true; + + if (data_len + text_len != length) { + device_mcu_error("Not matching length"); + return DEVICE_MCU_ERROR_INVALID_LENGTH; + } + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_MESSAGE, + device->brightness, + text + ); + break; + } + case DEVICE_MCU_MSG_P_END_HEARTBEAT: { + break; + } + default: + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_UNKNOWN, + device->brightness, + NULL + ); + break; + } + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +device_mcu_error_type device_mcu_poll_display_mode(device_mcu_type* device) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DISP_MODE, 0, NULL)) { + device_mcu_error("Requesting display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DISP_MODE, 1, &device->disp_mode)) { + device_mcu_error("Receiving display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +device_mcu_error_type device_mcu_update_display_mode(device_mcu_type* device) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_DISP_MODE, 1, &device->disp_mode)) { + device_mcu_error("Sending display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +device_mcu_error_type device_mcu_update_firmware(device_mcu_type* device, const char* path) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (!device->activated) { + device_mcu_error("Device is not activated"); + return DEVICE_MCU_ERROR_NO_ACTIVATION; + } + + size_t firmware_len = 0; + uint8_t* firmware = NULL; + + FILE* file = fopen(path, "rb"); + bool result = DEVICE_MCU_ERROR_UNKNOWN; + + if (file) { + fseek(file, 0, SEEK_END); + firmware_len = ftell(file); + + if (firmware_len > 0) { + firmware = (uint8_t*) malloc(firmware_len); + fread(firmware, sizeof(uint8_t), firmware_len, file); + } + + fclose(file); + } + + device_mcu_clear(device); + + printf("Prepare upload: %lu\n", firmware_len); + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) { + device_mcu_error("Failed preparing the device for MCU firmware update!\n"); + goto cleanup; + } + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_MCU_APP_JUMP_TO_BOOT, 0, NULL)) { + device_mcu_error("Failed mcu app jumping to boot"); + goto cleanup; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 0, NULL)) { + device_mcu_error("Requesting activation time failed"); + goto cleanup; + } + + uint8_t activated; + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 1, &activated)) { + device_mcu_error("Receiving activation time failed"); + goto cleanup; + } + + if (!activated) { + device_mcu_error("Device is not activated"); + goto jump_to_app; + } + + size_t offset = 0; + + while (offset < firmware_len) { + const size_t remaining = firmware_len - offset; + + printf("Upload: %lu / %lu\n", offset, firmware_len); + + uint8_t len; + uint16_t msgid; + if (offset == 0) { + len = remaining > 24? 24 : remaining; + msgid = DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_START; + } else { + len = remaining > 42? 42 : remaining; + msgid = DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT; + } + + if (!do_payload_action(device, msgid, len, firmware + offset)) { + device_mcu_error("Failed sending firmware upload"); + goto jump_to_app; + } + + offset += len; + } + + printf("Finish upload"); + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) { + device_mcu_error("Failed finishing firmware upload"); + goto jump_to_app; + } + + result = DEVICE_MCU_ERROR_NO_ERROR; + +jump_to_app: + if (!do_payload_action(device, DEVICE_MCU_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) { + device_mcu_error("Failed boot jumping back to app"); + goto cleanup; + } + +cleanup: + if (firmware) { + free(firmware); + } + + return result; +} + +device_mcu_error_type device_mcu_close(device_mcu_type* device) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (device->handle) { + hid_close(device->handle); + } + + memset(device, 0, sizeof(device_mcu_type)); + device_exit(); + + return DEVICE_MCU_ERROR_NO_ERROR; +} diff --git a/ardriver/xreal/device_mcu.h b/ardriver/xreal/device_mcu.h new file mode 100644 index 0000000..a253317 --- /dev/null +++ b/ardriver/xreal/device_mcu.h @@ -0,0 +1,212 @@ +#pragma once +// +// Created by thejackimonster on 29.03.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define DEVICE_MCU_MSG_R_BRIGHTNESS 0x03 +#define DEVICE_MCU_MSG_W_BRIGHTNESS 0x04 +#define DEVICE_MCU_MSG_R_DISP_MODE 0x07 +#define DEVICE_MCU_MSG_W_DISP_MODE 0x08 + +#define DEVICE_MCU_MSG_R_GLASSID 0x15 +#define DEVICE_MCU_MSG_R_DP7911_FW_VERSION 0x16 +#define DEVICE_MCU_MSG_R_DSP_VERSION 0x18 +#define DEVICE_MCU_MSG_W_CANCEL_ACTIVATION 0x19 +#define DEVICE_MCU_MSG_P_HEARTBEAT 0x1A +#define DEVICE_MCU_MSG_W_SLEEP_TIME 0x1E + +#define DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION 0x21 +#define DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION 0x26 +#define DEVICE_MCU_MSG_R_ACTIVATION_TIME 0x29 +#define DEVICE_MCU_MSG_W_ACTIVATION_TIME 0x2A + +#define DEVICE_MCU_MSG_R_DP7911_FW_IS_UPDATE 0x3C +#define DEVICE_MCU_MSG_W_UPDATE_DP 0x3D +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_PREPARE 0x3E +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_START 0x3F + +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT 0x40 +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_FINISH 0x41 +#define DEVICE_MCU_MSG_W_BOOT_JUMP_TO_APP 0x42 +#define DEVICE_MCU_MSG_W_MCU_APP_JUMP_TO_BOOT 0x44 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_PREPARE 0x45 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_START 0x46 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_TRANSMIT 0x47 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_FINISH 0x48 +#define DEVICE_MCU_MSG_R_IS_NEED_UPGRADE_DSP_FW 0x49 + +#define DEVICE_MCU_MSG_W_FORCE_UPGRADE_DSP_FW 0x69 + +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_MODE 0x1100 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_CONFIRM 0x1101 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_PREPARE 0x1102 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_START 0x1103 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_TRANSMIT 0x1104 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_FINISH 0x1105 + +#define DEVICE_MCU_MSG_P_START_HEARTBEAT 0x6c02 +#define DEVICE_MCU_MSG_P_DISPLAY_TOGGLED 0x6C04 +#define DEVICE_MCU_MSG_P_BUTTON_PRESSED 0x6C05 +#define DEVICE_MCU_MSG_P_END_HEARTBEAT 0x6c12 +#define DEVICE_MCU_MSG_P_ASYNC_TEXT_LOG 0x6c09 + +#define DEVICE_MCU_MSG_E_DSP_ONE_PACKGE_WRITE_FINISH 0x6C0E +#define DEVICE_MCU_MSG_E_DSP_UPDATE_PROGRES 0x6C10 +#define DEVICE_MCU_MSG_E_DSP_UPDATE_ENDING 0x6C11 + +#define DEVICE_MCU_BUTTON_PHYS_DISPLAY_TOGGLE 0x1 +#define DEVICE_MCU_BUTTON_PHYS_BRIGHTNESS_UP 0x2 +#define DEVICE_MCU_BUTTON_PHYS_BRIGHTNESS_DOWN 0x3 + +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_60 0x1 +#define DEVICE_MCU_DISPLAY_MODE_3840x1080_60_SBS 0x3 +#define DEVICE_MCU_DISPLAY_MODE_3840x1080_72_SBS 0x4 +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_72 0x5 +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_60_SBS 0x8 +#define DEVICE_MCU_DISPLAY_MODE_3840x1080_90_SBS 0x9 +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_90 0xA +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_120 0xB + +#define DEVICE_MCU_BUTTON_VIRT_DISPLAY_TOGGLE 0x1 +#define DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_UP 0x6 +#define DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_DOWN 0x7 +#define DEVICE_MCU_BUTTON_VIRT_UP 0x8 +#define DEVICE_MCU_BUTTON_VIRT_DOWN 0x9 +#define DEVICE_MCU_BUTTON_VIRT_MODE_2D 0xA +#define DEVICE_MCU_BUTTON_VIRT_MODE_3D 0xB +#define DEVICE_MCU_BUTTON_VIRT_BLEND_CYCLE 0xC +#define DEVICE_MCU_BUTTON_VIRT_CONTROL_TOGGLE 0xF + +#define DEVICE_MCU_BLEND_STATE_LOW 0x0 +#define DEVICE_MCU_BLEND_STATE_MEDIUM 0x2 +#define DEVICE_MCU_BLEND_STATE_FULL 0x3 + +#define DEVICE_MCU_CONTROL_MODE_BRIGHTNESS 0x0 +#define DEVICE_MCU_CONTROL_MODE_VOLUME 0x1 + +#ifdef __cplusplus +extern "C" { +#endif + +enum device_mcu_error_t { + DEVICE_MCU_ERROR_NO_ERROR = 0, + DEVICE_MCU_ERROR_NO_DEVICE = 1, + DEVICE_MCU_ERROR_NO_HANDLE = 2, + DEVICE_MCU_ERROR_NO_ACTIVATION = 3, + DEVICE_MCU_ERROR_WRONG_SIZE = 4, + DEVICE_MCU_ERROR_UNPLUGGED = 5, + DEVICE_MCU_ERROR_UNEXPECTED = 6, + DEVICE_MCU_ERROR_WRONG_HEAD = 7, + DEVICE_MCU_ERROR_INVALID_LENGTH = 8, + DEVICE_MCU_ERROR_NOT_INITIALIZED = 9, + DEVICE_MCU_ERROR_PAYLOAD_FAILED = 10, + DEVICE_MCU_ERROR_UNKNOWN = 11, +}; + +struct __attribute__((__packed__)) device_mcu_packet_t { + uint8_t head; + uint32_t checksum; + uint16_t length; + uint64_t timestamp; + uint16_t msgid; + uint8_t reserved [5]; + union { + char text [42]; + uint8_t data [42]; + }; +}; + +enum device_mcu_event_t { + DEVICE_MCU_EVENT_UNKNOWN = 0, + DEVICE_MCU_EVENT_SCREEN_ON = 1, + DEVICE_MCU_EVENT_SCREEN_OFF = 2, + DEVICE_MCU_EVENT_BRIGHTNESS_UP = 3, + DEVICE_MCU_EVENT_BRIGHTNESS_DOWN = 4, + DEVICE_MCU_EVENT_MESSAGE = 5, + DEVICE_MCU_EVENT_DISPLAY_MODE_2D = 6, + DEVICE_MCU_EVENT_DISPLAY_MODE_3D = 7, + DEVICE_MCU_EVENT_BLEND_CYCLE = 8, + DEVICE_MCU_EVENT_CONTROL_TOGGLE = 9, + DEVICE_MCU_EVENT_VOLUME_UP = 10, + DEVICE_MCU_EVENT_VOLUME_DOWN = 11, +}; + +typedef enum device_mcu_error_t device_mcu_error_type; +typedef struct device_mcu_packet_t device_mcu_packet_type; +typedef enum device_mcu_event_t device_mcu_event_type; +typedef void (*device_mcu_event_callback)( + uint64_t timestamp, + device_mcu_event_type event, + uint8_t brightness, + const char* msg +); + +struct device_mcu_t { + uint16_t vendor_id; + uint16_t product_id; + + void* handle; + + bool activated; + char mcu_app_fw_version [42]; + char dp_fw_version [42]; + char dsp_fw_version [42]; + + bool active; + uint8_t brightness; + uint8_t disp_mode; + uint8_t blend_state; + uint8_t control_mode; + + device_mcu_event_callback callback; +}; + +typedef struct device_mcu_t device_mcu_type; + +device_mcu_error_type device_mcu_open(device_mcu_type* device, device_mcu_event_callback callback); + +device_mcu_error_type device_mcu_clear(device_mcu_type* device); + +device_mcu_error_type device_mcu_read(device_mcu_type* device, int timeout); + +device_mcu_error_type device_mcu_poll_display_mode(device_mcu_type* device); + +device_mcu_error_type device_mcu_update_display_mode(device_mcu_type* device); + +device_mcu_error_type device_mcu_update_firmware(device_mcu_type* device, const char* path); + +device_mcu_error_type device_mcu_close(device_mcu_type* device); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/hid_ids.c b/ardriver/xreal/hid_ids.c new file mode 100644 index 0000000..b913d1b --- /dev/null +++ b/ardriver/xreal/hid_ids.c @@ -0,0 +1,108 @@ +// +// Created by wheaney on 12.11.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "hid_ids.h" + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +const uint16_t xreal_vendor_id = 0x3318; +const uint16_t xreal_product_ids[NUM_SUPPORTED_PRODUCTS] = { + 0x0424, // XREAL Air + 0x0428, // XREAL Air 2 + 0x0432, // XREAL Air 2 Pro + 0x0426 // XREAL Air 2 Ultra +}; + +const int xreal_imu_interface_ids[NUM_SUPPORTED_PRODUCTS] = { + 3, // XREAL Air + 3, // XREAL Air 2 + 3, // XREAL Air 2 Pro + 2 // XREAL Air 2 Ultra +}; + +const int xreal_mcu_interface_ids[NUM_SUPPORTED_PRODUCTS] = { + 4, // XREAL Air + 4, // XREAL Air 2 + 4, // XREAL Air 2 Pro + 0 // XREAL Air 2 Ultra MCU +}; + +const uint16_t xreal_imu_max_payload_sizes[NUM_SUPPORTED_PRODUCTS] = { + 64, // XREAL Air + 64, // XREAL Air 2 + 64, // XREAL Air 2 Pro + 512 // XREAL Air 2 Ultra +}; + +static int xreal_product_index(uint16_t product_id) { + for (int i = 0; i < NUM_SUPPORTED_PRODUCTS; i++) { + if (xreal_product_ids[i] == product_id) { + return i; + } + } + + return -1; +} + +bool is_xreal_product_id(uint16_t product_id) { + return xreal_product_index(product_id) >= 0; +} + +int xreal_imu_interface_id(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_imu_interface_ids[index]; + } else { + return -1; + } +} + +int xreal_mcu_interface_id(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_mcu_interface_ids[index]; + } else { + return -1; + } +} + +uint16_t xreal_imu_max_payload_size(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_imu_max_payload_sizes[index]; + } else { + return 0; + } +} diff --git a/ardriver/xreal/hid_ids.h b/ardriver/xreal/hid_ids.h new file mode 100644 index 0000000..7c6aa5c --- /dev/null +++ b/ardriver/xreal/hid_ids.h @@ -0,0 +1,54 @@ +#pragma once +// +// Created by wheaney on 20.11.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define NUM_SUPPORTED_PRODUCTS 4 + +#ifdef __cplusplus +extern "C" { +#endif + +extern const uint16_t xreal_vendor_id; +extern const uint16_t xreal_product_ids [NUM_SUPPORTED_PRODUCTS]; + +bool is_xreal_product_id(uint16_t product_id); + +int xreal_imu_interface_id(uint16_t product_id); +int xreal_mcu_interface_id(uint16_t product_id); + +uint16_t xreal_imu_max_payload_size(uint16_t product_id); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/xreal.go b/ardriver/xreal/xreal.go new file mode 100644 index 0000000..a99fe12 --- /dev/null +++ b/ardriver/xreal/xreal.go @@ -0,0 +1,12 @@ +//go:build xreal +// +build xreal + +package xreal + +// #include "evdi_lib.h" +// #include "go_ffi.h" +// #cgo CFLAGS: -w +// #cgo pkg-config: json-c libusb-1.0 hidapi-libusb +import "C" + +var IsXrealEnabled = true diff --git a/ardriver/xreal/xreal_disabled.go b/ardriver/xreal/xreal_disabled.go new file mode 100644 index 0000000..1cbe85c --- /dev/null +++ b/ardriver/xreal/xreal_disabled.go @@ -0,0 +1,6 @@ +//go:build !xreal +// +build !xreal + +package xreal + +var IsXrealEnabled = false