1 /*
2 * Copyright (c) 2021 Marti Riba Pons
3 * Copyright (c) 2021 Kevin Townsend
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <stdio.h>
9 #include <math.h>
10 #include <errno.h>
11 #include <zsl/orientation/compass.h>
12
zsl_comp_dms_to_dd(zsl_real_t d,zsl_real_t m,zsl_real_t s,zsl_real_t * dd)13 int zsl_comp_dms_to_dd(zsl_real_t d, zsl_real_t m, zsl_real_t s, zsl_real_t *dd)
14 {
15 int rc = 0;
16
17 *dd = 0.0;
18
19 #if CONFIG_ZSL_BOUNDS_CHECKS
20 /* Minutes should always be positive and less than 60. */
21 if ((m < 0.0) || (m > 60.0)) {
22 rc = -EINVAL;
23 goto err;
24 }
25
26 /* Seconds should always be positive and less than 60. */
27 if ((s < 0.0) || (s > 60.0)) {
28 rc = -EINVAL;
29 goto err;
30 }
31
32 /* Degrees longitude must be between -180.0 and 180.0, and latitude must be
33 * between -90.0 and 90.0. Since we don't know which is being converted,
34 * the best we can do is limit this to the worst-case scenario.
35 */
36 if ((d < -180.0) || (d > 180.0)) {
37 rc = -EINVAL;
38 goto err;
39 }
40 #endif
41
42 /* Adjust for negative values with south and west. */
43 if (d < 0.0) {
44 /* South and west. */
45 *dd = d - (m / 60.0) - (s / 3600.0);
46 } else {
47 /* North and east. */
48 *dd = d + (m / 60.0) + (s / 3600.0);
49 }
50
51 err:
52 return rc;
53 }
54
zsl_comp_magn_north(struct zsl_vec * m,zsl_real_t * d)55 int zsl_comp_magn_north(struct zsl_vec *m, zsl_real_t *d)
56 {
57 int rc = 0;
58
59 #if CONFIG_ZSL_BOUNDS_CHECKS
60 /* Make sure that the input vector have dimension 3. */
61 if (m->sz != 3) {
62 rc = -EINVAL;
63 goto err;
64 }
65 #endif
66
67 if (ZSL_ABS(m->data[1]) < 1E-6) {
68 if (m->data[0] > 0.0) {
69 *d = 0.0;
70 } else {
71 *d = 180.0;
72 }
73 } else if (m->data[1] > 0.0) {
74 *d = 90.0 - ZSL_ATAN2(m->data[0], m->data[1]) * 180.0 / ZSL_PI;
75 } else if (m->data[1] < 0.0) {
76 *d = 270.0 - ZSL_ATAN2(m->data[0], m->data[1]) * 180.0 / ZSL_PI;
77 }
78
79 err:
80 return rc;
81 }
82
zsl_comp_geo_north(struct zsl_vec * m,zsl_real_t dec,zsl_real_t * d)83 int zsl_comp_geo_north(struct zsl_vec *m, zsl_real_t dec, zsl_real_t *d)
84 {
85 int rc = 0;
86
87 #if CONFIG_ZSL_BOUNDS_CHECKS
88 /* Make sure that the input vector have dimension 3. */
89 if (m->sz != 3) {
90 rc = -EINVAL;
91 goto err;
92 }
93 #endif
94
95 zsl_comp_magn_north(m, d);
96 *d += dec;
97
98 err:
99 return rc;
100 }
101
