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/complementary.h>
12
13 static uint32_t zsl_fus_comp_freq = 0;
14
zsl_fus_comp(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * alpha,struct zsl_quat * q)15 static int zsl_fus_comp(struct zsl_vec *a, struct zsl_vec *m,
16 struct zsl_vec *g, zsl_real_t *alpha, 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. Alpha must be
22 * between zero and one (included). */
23 if ((a != NULL && (a->sz != 3)) || (m != NULL && (m->sz != 3)) ||
24 g->sz != 3 || *alpha < 0.0 || *alpha > 1.0) {
25 rc = -EINVAL;
26 goto err;
27 }
28 /* Make sure that the input quaternion is not zero. */
29 if (ZSL_ABS(zsl_quat_magn(q)) < 1E-6) {
30 rc = -EINVAL;
31 goto err;
32 }
33 #endif
34
35 /* Normalize the input quaternion. */
36 zsl_quat_to_unit_d(q);
37
38 /* Estimate the orientation (q_w) using the angular velocity data from the
39 * gyroscope. */
40 struct zsl_quat q_w;
41 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_comp_freq, &q_w);
42
43 /* Continue with the calculations only if the data from the accelerometer
44 * and magnetometer is valid (non zero). */
45 if ((m != NULL) && (ZSL_ABS(zsl_vec_norm(m)) > 1E-6) &&
46 (a != NULL) && (ZSL_ABS(zsl_vec_norm(a)) > 1E-6)) {
47 /* Estimate the orientation (q_am) using the acceleration and magnetic
48 * field data to calculate the attitude. Then convert this orientation
49 * to a unit quaternion. */
50 struct zsl_attitude att;
51 struct zsl_euler e;
52 struct zsl_quat q_am;
53 zsl_att_from_accelmag(a, m, &att);
54 zsl_att_to_euler(&att, &e);
55 zsl_quat_from_euler(&e, &q_am);
56
57 /* The final estimation is a linear interpolation the previous
58 * estimations (q_w and q_am). Alpha is arbitrary and depends on the
59 * reliability of the sensors. For more accurate gyroscope data, alpha
60 * should be closer to zero. For more accurate accelerometer and
61 * magnetometer data, alpha should be closer to 1. */
62 zsl_quat_lerp(&q_w, &q_am, *alpha, q);
63 } else {
64 /* If the data from the accelerometer and magnetometer is invalid, then
65 * set the output quaternion as just q_w. */
66 q->r = q_w.r;
67 q->i = q_w.i;
68 q->j = q_w.j;
69 q->k = q_w.k;
70 }
71
72 /* Normalize the output quaternion. */
73 zsl_quat_to_unit_d(q);
74
75 err:
76 return rc;
77 }
78
zsl_fus_comp_init(uint32_t freq,void * cfg)79 int zsl_fus_comp_init(uint32_t freq, void *cfg)
80 {
81 int rc = 0;
82
83 struct zsl_fus_comp_cfg *mcfg = cfg;
84
85 (void)mcfg;
86
87 #if CONFIG_ZSL_BOUNDS_CHECKS
88 /* Make sure that the sample frequency is positive. */
89 if (freq <= 0.0) {
90 rc = -EINVAL;
91 goto err;
92 }
93 #endif
94
95 zsl_fus_comp_freq = freq;
96
97 err:
98 return rc;
99 }
100
zsl_fus_comp_feed(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * incl,struct zsl_quat * q,void * cfg)101 int zsl_fus_comp_feed(struct zsl_vec *a, struct zsl_vec *m,
102 struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q, void *cfg)
103 {
104 struct zsl_fus_comp_cfg *mcfg = cfg;
105
106 if (mcfg->alpha < 0.0 || mcfg->alpha > 1.0) {
107 return -EINVAL;
108 }
109
110 return zsl_fus_comp(a, m, g, &(mcfg->alpha), q);
111 }
112
zsl_fus_comp_error(int error)113 void zsl_fus_comp_error(int error)
114 {
115 /* ToDo: Log error in default handler. */
116 }
117
