Pack and unpack using shifting and masking
This commit is contained in:
parent
5e887a95c5
commit
8cdc1ffe7f
2 changed files with 35 additions and 22 deletions
|
@ -49,21 +49,17 @@ bool CFMControl::writeModem(const unsigned char* data, unsigned int length)
|
||||||
for (unsigned int i = 0U; i < length; i += 3U) {
|
for (unsigned int i = 0U; i < length; i += 3U) {
|
||||||
unsigned short sample1 = 0U;
|
unsigned short sample1 = 0U;
|
||||||
unsigned short sample2 = 0U;
|
unsigned short sample2 = 0U;
|
||||||
unsigned short MASK = 0x0001U;
|
unsigned short MASK = ~(0xFFFFF000);
|
||||||
|
|
||||||
const unsigned char* base = data + i;
|
int pack = 0;
|
||||||
for (unsigned int j = 0U; j < 12U; j++, MASK <<= 1) {
|
char* packPointer = (char*)&pack;
|
||||||
unsigned int pos1 = j;
|
|
||||||
unsigned int pos2 = j + 12U;
|
|
||||||
|
|
||||||
bool b1 = READ_BIT(base, pos1) != 0U;
|
packPointer[1] = data[i];
|
||||||
bool b2 = READ_BIT(base, pos2) != 0U;
|
packPointer[2] = data[i + 1];
|
||||||
|
packPointer[3] = data[i + 2];
|
||||||
|
|
||||||
if (b1)
|
sample2 = (short)(pack & MASK);
|
||||||
sample1 |= MASK;
|
sample1 = (short)(pack >> 12);
|
||||||
if (b2)
|
|
||||||
sample2 |= MASK;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Convert from unsigned short (0 - +4095) to float (-1.0 - +1.0)
|
// Convert from unsigned short (0 - +4095) to float (-1.0 - +1.0)
|
||||||
samples[nSamples++] = (float(sample1) - 2048.0F) / 2048.0F;
|
samples[nSamples++] = (float(sample1) - 2048.0F) / 2048.0F;
|
||||||
|
@ -108,18 +104,24 @@ unsigned int CFMControl::readModem(unsigned char* data, unsigned int space)
|
||||||
// Pre-emphasise the data and other stuff.
|
// Pre-emphasise the data and other stuff.
|
||||||
|
|
||||||
// Pack the floating point data (+1.0 to -1.0) to packed 12-bit samples (+2047 - -2048)
|
// Pack the floating point data (+1.0 to -1.0) to packed 12-bit samples (+2047 - -2048)
|
||||||
unsigned int offset = 0U;
|
int pack = 0;
|
||||||
for (unsigned int i = 0U; i < nSamples; i++) {
|
char* packPointer = (char*)&pack;
|
||||||
unsigned short sample = (unsigned short)((samples[i] + 1.0F) * 2048.0F + 0.5F);
|
unsigned int j = 0U;
|
||||||
unsigned short MASK = 0x0001U;
|
unsigned int i = 0U;
|
||||||
for (unsigned int j = 0U; j < 12U; j++, MASK <<= 1) {
|
for (; i < nSamples && j < space; i += 2, j += 3) {
|
||||||
bool b = (sample & MASK) != 0U;
|
unsigned short sample1 = (unsigned short)((samples[i] + 1.0F) * 2048.0F + 0.5F);
|
||||||
WRITE_BIT(data, offset, b);
|
unsigned short sample2 = (unsigned short)((samples[i + 1] + 1.0F) * 2048.0F + 0.5F);
|
||||||
offset++;
|
|
||||||
}
|
pack = 0;
|
||||||
|
pack = ((int)sample1) << 12;
|
||||||
|
pack |= sample2;
|
||||||
|
|
||||||
|
data[j] = packPointer[1];
|
||||||
|
data[j + 1] = packPointer[2];
|
||||||
|
data[j + 2] = packPointer[3];
|
||||||
}
|
}
|
||||||
|
|
||||||
return nSamples;
|
return j;//return the number of bytes written
|
||||||
}
|
}
|
||||||
|
|
||||||
void CFMControl::clock(unsigned int ms)
|
void CFMControl::clock(unsigned int ms)
|
||||||
|
|
11
FMControl.h
11
FMControl.h
|
@ -22,6 +22,17 @@
|
||||||
#include "FMNetwork.h"
|
#include "FMNetwork.h"
|
||||||
#include "Defines.h"
|
#include "Defines.h"
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int pack;
|
||||||
|
char packBytes[4];
|
||||||
|
};
|
||||||
|
|
||||||
|
} SamplePack;
|
||||||
|
|
||||||
|
|
||||||
class CFMControl {
|
class CFMControl {
|
||||||
public:
|
public:
|
||||||
CFMControl(CFMNetwork* network);
|
CFMControl(CFMNetwork* network);
|
||||||
|
|
Loading…
Reference in a new issue