diff --git a/include/sstv.h b/include/sstv.h new file mode 100644 index 0000000..0be6438 --- /dev/null +++ b/include/sstv.h @@ -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(); diff --git a/src/sstv.c b/src/sstv.c new file mode 100644 index 0000000..3808249 --- /dev/null +++ b/src/sstv.c @@ -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; +}