-
Notifications
You must be signed in to change notification settings - Fork 1
/
FMDownSampler.cpp
80 lines (66 loc) · 1.96 KB
/
FMDownSampler.cpp
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
/*
* Copyright (C) 2020 by Jonathan Naylor G4KLX
* Copyright (C) 2020 by Geoffrey Merck F4FXL - KC3FRA
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "Config.h"
#if defined(MODE_FM)
#include "FMDownSampler.h"
CFMDownSampler::CFMDownSampler(uint16_t length) :
m_ringBuffer(length),
m_samplePack(0U),
m_samplePackPointer(NULL),
m_sampleIndex(0U)
{
m_samplePackPointer = (uint8_t*)&m_samplePack;
}
void CFMDownSampler::addSample(q15_t sample)
{
uint32_t usample = uint32_t(int32_t(sample) + 2048);
//only take one of three samples
switch(m_sampleIndex){
case 0:
m_samplePack = usample << 12;
break;
case 3:{
m_samplePack |= usample;
//we did not use MSB; skip it
TSamplePairPack pair{m_samplePackPointer[0U], m_samplePackPointer[1U], m_samplePackPointer[2U]};
m_ringBuffer.put(pair);
m_samplePack = 0U;//reset the sample pack
}
break;
default:
//Just skip this sample
break;
}
m_sampleIndex++;
if (m_sampleIndex >= 6U)//did we pack two samples ?
m_sampleIndex = 0U;
}
bool CFMDownSampler::getPackedData(TSamplePairPack& data)
{
return m_ringBuffer.get(data);
}
uint16_t CFMDownSampler::getData()
{
return m_ringBuffer.getData();
}
void CFMDownSampler::reset()
{
m_sampleIndex = 0U;
}
#endif