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/madgwick.h>
12
13 static uint32_t zsl_fus_madg_freq;
14
zsl_fus_madgwick_imu(struct zsl_vec * g,struct zsl_vec * a,zsl_real_t * beta,zsl_real_t * incl,struct zsl_quat * q)15 static int zsl_fus_madgwick_imu(struct zsl_vec *g, struct zsl_vec *a,
16 zsl_real_t *beta, zsl_real_t *incl, struct zsl_quat *q)
17 {
18 int rc = 0;
19
20 #if CONFIG_ZSL_BOUNDS_CHECKS
21 /* Make sure that the input vectors are tridimensional. */
22 if ((a != NULL && (a->sz != 3)) || g->sz != 3) {
23 rc = -EINVAL;
24 goto err;
25 }
26 /* Make sure that the input quaternion is not zero. */
27 if (ZSL_ABS(zsl_quat_magn(q)) < 1E-6) {
28 rc = -EINVAL;
29 goto err;
30 }
31 #endif
32
33 /* Convert the input quaternion to a unit quaternion. */
34 zsl_quat_to_unit_d(q);
35
36 /* Declare the gradient vector and set it to zero. It will only be
37 * modified if the acceleration vector is valid. */
38 ZSL_VECTOR_DEF(grad, 4);
39 zsl_vec_init(&grad);
40
41 /* Continue with the calculations only if the data from the accelerometer
42 * is valid (non zero). */
43 if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6) {
44
45 /* Normalize the acceleration vector. */
46 zsl_vec_to_unit(a);
47
48 /* Define the normalized quaternion of acceleration on the earth's
49 * reference frame, which only has a vertical component. */
50 struct zsl_quat qa = {
51 .r = 0.0,
52 .i = 0.0,
53 .j = 0.0,
54 .k = 1.0
55 };
56
57 /* Calculate the function f by using the input quaternion to rotate the
58 * normalised acceleration vector in the earth's reference frame, and
59 * then substracting the acceleration vector. */
60 ZSL_MATRIX_DEF(f, 3, 1);
61 struct zsl_quat qaq;
62 zsl_quat_rot(q, &qa, &qaq);
63 f.data[0] = qaq.i - a->data[0];
64 f.data[1] = qaq.j - a->data[1];
65 f.data[2] = qaq.k - a->data[2];
66
67 /* Define and compute the transpose of the Jacobian matrix of the
68 * function f. */
69 ZSL_MATRIX_DEF(jt, 4, 3);
70 zsl_real_t jt_data[12] = {
71 -2.0 * q->j, 2.0 * q->i, 0.0,
72 2.0 * q->k, 2.0 * q->r, -4.0 * q->i,
73 -2.0 * q->r, 2.0 * q->k, -4.0 * q->j,
74 2.0 * q->i, 2.0 * q->j, 0.0
75 };
76
77 zsl_mtx_from_arr(&jt, jt_data);
78
79 /* Calculate the gradient of f by multiplying the transposed Jacobian
80 * matrix by the function itself. */
81 ZSL_MATRIX_DEF(jtf, 4, 1);
82 zsl_mtx_mult(&jt, &f, &jtf);
83
84 /* Normalize the gradient vector. */
85 zsl_vec_from_arr(&grad, jtf.data);
86 zsl_vec_to_unit(&grad);
87 }
88
89 /* Update the input quaternion with a modified quaternion integration. */
90 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_madg_freq, q);
91 q->r -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[0]);
92 q->i -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[1]);
93 q->j -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[2]);
94 q->k -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[3]);
95
96 /* Normalize the output quaternion. */
97 zsl_quat_to_unit_d(q);
98
99 err:
100 return rc;
101 }
102
103 /**
104 * @brief
105 *
106 * @param g
107 * @param a
108 * @param m
109 * @param beta
110 * @param q
111 *
112 * @return int
113 */
zsl_fus_madgwick(struct zsl_vec * g,struct zsl_vec * a,struct zsl_vec * m,zsl_real_t * beta,zsl_real_t * incl,struct zsl_quat * q)114 static int zsl_fus_madgwick(struct zsl_vec *g, struct zsl_vec *a,
115 struct zsl_vec *m, zsl_real_t *beta, zsl_real_t *incl,
116 struct zsl_quat *q)
117 {
118 int rc = 0;
119
120 #if CONFIG_ZSL_BOUNDS_CHECKS
121 /* Make sure that the input vectors are tridimensional. */
122 if ((a != NULL && (a->sz != 3)) || (m != NULL && (m->sz != 3)) ||
123 g->sz != 3) {
124 rc = -EINVAL;
125 goto err;
126 }
127 /* Make sure that the input quaternion is not zero. */
128 if (ZSL_ABS(zsl_quat_magn(q)) < 1E-6) {
129 rc = -EINVAL;
130 goto err;
131 }
132 #endif
133
134 /* Use IMU algorithm if the magnetometer measurement is invalid. */
135 if ((m == NULL) || (ZSL_ABS(zsl_vec_norm(m)) < 1E-6)) {
136 return zsl_fus_madgwick_imu(g, a, beta, incl, q);
137 }
138
139 /* Convert the input quaternion to a unit quaternion. */
140 zsl_quat_to_unit_d(q);
141
142 /* Declare the gradient vector and set it to zero. It will only be
143 * modified if the acceleration vector is valid. */
144 ZSL_VECTOR_DEF(grad, 4);
145 zsl_vec_init(&grad);
146
147 /* Continue with the calculations only if the data from the accelerometer
148 * is valid (non zero). */
149 if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6) {
150
151 /* Normalize the acceleration vector. */
152 zsl_vec_to_unit(a);
153
154 /* Normalize the magnetic field vector. */
155 zsl_vec_to_unit(m);
156
157 /* Define the normalized vector of acceleration on the earth's
158 * reference frame, which only has a vertical component. */
159 struct zsl_quat qa = {
160 .r = 0.0,
161 .i = 0.0,
162 .j = 0.0,
163 .k = 1.0
164 };
165
166 /* Calculate the function f_g by using the input quaternion to rotate
167 * the normalised acceleration quaternion in the earth's reference
168 * frame 'qa', and then substracting the acceleration vector. */
169 ZSL_MATRIX_DEF(f_g, 3, 1);
170 struct zsl_quat qaq;
171 zsl_quat_rot(q, &qa, &qaq);
172 f_g.data[0] = qaq.i - a->data[0];
173 f_g.data[1] = qaq.j - a->data[1];
174 f_g.data[2] = qaq.k - a->data[2];
175
176 /* Define and compute the transpose of the Jacobian matrix of the
177 * function f_g. */
178 ZSL_MATRIX_DEF(jt_g, 4, 3);
179 zsl_real_t jt_g_data[12] = {
180 -2.0 * q->j, 2.0 * q->i, 0.0,
181 2.0 * q->k, 2.0 * q->r, -4.0 * q->i,
182 -2.0 * q->r, 2.0 * q->k, -4.0 * q->j,
183 2.0 * q->i, 2.0 * q->j, 0.0
184 };
185
186 zsl_mtx_from_arr(&jt_g, jt_g_data);
187
188 /* Calculate the gradient of f_g by multiplying the transposed Jacobian
189 * matrix by the function itself. */
190 ZSL_MATRIX_DEF(jtf_g, 4, 1);
191 zsl_mtx_mult(&jt_g, &f_g, &jtf_g);
192
193 /* Turn the data of the magnetometer into a pure quaterion. */
194 struct zsl_quat qm = {
195 .r = 0.0,
196 .i = m->data[0],
197 .j = m->data[1],
198 .k = m->data[2]
199 };
200
201 /* Define the normalized quaternion 'b' of magnetic field on the
202 * earth's reference frame, which only has a x (north) and z (vertical)
203 * components. If the declination angle is known, use it to calculate
204 * this vector. */
205 zsl_real_t bx;
206 zsl_real_t bz;
207
208 if (incl == NULL) {
209 struct zsl_quat h;
210 zsl_quat_rot(q, &qm, &h);
211 bx = ZSL_SQRT(h.i * h.i + h.j * h.j);
212 bz = h.k;
213 } else {
214 bx = ZSL_COS(*incl * ZSL_PI / 180.0);
215 bz = ZSL_SIN(*incl * ZSL_PI / 180.0);
216 }
217
218 struct zsl_quat b = { .r = 0.0, .i = bx, .j = 0.0, .k = bz };
219
220 /* Calculate the function f_b by using the input quaternion to rotate
221 * the normalised magnetic field quaternion in the earth's reference
222 * frame 'b', and then substracting the magnetometer data vector. */
223 ZSL_MATRIX_DEF(f_b, 3, 1);
224 struct zsl_quat qbq;
225 zsl_quat_rot(q, &b, &qbq);
226 f_b.data[0] = qbq.i - m->data[0];
227 f_b.data[1] = qbq.j - m->data[1];
228 f_b.data[2] = qbq.k - m->data[2];
229
230 /* Define and compute the transpose of the Jacobian matrix of the
231 * function f_b. */
232 ZSL_MATRIX_DEF(jt_b, 4, 3);
233 zsl_real_t jt_b_data[12] = {
234 /* 1x1..1x3 */
235 -2.0 * bz * q->j,
236 -2.0 * bx * q->k + 2.0 * bz * q->i,
237 2.0 * bx * q->j,
238 /* 2x1..2x3 */
239 2.0 * bz * q->k,
240 2.0 * bz * q->j + 2.0 * bz * q->r,
241 2.0 * bx * q->k - 4.0 * bz * q->i,
242 /* 3x1..3x3 */
243 -4.0 * bx * q->j - 2.0 * bz * q->r,
244 2.0 * bx * q->i - 2.0 * bz * q->k,
245 2.0 * bx * q->r - 2.0 * bz * q->j,
246 /* 4x1..4x3 */
247 -4.0 * bx * q->k + 2.0 * bz * q->i,
248 -2.0 * bx * q->r + 2.0 * bz * q->j,
249 2.0 * bx * q->i
250 };
251
252 zsl_mtx_from_arr(&jt_b, jt_b_data);
253
254 /* Calculate the gradient of f_b by multiplying the transposed Jacobian
255 * matrix by the function itself. */
256 ZSL_MATRIX_DEF(jtf_b, 4, 1);
257 zsl_mtx_mult(&jt_b, &f_b, &jtf_b);
258
259 /* Compute the total gradient vector by adding the gradient of f_g and
260 * total gradient of f_b. Then normalize the total gradient vector. */
261 grad.data[0] = jtf_g.data[0] + jtf_b.data[0];
262 grad.data[1] = jtf_g.data[1] + jtf_b.data[1];
263 grad.data[2] = jtf_g.data[2] + jtf_b.data[2];
264 grad.data[3] = jtf_g.data[3] + jtf_b.data[3];
265
266 zsl_vec_to_unit(&grad);
267 }
268
269 /* Update the input quaternion with a modified quaternion integration. */
270 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_madg_freq, q);
271 q->r -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[0]);
272 q->i -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[1]);
273 q->j -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[2]);
274 q->k -= (1.0 / zsl_fus_madg_freq) * (*beta * grad.data[3]);
275
276 /* Normalize the output quaternion. */
277 zsl_quat_to_unit_d(q);
278
279 err:
280 return rc;
281 }
282
zsl_fus_madg_init(uint32_t freq,void * cfg)283 int zsl_fus_madg_init(uint32_t freq, void *cfg)
284 {
285 int rc = 0;
286
287 struct zsl_fus_madg_cfg *mcfg = cfg;
288
289 (void)mcfg;
290
291 #if CONFIG_ZSL_BOUNDS_CHECKS
292 /* Make sure that the sample frequency is positive. */
293 if (freq <= 0) {
294 rc = -EINVAL;
295 goto err;
296 }
297 #endif
298
299 zsl_fus_madg_freq = freq;
300
301 err:
302 return rc;
303 }
304
zsl_fus_madg_feed(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * incl,struct zsl_quat * q,void * cfg)305 int zsl_fus_madg_feed(struct zsl_vec *a, struct zsl_vec *m, struct zsl_vec *g,
306 zsl_real_t *incl, struct zsl_quat *q, void *cfg)
307 {
308 struct zsl_fus_madg_cfg *mcfg = cfg;
309
310 if (mcfg->beta < 0.0) {
311 return -EINVAL;
312 }
313
314 return zsl_fus_madgwick(g, a, m, &(mcfg->beta), incl, q);
315 }
316
zsl_fus_madg_error(int error)317 void zsl_fus_madg_error(int error)
318 {
319 /* ToDo: Log error in default handler. */
320 }
321
