1 /*
2 * Copyright (c) 2021 Stephanos Ioannidis <root@stephanos.io>
3 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/ztest.h>
9 #include <zephyr/kernel.h>
10 #include <stdlib.h>
11 #include <arm_math.h>
12 #include "../../common/test_common.h"
13
14 #include "f32.pat"
15
16 #define SNR_ERROR_THRESH ((float32_t)120)
17 #define REL_ERROR_THRESH (1.0e-6)
18 #define ABS_ERROR_THRESH (1.0e-7)
19
ZTEST(quaternionmath_f32,test_arm_quaternion_norm_f32)20 ZTEST(quaternionmath_f32, test_arm_quaternion_norm_f32)
21 {
22 size_t length = ARRAY_SIZE(ref_norm);
23 const float32_t *input = (const float32_t *)in_com1;
24 const float32_t *ref = (const float32_t *)ref_norm;
25 float32_t *output;
26
27 /* Allocate output buffer */
28 output = malloc(length * sizeof(float32_t));
29 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
30
31 /* Run test function */
32 arm_quaternion_norm_f32(input, output, length);
33
34 /* Validate output */
35 zassert_true(
36 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
37 ASSERT_MSG_SNR_LIMIT_EXCEED);
38
39 zassert_true(
40 test_close_error_f32(length, output, ref,
41 ABS_ERROR_THRESH, REL_ERROR_THRESH),
42 ASSERT_MSG_ERROR_LIMIT_EXCEED);
43
44 /* Free output buffer */
45 free(output);
46 }
47
ZTEST(quaternionmath_f32,test_arm_quaternion_inverse_f32)48 ZTEST(quaternionmath_f32, test_arm_quaternion_inverse_f32)
49 {
50 size_t length = ARRAY_SIZE(ref_inv);
51 const float32_t *input = (const float32_t *)in_com1;
52 const float32_t *ref = (const float32_t *)ref_inv;
53 float32_t *output;
54
55 /* Allocate output buffer */
56 output = malloc(length * sizeof(float32_t));
57 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
58
59 /* Run test function */
60 arm_quaternion_inverse_f32(input, output, length / 4);
61
62 /* Validate output */
63 zassert_true(
64 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
65 ASSERT_MSG_SNR_LIMIT_EXCEED);
66
67 zassert_true(
68 test_close_error_f32(length, output, ref,
69 ABS_ERROR_THRESH, REL_ERROR_THRESH),
70 ASSERT_MSG_ERROR_LIMIT_EXCEED);
71
72 /* Free output buffer */
73 free(output);
74 }
75
ZTEST(quaternionmath_f32,test_arm_quaternion_conjugate_f32)76 ZTEST(quaternionmath_f32, test_arm_quaternion_conjugate_f32)
77 {
78 size_t length = ARRAY_SIZE(ref_conj);
79 const float32_t *input = (const float32_t *)in_com1;
80 const float32_t *ref = (const float32_t *)ref_conj;
81 float32_t *output;
82
83 /* Allocate output buffer */
84 output = malloc(length * sizeof(float32_t));
85 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
86
87 /* Run test function */
88 arm_quaternion_conjugate_f32(input, output, length / 4);
89
90 /* Validate output */
91 zassert_true(
92 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
93 ASSERT_MSG_SNR_LIMIT_EXCEED);
94
95 zassert_true(
96 test_close_error_f32(length, output, ref,
97 ABS_ERROR_THRESH, REL_ERROR_THRESH),
98 ASSERT_MSG_ERROR_LIMIT_EXCEED);
99
100 /* Free output buffer */
101 free(output);
102 }
103
ZTEST(quaternionmath_f32,test_arm_quaternion_normalize_f32)104 ZTEST(quaternionmath_f32, test_arm_quaternion_normalize_f32)
105 {
106 size_t length = ARRAY_SIZE(ref_normalize);
107 const float32_t *input = (const float32_t *)in_com1;
108 const float32_t *ref = (const float32_t *)ref_normalize;
109 float32_t *output;
110
111 /* Allocate output buffer */
112 output = malloc(length * sizeof(float32_t));
113 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
114
115 /* Run test function */
116 arm_quaternion_normalize_f32(input, output, length / 4);
117
118 /* Validate output */
119 zassert_true(
120 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
121 ASSERT_MSG_SNR_LIMIT_EXCEED);
122
123 zassert_true(
124 test_close_error_f32(length, output, ref,
125 ABS_ERROR_THRESH, REL_ERROR_THRESH),
126 ASSERT_MSG_ERROR_LIMIT_EXCEED);
127
128 /* Free output buffer */
129 free(output);
130 }
131
ZTEST(quaternionmath_f32,test_arm_quaternion_product_single_f32)132 ZTEST(quaternionmath_f32, test_arm_quaternion_product_single_f32)
133 {
134 size_t index;
135 size_t length = ARRAY_SIZE(ref_mult);
136 const float32_t *input1 = (const float32_t *)in_com1;
137 const float32_t *input2 = (const float32_t *)in_com2;
138 const float32_t *ref = (const float32_t *)ref_mult;
139 float32_t *output, *output_buf;
140
141 /* Allocate output buffer */
142 output_buf = malloc(length * sizeof(float32_t));
143 zassert_not_null(output_buf, ASSERT_MSG_BUFFER_ALLOC_FAILED);
144
145 output = output_buf;
146
147 /* Run test function */
148 for (index = 0; index < (length / 4); index++) {
149 arm_quaternion_product_single_f32(input1, input2, output);
150
151 output += 4;
152 input1 += 4;
153 input2 += 4;
154 }
155
156 /* Validate output */
157 zassert_true(
158 test_snr_error_f32(length, output_buf, ref, SNR_ERROR_THRESH),
159 ASSERT_MSG_SNR_LIMIT_EXCEED);
160
161 zassert_true(
162 test_close_error_f32(length, output_buf, ref,
163 ABS_ERROR_THRESH, REL_ERROR_THRESH),
164 ASSERT_MSG_ERROR_LIMIT_EXCEED);
165
166 /* Free output buffer */
167 free(output_buf);
168 }
169
ZTEST(quaternionmath_f32,test_arm_quaternion_product_f32)170 ZTEST(quaternionmath_f32, test_arm_quaternion_product_f32)
171 {
172 size_t length = ARRAY_SIZE(ref_mult);
173 const float32_t *input1 = (const float32_t *)in_com1;
174 const float32_t *input2 = (const float32_t *)in_com2;
175 const float32_t *ref = (const float32_t *)ref_mult;
176 float32_t *output;
177
178 /* Allocate output buffer */
179 output = malloc(length * sizeof(float32_t));
180 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
181
182 /* Run test function */
183 arm_quaternion_product_f32(input1, input2, output, length / 4);
184
185 /* Validate output */
186 zassert_true(
187 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
188 ASSERT_MSG_SNR_LIMIT_EXCEED);
189
190 zassert_true(
191 test_close_error_f32(length, output, ref,
192 ABS_ERROR_THRESH, REL_ERROR_THRESH),
193 ASSERT_MSG_ERROR_LIMIT_EXCEED);
194
195 /* Free output buffer */
196 free(output);
197 }
198
ZTEST(quaternionmath_f32,test_arm_quaternion2rotation_f32)199 ZTEST(quaternionmath_f32, test_arm_quaternion2rotation_f32)
200 {
201 size_t length = ARRAY_SIZE(ref_quat2rot);
202 const float32_t *input = (const float32_t *)in_com1;
203 const float32_t *ref = (const float32_t *)ref_quat2rot;
204 float32_t *output;
205
206 /* Allocate output buffer */
207 output = malloc(length * sizeof(float32_t));
208 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
209
210 /* Run test function */
211 arm_quaternion2rotation_f32(input, output, ARRAY_SIZE(in_com1) / 4);
212
213 /* Validate output */
214 zassert_true(
215 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
216 ASSERT_MSG_SNR_LIMIT_EXCEED);
217
218 zassert_true(
219 test_close_error_f32(length, output, ref,
220 ABS_ERROR_THRESH, REL_ERROR_THRESH),
221 ASSERT_MSG_ERROR_LIMIT_EXCEED);
222
223 /* Free output buffer */
224 free(output);
225 }
226
ZTEST(quaternionmath_f32,test_arm_rotation2quaternion_f32)227 ZTEST(quaternionmath_f32, test_arm_rotation2quaternion_f32)
228 {
229 size_t length = ARRAY_SIZE(ref_rot2quat);
230 const float32_t *input = (const float32_t *)in_rot;
231 const float32_t *ref = (const float32_t *)ref_rot2quat;
232 float32_t *output;
233
234 /* Allocate output buffer */
235 output = malloc(length * sizeof(float32_t));
236 zassert_not_null(output, ASSERT_MSG_BUFFER_ALLOC_FAILED);
237
238 /* Run test function */
239 arm_rotation2quaternion_f32(input, output, ARRAY_SIZE(in_com1) / 4);
240
241 /* Remove ambiguity */
242 for (float32_t *ptr = output; ptr < (output + length); ptr += 4) {
243 if (ptr[0] < 0.0f) {
244 ptr[0] = -ptr[0];
245 ptr[1] = -ptr[1];
246 ptr[2] = -ptr[2];
247 ptr[3] = -ptr[3];
248 }
249 }
250
251 /* Validate output */
252 zassert_true(
253 test_snr_error_f32(length, output, ref, SNR_ERROR_THRESH),
254 ASSERT_MSG_SNR_LIMIT_EXCEED);
255
256 zassert_true(
257 test_close_error_f32(length, output, ref,
258 ABS_ERROR_THRESH, REL_ERROR_THRESH),
259 ASSERT_MSG_ERROR_LIMIT_EXCEED);
260
261 /* Free output buffer */
262 free(output);
263 }
264
265 ZTEST_SUITE(quaternionmath_f32, NULL, NULL, NULL, NULL, NULL);
266
