-
Notifications
You must be signed in to change notification settings - Fork 241
/
main.c
71 lines (57 loc) · 3.3 KB
/
main.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#include "../../Fusion/Fusion.h"
#include <stdbool.h>
#include <stdio.h>
#include <time.h>
#define SAMPLE_RATE (100) // replace this with actual sample rate
int main() {
// Define calibration (replace with actual calibration data if available)
const FusionMatrix gyroscopeMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
const FusionVector gyroscopeSensitivity = {1.0f, 1.0f, 1.0f};
const FusionVector gyroscopeOffset = {0.0f, 0.0f, 0.0f};
const FusionMatrix accelerometerMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
const FusionVector accelerometerSensitivity = {1.0f, 1.0f, 1.0f};
const FusionVector accelerometerOffset = {0.0f, 0.0f, 0.0f};
const FusionMatrix softIronMatrix = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
const FusionVector hardIronOffset = {0.0f, 0.0f, 0.0f};
// Initialise algorithms
FusionOffset offset;
FusionAhrs ahrs;
FusionOffsetInitialise(&offset, SAMPLE_RATE);
FusionAhrsInitialise(&ahrs);
// Set AHRS algorithm settings
const FusionAhrsSettings settings = {
.convention = FusionConventionNwu,
.gain = 0.5f,
.gyroscopeRange = 2000.0f, /* replace this with actual gyroscope range in degrees/s */
.accelerationRejection = 10.0f,
.magneticRejection = 10.0f,
.recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */
};
FusionAhrsSetSettings(&ahrs, &settings);
// This loop should repeat each time new gyroscope data is available
while (true) {
// Acquire latest sensor data
const clock_t timestamp = clock(); // replace this with actual gyroscope timestamp
FusionVector gyroscope = {0.0f, 0.0f, 0.0f}; // replace this with actual gyroscope data in degrees/s
FusionVector accelerometer = {0.0f, 0.0f, 1.0f}; // replace this with actual accelerometer data in g
FusionVector magnetometer = {1.0f, 0.0f, 0.0f}; // replace this with actual magnetometer data in arbitrary units
// Apply calibration
gyroscope = FusionCalibrationInertial(gyroscope, gyroscopeMisalignment, gyroscopeSensitivity, gyroscopeOffset);
accelerometer = FusionCalibrationInertial(accelerometer, accelerometerMisalignment, accelerometerSensitivity, accelerometerOffset);
magnetometer = FusionCalibrationMagnetic(magnetometer, softIronMatrix, hardIronOffset);
// Update gyroscope offset correction algorithm
gyroscope = FusionOffsetUpdate(&offset, gyroscope);
// Calculate delta time (in seconds) to account for gyroscope sample clock error
static clock_t previousTimestamp;
const float deltaTime = (float) (timestamp - previousTimestamp) / (float) CLOCKS_PER_SEC;
previousTimestamp = timestamp;
// Update gyroscope AHRS algorithm
FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
// Print algorithm outputs
const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
const FusionVector earth = FusionAhrsGetEarthAcceleration(&ahrs);
printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f, X %0.1f, Y %0.1f, Z %0.1f\n",
euler.angle.roll, euler.angle.pitch, euler.angle.yaw,
earth.axis.x, earth.axis.y, earth.axis.z);
}
}