mirror of
https://github.com/google/pebble.git
synced 2025-03-21 11:21:21 +00:00
148 lines
3.5 KiB
C
148 lines
3.5 KiB
C
/*
|
|
* 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 <stdint.h>
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|