Skip to content

Commit 6fb3881

Browse files
danngreendanngreen
committed
FilteredInterpArray: Fix one-cycle lag in step size when num_updates changes
And add unit tests
1 parent fad142e commit 6fb3881

2 files changed

Lines changed: 229 additions & 1 deletion

File tree

‎tests/filtered_interp_array_tests.cc‎

Lines changed: 217 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,217 @@
1+
#include "doctest.h"
2+
#include "util/filter.hh"
3+
#include "util/filtered_interp_array.hh"
4+
5+
// NoFilter<float> passes values through unchanged, giving exact predictable outputs.
6+
using TestArray1 = FilteredInterpArray<1, NoFilter<float>>;
7+
using TestArray2 = FilteredInterpArray<2, NoFilter<float>>;
8+
9+
TEST_CASE("outputs zero before mark") {
10+
TestArray1 arr;
11+
arr.set_num_updates(4);
12+
float out = -1.f;
13+
arr.add_new_readings([](unsigned) { return 99.f; });
14+
arr.get_interp_values([&](unsigned, float v) { out = v; });
15+
CHECK(out == 0.f);
16+
}
17+
18+
TEST_CASE("no change without mark") {
19+
TestArray1 arr;
20+
arr.set_num_updates(4);
21+
float out = -1.f;
22+
for (int i = 0; i < 8; i++) {
23+
arr.add_new_readings([](unsigned) { return 8.f; });
24+
arr.get_interp_values([&](unsigned, float v) { out = v; });
25+
CHECK(out == 0.f);
26+
}
27+
}
28+
29+
TEST_CASE("basic interpolation") {
30+
TestArray1 arr;
31+
arr.set_num_updates(4);
32+
arr.mark_new_data_ready();
33+
float out = 0.f;
34+
auto output_fn = [&](unsigned, float v) { out = v; };
35+
auto read_fn = [](unsigned) { return 8.f; };
36+
37+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
38+
CHECK(out == doctest::Approx(2.f));
39+
40+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
41+
CHECK(out == doctest::Approx(4.f));
42+
43+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
44+
CHECK(out == doctest::Approx(6.f));
45+
46+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
47+
CHECK(out == 8.f); // snap: exact match, no floating-point accumulation
48+
}
49+
50+
TEST_CASE("snaps to exact target") {
51+
// Verify that the final snap writes target_val directly rather than
52+
// relying on accumulated floating-point steps.
53+
TestArray1 arr;
54+
arr.set_num_updates(3);
55+
arr.mark_new_data_ready();
56+
float out = 0.f;
57+
for (int i = 0; i < 3; i++) {
58+
arr.add_new_readings([](unsigned) { return 1.f / 3.f; });
59+
arr.get_interp_values([&](unsigned, float v) { out = v; });
60+
}
61+
CHECK(out == 1.f / 3.f);
62+
}
63+
64+
TEST_CASE("holds target after snap") {
65+
TestArray1 arr;
66+
arr.set_num_updates(4);
67+
arr.mark_new_data_ready();
68+
float out = 0.f;
69+
auto output_fn = [&](unsigned, float v) { out = v; };
70+
auto read_fn = [](unsigned) { return 8.f; };
71+
72+
for (int i = 0; i < 4; i++) {
73+
arr.add_new_readings(read_fn);
74+
arr.get_interp_values(output_fn);
75+
}
76+
CHECK(out == 8.f);
77+
78+
for (int i = 0; i < 8; i++) {
79+
arr.add_new_readings(read_fn);
80+
arr.get_interp_values(output_fn);
81+
CHECK(out == 8.f);
82+
}
83+
}
84+
85+
TEST_CASE("multi channel interpolates independently") {
86+
TestArray2 arr;
87+
arr.set_num_updates(4);
88+
arr.mark_new_data_ready();
89+
float out0 = 0.f, out1 = 0.f;
90+
auto output_fn = [&](unsigned i, float v) {
91+
if (i == 0) out0 = v;
92+
else out1 = v;
93+
};
94+
// ch0 target=4 (step=1), ch1 target=8 (step=2)
95+
auto read_fn = [](unsigned i) { return i == 0 ? 4.f : 8.f; };
96+
97+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
98+
CHECK(out0 == doctest::Approx(1.f));
99+
CHECK(out1 == doctest::Approx(2.f));
100+
101+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
102+
CHECK(out0 == doctest::Approx(2.f));
103+
CHECK(out1 == doctest::Approx(4.f));
104+
105+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
106+
CHECK(out0 == doctest::Approx(3.f));
107+
CHECK(out1 == doctest::Approx(6.f));
108+
109+
arr.add_new_readings(read_fn); arr.get_interp_values(output_fn);
110+
CHECK(out0 == 4.f); // snap
111+
CHECK(out1 == 8.f); // snap
112+
}
113+
114+
TEST_CASE("adapts num updates to measured period") {
115+
// The ISR fires every 2 frames, but set_num_updates(4) initially.
116+
// Cycle 1: count=0 on entry, set_num_updates skipped, step uses initial num_updates=4.
117+
// Cycle 2: count=2 on entry, set_num_updates(2) fires BEFORE set_new_value,
118+
// so step immediately uses the correct measured period.
119+
TestArray1 arr;
120+
arr.set_num_updates(4);
121+
float out = 0.f;
122+
auto output_fn = [&](unsigned, float v) { out = v; };
123+
124+
// --- Cycle 1: ISR fires, target=8, num_updates=4 ---
125+
arr.mark_new_data_ready();
126+
arr.add_new_readings([](unsigned) { return 8.f; }); // count=0 -> skip set_num_updates
127+
arr.get_interp_values(output_fn); // count=1, step=8/4=2, out=2
128+
CHECK(out == doctest::Approx(2.f));
129+
arr.add_new_readings([](unsigned) { return 8.f; });
130+
arr.get_interp_values(output_fn); // count=2, out=4
131+
CHECK(out == doctest::Approx(4.f));
132+
// ISR fires after 2 frames; count=2 at entry to next cycle
133+
134+
// --- Cycle 2: ISR fires, target=16, immediately adapted ---
135+
arr.mark_new_data_ready();
136+
arr.add_new_readings([](unsigned) { return 16.f; }); // count=2 -> set_num_updates(2), step=(16-4)/2=6
137+
arr.get_interp_values(output_fn); // count=1, out=4+6=10
138+
CHECK(out == doctest::Approx(10.f)); // correctly at the midpoint
139+
arr.add_new_readings([](unsigned) { return 16.f; });
140+
arr.get_interp_values(output_fn); // count=2 >= num_updates=2: snap=16
141+
CHECK(out == 16.f);
142+
143+
// --- Cycle 3: ISR fires, target=0 ---
144+
arr.mark_new_data_ready();
145+
arr.add_new_readings([](unsigned) { return 0.f; }); // count=2 -> set_num_updates(2), step=(0-16)/2=-8
146+
arr.get_interp_values(output_fn); // count=1, out=16-8=8
147+
CHECK(out == doctest::Approx(8.f)); // correctly at the midpoint
148+
arr.add_new_readings([](unsigned) { return 0.f; });
149+
arr.get_interp_values(output_fn); // count=2 >= num_updates=2: snap=0
150+
CHECK(out == 0.f);
151+
}
152+
153+
TEST_CASE("mark multiple times is processed once") {
154+
TestArray1 arr;
155+
arr.set_num_updates(4);
156+
arr.mark_new_data_ready();
157+
arr.mark_new_data_ready(); // redundant: flag is already set
158+
159+
int call_count = 0;
160+
float out = 0.f;
161+
arr.add_new_readings([](unsigned) { return 8.f; });
162+
arr.get_interp_values([&](unsigned, float v) { out = v; call_count++; });
163+
164+
CHECK(call_count == 1); // output_fn called exactly once (1 channel)
165+
CHECK(out == doctest::Approx(2.f)); // normal step, not doubled
166+
}
167+
168+
TEST_CASE("second mark before add uses latest reading") {
169+
// If mark fires multiple times before add_new_readings, the flag is still
170+
// processed exactly once and read_fn is called at add_new_readings time.
171+
TestArray1 arr;
172+
arr.set_num_updates(4);
173+
float out = 0.f;
174+
int call_count = 0;
175+
auto output_fn = [&](unsigned, float v) { out = v; call_count++; };
176+
177+
arr.mark_new_data_ready();
178+
arr.add_new_readings([](unsigned) { return 4.f; });
179+
arr.get_interp_values(output_fn);
180+
CHECK(out == doctest::Approx(1.f)); // stepped once toward 4.f
181+
182+
call_count = 0;
183+
arr.mark_new_data_ready();
184+
arr.mark_new_data_ready(); // redundant second mark before next add
185+
// add_new_readings reads the current value from read_fn (8.f), processed once.
186+
// _update_count is 1 here, so set_num_updates(1) fires -> snap on next get.
187+
arr.add_new_readings([](unsigned) { return 8.f; });
188+
arr.get_interp_values(output_fn);
189+
CHECK(call_count == 1); // flag processed exactly once (1 channel, 1 call)
190+
CHECK(out == 8.f); // snapped to 8.f (num_updates adapted to 1)
191+
}
192+
193+
TEST_CASE("size returns N") {
194+
static_assert(FilteredInterpArray<1, NoFilter<float>>::size() == 1);
195+
static_assert(FilteredInterpArray<3, NoFilter<float>>::size() == 3);
196+
static_assert(FilteredInterpArray<10, NoFilter<float>>::size() == 10);
197+
}
198+
199+
TEST_CASE("filter concept is enforced") {
200+
struct ValidFilter {
201+
float add_val(float v) { return v; }
202+
};
203+
struct NoAddVal {
204+
float process(float v) { return v; } // wrong method name
205+
};
206+
struct WrongReturnType {
207+
void add_val(float) {} // void is not convertible to float
208+
};
209+
210+
// Test the requires expression directly (same constraint as on the class template)
211+
auto satisfies = []<typename F>() {
212+
return IsSimpleFilter<F, float>;
213+
};
214+
static_assert(satisfies.template operator()<ValidFilter>());
215+
static_assert(!satisfies.template operator()<NoAddVal>());
216+
static_assert(!satisfies.template operator()<WrongReturnType>());
217+
}

‎util/filtered_interp_array.hh‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,19 @@
22
#include "util/interp_param.hh"
33
#include <array>
44
#include <atomic>
5+
#include <concepts>
56
#include <cstddef>
7+
#include <span>
8+
9+
template<typename FilterT, typename T>
10+
concept IsSimpleFilter = requires(FilterT v, T t) {
11+
{
12+
v.add_val(t)
13+
} -> std::convertible_to<T>;
14+
};
615

716
template<size_t N, typename Filter, typename T = float>
17+
requires IsSimpleFilter<Filter, T>
818
struct FilteredInterpArray {
919

1020
void mark_new_data_ready() {
@@ -19,9 +29,10 @@ struct FilteredInterpArray {
1929
_new_data_ready = false;
2030

2131
for (unsigned i = 0; i < N; i++) {
22-
_interps[i].set_new_value(_filters[i].add_val(read_fn(i)));
2332
if (_update_count > 0)
2433
_interps[i].set_num_updates(_update_count);
34+
35+
_interps[i].set_new_value(_filters[i].add_val(read_fn(i)));
2536
}
2637
_update_count = 0;
2738
}

0 commit comments

Comments
 (0)