FluxSand 1.0
FluxSand - Interactive Digital Hourglass
Loading...
Searching...
No Matches
comp_ahrs.hpp
1#pragma once
2
3/*
4 MadgwickAHRS
5*/
6
7#include <fstream>
8#include <functional>
9#include <semaphore>
10
11#include "bsp.hpp"
12#include "comp_type.hpp"
13
14class AHRS {
15 public:
16 AHRS() : ready_(0) {
17 quat_.q0 = -1.0f;
18 quat_.q1 = 0.0f;
19 quat_.q2 = 0.0f;
20 quat_.q3 = 0.0f;
21
22 start_ = now_ = last_wakeup_ =
23 std::chrono::duration_cast<std::chrono::microseconds>(
24 std::chrono::system_clock::now().time_since_epoch());
25
26 thread_ = std::thread(&AHRS::ThreadTask, this);
27 }
28
29 void OnData(const Type::Vector3& accel, const Type::Vector3& gyro) {
30 accel_ = accel;
31 gyro_ = gyro;
32 ready_.release();
33 }
34
35 void ThreadTask() {
36 while (true) {
37 ready_.acquire();
38 Update();
39 GetEulr();
40 }
41 }
42
43 void Update() {
44 static float recip_norm;
45 static float s0, s1, s2, s3;
46 static float q_dot1, q_dot2, q_dot3, q_dot4;
47 static float q_2q0, q_2q1, q_2q2, q_2q3, q_4q0, q_4q1, q_4q2, q_8q1, q_8q2,
48 q0q0, q1q1, q2q2, q3q3;
49
50 now_ = std::chrono::duration_cast<std::chrono::microseconds>(
51 std::chrono::system_clock::now().time_since_epoch());
52
53 dt_ = 0.001f;
54 last_wakeup_ = now_;
55
56 float ax = accel_.x;
57 float ay = accel_.y;
58 float az = accel_.z;
59
60 float gx = gyro_.x;
61 float gy = gyro_.y;
62 float gz = gyro_.z;
63
64 /* Rate of change of quaternion from gyroscope */
65 q_dot1 = 0.5f * (-quat_.q1 * gx - quat_.q2 * gy - quat_.q3 * gz);
66 q_dot2 = 0.5f * (quat_.q0 * gx + quat_.q2 * gz - quat_.q3 * gy);
67 q_dot3 = 0.5f * (quat_.q0 * gy - quat_.q1 * gz + quat_.q3 * gx);
68 q_dot4 = 0.5f * (quat_.q0 * gz + quat_.q1 * gy - quat_.q2 * gx);
69
70 /* Compute feedback only if accelerometer measurement valid (avoids NaN in
71 * accelerometer normalisation) */
72 if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
73 /* Normalise accelerometer measurement */
74 recip_norm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
75 ax *= recip_norm;
76 ay *= recip_norm;
77 az *= recip_norm;
78
79 /* Auxiliary variables to avoid repeated arithmetic */
80 q_2q0 = 2.0f * quat_.q0;
81 q_2q1 = 2.0f * quat_.q1;
82 q_2q2 = 2.0f * quat_.q2;
83 q_2q3 = 2.0f * quat_.q3;
84 q_4q0 = 4.0f * quat_.q0;
85 q_4q1 = 4.0f * quat_.q1;
86 q_4q2 = 4.0f * quat_.q2;
87 q_8q1 = 8.0f * quat_.q1;
88 q_8q2 = 8.0f * quat_.q2;
89 q0q0 = quat_.q0 * quat_.q0;
90 q1q1 = quat_.q1 * quat_.q1;
91 q2q2 = quat_.q2 * quat_.q2;
92 q3q3 = quat_.q3 * quat_.q3;
93
94 /* Gradient decent algorithm corrective step */
95 s0 = q_4q0 * q2q2 + q_2q2 * ax + q_4q0 * q1q1 - q_2q1 * ay;
96 s1 = q_4q1 * q3q3 - q_2q3 * ax + 4.0f * q0q0 * quat_.q1 - q_2q0 * ay -
97 q_4q1 + q_8q1 * q1q1 + q_8q1 * q2q2 + q_4q1 * az;
98 s2 = 4.0f * q0q0 * quat_.q2 + q_2q0 * ax + q_4q2 * q3q3 - q_2q3 * ay -
99 q_4q2 + q_8q2 * q1q1 + q_8q2 * q2q2 + q_4q2 * az;
100 s3 = 4.0f * q1q1 * quat_.q3 - q_2q1 * ax + 4.0f * q2q2 * quat_.q3 -
101 q_2q2 * ay;
102
103 /* normalise step magnitude */
104 recip_norm = 1.0f / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
105
106 s0 *= recip_norm;
107 s1 *= recip_norm;
108 s2 *= recip_norm;
109 s3 *= recip_norm;
110
111 float beta_imu = 0;
112
113 if (now_.count() - start_.count() > 1000000) {
114 beta_imu = 2.0f;
115 } else {
116 beta_imu = 10.0f;
117 }
118
119 /* Apply feedback step */
120 q_dot1 -= beta_imu * s0;
121 q_dot2 -= beta_imu * s1;
122 q_dot3 -= beta_imu * s2;
123 q_dot4 -= beta_imu * s3;
124 }
125
126 /* Integrate rate of change of quaternion to yield quaternion */
127 quat_.q0 += q_dot1 * dt_;
128 quat_.q1 += q_dot2 * dt_;
129 quat_.q2 += q_dot3 * dt_;
130 quat_.q3 += q_dot4 * dt_;
131
132 /* Normalise quaternion */
133 recip_norm = 1.0f / sqrtf(quat_.q0 * quat_.q0 + quat_.q1 * quat_.q1 +
134 quat_.q2 * quat_.q2 + quat_.q3 * quat_.q3);
135 quat_.q0 *= recip_norm;
136 quat_.q1 *= recip_norm;
137 quat_.q2 *= recip_norm;
138 quat_.q3 *= recip_norm;
139 }
140
141 void GetEulr() {
142 float yaw = std::atan2(2 * (quat_.q0 * quat_.q3 + quat_.q1 * quat_.q2),
143 1 - 2 * (quat_.q3 * quat_.q3 + quat_.q2 * quat_.q2));
144 float pitch = std::asin(2 * (quat_.q0 * quat_.q2 - quat_.q1 * quat_.q3));
145 float roll =
146 std::atan2(2 * (quat_.q0 * quat_.q1 + quat_.q2 * quat_.q3),
147 1 - 2 * (quat_.q2 * quat_.q2 + quat_.q1 * quat_.q1));
148
149 eulr_.pit = pitch;
150 eulr_.rol = roll;
151 eulr_.yaw = yaw;
152
153 if (data_callback_) {
154 data_callback_(accel_, gyro_, eulr_);
155 }
156 }
157
158 void DisplayData() {
159 std::cout << std::format(
160 "Quaternion: [q0={:+.4f}, q1={:+.4f}, q2={:+.4f}, q3={:+.4f}] | "
161 "Eulr: [rol={:+.4f}, pit={:+.4f}, yaw={:+.4f}] ",
162 quat_.q0, quat_.q1, quat_.q2, quat_.q3, eulr_.rol.Value(),
163 eulr_.pit.Value(), eulr_.yaw.Value());
164
165 std::cout << std::format(
166 "Accel: [x={:+.4f}, y={:+.4f}, z={:+.4f}]] dt={:+.8f}\n",
167 filtered_accel_.x, filtered_accel_.y, filtered_accel_.z, dt_);
168 }
169
170 void DisplyDataWithoutYaw() {
171 std::cout << std::format(
172 "Quaternion: [q0={:+.4f}, q1={:+.4f}, q2={:+.4f}, q3={:+.4f}] | "
173 "Eulr: [rol={:+.4f}, pit={:+.4f}, yaw={:+.4f}]\n",
174 quat_without_z_.q0, quat_without_z_.q1, quat_without_z_.q2,
175 quat_without_z_.q3, eulr_without_yaw_.rol.Value(),
176 eulr_without_yaw_.pit.Value(), eulr_without_yaw_.yaw.Value());
177 }
178
179 void StartRecordData() {
180 record_thread_ = std::thread(&AHRS::RecordTask, this);
181 }
182
183 void RecordTask() {
184 std::ofstream csv_file("imu_data.csv");
185 std::chrono::microseconds period_us(static_cast<int>(1000));
186
187 std::chrono::steady_clock::time_point next_time, start_time;
188 next_time = start_time = std::chrono::steady_clock::now();
189 csv_file << "timestamp,ax,ay,az,gx,gy,gz,q0,q1,q2,q3,rol,pit,yaw,label\n";
190 while (true) {
191 csv_file << std::to_string((next_time.time_since_epoch().count() -
192 start_time.time_since_epoch().count()) /
193 1000)
194 << "," << accel_.x << "," << accel_.y << "," << accel_.z << ","
195 << gyro_.x << "," << gyro_.y << "," << gyro_.z << "," << quat_.q0
196 << "," << quat_.q1 << "," << quat_.q2 << "," << quat_.q3 << ","
197 << eulr_.rol.Value() << "," << eulr_.pit.Value() << ","
198 << eulr_.yaw.Value() << "," << "1\n";
199 csv_file.flush();
200
201 next_time += period_us;
202 std::this_thread::sleep_until(next_time);
203 }
204 }
205
206 void RegisterDataCallback(
207 const std::function<void(const Type::Vector3&, const Type::Vector3&,
208 const Type::Eulr&)>& callback) {
209 data_callback_ = callback;
210 }
211
212 void RunUnitTest() {
213 std::cout << "[UnitTest] Starting AHRS unit test...\n";
214
215 // ---- Initialization section ----
216 quat_ = {1.0f, 0.0f, 0.0f, 0.0f};
217 accel_ = {0.01f, 0.01f, 1.0f}; // Slight tilt
218 gyro_ = {0.0f, 0.0f, 0.0f};
219
220 auto current_time = std::chrono::duration_cast<std::chrono::microseconds>(
221 std::chrono::system_clock::now().time_since_epoch());
222
223 start_ = now_ = last_wakeup_ = current_time;
224 dt_ = 0.001f; // 1ms sample period
225
226 auto t0 = std::chrono::high_resolution_clock::now();
227
228 // ---- Run Update ----
229 auto t1 = std::chrono::high_resolution_clock::now();
230 Update();
231 auto t2 = std::chrono::high_resolution_clock::now();
232
233 // ---- Convert to Euler angles ----
234 GetEulr();
235 auto t3 = std::chrono::high_resolution_clock::now();
236
237 // ---- Output ----
238 std::cout << "[UnitTest] Quaternion: ";
239 std::cout << std::format(
240 "[q0={:+.4f}, q1={:+.4f}, q2={:+.4f}, q3={:+.4f}]\n", quat_.q0,
241 quat_.q1, quat_.q2, quat_.q3);
242
243 std::cout << "[UnitTest] Euler Angles (rad): ";
244 std::cout << std::format("[rol={:+.4f}, pit={:+.4f}, yaw={:+.4f}]\n",
245 eulr_.rol.Value(), eulr_.pit.Value(),
246 eulr_.yaw.Value());
247
248 auto t4 = std::chrono::high_resolution_clock::now();
249
250 // ---- Verification ----
251 auto normalize_angle = [](float rad) -> float {
252 while (rad > M_PI) rad -= 2.0f * M_PI;
253 while (rad < -M_PI) rad += 2.0f * M_PI;
254 return rad;
255 };
256
257 float norm_rol = normalize_angle(eulr_.rol.Value());
258 float norm_pit = normalize_angle(eulr_.pit.Value());
259 float norm_yaw = normalize_angle(eulr_.yaw.Value());
260
261 const float tolerance = 0.1f; // ~5.7 degrees
262
263 if (std::abs(norm_rol) < tolerance && std::abs(norm_pit) < tolerance &&
264 std::abs(norm_yaw) < tolerance) {
265 std::cout
266 << "[UnitTest] ✅ Test Passed: Orientation is correct at rest.\n";
267 } else {
268 std::cout << "[UnitTest] ❌ Test Failed: Unexpected orientation.\n";
269 }
270
271 auto t5 = std::chrono::high_resolution_clock::now();
272
273 // ---- Timing report ----
274 std::cout << std::format(
275 "[Timing] Init & assignment : {:>8.2f} µs\n",
276 std::chrono::duration<float, std::micro>(t1 - t0).count());
277 std::cout << std::format(
278 "[Timing] Update() : {:>8.2f} µs\n",
279 std::chrono::duration<float, std::micro>(t2 - t1).count());
280 std::cout << std::format(
281 "[Timing] GetEulr() : {:>8.2f} µs\n",
282 std::chrono::duration<float, std::micro>(t3 - t2).count());
283 std::cout << std::format(
284 "[Timing] Output : {:>8.2f} µs\n",
285 std::chrono::duration<float, std::micro>(t4 - t3).count());
286 std::cout << std::format(
287 "[Timing] Verify : {:>8.2f} µs\n",
288 std::chrono::duration<float, std::micro>(t5 - t4).count());
289 std::cout << std::format(
290 "[Timing] Total : {:>8.2f} µs\n",
291 std::chrono::duration<float, std::micro>(t5 - t0).count());
292 }
293
294 Type::Quaternion quat_{};
295 Type::Eulr eulr_{};
296 Type::Vector3 accel_{};
297 Type::Vector3 gyro_{};
298
299 private:
300 std::chrono::duration<uint64_t, std::ratio<1, 1000000>> last_wakeup_;
301 std::chrono::duration<uint64_t, std::ratio<1, 1000000>> now_;
302 std::chrono::duration<uint64_t, std::ratio<1, 1000000>> start_;
303 float dt_ = 0.0f;
304
305 Type::Quaternion quat_without_z_{};
306 Type::Eulr eulr_without_yaw_{};
307
308 Type::Vector3 filtered_accel_{};
309
310 std::binary_semaphore ready_;
311
312 std::function<void(const Type::Vector3& acc, const Type::Vector3& gyro,
313 const Type::Eulr& eulr)>
314 data_callback_;
315
316 std::thread thread_, record_thread_; /* Thread */
317};
float Value()
Returns the current value.
Represents Euler angles with cyclic values for yaw, pitch, and roll.
Represents a quaternion (w, x, y, z components).
Represents a 3D vector.