1 /*
2 * Copyright (c) 2021 Kevin Townsend
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <math.h>
8 #include <errno.h>
9 #include <zsl/zsl.h>
10 #include <zsl/orientation/orientation.h>
11
zsl_att_to_vec(struct zsl_attitude * a,struct zsl_vec * v)12 int zsl_att_to_vec(struct zsl_attitude *a, struct zsl_vec *v)
13 {
14 int rc = 0;
15
16 v->sz = 3;
17 v->data = a->idx;
18
19 return rc;
20 }
21
zsl_att_to_euler(struct zsl_attitude * a,struct zsl_euler * e)22 int zsl_att_to_euler(struct zsl_attitude *a, struct zsl_euler *e)
23 {
24 int rc = 0;
25
26 e->x = a->roll * ZSL_DEG_TO_RAD;
27 e->y = a->pitch * ZSL_DEG_TO_RAD;
28 e->z = a->yaw * ZSL_DEG_TO_RAD;
29
30 return rc;
31 }
32
zsl_att_from_euler(struct zsl_euler * e,struct zsl_attitude * a)33 int zsl_att_from_euler(struct zsl_euler *e, struct zsl_attitude *a)
34 {
35 int rc = 0;
36
37 a->roll = e->x * ZSL_RAD_TO_DEG;
38 a->pitch = e->y * ZSL_RAD_TO_DEG;
39 a->yaw = e->z * ZSL_RAD_TO_DEG;
40 a->status_bits = 0;
41
42 return rc;
43 }
44
zsl_att_from_accelmag(struct zsl_vec * accel,struct zsl_vec * mag,struct zsl_attitude * a)45 int zsl_att_from_accelmag(struct zsl_vec *accel, struct zsl_vec *mag,
46 struct zsl_attitude *a)
47 {
48 int rc = 0;
49
50 #if CONFIG_ZSL_BOUNDS_CHECKS
51 /* Make sure that the accelerometer and magnetometer vectors are size 3. */
52 if (accel->sz != 3 || mag->sz != 3) {
53 rc = -EINVAL;
54 goto err;
55 }
56 #endif
57
58 ZSL_VECTOR_DEF(mag_unit, 3);
59 zsl_vec_copy(&mag_unit, mag);
60 zsl_vec_to_unit(&mag_unit);
61
62 struct zsl_attitude att;
63
64 zsl_att_from_accel(accel, &att);
65
66 zsl_real_t nom = mag_unit.data[2] * ZSL_SIN(att.pitch * ZSL_DEG_TO_RAD) -
67 mag_unit.data[1] * ZSL_COS(att.pitch * ZSL_DEG_TO_RAD);
68 zsl_real_t den = mag_unit.data[0] * ZSL_COS(att.roll * ZSL_DEG_TO_RAD) +
69 ZSL_SIN(att.roll * ZSL_DEG_TO_RAD) *
70 (mag_unit.data[1] * ZSL_SIN(att.pitch * ZSL_DEG_TO_RAD) +
71 mag_unit.data[2] * ZSL_COS(att.pitch * ZSL_DEG_TO_RAD));
72
73 a->roll = att.roll;
74 a->pitch = att.pitch;
75 a->yaw = ZSL_ATAN2(nom, den) * ZSL_RAD_TO_DEG;
76
77 #if CONFIG_ZSL_BOUNDS_CHECKS
78 err:
79 #endif
80 return rc;
81 }
82
zsl_att_from_accel(struct zsl_vec * accel,struct zsl_attitude * a)83 int zsl_att_from_accel(struct zsl_vec *accel, struct zsl_attitude *a)
84 {
85 int rc = 0;
86
87 #if CONFIG_ZSL_BOUNDS_CHECKS
88 /* Make sure that the accelerometer vector is size 3. */
89 if (accel->sz != 3) {
90 rc = -EINVAL;
91 goto err;
92 }
93 #endif
94
95 ZSL_VECTOR_DEF(accel_unit, 3);
96 zsl_vec_copy(&accel_unit, accel);
97 zsl_vec_to_unit(&accel_unit);
98
99 zsl_real_t ss = accel_unit.data[1] * accel_unit.data[1] +
100 accel_unit.data[2] * accel_unit.data[2];
101
102 a->roll = ZSL_ATAN2(accel_unit.data[1], accel_unit.data[2])
103 * ZSL_RAD_TO_DEG;
104 a->pitch = ZSL_ATAN2(-accel_unit.data[0], ZSL_SQRT(ss))
105 * ZSL_RAD_TO_DEG;
106 a->yaw = 0.0;
107
108 #if CONFIG_ZSL_BOUNDS_CHECKS
109 err:
110 #endif
111 return rc;
112 }
113
zsl_att_accel_angle(struct zsl_vec * a1,struct zsl_vec * a2,zsl_real_t * b)114 int zsl_att_accel_angle(struct zsl_vec *a1, struct zsl_vec *a2, zsl_real_t *b)
115 {
116 int rc = 0;
117
118 #if CONFIG_ZSL_BOUNDS_CHECKS
119 /* Make sure that the accelerometer vectors are size 3. */
120 if (a1->sz != 3 || a2->sz != 3) {
121 rc = -EINVAL;
122 goto err;
123 }
124 #endif
125
126 zsl_real_t a1_norm, a2_norm, dot;
127
128 a1_norm = zsl_vec_norm(a1);
129 a2_norm = zsl_vec_norm(a2);
130 zsl_vec_dot(a1, a2, &dot);
131
132 *b = ZSL_ACOS(dot / (a1_norm * a2_norm));
133
134 #if CONFIG_ZSL_BOUNDS_CHECKS
135 err:
136 #endif
137 return rc;
138 }
139
