UnrealXR/unrealxr
2
0
Fork 0
Code Issues 1 Pull requests Projects Releases Packages Wiki Activity Actions

chore: Add basic skeleton of the Xreal driver

Browse source
This commit is contained in:
Tera << 8 2025-06-23 13:49:53 -04:00
parent b22931ffaf
commit ad3045fc29
Signed by: imterah
GPG key ID: 8FA7DD57BA6CEA37
26 changed files with 4470 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

4
Makefile
View file

@ -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)

22
ardriver/commons/interface.go Normal file
View file

@ -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)
}

32
ardriver/xreal/Fusion.h Normal file
View file

@ -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

510
ardriver/xreal/FusionAhrs.c Normal file
View file

@ -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 <float.h>
#include "FusionAhrs.h"
#include <math.h>
//------------------------------------------------------------------------------
// 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

111
ardriver/xreal/FusionAhrs.h Normal file
View file

@ -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 <stdbool.h>
//------------------------------------------------------------------------------
// 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

187
ardriver/xreal/FusionAxes.h Normal file
View file

@ -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

44
ardriver/xreal/FusionCalibration.h Normal file
View file

@ -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

49
ardriver/xreal/FusionCompass.c Normal file
View file

@ -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 <math.h>
//------------------------------------------------------------------------------
// 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

25
ardriver/xreal/FusionCompass.h Normal file
View file

@ -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

25
ardriver/xreal/FusionConvention.h Normal file
View file

@ -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

481
ardriver/xreal/FusionMath.h Normal file
View file

@ -0,0 +1,481 @@
/**
* @file FusionMath.h
* @author Seb Madgwick
* @brief Math library.
*/
#ifndef FUSION_MATH_H
#define FUSION_MATH_H
//------------------------------------------------------------------------------
// Includes
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
//------------------------------------------------------------------------------
// 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

77
ardriver/xreal/FusionOffset.c Normal file
View file

@ -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 <math.h>
//------------------------------------------------------------------------------
// 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

40
ardriver/xreal/FusionOffset.h Normal file
View file

@ -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

5
ardriver/xreal/README.md Normal file
View file

@ -0,0 +1,5 @@
# Xreal driver
## Attribution
- [Xreal driver](https://gitlab.com/TheJackiMonster/nrealAirLinuxDriver)
- [Fusion](https://github.com/xioTechnologies/Fusion)

102
ardriver/xreal/crc32.c Normal file
View file

@ -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;
}

40
ardriver/xreal/crc32.h Normal file
View file

@ -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 <stdint.h>
#else
#include <cstdint>
#endif
#ifdef __cplusplus
extern "C" {
#endif
uint32_t crc32_checksum(const uint8_t* buf, uint32_t len);
#ifdef __cplusplus
} // extern "C"
#endif

50
ardriver/xreal/device.c Normal file
View file

@ -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 <hidapi/hidapi.h>
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();
}
}

46
ardriver/xreal/device.h Normal file
View file

@ -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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#endif
#ifdef __cplusplus
extern "C" {
#endif
bool device_init();
void device_exit();
#ifdef __cplusplus
} // extern "C"
#endif

1272
ardriver/xreal/device_imu.c Normal file
View file

File diff suppressed because it is too large Load diff

227
ardriver/xreal/device_imu.h Normal file
View file

@ -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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#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

731
ardriver/xreal/device_mcu.c Normal file
View file

@ -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 <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <hidapi/hidapi.h>
#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;
}

212
ardriver/xreal/device_mcu.h Normal file
View file

@ -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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#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

108
ardriver/xreal/hid_ids.c Normal file
View file

@ -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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#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;
}
}

54
ardriver/xreal/hid_ids.h Normal file
View file

@ -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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#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

12
ardriver/xreal/xreal.go Normal file
View file

@ -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

6
ardriver/xreal/xreal_disabled.go Normal file
View file

@ -0,0 +1,6 @@
//go:build !xreal
// +build !xreal
package xreal
var IsXrealEnabled = false