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/saam.h>
12
13 static uint32_t zsl_fus_saam_freq;
14
zsl_fus_saam(struct zsl_vec * a,struct zsl_vec * m,struct zsl_quat * q)15 static int zsl_fus_saam(struct zsl_vec *a, struct zsl_vec *m,
16 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->sz != 3 || m->sz != 3) {
23 rc = -EINVAL;
24 goto err;
25 }
26 #endif
27
28 /* Return an error if either of the data in the sensors is missing or
29 * zero. */
30 if ((m == NULL) || (ZSL_ABS(zsl_vec_norm(m)) < 1E-6) ||
31 (a == NULL) || (ZSL_ABS(zsl_vec_norm(a)) < 1E-6)) {
32 rc = -EINVAL;
33 goto err;
34 }
35
36 /* Normalize the acceleration vector. */
37 zsl_vec_to_unit(a);
38
39 /* Normalize the magnetic field vector. */
40 zsl_vec_to_unit(m);
41
42 /* Determine the x (north) and z (vertical) components of the magnetic
43 * field of the earth on the earth's reference frame. */
44 zsl_real_t bz;
45 zsl_vec_dot(a, m, &bz);
46 zsl_real_t bx = ZSL_SQRT(1.0 - bz * bz);
47
48 /* Calculate the orientation quaternion. */
49 q->r = -a->data[1] * (bx + m->data[0]) + a->data[0] * m->data[1];
50 q->i = (a->data[2] - 1.0) * (bx + m->data[0]) +
51 a->data[0] * (bz - m->data[2]);
52 q->j = (a->data[2] - 1.0) * m->data[1] + a->data[1] * (bz - m->data[2]);
53 q->k = a->data[2] * bz - a->data[0] * bx - m->data[2];
54
55 /* Normalize the output quaternion. */
56 zsl_quat_to_unit_d(q);
57
58 err:
59 return rc;
60 }
61
62
63
zsl_fus_saam_init(uint32_t freq,void * cfg)64 int zsl_fus_saam_init(uint32_t freq, void *cfg)
65 {
66 int rc = 0;
67
68 #if CONFIG_ZSL_BOUNDS_CHECKS
69 /* Make sure that the sample frequency is positive. */
70 if (freq <= 0.0) {
71 rc = -EINVAL;
72 goto err;
73 }
74 #endif
75
76 zsl_fus_saam_freq = freq;
77
78 err:
79 return rc;
80 }
81
zsl_fus_saam_feed(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * incl,struct zsl_quat * q,void * cfg)82 int zsl_fus_saam_feed(struct zsl_vec *a, struct zsl_vec *m,
83 struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q, void *cfg)
84 {
85 return zsl_fus_saam(a, m, q);
86 }
87
zsl_fus_saam_error(int error)88 void zsl_fus_saam_error(int error)
89 {
90 /* ToDo: Log error in default handler. */
91 }
92
