mirror
/
radiosonde_auto_rx
kopia lustrzana https://github.com/projecthorus/radiosonde_auto_rx
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
radiosonde_auto_rx/m10/M10Decoder.cpp

446 wiersze
13 KiB
C++
Czysty Wina Historia

/*
* File: M10Decoder.cpp
* Author: Viproz
* Used code from rs1729
* Created on December 12, 2018, 11:31 PM
*/
#include "M10Decoder.h"
#include "M10GtopParser.h"
#include "M10TrimbleParser.h"
char M10Decoder::header[] = "10011001100110010100110010011001";
M10Decoder::M10Decoder() {
m10Gtop = new M10GtopParser();
m10Trimble = new M10TrimbleParser();
m10Parser = m10Gtop;
frameSamples = NULL;
audioFile = NULL;
}
M10Decoder::~M10Decoder() {
delete m10Gtop;
delete m10Trimble;
if (frameSamples)
delete frameSamples;
if (audioFile)
delete audioFile;
}
int M10Decoder::startDecode(std::string fname) {
filename = fname;
int error = 0;
audioFile = new AudioFile(fname, baudRate, &error);
if (error) {
return error;
}
samplesPerBit = audioFile->getSamplesPerBit();
samplesBufLength = (DATA_LENGTH * 8 + 100) * samplesPerBit * 2;
frameSamples = new std::vector<int>(samplesBufLength);
double res = 1;
bool supported = true;
while (res != EOF_INT) {
res = findFrameStart();
if (res == EOF_INT)
break;
totalFrames++;
if (decodeMessage(res) == EOF_INT)
break;
long sondeType = ((long) frame_bytes[1] << 8) + (long) frame_bytes[2];
frameLength = frame_bytes[0];
supported = true;
switch (sondeType) {
case 0xAF02:
m10Parser = m10Gtop;
break;
case 0x9F20:
m10Parser = m10Trimble;
break;
default:
supported = false;
}
bool correctCRC = checkCRC();
if (correctCRC || verboseLevel >= 1) {
if (!supported) {
fprintf(stderr, "Not supported : %#06x\n", (unsigned int) sondeType);
continue;
}
if (correctCRC) {
correctFrames++;
lastGoodFrame = frame_bytes;
}
m10Parser->changeData(frame_bytes, correctCRC);
m10Parser->printFrame();
if (!correctCRC) {
if (correctFrames == 0 && tryStats) // Add the frame to the record
m10Parser->addToStats();
else if (tryRepair && correctFrames != 0) // Put the last correct to repair
m10Parser->changeData(lastGoodFrame, correctFrames > 0);
}
}
}
if (tryStats && verboseLevel >= 1)
m10Parser->printStatsFrame();
if (dispResult)
fprintf(stderr, "Result of %i/%i decoding\n", correctFrames, totalFrames);
return 0;
}
double M10Decoder::findFrameStart() {
int headerLength = strlen(header);
double posData[headerLength];
char decoded[headerLength];
int currentIndex = 0;
for (int i = 0; i < headerLength; ++i)
decoded[i] = '0'; // Fill in decoded to compare later
int prevV = 1;
int len = 0;
int v;
int smallBufLen = (int) (samplesPerBit * 5.);
std::vector<int> vals(smallBufLen);
int valIndex = 0;
double activeSum = 0;
for (int j = 0; 1; ++j) {
v = audioFile->readSignedSample();
vals[valIndex++ % smallBufLen] = v;
if (v == EOF_INT)
return EOF_INT;
// Average over the last 6 samples to comply with a global offset
activeSum = (activeSum + (double) v)*(samplesPerBit * AVG_NUM) / (samplesPerBit * AVG_NUM + 1.);
v = v - activeSum / (samplesPerBit * AVG_NUM);
len++;
if (v * prevV > 0) // If the signs are the same
continue;
for (int i = 0; i < round((double) len / samplesPerBit); ++i) { // Multiple bits in one detection
if (prevV < 0)
decoded[currentIndex] = '1';
else
decoded[currentIndex] = '0';
// Store the position of the start of the bit
posData[currentIndex] = (double) j - (1. - (double) i / round((double) len / samplesPerBit))*(double) len;
char normal = 1;
char inv = 1;
int headerIndex = 0;
// Check if the header is correct
for (int k = 0; k < headerLength; ++k) {
if (decoded[(k + currentIndex + 1) % headerLength] == header[headerIndex])
inv = 0;
else
normal = 0;
headerIndex++;
}
if (normal || inv) {
// Calculate the real position of the data averaging over the headerLength samples
double pos = 0;
for (int k = 0; k < headerLength; ++k) {
pos += posData[(k + currentIndex + 1) % headerLength] + (double) (headerLength - k) * samplesPerBit;
//fprintf(stderr, "%.2f\n", posData[(k + currentIndex + 1) % headerLength] + (double) (headerLength - k) * samplesPerBit);
}
pos /= (double) headerLength;
int tmpIndex = 0;
valIndex--;
valIndex %= smallBufLen;
if (j - (int) pos > smallBufLen) // If absurdly long way back
continue;
// Store the previous values in the buffer
for (curIndex = 0; curIndex < (j - (int) pos); ++curIndex) {
tmpIndex = valIndex + curIndex - (j - (int) pos);
if (tmpIndex < 0)
tmpIndex += smallBufLen;
frameSamples->at(curIndex) = vals.at(tmpIndex);
}
// Only the weight of the first bit is useful, they are stored already
return pos - (int) pos;
}
currentIndex = (currentIndex + 1) % headerLength;
}
len = 0;
prevV = v;
}
}
int M10Decoder::decodeMessage(double initialPos) {
std::array<unsigned char, DATA_LENGTH> frameBackup;
int v;
for (; curIndex < samplesBufLength; ++curIndex) {
v = audioFile->readSignedSample();
if (v == EOF_INT)
return EOF_INT;
frameSamples->at(curIndex) = v;
}
// Reset the index
curIndex = 0;
int ret;
ret = decodeMethodCompare(initialPos);
if (ret == 0)
return 0;
if (ret == EOF_INT)
return EOF_INT;
if ((tryRepair && correctFrames != 0) || (correctFrames == 0 && tryStats)) {
frameBackup = frame_bytes;
frame_bytes = m10Parser->replaceWithPrevious(frame_bytes);
if (checkCRC())
return 0;
frame_bytes = frameBackup;
}
if (trySign) {
// Reset the index
curIndex = 0;
ret = decodeMethodSign(initialPos);
if (ret == 0)
return 0;
if (ret == EOF_INT)
return EOF_INT;
if ((tryRepair && correctFrames != 0) || (correctFrames == 0 && tryStats)) {
frameBackup = frame_bytes;
frame_bytes = m10Parser->replaceWithPrevious(frame_bytes);
if (checkCRC())
return 0;
frame_bytes = frameBackup;
}
}
return 1;
}
int M10Decoder::decodeMethodCompare(double initialPos) {
char bit0 = 2;
double j = initialPos;
double sum = 0;
int val = getNextBufferValue(); // Read the first value
if (val == EOF_INT)
return EOF_INT;
for (int k = 0; k < FRAME_LEN * 8 + AUX_LEN * 8 + 8; ++k) { // Iterate through needed bits
// val is set in the previous round
// Add the first part of the value weighted correctly
sum = (1. - (j - floor(j)))*(double) val;
for (int i = ceil(j); i < j + samplesPerBit - 1; ++i) { // Full vals in the middle
val = getNextBufferValue();
if (val == EOF_INT)
return EOF_INT;
sum += (double) val;
}
val = getNextBufferValue();
if (val == EOF_INT)
return EOF_INT;
// Add the rest of the value
sum += (j + samplesPerBit - floor(j + samplesPerBit))*(double) val;
double moy1 = sum / samplesPerBit;
j += samplesPerBit;
// Same for the second bit
sum = (1 - (j - floor(j)))*(double) val;
for (int i = ceil(j); i < j + samplesPerBit - 1; ++i) { // Full vals in the middle
val = getNextBufferValue();
if (val == EOF_INT)
return EOF_INT;
sum += (double) val;
}
val = getNextBufferValue();
if (val == EOF_INT)
return EOF_INT;
sum += (j + samplesPerBit - floor(j + samplesPerBit))*(double) val;
double moy2 = sum / samplesPerBit;
j += samplesPerBit;
// Determination of a combination of raw bits or 10 or 01.
if (moy1 > moy2) {
if (bit0 == 0)
frame_bits[k] = '1';
else
frame_bits[k] = '0';
bit0 = 0;
} else {
if (bit0 == 1)
frame_bits[k] = '1';
else
frame_bits[k] = '0';
bit0 = 1;
}
}
bits2bytes();
return !checkCRC();
}
int M10Decoder::decodeMethodSign(double initialPos) {
char bit0 = 2;
double j = initialPos;
double sum = 0;
int val = audioFile->averageNormalizeSample(getNextBufferValue()); // Read the first value
if (val == EOF_INT)
return EOF_INT;
for (int k = 0; k < FRAME_LEN * 8 + AUX_LEN * 8 + 8; ++k) { // Iterate through needed bits
// val is set in the previous round
// Add the first part of the value weighted correctly
sum = (1. - (j - floor(j)))*(double) val;
for (int i = ceil(j); i < j + samplesPerBit - 1; ++i) { // Full vals in the middle
val = audioFile->averageNormalizeSample(getNextBufferValue());
if (val == EOF_INT)
return EOF_INT;
sum += val;
}
val = audioFile->averageNormalizeSample(getNextBufferValue());
if (val == EOF_INT)
return EOF_INT;
// Add the rest of the value
sum += (j + samplesPerBit - floor(j + samplesPerBit))*(double) val;
double moy1 = sum / samplesPerBit;
j += samplesPerBit;
// Same for the second bit
sum = (1 - (j - floor(j)))*(double) val;
for (int i = ceil(j); i < j + samplesPerBit - 1; ++i) { // Full vals in the middle
val = audioFile->averageNormalizeSample(getNextBufferValue());
if (val == EOF_INT)
return EOF_INT;
sum += (double) val;
}
val = audioFile->averageNormalizeSample(getNextBufferValue());
if (val == EOF_INT)
return EOF_INT;
sum += (j + samplesPerBit - floor(j + samplesPerBit))*(double) val;
double moy2 = sum / samplesPerBit;
j += samplesPerBit;
// Determination of a combination of raw bits or 10 or 01.
if (moy1 > moy2) {
if (bit0 == 0)
frame_bits[k] = '1';
else
frame_bits[k] = '0';
bit0 = 0;
} else {
if (bit0 == 1)
frame_bits[k] = '1';
else
frame_bits[k] = '0';
bit0 = 1;
}
}
bits2bytes();
return !checkCRC();
}
void M10Decoder::setRaw(bool b) {
dispRaw = b;
m10Gtop->setRaw(b);
m10Trimble->setRaw(b);
}
int M10Decoder::getNextBufferValue() {
if (curIndex < samplesBufLength)
return frameSamples->at(curIndex++);
else {
fprintf(stderr, "Error, end of buffer.\n");
return EOF_INT;
}
}
bool M10Decoder::checkCRC() {
int i, cs;
cs = 0;
for (i = 0; i < frameLength-1; i++) {
cs = update_checkM10(cs, frame_bytes[i]);
}
return ((cs & 0xFFFF) != 0) && ((cs & 0xFFFF) == ((frame_bytes[frameLength-1] << 8) | frame_bytes[frameLength]));
}
int M10Decoder::update_checkM10(int c, unsigned short b) {
int c0, c1, t, t6, t7, s;
c1 = c & 0xFF;
// B
b = (b >> 1) | ((b & 1) << 7);
b ^= (b >> 2) & 0xFF;
// A1
t6 = (c & 1) ^ ((c >> 2) & 1) ^ ((c >> 4) & 1);
t7 = ((c >> 1) & 1) ^ ((c >> 3) & 1) ^ ((c >> 5) & 1);
t = (c & 0x3F) | (t6 << 6) | (t7 << 7);
// A2
s = (c >> 7) & 0xFF;
s ^= (s >> 2) & 0xFF;
c0 = b ^ t ^ s;
return ((c1 << 8) | c0) & 0xFFFF;
}
void M10Decoder::bits2bytes() {
int i, bit, d, byteval;
int bitpos, bytepos;
bitpos = 0;
bytepos = 0;
while (bytepos < FRAME_LEN + AUX_LEN) {
byteval = 0;
d = 1;
for (i = 0; i < 8; i++) {
//bit=*(bitstr+bitpos+i); /* little endian */
bit = frame_bits[bitpos + 7 - i]; /* big endian */
if (bit == '1')
byteval += d;
else
byteval += 0;
d <<= 1;
}
bitpos += 8;
frame_bytes[bytepos++] = byteval & 0xFF;
}
}
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik