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