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/mahony.h>
12
13 static uint32_t zsl_fus_mahn_freq;
14
zsl_fus_mahony_imu(struct zsl_vec * g,struct zsl_vec * a,zsl_real_t * Kp,zsl_real_t * Ki,struct zsl_vec * integralFB,zsl_real_t integral_limit,zsl_real_t * incl,struct zsl_quat * q)15 static int zsl_fus_mahony_imu(struct zsl_vec *g, struct zsl_vec *a,
16 zsl_real_t *Kp, zsl_real_t *Ki,
17 struct zsl_vec *integralFB,
18 zsl_real_t integral_limit,
19 zsl_real_t *incl,
20 struct zsl_quat *q)
21 {
22 int rc = 0;
23
24 #if CONFIG_ZSL_BOUNDS_CHECKS
25 /* Make sure that the input vectors are tridimensional. */
26 if ((a != NULL && (a->sz != 3)) || g->sz != 3 || integralFB->sz != 3) {
27 rc = -EINVAL;
28 goto err;
29 }
30 /* Make sure that the input quaternion is not zero. */
31 if (ZSL_ABS(zsl_quat_magn(q)) < 1E-6) {
32 rc = -EINVAL;
33 goto err;
34 }
35 #endif
36
37 /* Continue with the calculations only if the data from the accelerometer
38 * is valid (non zero). */
39 if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6) {
40
41 /* Normalise the accelerometer vector. */
42 zsl_vec_to_unit(a);
43
44 /* Define the normalized quaternion of acceleration on the earth's
45 * reference frame, which only has a vertical component. */
46 struct zsl_quat qa = {
47 .r = 0.0,
48 .i = 0.0,
49 .j = 0.0,
50 .k = 1.0
51 };
52
53 /* Calculate an estimation of the gravity field 'v' using the input
54 * quaternion to rotate the normalised acceleration vector in the
55 * earth's reference frame 'qa'. */
56 ZSL_VECTOR_DEF(v, 3);
57 struct zsl_quat qaq;
58 zsl_quat_rot(q, &qa, &qaq);
59 v.data[0] = qaq.i;
60 v.data[1] = qaq.j;
61 v.data[2] = qaq.k;
62
63 /* The orientatiom error is the cross product between measured
64 * direction of gravity 'a' and estimated direction of gravity 'v'. */
65 ZSL_VECTOR_DEF(e, 3);
66 zsl_vec_cross(a, &v, &e);
67
68 /* Compute integral feedback. */
69 integralFB->data[0] += e.data[0] * (1.0 / zsl_fus_mahn_freq);
70 integralFB->data[1] += e.data[1] * (1.0 / zsl_fus_mahn_freq);
71 integralFB->data[2] += e.data[2] * (1.0 / zsl_fus_mahn_freq);
72
73 /* Limit integral values */
74 if(integralFB->data[0] > integral_limit) {
75 integralFB->data[0] = integral_limit;
76 } else if (integralFB->data[0] < -integral_limit) {
77 integralFB->data[0] = -integral_limit;
78 }
79
80 if(integralFB->data[1] > integral_limit) {
81 integralFB->data[1] = integral_limit;
82 } else if (integralFB->data[1] < -integral_limit) {
83 integralFB->data[1] = -integral_limit;
84 }
85
86 if(integralFB->data[2] > integral_limit) {
87 integralFB->data[2] = integral_limit;
88 } else if (integralFB->data[2] < -integral_limit) {
89 integralFB->data[2] = -integral_limit;
90 }
91
92 /* Apply integral feedback multiplied by Ki. */
93 g->data[0] += *Ki * integralFB->data[0];
94 g->data[1] += *Ki * integralFB->data[1];
95 g->data[2] += *Ki * integralFB->data[2];
96
97 /* Apply proportional feedback. */
98 g->data[0] += *Kp * e.data[0];
99 g->data[1] += *Kp * e.data[1];
100 g->data[2] += *Kp * e.data[2];
101 }
102
103 /* Integrate rate of change of the input quaternion using the modified
104 * angular velocity data from the gyroscope. */
105 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_mahn_freq, q);
106
107 /* Normalize the output quaternion. */
108 zsl_quat_to_unit_d(q);
109
110 err:
111 return rc;
112 }
113
zsl_fus_mahony(struct zsl_vec * g,struct zsl_vec * a,struct zsl_vec * m,zsl_real_t * Kp,zsl_real_t * Ki,struct zsl_vec * integralFB,zsl_real_t integral_limit,zsl_real_t * incl,struct zsl_quat * q)114 static int zsl_fus_mahony(struct zsl_vec *g, struct zsl_vec *a,
115 struct zsl_vec *m, zsl_real_t *Kp, zsl_real_t *Ki,
116 struct zsl_vec *integralFB,
117 zsl_real_t integral_limit, zsl_real_t *incl, struct zsl_quat *q)
118 {
119 int rc = 0;
120
121 #if CONFIG_ZSL_BOUNDS_CHECKS
122 /* Make sure that the input vectors are tridimensional. */
123 if ((a != NULL && (a->sz != 3)) || (m != NULL && (m->sz != 3)) ||
124 g->sz != 3 || integralFB->sz != 3) {
125 rc = -EINVAL;
126 goto err;
127 }
128 /* Make sure that the input quaternion is not zero. */
129 if (ZSL_ABS(zsl_quat_magn(q)) < 1E-6) {
130 rc = -EINVAL;
131 goto err;
132 }
133 #endif
134
135 /* Use IMU algorithm if the magnetometer measurement is invalid. */
136 if ((m == NULL) || (ZSL_ABS(zsl_vec_norm(m)) < 1E-6)) {
137 return zsl_fus_mahony_imu(g, a, Kp, Ki, integralFB, integral_limit, incl, q);
138 }
139
140 /* Continue with the calculations only if the data from the accelerometer
141 * is valid (non zero). */
142 if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6) {
143
144 /* Normalise the accelerometer vector. */
145 zsl_vec_to_unit(a);
146
147 /* Normalise the magnetometer vector. */
148 zsl_vec_to_unit(m);
149
150 /* Define the normalized quaternion of acceleration on the earth's
151 * reference frame, which only has a vertical component. */
152 struct zsl_quat qa = {
153 .r = 0.0,
154 .i = 0.0,
155 .j = 0.0,
156 .k = 1.0
157 };
158
159 /* Calculate an estimation of the gravity field 'v' using the input
160 * quaternion to rotate the normalised acceleration vector in the
161 * earth's reference frame 'qa'. */
162 ZSL_VECTOR_DEF(v, 3);
163 struct zsl_quat qaq;
164 zsl_quat_rot(q, &qa, &qaq);
165 v.data[0] = qaq.i;
166 v.data[1] = qaq.j;
167 v.data[2] = qaq.k;
168
169 /* The gravity orientatiom error is the cross product between measured
170 * direction of gravity 'a' and estimated direction of gravity 'v'. */
171 ZSL_VECTOR_DEF(e_g, 3);
172 zsl_vec_cross(a, &v, &e_g);
173
174 /* Turn the data of the magnetometer into a pure quaterion. */
175 struct zsl_quat qm = {
176 .r = 0.0,
177 .i = m->data[0],
178 .j = m->data[1],
179 .k = m->data[2]
180 };
181
182 /* Define the normalized quaternion 'b' of magnetic field on the
183 * earth's reference frame, which only has a x (north) and z (vertical)
184 * components. If the declination angle is known, use it to calculate
185 * this vector. */
186 zsl_real_t bx;
187 zsl_real_t bz;
188
189 if (incl == NULL) {
190 struct zsl_quat h;
191 zsl_quat_rot(q, &qm, &h);
192 bx = ZSL_SQRT(h.i * h.i + h.j * h.j);
193 bz = h.k;
194 } else {
195 bx = ZSL_COS(*incl * ZSL_PI / 180.0);
196 bz = ZSL_SIN(*incl * ZSL_PI / 180.0);
197 }
198
199 struct zsl_quat b = { .r = 0.0, .i = bx, .j = 0.0, .k = bz };
200
201 /* Calculate an estimation of the magnetic field 'bf' using the input
202 * quaternion to rotate the normalised magnetic field quaternion in the
203 * earth's reference frame 'b'. */
204 ZSL_VECTOR_DEF(bf, 3);
205 struct zsl_quat qbq;
206 zsl_quat_rot(q, &b, &qbq);
207 bf.data[0] = qbq.i;
208 bf.data[1] = qbq.j;
209 bf.data[2] = qbq.k;
210
211 /* The magnetic field orientation error is the cross product between
212 * measured direction of magnetic field 'm' and estimated direction of
213 * magnetic field 'bf'. */
214 ZSL_VECTOR_DEF(e_b, 3);
215 zsl_vec_cross(m, &bf, &e_b);
216
217 /* The total orientation error is the sum of the gravity and the
218 * magnetic field errors. */
219 ZSL_VECTOR_DEF(e, 3);
220 zsl_vec_add(&e_g, &e_b, &e);
221
222 /* Compute and apply integral feedback if enabled. */
223 integralFB->data[0] += e.data[0] * (1.0 / zsl_fus_mahn_freq);
224 integralFB->data[1] += e.data[1] * (1.0 / zsl_fus_mahn_freq);
225 integralFB->data[2] += e.data[2] * (1.0 / zsl_fus_mahn_freq);
226
227 /* Limit integral values */
228 if(integralFB->data[0] > integral_limit) {
229 integralFB->data[0] = integral_limit;
230 } else if (integralFB->data[0] < -integral_limit) {
231 integralFB->data[0] = -integral_limit;
232 }
233
234 if(integralFB->data[1] > integral_limit) {
235 integralFB->data[1] = integral_limit;
236 } else if (integralFB->data[1] < -integral_limit) {
237 integralFB->data[1] = -integral_limit;
238 }
239
240 if(integralFB->data[2] > integral_limit) {
241 integralFB->data[2] = integral_limit;
242 } else if (integralFB->data[2] < -integral_limit) {
243 integralFB->data[2] = -integral_limit;
244 }
245
246 /* Apply integral feedback multiplied by Ki. */
247 g->data[0] += *Ki * integralFB->data[0];
248 g->data[1] += *Ki * integralFB->data[1];
249 g->data[2] += *Ki * integralFB->data[2];
250
251 /* Apply proportional feedback. */
252 g->data[0] += *Kp * e.data[0];
253 g->data[1] += *Kp * e.data[1];
254 g->data[2] += *Kp * e.data[2];
255 }
256
257 /* Integrate rate of change of the input quaternion using the modified
258 * angular velocity data from the gyroscope. */
259 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_mahn_freq, q);
260
261 /* Normalize the output quaternion. */
262 zsl_quat_to_unit_d(q);
263
264 err:
265 return rc;
266 }
267
zsl_fus_mahn_init(uint32_t freq,void * cfg)268 int zsl_fus_mahn_init(uint32_t freq, void *cfg)
269 {
270 int rc = 0;
271
272 struct zsl_fus_mahn_cfg *mcfg = cfg;
273
274 (void)mcfg;
275
276 #if CONFIG_ZSL_BOUNDS_CHECKS
277 /* Make sure that the sample frequency is positive. */
278 if (freq <= 0.0) {
279 rc = -EINVAL;
280 goto err;
281 }
282 #endif
283
284 zsl_fus_mahn_freq = freq;
285
286 err:
287 return rc;
288 }
289
zsl_fus_mahn_feed(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * incl,struct zsl_quat * q,void * cfg)290 int zsl_fus_mahn_feed(struct zsl_vec *a, struct zsl_vec *m, struct zsl_vec *g,
291 zsl_real_t *incl, struct zsl_quat *q, void *cfg)
292 {
293 struct zsl_fus_mahn_cfg *mcfg = cfg;
294
295 if (mcfg->kp < 0.0 || mcfg->ki < 0.0) {
296 return -EINVAL;
297 }
298
299 return zsl_fus_mahony(g, a, m, &(mcfg->kp), &(mcfg->ki), &(mcfg->intfb),
300 mcfg->integral_limit, incl, q);
301 }
302
zsl_fus_mahn_error(int error)303 void zsl_fus_mahn_error(int error)
304 {
305 /* ToDo: Log error in default handler. */
306 }
307
