/* * Copyright 2024 Google LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "services/common/ecompass.h" #include "util/math.h" #include "clar.h" #include "stubs_language_ui.h" #include "stubs_logging.h" #include "stubs_pbl_malloc.h" #include "stubs_serial.h" #include typedef struct { int16_t raw_samples[4][3]; int16_t sphere_fit_corr[3]; } SampleData; static SampleData s_sample_data[6] = { [0] = { { { 2779, -2079, -1309 }, { 2616, -2007, -1679 }, { 3179, -2119, -1329 }, { 3151, -1725, -1359 } }, { 2979, -1954, -1600 } }, [1] = { { { 3113, -1684, -1384 }, { 2770, -1627, -1577 }, { 2636, -1978, -1550 }, { 2824, -1709, -1969 } }, { 3012, -1930, -1688 } }, [2] = { { { 2854, -1748, -2000 }, { 2636, -1847, -1619 }, { 2812, -2137, -1388 }, { 3326, -1995, -1372 }, }, { 3042, -1935, -1675 } }, [3] = { { { 3348, -1963, -1391 }, { 3208, -1615, -1511 }, { 2814, -1584, -1758 }, { 3001, -1840, -2066 }, }, { 2988, -1972, -1646 } }, [4] = { { { 3054, -1881, -2082 }, { 2789, -1672, -1888 }, { 2664, -1863, -1500 }, { 3161, -1997, -1293 } }, { 3029, -1927, -1675 } }, [5] = { { { 3195, -1941, -1300 }, { 3183, -1615, -1482 }, { 2927, -1579, -1845 }, { 3064, -2022, -2094 } }, { 3036 -1947 -1685 } } }; static int16_t expected_final_solution[3] = { 3017, -1948, -1668 }; void test_analytics__initialize(void) { } void test_analytics__cleanup(void) { } int32_t integer_sqrt(int64_t x) { if (x < 0) { return 0; } int64_t last_res = 0x3fff; uint16_t iterations = 0; while ((last_res > 0) && (iterations < 15)) { last_res = ((x / last_res) + last_res)/2; iterations++; } return (last_res); } static void solution_and_estimate_match(int16_t *solution, int16_t *correction) { for (int i = 0; i < 3; i++) { int diff = ABS(solution[i] - correction[i]); cl_assert(diff < 2); } } void test_compass_cal__sphere_fit(void) { int num_entries = sizeof(s_sample_data) / sizeof(SampleData); int16_t solution[3]; int rv; for (int i = 0; i < num_entries; i++) { for (int j = 0; j < 4; j++) { rv = ecomp_corr_add_raw_mag_sample(s_sample_data[i].raw_samples[j], NULL, solution); if (j != 3) { // add the same sample twice to make sure close values are thrown away rv = ecomp_corr_add_raw_mag_sample(s_sample_data[i].raw_samples[j], NULL, solution); cl_assert_equal_i(rv, MagCalStatusNoSolution); } } cl_assert_equal_i(rv, ((num_entries - 1) == i) ? MagCalStatusNewLockedSolutionAvail : MagCalStatusNewSolutionAvail); if (rv == MagCalStatusNewSolutionAvail) { solution_and_estimate_match(solution, s_sample_data[i].sphere_fit_corr); // should be avg of last 3 solutions } else if (rv == MagCalStatusNewLockedSolutionAvail) { solution_and_estimate_match(solution, expected_final_solution); } } }