Skip to content

Commit 64315eb

Browse files
AlexandreRoumaAlexandreRouma
committed
work on ATV demod
1 parent 553204b commit 64315eb

File tree

2 files changed

+247
-181
lines changed

2 files changed

+247
-181
lines changed

‎decoder_modules/atv_decoder/src/linesync.h

+133-79
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,25 @@
55
#include <dsp/multirate/polyphase_bank.h>
66
#include <dsp/math/step.h>
77

8+
#define LINE_SIZE 945
9+
10+
#define SYNC_LEN 70
11+
#define SYNC_SIDE_LEN 17
12+
#define SYNC_L_START (LINE_SIZE - SYNC_SIDE_LEN)
13+
#define SYNC_R_START (SYNC_LEN/2)
14+
#define SYNC_R_END (SYNC_R_START + (SYNC_LEN/2) + SYNC_SIDE_LEN)
15+
#define SYNC_HALF_LEN ((SYNC_LEN/2) + SYNC_SIDE_LEN)
16+
17+
#define EQUAL_LEN 35
18+
19+
#define HBLANK_START SYNC_LEN
20+
#define HBLANK_END 155
21+
#define HBLANK_LEN (HBLANK_END - HBLANK_START + 1)
22+
23+
#define SYNC_LEVEL (-0.428)
24+
25+
#define MAX_LOCK 1000
26+
827
class LineSync : public dsp::Processor<float, float> {
928
using base_type = dsp::Processor<float, float>;
1029
public:
@@ -27,41 +46,17 @@ class LineSync : public dsp::Processor<float, float> {
2746
_interpPhaseCount = interpPhaseCount;
2847
_interpTapCount = interpTapCount;
2948

30-
pcl.init(_muGain, _omegaGain, 0.0, 0.0, 1.0, _omega, _omega * (1.0 - omegaRelLimit), _omega * (1.0 + omegaRelLimit));
3149
generateInterpTaps();
3250
buffer = dsp::buffer::alloc<float>(STREAM_BUFFER_SIZE + _interpTapCount);
3351
bufStart = &buffer[_interpTapCount - 1];
52+
53+
// TODO: Needs tuning, so do the gains
54+
maxPeriod = (int32_t)(1.0001 * (float)(1 << 30));
55+
minPeriod = (int32_t)(0.9999 * (float)(1 << 30));
3456

3557
base_type::init(in);
3658
}
3759

38-
void setOmegaGain(double omegaGain) {
39-
assert(base_type::_block_init);
40-
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
41-
_omegaGain = omegaGain;
42-
pcl.setCoefficients(_muGain, _omegaGain);
43-
}
44-
45-
void setMuGain(double muGain) {
46-
assert(base_type::_block_init);
47-
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
48-
_muGain = muGain;
49-
pcl.setCoefficients(_muGain, _omegaGain);
50-
}
51-
52-
void setOmegaRelLimit(double omegaRelLimit) {
53-
assert(base_type::_block_init);
54-
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
55-
_omegaRelLimit = omegaRelLimit;
56-
pcl.setFreqLimits(_omega * (1.0 - _omegaRelLimit), _omega * (1.0 + _omegaRelLimit));
57-
}
58-
59-
void setSyncLevel(float level) {
60-
assert(base_type::_block_init);
61-
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
62-
syncLevel = level;
63-
}
64-
6560
void setInterpParams(int interpPhaseCount, int interpTapCount) {
6661
assert(base_type::_block_init);
6762
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
@@ -81,8 +76,7 @@ class LineSync : public dsp::Processor<float, float> {
8176
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
8277
base_type::tempStop();
8378
offset = 0;
84-
pcl.phase = 0.0f;
85-
pcl.freq = _omega;
79+
phase = 0;
8680
base_type::tempStart();
8781
}
8882

@@ -93,78 +87,136 @@ class LineSync : public dsp::Processor<float, float> {
9387
// Copy data to work buffer
9488
memcpy(bufStart, base_type::_in->readBuf, count * sizeof(float));
9589

96-
if (test2) {
97-
test2 = false;
98-
offset += 5;
99-
}
100-
101-
// Process all samples
90+
// Process samples while they are available
10291
while (offset < count) {
103-
// Calculate new output value
104-
int phase = std::clamp<int>(floorf(pcl.phase * (float)_interpPhaseCount), 0, _interpPhaseCount - 1);
105-
float outVal;
106-
volk_32f_x2_dot_prod_32f(&outVal, &buffer[offset], interpBank.phases[phase], _interpTapCount);
107-
base_type::out.writeBuf[outCount++] = outVal;
108-
109-
// If the end of the line is reached, process it and determin error
110-
float error = 0;
111-
if (outCount >= 720) {
112-
// Compute averages.
92+
// While the offset is negative, out put zeros
93+
while (offset < 0 && pixel < LINE_SIZE) {
94+
// Output a zero
95+
base_type::out.writeBuf[pixel++] = 0.0f;
96+
97+
// Increment the phase
98+
phase += period;
99+
offset += (phase >> 30);
100+
phase &= 0x3FFFFFFF;
101+
}
102+
103+
// Process as much of a line as possible
104+
while (offset < count && pixel < LINE_SIZE) {
105+
// Compute the output sample
106+
volk_32f_x2_dot_prod_32f(&base_type::out.writeBuf[pixel++], &buffer[offset], interpBank.phases[(phase >> 23) & 0x7F], _interpTapCount);
107+
108+
// Increment the phase
109+
phase += period;
110+
offset += (phase >> 30);
111+
phase &= 0x3FFFFFFF;
112+
}
113+
114+
// If the line is done, process it
115+
if (pixel == LINE_SIZE) {
116+
// Compute averages. (TODO: Try faster method)
113117
float left = 0.0f, right = 0.0f;
114-
for (int i = (720-17); i < 720; i++) {
118+
int lc = 0, rc = 0;
119+
for (int i = SYNC_L_START; i < LINE_SIZE; i++) {
115120
left += base_type::out.writeBuf[i];
121+
lc++;
116122
}
117-
for (int i = 0; i < 27; i++) {
123+
for (int i = 0; i < SYNC_R_START; i++) {
118124
left += base_type::out.writeBuf[i];
125+
lc++;
119126
}
120-
for (int i = 27; i < (54+17); i++) {
127+
for (int i = SYNC_R_START; i < SYNC_R_END; i++) {
121128
right += base_type::out.writeBuf[i];
129+
rc++;
130+
}
131+
132+
// Compute the error
133+
float error = (left - right) * (1.0f/((float)SYNC_HALF_LEN));
134+
135+
// Compute the change in phase and frequency due to the error
136+
float periodDelta = error * _omegaGain;
137+
float phaseDelta = error * _muGain;
138+
139+
// Normalize the phase delta (TODO: Make faster)
140+
while (phaseDelta <= -1.0f) {
141+
phaseDelta += 1.0f;
142+
offset--;
143+
}
144+
while (phaseDelta >= 1.0f) {
145+
phaseDelta -= 1.0f;
146+
offset++;
122147
}
123-
left *= (1.0f/44.0f);
124-
right *= (1.0f/44.0f);
125148

126-
// If the sync is present, compute error
127-
if ((left < syncLevel && right < syncLevel) && !forceLock) {
128-
error = (left + syncBias - right);
129-
locked = true;
149+
// Update the period (TODO: Clamp error*omegaGain to prevent weird shit with corrupt samples)
150+
period += (int32_t)(periodDelta * (float)(1 << 30));
151+
period = std::clamp<uint32_t>(period, minPeriod, maxPeriod);
152+
153+
// Update the phase
154+
phase += (int32_t)(phaseDelta * (float)(1 << 30));
155+
156+
// Normalize the phase
157+
uint32_t overflow = phase >> 30;
158+
if (overflow) {
159+
if (error < 0) {
160+
offset -= 4 - overflow;
161+
}
162+
else {
163+
offset += overflow;
164+
}
130165
}
131-
else {
132-
locked = false;
166+
phase &= 0x3FFFFFFF;
167+
168+
// Find the lowest value
169+
float lowest = INFINITY;
170+
int lowestId = -1;
171+
for (int i = 0; i < LINE_SIZE; i++) {
172+
float val = base_type::out.writeBuf[i];
173+
if (val < lowest) {
174+
lowest = val;
175+
lowestId = i;
176+
}
133177
}
134178

135-
if (++counter >= 100) {
136-
counter = 0;
137-
//flog::warn("Left: {}, Right: {}, Error: {}, Freq: {}, Phase: {}", left, right, error, pcl.freq, pcl.phase);
179+
// Check the the line is in lock
180+
bool lineLocked = (lowestId < SYNC_R_END || lowestId >= SYNC_L_START);
181+
182+
// Update the lock status based on the line lock
183+
if (!lineLocked && locked) {
184+
locked--;
185+
}
186+
else if (lineLocked && locked < MAX_LOCK) {
187+
locked++;
188+
}
189+
190+
// If not locked, attempt to lock by forcing the sync to happen at the right spot
191+
// TODO: This triggers waaaay too easily at low SNR
192+
if (!locked && fastLock) {
193+
offset += lowestId - SYNC_R_START;
194+
locked = MAX_LOCK / 2;
138195
}
139196

140197
// Output line
141-
if (!base_type::out.swap(outCount)) { break; }
142-
outCount = 0;
198+
if (!base_type::out.swap(LINE_SIZE)) { break; }
199+
pixel = 0;
143200
}
144-
145-
// Advance symbol offset and phase
146-
pcl.advance(error);
147-
float delta = floorf(pcl.phase);
148-
offset += delta;
149-
pcl.phase -= delta;
150201
}
202+
203+
// Get the offset ready for the next buffer
151204
offset -= count;
152205

153206
// Update delay buffer
154207
memmove(buffer, &buffer[count], (_interpTapCount - 1) * sizeof(float));
155208

156209
// Swap if some data was generated
157210
base_type::_in->flush();
158-
return outCount;
211+
return 0;
159212
}
160213

161-
bool locked = false;
162-
bool test2 = false;
214+
float syncBias = 0;
163215

164-
float syncBias = 0.0f;
165-
bool forceLock = false;
216+
uint32_t period = (0x800072F3 >> 1);//(1 << 31) + 1;
166217

167-
int counter = 0;
218+
int locked = 0;
219+
bool fastLock = true;
168220

169221
protected:
170222
void generateInterpTaps() {
@@ -175,19 +227,21 @@ class LineSync : public dsp::Processor<float, float> {
175227
}
176228

177229
dsp::multirate::PolyphaseBank<float> interpBank;
178-
dsp::loop::PhaseControlLoop<double, false> pcl;
179230

180231
double _omega;
181232
double _omegaGain;
182233
double _muGain;
183234
double _omegaRelLimit;
184235
int _interpPhaseCount;
185236
int _interpTapCount;
186-
187-
int offset = 0;
188-
int outCount = 0;
189237
float* buffer;
190238
float* bufStart;
191-
239+
240+
uint32_t phase = 0;
241+
uint32_t maxPeriod;
242+
uint32_t minPeriod;
192243
float syncLevel = -0.03f;
244+
245+
int offset = 0;
246+
int pixel = 0;
193247
};

0 commit comments

Comments
 (0)