feat: sstv decode
This commit is contained in:
parent
c0c2687135
commit
dc40229644
21
include/sstv.h
Normal file
21
include/sstv.h
Normal file
@ -0,0 +1,21 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "inttypes.h"
|
||||||
|
|
||||||
|
#define float32_t float
|
||||||
|
|
||||||
|
#define N 1024
|
||||||
|
#define FS 16000
|
||||||
|
#define RE FS / N
|
||||||
|
#define MN 256
|
||||||
|
|
||||||
|
#define FREQ_BIT1 1100
|
||||||
|
#define FREQ_SYNC 1200
|
||||||
|
#define FREQ_BIT0 1300
|
||||||
|
#define FREQ_START 1900
|
||||||
|
#define FREQ_BLACK 1500
|
||||||
|
#define FREQ_WHITE 7500
|
||||||
|
#define FREQ_RANGE FREQ_WHITE - FREQ_BLACK
|
||||||
|
#define COLOR_SCALE 256
|
||||||
|
|
||||||
|
uint8_t decode_sstv();
|
||||||
214
src/sstv.c
Normal file
214
src/sstv.c
Normal file
@ -0,0 +1,214 @@
|
|||||||
|
#include "fft/fft.h"
|
||||||
|
#include "fft/table.h"
|
||||||
|
#include "math.h"
|
||||||
|
#include "sstv.h"
|
||||||
|
#include "stdio.h"
|
||||||
|
|
||||||
|
static q15_t buffer[MN];
|
||||||
|
static q15_t data[N];
|
||||||
|
static q15_t* pData = &data[0];
|
||||||
|
static q15_t img[1843202];
|
||||||
|
static q15_t* pImg = &img[0];
|
||||||
|
static uint8_t colorBuf[960];
|
||||||
|
|
||||||
|
static const float32_t color_re = (float32_t)(FREQ_RANGE) / (COLOR_SCALE - 1);
|
||||||
|
#define fre2lum(fre) (uint8_t)((fre - FREQ_BLACK) / color_re)
|
||||||
|
|
||||||
|
// #define Rcos(x, N) (0.5 - 0.5 * cos(2.0 * PI * x / (N - 1.0)))
|
||||||
|
|
||||||
|
// static q15_t hann(uint16_t x, uint16_t len) {
|
||||||
|
// q31_t v = round(Rcos(x, len) * 32768);
|
||||||
|
// if (v > 32767) v = 32767;
|
||||||
|
// if (v < -32768) v = -32768;
|
||||||
|
// return (q15_t)v;
|
||||||
|
// }
|
||||||
|
|
||||||
|
void init() {
|
||||||
|
FILE* fp;
|
||||||
|
fp = fopen("temp/data.txt", "r");
|
||||||
|
for (int i = 0; i < 1843202; i++) fscanf(fp, "%" SCNd16, &img[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void read_data(q15_t* pSrc, uint16_t len) {
|
||||||
|
for (int i = 0; i < len; i++) pSrc[i] = *pImg++;
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint16_t find_peak(uint16_t L, uint16_t R) {
|
||||||
|
uint16_t index = 0, peak = 0;
|
||||||
|
for (uint16_t i = L; i <= R; i++)
|
||||||
|
if (data[i] > peak) {
|
||||||
|
index = i;
|
||||||
|
peak = data[i];
|
||||||
|
}
|
||||||
|
return index;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void hanning(uint16_t len) {
|
||||||
|
for (uint16_t i = len; i < N; i++) data[i] = 0;
|
||||||
|
// for (uint16_t i = 0; i < len; i++) data[i] = ((q31_t)data[i] * hann(i, len)) >> 15;
|
||||||
|
if (len == 160)
|
||||||
|
for (uint16_t i = 0; i < len; i++) data[i] = ((q31_t)data[i] * hanning_160[i]) >> 15;
|
||||||
|
else
|
||||||
|
for (uint16_t i = 0; i < len; i++) data[i] = ((q31_t)data[i] * hanning_17[i]) >> 15;
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint32_t cSample = 0;
|
||||||
|
static uint16_t preLen = 0;
|
||||||
|
static void read_samples(uint32_t start, uint16_t len) {
|
||||||
|
int16_t offset = start - cSample;
|
||||||
|
if (offset >= 0) {
|
||||||
|
read_data(pData, offset);
|
||||||
|
read_data(pData, len);
|
||||||
|
} else {
|
||||||
|
for (uint16_t i = 0; i < -offset; i++) data[i] = buffer[preLen + offset + i];
|
||||||
|
read_data(pData - offset, len + offset);
|
||||||
|
}
|
||||||
|
for (uint16_t i = 0; i < len; i++) buffer[i] = data[i];
|
||||||
|
|
||||||
|
cSample += offset + len;
|
||||||
|
preLen = len;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void read_header() {
|
||||||
|
uint8_t cLeader1, cLeader2, cBreak, cVis;
|
||||||
|
uint16_t len = (uint16_t)(FS * 0.01);
|
||||||
|
uint16_t idx, fre;
|
||||||
|
cLeader1 = cLeader2 = cBreak = cVis = 0;
|
||||||
|
|
||||||
|
for (;;) {
|
||||||
|
read_data(pData, len);
|
||||||
|
hanning(len);
|
||||||
|
|
||||||
|
FFT(pData);
|
||||||
|
idx = find_peak(64, 128); // 1000 - 2000 Hz
|
||||||
|
fre = FFT_BIN(idx, RE);
|
||||||
|
|
||||||
|
if (abs(fre - FREQ_START) < 80) {
|
||||||
|
if (cBreak) cLeader2++;
|
||||||
|
else cLeader1++;
|
||||||
|
} else if (abs(fre - FREQ_SYNC) < 80) {
|
||||||
|
if (cLeader2) cVis++;
|
||||||
|
else cBreak++;
|
||||||
|
} else cLeader1 = cLeader2 = cBreak = cVis = 0;
|
||||||
|
|
||||||
|
if (cLeader1 > 5 // Leader tone
|
||||||
|
&& cBreak < 3 // Break
|
||||||
|
&& abs(cLeader2 - 30) < 5 // Leader tone
|
||||||
|
&& (cVis == 3 || cVis == 4) // VIS start bit
|
||||||
|
) {
|
||||||
|
printf("Header: %d %d %d %d\r\n", cLeader1, cBreak, cLeader2, cVis);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint8_t read_mode() {
|
||||||
|
uint16_t len = (uint16_t)(FS * 0.01);
|
||||||
|
uint16_t idx, fre;
|
||||||
|
uint8_t cBit1 = 0, cParity = 0, mode = 0;
|
||||||
|
|
||||||
|
for (uint8_t i = 0; i < 24; i++) {
|
||||||
|
read_data(pData, len);
|
||||||
|
hanning(len);
|
||||||
|
|
||||||
|
FFT(pData);
|
||||||
|
idx = find_peak(64, 128); // 1000 - 2000 Hz
|
||||||
|
fre = FFT_BIN(idx, RE);
|
||||||
|
|
||||||
|
if (abs(fre - FREQ_BIT1) < 80) cBit1++;
|
||||||
|
else if (abs(fre - FREQ_BIT0) > 80) {
|
||||||
|
printf("Error VIS BIT: %d\r\n", fre);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (i % 3 == 2) {
|
||||||
|
mode = mode >> 1;
|
||||||
|
if (cBit1 > 1) {
|
||||||
|
mode |= BIT(7);
|
||||||
|
if (i != 23) cParity++;
|
||||||
|
}
|
||||||
|
cBit1 = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cParity = (mode & BIT(7) ? 1 : 0) == (cParity % 2);
|
||||||
|
mode = mode & (~BIT(7));
|
||||||
|
|
||||||
|
if (!cParity) {
|
||||||
|
printf("Error Parity Check: %d\r\n", mode);
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
read_data(pData, len * 3);
|
||||||
|
return mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void decode_martin1() {
|
||||||
|
FILE* fp;
|
||||||
|
fp = fopen("temp/decode.txt", "w");
|
||||||
|
|
||||||
|
printf("Mode: 44 - Martin 1\r\n");
|
||||||
|
|
||||||
|
const uint16_t WIDTH = 320;
|
||||||
|
const uint16_t HEIGHT = 256;
|
||||||
|
|
||||||
|
const float32_t SCAN_TIME = 0.146432;
|
||||||
|
const float32_t SYNC_PULSE = 0.004862;
|
||||||
|
const float32_t SYNC_PORCH = 0.000572;
|
||||||
|
|
||||||
|
const float32_t CHAN_TIME = SYNC_PORCH + SCAN_TIME;
|
||||||
|
const float32_t LINE_TIME = SYNC_PULSE + SYNC_PORCH + CHAN_TIME * 3;
|
||||||
|
const float32_t PIXEL_TIME = SCAN_TIME / WIDTH;
|
||||||
|
const float32_t CHAN_OFFSETS[] = {
|
||||||
|
SYNC_PULSE + SYNC_PORCH,
|
||||||
|
SYNC_PULSE + SYNC_PORCH + CHAN_TIME,
|
||||||
|
SYNC_PULSE + SYNC_PORCH + CHAN_TIME * 2,
|
||||||
|
};
|
||||||
|
|
||||||
|
const float32_t FWHM = 2.34;
|
||||||
|
const float32_t HALF_SAMPLE_TIME = (PIXEL_TIME * FWHM) / 2;
|
||||||
|
const uint8_t len = (uint8_t)(HALF_SAMPLE_TIME * 2 * FS);
|
||||||
|
|
||||||
|
uint16_t idx, fre;
|
||||||
|
uint32_t start, cSampleI = 0;
|
||||||
|
float32_t cSampleF = 0;
|
||||||
|
|
||||||
|
printf("Start Decoding\r\n");
|
||||||
|
for (uint16_t h = 0; h < HEIGHT; h++) {
|
||||||
|
printf("Line: %d / 255\r\n", h);
|
||||||
|
|
||||||
|
if (h) {
|
||||||
|
cSampleF += LINE_TIME * FS;
|
||||||
|
cSampleI = (uint32_t)round(cSampleF);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (uint8_t c = 0; c < 3; c++) {
|
||||||
|
for (uint16_t w = 0; w < WIDTH; w++) {
|
||||||
|
start = (uint32_t)round(cSampleI + (CHAN_OFFSETS[c] + w * PIXEL_TIME - HALF_SAMPLE_TIME) * FS);
|
||||||
|
read_samples(start, len);
|
||||||
|
hanning(len);
|
||||||
|
|
||||||
|
FFT(pData);
|
||||||
|
idx = find_peak(119, 457); // 1875 - 7125 Hz
|
||||||
|
fre = FFT_BIN(idx, RE);
|
||||||
|
colorBuf[w + WIDTH * c] = fre2lum(fre);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (uint16_t w = 0; w < WIDTH; w++)
|
||||||
|
fprintf(fp, "%d %d %d \r\n", colorBuf[w + WIDTH * 2], colorBuf[w], colorBuf[w + WIDTH]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t decode_sstv() {
|
||||||
|
uint8_t mode;
|
||||||
|
|
||||||
|
init();
|
||||||
|
read_header();
|
||||||
|
mode = read_mode();
|
||||||
|
|
||||||
|
if (mode == 44) decode_martin1();
|
||||||
|
else {
|
||||||
|
printf("Mode Error: %d\r\n", mode);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user