1 /*
2 * Copyright (c) 2021 Marti Riba Pons
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdio.h>
8 #include <math.h>
9 #include <errno.h>
10 #include <zsl/orientation/fusion/fusion.h>
11 #include <zsl/orientation/fusion/aqua.h>
12
13 static uint32_t zsl_fus_aqua_freq;
14 static uint32_t zsl_fus_aqua_initialised;
15
zsl_fus_aqua(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * e_a,zsl_real_t * e_m,zsl_real_t * alpha,zsl_real_t * beta,struct zsl_quat * q)16 static int zsl_fus_aqua(struct zsl_vec *a, struct zsl_vec *m,
17 struct zsl_vec *g, zsl_real_t *e_a, zsl_real_t *e_m,
18 zsl_real_t *alpha, zsl_real_t *beta, struct zsl_quat *q)
19 {
20 int rc = 0;
21
22 #if CONFIG_ZSL_BOUNDS_CHECKS
23 /* Make sure that the input vectors are tridimensional. */
24 if ((a != NULL && (a->sz != 3)) || (m != NULL && (m->sz != 3)) ||
25 g->sz != 3) {
26 rc = -EINVAL;
27 goto err;
28 }
29 /* Make sure that the input quaternion is not zero. */
30 if (ZSL_ABS(zsl_quat_magn(q)) < 1E-6) {
31 rc = -EINVAL;
32 goto err;
33 }
34 #endif
35
36 /* Convert the input quaternion to a unit quaternion. */
37 zsl_quat_to_unit_d(q);
38
39 /* Calculate an estimation of the orientation using only the data of the
40 * gyroscope and quaternion integration. */
41 zsl_vec_scalar_mult(g, -1.0);
42 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_aqua_freq, q);
43
44 /* Continue with the calculations only if the data from the accelerometer
45 * is valid (non zero). */
46 if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6) {
47
48 /* Normalize the acceleration vector. */
49 zsl_vec_to_unit(a);
50
51 /* Turn the data of the accelerometer into a pure quaterion. */
52 struct zsl_quat qa = {
53 .r = 0.0,
54 .i = a->data[0],
55 .j = a->data[1],
56 .k = a->data[2]
57 };
58
59 /* Invert q. */
60 struct zsl_quat q_inv;
61 zsl_quat_inv(q, &q_inv);
62
63 /* Rotate the accelerometer data quaternion (qa) using the inverse of
64 * previous estimation of the orientation (q_inv). */
65 struct zsl_quat qg;
66 zsl_quat_rot(&q_inv, &qa, &qg);
67
68 /* Compute the delta q_acc quaternion. */
69 struct zsl_quat Dq_acc = {
70 .r = ZSL_SQRT((qg.k + 1.0) / 2.0),
71 .i = -qg.j / ZSL_SQRT(2.0 * (qg.k + 1.0)),
72 .j = qg.i / ZSL_SQRT(2.0 * (qg.k + 1.0)),
73 .k = 0.0
74 };
75
76 /* Scale down the delta q_acc by using spherical linear interpolation
77 * or simply linear interpolation to minimize the effects of high
78 * frequency noise in the accelerometer. */
79 struct zsl_quat q_acc;
80 struct zsl_quat qi;
81 zsl_quat_init(&qi, ZSL_QUAT_TYPE_IDENTITY);
82 if (Dq_acc.r > *e_a) {
83 zsl_quat_lerp(&qi, &Dq_acc, *alpha, &q_acc);
84 } else {
85 zsl_quat_slerp(&qi, &Dq_acc, *alpha, &q_acc);
86 }
87
88 /* New orientation estimation with accelerometer correction. */
89 zsl_quat_mult(q, &q_acc, q);
90
91 /* Continue with the calculations only if the data from the
92 * magnetometer is valid (non zero). */
93 if ((m != NULL) && ZSL_ABS(zsl_vec_norm(m)) > 1E-6) {
94
95 /* Normalize the magnetic field vector. */
96 zsl_vec_to_unit(m);
97
98 /* Turn the data of the magnetometer into a pure quaterion. */
99 struct zsl_quat qm = {
100 .r = 0.0,
101 .i = m->data[0],
102 .j = m->data[1],
103 .k = m->data[2]
104 };
105
106 /* Invert q again. */
107 zsl_quat_inv(q, &q_inv);
108
109 /* Rotate the magnetometer data quaternion (qm) using the inverse
110 * of the previous estimation of the orientation (q_inv). */
111 struct zsl_quat ql;
112 zsl_quat_rot(&q_inv, &qm, &ql);
113
114 /* Compute the delta q_mag quaternion. */
115 zsl_real_t y = ZSL_SQRT(ql.i * ql.i + ql.j * ql.j);
116 struct zsl_quat Dq_mag = {
117 .r = ZSL_SQRT((y * y + ql.i * y) / (2.0 * y * y)),
118 .i = 0.0,
119 .j = 0.0,
120 .k = ql.j / ZSL_SQRT(2.0 * (y * y + ql.i * y))
121 };
122
123 /* Scale down the delta q_mag quaternion by using spherical linear
124 * interpolation or simply linear interpolation to minimize the
125 * effects of high frequency noise in the magnetometer. */
126 struct zsl_quat q_mag;
127 if (Dq_mag.r > *e_m) {
128 zsl_quat_lerp(&qi, &Dq_mag, *beta, &q_mag);
129 } else {
130 zsl_quat_slerp(&qi, &Dq_mag, *beta, &q_mag);
131 }
132
133 /* New orientation estimation with magnetometer correction. */
134 zsl_quat_mult(q, &q_mag, q);
135 }
136 }
137
138 /* Normalize the output quaternion. */
139 zsl_quat_to_unit_d(q);
140
141 err:
142 return rc;
143 }
144
zsl_fus_aqua_alpha_init(struct zsl_vec * a,zsl_real_t * alpha)145 static int zsl_fus_aqua_alpha_init(struct zsl_vec *a, zsl_real_t *alpha)
146 {
147 int rc = 0;
148
149 #if CONFIG_ZSL_BOUNDS_CHECKS
150 /* Make sure that the input vector is tridimensional. */
151 if (a->sz != 3) {
152 rc = -EINVAL;
153 goto err;
154 }
155 #endif
156
157 /* Calculate the value of alpha, which depends on the magnitude error
158 * (m_e) and the value of alpha in static conditions. */
159 zsl_real_t n = zsl_vec_norm(a);
160 zsl_real_t gr = 9.81; /* Earth's gravity. */
161 zsl_real_t m_e = ZSL_ABS(n - gr) / gr; /* Magnitude error. */
162 if (m_e >= 0.2) {
163 *alpha = 0.0;
164 } else if (m_e > 0.1 && m_e < 0.2) {
165 *alpha *= (0.2 - m_e) / 0.1;
166 }
167
168 /* Inidicate that we have already called this function. */
169 zsl_fus_aqua_initialised++;
170
171 err:
172 return rc;
173 }
174
zsl_fus_aqua_init(uint32_t freq,void * cfg)175 int zsl_fus_aqua_init(uint32_t freq, void *cfg)
176 {
177 int rc = 0;
178
179 struct zsl_fus_aqua_cfg *mcfg = cfg;
180
181 (void)mcfg;
182
183 #if CONFIG_ZSL_BOUNDS_CHECKS
184 /* Make sure that the sample frequency is positive. */
185 if (freq <= 0.0) {
186 rc = -EINVAL;
187 goto err;
188 }
189 #endif
190
191 zsl_fus_aqua_freq = freq;
192
193 err:
194 return rc;
195 }
196
zsl_fus_aqua_feed(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * incl,struct zsl_quat * q,void * cfg)197 int zsl_fus_aqua_feed(struct zsl_vec *a, struct zsl_vec *m,
198 struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q,
199 void *cfg)
200 {
201 struct zsl_fus_aqua_cfg *mcfg = cfg;
202
203 if (mcfg->alpha < 0.0 || mcfg->alpha > 1.0 || mcfg->beta < 0.0 ||
204 mcfg->beta > 1.0) {
205 return -EINVAL;
206 }
207
208 /* This functions should only be called once. */
209 if (!zsl_fus_aqua_initialised) {
210 zsl_fus_aqua_alpha_init(a, &(mcfg->alpha));
211 }
212
213 return zsl_fus_aqua(a, m, g, &(mcfg->e_a), &(mcfg->e_m), &(mcfg->alpha),
214 &(mcfg->beta), q);
215 }
216
zsl_fus_aqua_error(int error)217 void zsl_fus_aqua_error(int error)
218 {
219 /* ToDo: Log error in default handler. */
220 }
221
