22 start_ = now_ = last_wakeup_ =
23 std::chrono::duration_cast<std::chrono::microseconds>(
24 std::chrono::system_clock::now().time_since_epoch());
26 thread_ = std::thread(&AHRS::ThreadTask,
this);
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;
50 now_ = std::chrono::duration_cast<std::chrono::microseconds>(
51 std::chrono::system_clock::now().time_since_epoch());
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);
72 if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
74 recip_norm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
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;
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 -
104 recip_norm = 1.0f / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
113 if (now_.count() - start_.count() > 1000000) {
120 q_dot1 -= beta_imu * s0;
121 q_dot2 -= beta_imu * s1;
122 q_dot3 -= beta_imu * s2;
123 q_dot4 -= beta_imu * s3;
127 quat_.q0 += q_dot1 * dt_;
128 quat_.q1 += q_dot2 * dt_;
129 quat_.q2 += q_dot3 * dt_;
130 quat_.q3 += q_dot4 * dt_;
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;
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));
146 std::atan2(2 * (quat_.q0 * quat_.q1 + quat_.q2 * quat_.q3),
147 1 - 2 * (quat_.q2 * quat_.q2 + quat_.q1 * quat_.q1));
153 if (data_callback_) {
154 data_callback_(accel_, gyro_, eulr_);
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(),
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_);
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());
179 void StartRecordData() {
180 record_thread_ = std::thread(&AHRS::RecordTask,
this);
184 std::ofstream csv_file(
"imu_data.csv");
185 std::chrono::microseconds period_us(
static_cast<int>(1000));
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";
191 csv_file << std::to_string((next_time.time_since_epoch().count() -
192 start_time.time_since_epoch().count()) /
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";
201 next_time += period_us;
202 std::this_thread::sleep_until(next_time);
206 void RegisterDataCallback(
209 data_callback_ = callback;
213 std::cout <<
"[UnitTest] Starting AHRS unit test...\n";
216 quat_ = {1.0f, 0.0f, 0.0f, 0.0f};
217 accel_ = {0.01f, 0.01f, 1.0f};
218 gyro_ = {0.0f, 0.0f, 0.0f};
220 auto current_time = std::chrono::duration_cast<std::chrono::microseconds>(
221 std::chrono::system_clock::now().time_since_epoch());
223 start_ = now_ = last_wakeup_ = current_time;
226 auto t0 = std::chrono::high_resolution_clock::now();
229 auto t1 = std::chrono::high_resolution_clock::now();
231 auto t2 = std::chrono::high_resolution_clock::now();
235 auto t3 = std::chrono::high_resolution_clock::now();
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);
243 std::cout <<
"[UnitTest] Euler Angles (rad): ";
244 std::cout << std::format(
"[rol={:+.4f}, pit={:+.4f}, yaw={:+.4f}]\n",
248 auto t4 = std::chrono::high_resolution_clock::now();
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;
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());
261 const float tolerance = 0.1f;
263 if (std::abs(norm_rol) < tolerance && std::abs(norm_pit) < tolerance &&
264 std::abs(norm_yaw) < tolerance) {
266 <<
"[UnitTest] ✅ Test Passed: Orientation is correct at rest.\n";
268 std::cout <<
"[UnitTest] ❌ Test Failed: Unexpected orientation.\n";
271 auto t5 = std::chrono::high_resolution_clock::now();
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());
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_;
310 std::binary_semaphore ready_;
316 std::thread thread_, record_thread_;