-
Notifications
You must be signed in to change notification settings - Fork 2
/
freq_ignore.c
176 lines (144 loc) · 4.73 KB
/
freq_ignore.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
#include "driver/uart.h"
#endif
#include "freq_ignore.h"
#include "misc.h"
//#define FI_CLOSE_ENOUGH_HZ 300
// a list of frequencies to ignore/skip when scanning
uint32_t ignore_frequencies[64];
int ignore_frequencies_count = 0;
void FI_clear_freq_ignored(void)
{ // clear the ignore list
ignore_frequencies_count = 0;
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_SendText("ignore cleared\r\n");
#endif
}
int FI_freq_ignored(const uint32_t frequency)
{ // return index of the ignored frequency
#ifdef FI_CLOSE_ENOUGH_HZ
if (frequency <= FI_CLOSE_ENOUGH_HZ || frequency >= (0xffffffff - FI_CLOSE_ENOUGH_HZ) || ignore_frequencies_count <= 0)
return -1; // invalid frequency
#else
if (frequency == 0 || frequency == 0xffffffff || ignore_frequencies_count <= 0)
return -1; // invalid frequency
#endif
if (ignore_frequencies_count >= 20)
{ // binary search becomes faster than sequencial as the list grows beyound a certain size
int low = 0;
int high = ignore_frequencies_count;
while (low < high)
{
register int mid = (low + high) / 2;
register uint32_t freq = ignore_frequencies[mid];
#ifdef FI_CLOSE_ENOUGH_HZ
if (abs((int32_t)frequency - (int32_t)freq) <= FI_CLOSE_ENOUGH_HZ)
#else
if (frequency == freq)
#endif
{ // found it
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_printf("ignored bin %u %u\r\n", frequency, mid);
#endif
return mid;
}
if (freq > frequency)
high = mid;
else
low = mid + 1;
}
}
else
{ // sequencial search
register int i;
for (i = 0; i < ignore_frequencies_count; i++)
{
register uint32_t freq = ignore_frequencies[i];
#ifdef FI_CLOSE_ENOUGH_HZ
if (abs((int32_t)frequency - (int32_t)freq) <= FI_CLOSE_ENOUGH_HZ)
#else
if (frequency == freq)
#endif
{ // found it
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_printf("ignored seq %u %u\r\n", frequency, i);
#endif
return i;
}
if (frequency < freq)
return -1; // exit loop early as the list is sorted by frequency
}
}
return -1; // not found
}
bool FI_add_freq_ignored(const uint32_t frequency)
{ // add a new frequency to the ignore list
int i;
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_printf("ignore add %u\r\n", frequency);
#endif
#ifdef FI_CLOSE_ENOUGH_HZ
if (frequency <= FI_CLOSE_ENOUGH_HZ || frequency >= (0xffffffff - FI_CLOSE_ENOUGH_HZ))
return false; // invalid frequency
#else
if (frequency == 0 || frequency == 0xffffffff)
return false; // invalid frequency
#endif
if (ignore_frequencies_count >= (int)ARRAY_SIZE(ignore_frequencies))
{ // the list is full
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_SendText("ignore add full\r\n");
#endif
return false; // failed
}
for (i = 0; i < ignore_frequencies_count; i++)
{
register uint32_t freq = ignore_frequencies[i];
#ifdef FI_CLOSE_ENOUGH_HZ
if (abs((int32_t)frequency - (int32_t)freq) <= FI_CLOSE_ENOUGH_HZ)
#else
if (frequency == freq)
#endif
{ // already in the list
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_SendText("ignore add already\r\n");
#endif
return true;
}
#ifdef FI_CLOSE_ENOUGH_HZ
if (frequency < (freq + FI_CLOSE_ENOUGH_HZ))
break; // exit loop early as the list is sorted by frequency
#else
if (frequency < freq)
break; // exit loop early as the list is sorted by frequency
#endif
}
// found the location to store the new frequency - the list is kept sorted by frequency
// make room for the new frequency
if (i < ignore_frequencies_count)
memmove(&ignore_frequencies[i + 1], &ignore_frequencies[i], sizeof(ignore_frequencies[0]) * (ignore_frequencies_count - i));
// add the frequency to the list
ignore_frequencies[i] = frequency;
ignore_frequencies_count++;
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
for (i = 0; i < ignore_frequencies_count; i++)
UART_printf("%2u %10u\r\n", i, ignore_frequencies[i]);
#endif
return true;
}
void FI_sub_freq_ignored(const uint32_t frequency)
{ // remove a frequency from the ignore list
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
UART_printf("ignore sub %u\r\n", frequency);
#endif
int index = FI_freq_ignored(frequency);
if (index < 0)
return;
if (index < (ignore_frequencies_count - 1))
memmove(&ignore_frequencies[index], &ignore_frequencies[index + 1], sizeof(ignore_frequencies[0]) * (ignore_frequencies_count - 1));
ignore_frequencies_count--;
#if defined(ENABLE_UART) && defined(ENABLE_UART_DEBUG)
for (index = 0; index < ignore_frequencies_count; index++)
UART_printf("%2u %10u\r\n", index, ignore_frequencies[index]);
#endif
}