kopia lustrzana https://github.com/xaelsouth/rtl-wmbus
initial commit
xaelsouth
commit
ebe0a8bef6
|
|
@ -0,0 +1,31 @@
|
|||
RM=rm
|
||||
MKDIR=mkdir
|
||||
CC=gcc
|
||||
STRIP=strip
|
||||
|
||||
OUTDIR=build
|
||||
OUTFILE="$(OUTDIR)/rtl_wmbus"
|
||||
CFLAGS=-Iinclude -std=gnu99
|
||||
CFLAGS_WARNINGS=-Wall -W -Waggregate-return -Wbad-function-cast -Wcast-align -Wcast-qual -Wchar-subscripts -Wcomment -Wfloat-equal -Winline -Wmain -Wmissing-noreturn -Wmissing-prototypes -Wparentheses -Wpointer-arith -Wredundant-decls -Wreturn-type -Wshadow -Wsign-compare -Wstrict-prototypes -Wswitch -Wunreachable-code -Wunused -Wuninitialized
|
||||
LIB=-lm
|
||||
SRC=rtl_wmbus.c
|
||||
|
||||
all: $(OUTDIR) release
|
||||
|
||||
$(OUTDIR):
|
||||
$(MKDIR) -p "$(OUTDIR)"
|
||||
|
||||
release: $(OUTDIR)
|
||||
$(CC) -msse4.2 -DNDEBUG -O3 $(CFLAGS) $(CFLAGS_WARNINGS) -o $(OUTFILE) $(SRC) $(LIB)
|
||||
|
||||
debug: $(OUTDIR)
|
||||
$(CC) -msse4.2 -DDEBUG -O0 -g3 -ggdb -p -pg $(CFLAGS) $(CFLAGS_WARNINGS) -o $(OUTFILE) $(SRC) $(LIB)
|
||||
|
||||
# Will build on Raspberry Pi 1 only
|
||||
pi1:
|
||||
$(CC) -DNDEBUG -O3 -march=armv6 -mtune=arm1176jzf-s -mfloat-abi=hard -mfpu=vfp -ffast-math $(CFLAGS) $(CFLAGS_WARNINGS) -o $(OUTFILE) $(SRC) $(LIB)
|
||||
|
||||
rebuild: clean all
|
||||
|
||||
clean:
|
||||
$(RM) -rf "$(OUTDIR)"
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
|
||||
rtl-wmbus - Software Decoder for Wireless-M-Bus with RTL-SDR
|
||||
------------------------------------------------------------
|
||||
|
||||
rtl-wmbus is a software-defined radio receiver for Wireless-M-Bus. It is
|
||||
written in plain C and uses RTL-SDR to interface with RTL2832-based hardware.
|
||||
|
||||
Wireless-M-Bus is the wireless version of M-Bus ("Meter-Bus", http://www.m-bus.com),
|
||||
which is an European standard for remote reading of smart meters.
|
||||
|
||||
The primary purpose of rtl-wmbus is experimenting with digital signal
|
||||
processing and software radio. rtl-wmbus can be used on resource constrained
|
||||
devices such Raspberry Pi Zero or Raspberry PI B+ overclocked to 1GHz.
|
||||
Any Android based tablet will do the same too.
|
||||
|
||||
rtl-wmbus provides:
|
||||
* FSK demodulating
|
||||
* clock recovery
|
||||
* mode T1 and mode C1 packet decoding
|
||||
|
||||
rtl-wmbus requires:
|
||||
* Linux or Android
|
||||
* C99
|
||||
* supported DVB-T receiver
|
||||
* RTL-SDR library for Linux: http://sdr.osmocom.org/trac/wiki/rtl-sdr or
|
||||
* (optional, only for android builds) RTL-SDR library port for Android: https://github.com/martinmarinov/rtl_tcp_andro-
|
||||
|
||||
For the latest version, see https://github.com/xaelsouth/rtl-wmbus
|
||||
|
||||
|
||||
Installing
|
||||
----------
|
||||
|
||||
The Osmocom RTL-SDR library must be installed before you can build
|
||||
rtl-wmbus. See http://sdr.osmocom.org/trac/wiki/rtl-sdr for more
|
||||
information. RTL-SDR library for Android would be installed via
|
||||
Google Play.
|
||||
|
||||
To install rtl-wmbus, download, unpack the source code and go to the
|
||||
top level directory. Then use one of these three options:
|
||||
|
||||
$ make debug # (no optimization at all, with debug options)
|
||||
|
||||
$ make release # (-O3 optimized version, without any debugging options)
|
||||
|
||||
$ make pi1 # (Raspberry Pi optimized version, without any debugging options,
|
||||
will build on RasPi1) only
|
||||
|
||||
Before building Android version the SDK and NDK have to be installed.
|
||||
See androidbuild.bat for how to build.
|
||||
|
||||
Usage
|
||||
-----
|
||||
Shown using "release"-version:
|
||||
|
||||
To save IQ-stream on disk and decode them off-line:
|
||||
|
||||
$ rtl_sdr samples.bin -f 868.9M -s 1600000
|
||||
|
||||
$ cat samples.bin | build/rtl_wmbus
|
||||
|
||||
To run continuously:
|
||||
|
||||
$ rtl_sdr -f 868.9M -s 1600000 - 2>/dev/null | build/rtl_wmbus
|
||||
|
||||
To count "good" (no 3 out of 6 errors, no checksum errors) packets:
|
||||
|
||||
$ cat samples.bin | build/rtl_wmbus 2>/dev/null | grep "[T,C]1;1;1" | wc -l
|
||||
|
||||
Carrier-frequency given at "-f" must be set properly. With my DVB-T-
|
||||
Receiver I had to choose carrier 50kHz under the standard of 868.95MHz.
|
||||
Sample rate at 1.6Ms/s is hardcoded and cannot be changed.
|
||||
|
||||
samples2.bin is a "life" example with two devices received.
|
||||
|
||||
On Android first the driver must be started with options given above.
|
||||
IQ-data goes to a port which is would be already set by driver settings.
|
||||
Use get_net to get IQ-data into rtl_wmbus.
|
||||
|
||||
License
|
||||
-------
|
||||
|
||||
Copyright (c) 2017 <xael.south@yandex.com>
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
@echo off
|
||||
|
||||
rem SETTINGS
|
||||
set SDK_ROOT=c:\Android\sdk
|
||||
set NDK_ROOT=c:\Android\android-ndk-r10e
|
||||
set SYSROOT=%NDK_ROOT%\platforms\android-21\arch-arm
|
||||
set CC=%NDK_ROOT%\toolchains\arm-linux-androideabi-4.9\prebuilt\windows-x86_64\bin\arm-linux-androideabi-gcc --sysroot=%SYSROOT%
|
||||
|
||||
rem BUILD
|
||||
rd /S /Q Android
|
||||
md Android
|
||||
%CC% -o Android/rtl_wmbus rtl_wmbus.c -std=gnu99 -Iinclude -Wall -O3 -g0 -lm
|
||||
|
||||
rem INSTALL
|
||||
%SDK_ROOT%\platform-tools\adb push Android\rtl_wmbus /mnt/sdcard/wmbus
|
||||
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#ifndef ATAN2_H
|
||||
#define ATAN2_H
|
||||
|
||||
#include <math.h>
|
||||
|
||||
|
||||
static inline float atan2_libm(float complex y)
|
||||
{
|
||||
return carg(y) * (float)M_1_PI;
|
||||
}
|
||||
|
||||
|
||||
/** https://gist.github.com/volkansalma/2972237 */
|
||||
static inline float atan2_approximation(float complex s)
|
||||
{
|
||||
static const float ONEQTR_PI = M_PI / 4.0;
|
||||
static const float THRQTR_PI = 3.0 * M_PI / 4.0;
|
||||
|
||||
float y = cimagf(s), x = crealf(s);
|
||||
float r, angle;
|
||||
float abs_y = fabs(y) + 1e-10f; // kludge to prevent 0/0 condition
|
||||
|
||||
if (x < 0.0f)
|
||||
{
|
||||
r = (x + abs_y) / (abs_y - x);
|
||||
angle = THRQTR_PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
r = (x - abs_y) / (x + abs_y);
|
||||
angle = ONEQTR_PI;
|
||||
}
|
||||
|
||||
angle += (0.1963f*(float)M_1_PI * r * r - 0.9817f*(float)M_1_PI) * r;
|
||||
|
||||
if (y < 0.0f)
|
||||
angle = -angle; // negate if in quad III or IV
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
|
||||
/** https://gist.github.com/volkansalma/2972237 */
|
||||
static inline float atan2_approximation2(float complex s)
|
||||
{
|
||||
float y = cimagf(s), x = crealf(s);
|
||||
|
||||
if (x == 0.0f)
|
||||
{
|
||||
if (y > 0.0f) return 0.5f;
|
||||
if (y == 0.0f) return 0.0f;
|
||||
return -0.5f;
|
||||
}
|
||||
|
||||
float atan;
|
||||
const float z = y / x;
|
||||
|
||||
if (fabs(z) < 1.0f)
|
||||
{
|
||||
atan = z / (1.0f*(float)M_PI + 0.28086f*(float)M_PI*z*z);
|
||||
if (x < 0.0f)
|
||||
{
|
||||
if (y < 0.0f) return atan - 1.0f;
|
||||
return atan + 1.0f;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
atan = 0.5f - z / (z*z + 0.28086f) * (float)M_1_PI;
|
||||
if ( y < 0.0f ) return atan - 1.0f;
|
||||
}
|
||||
|
||||
return atan;
|
||||
}
|
||||
|
||||
|
||||
#endif /* ATAN2_H */
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
clear all;
|
||||
|
||||
samplerate = 1600e3;
|
||||
nyqistrate = samplerate/2;
|
||||
|
||||
Ws1 = 90e3/nyqistrate;
|
||||
Wp1 = 98e3/nyqistrate;
|
||||
Wp2 = 102e3/nyqistrate;
|
||||
Ws2 = 110e3/nyqistrate;
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
% seems to be the best
|
||||
[n, Wc] = cheb1ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
[b, a] = cheby1(n, Rp, Wc);
|
||||
|
||||
% does not work at all
|
||||
%[n, Wc] = cheb2ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = cheby2(n, Rp, Wc);
|
||||
|
||||
% performs badly
|
||||
%[n, Wc] = ellipord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = ellip(n, Rp, Rs, Wc);
|
||||
|
||||
% big filter order - unpracticable
|
||||
%[n, Wc] = buttord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = butter(n, Wc);
|
||||
|
||||
print_iir_filter_coef(b ,a);
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
clear all;
|
||||
|
||||
samplerate = 400e3;
|
||||
nyqistrate = samplerate/2;
|
||||
|
||||
Ws1 = 90e3/nyqistrate;
|
||||
Wp1 = 98e3/nyqistrate;
|
||||
Wp2 = 102e3/nyqistrate;
|
||||
Ws2 = 110e3/nyqistrate;
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
% seems to be the best
|
||||
[n, Wc] = cheb1ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
[b, a] = cheby1(n, Rp, Wc);
|
||||
|
||||
% does not work at all
|
||||
%[n, Wc] = cheb2ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = cheby2(n, Rp, Wc);
|
||||
|
||||
% performs badly
|
||||
%[n, Wc] = ellipord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = ellip(n, Rp, Rs, Wc);
|
||||
|
||||
% big filter order - unpracticable
|
||||
%[n, Wc] = buttord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = butter(n, Wc);
|
||||
|
||||
print_iir_filter_coef(b ,a);
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
clear all;
|
||||
|
||||
samplerate = 800e3;
|
||||
nyqistrate = samplerate/2;
|
||||
|
||||
Ws1 = 90e3/nyqistrate;
|
||||
Wp1 = 98e3/nyqistrate;
|
||||
Wp2 = 102e3/nyqistrate;
|
||||
Ws2 = 110e3/nyqistrate;
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
% seems to be the best
|
||||
[n, Wc] = cheb1ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
[b, a] = cheby1(n, Rp, Wc);
|
||||
|
||||
% does not work at all
|
||||
%[n, Wc] = cheb2ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = cheby2(n, Rp, Wc);
|
||||
|
||||
% performs badly
|
||||
%[n, Wc] = ellipord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = ellip(n, Rp, Rs, Wc);
|
||||
|
||||
% big filter order - unpracticable
|
||||
%[n, Wc] = buttord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
%[b, a] = butter(n, Wc);
|
||||
|
||||
print_iir_filter_coef(b ,a);
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
clear all;
|
||||
|
||||
samplerate = 1600e3;
|
||||
nyqistrate = samplerate/2;
|
||||
|
||||
Wp1 = 160e3/nyqistrate;
|
||||
Ws1 = 200e3/nyqistrate;
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
[n, Wc] = buttord(Wp1, Ws1, Rp, Rs);
|
||||
[b] = fir1(n, Wc);
|
||||
|
||||
print_fir_filter_coef(b);
|
||||
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
clear all;
|
||||
|
||||
samplerate = 800e3;
|
||||
nyqistrate = samplerate/2;
|
||||
|
||||
Wp1 = 100e3/nyqistrate;
|
||||
Ws1 = 10e3/nyqistrate;
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
[n, Wc] = buttord(Wp1, Ws1, Rp, Rs);
|
||||
[b] = fir1(n, Wc);
|
||||
|
||||
print_fir_filter_coef(b);
|
||||
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
clear all;
|
||||
|
||||
samplerate = 1600e3;
|
||||
nyqistrate = samplerate/2;
|
||||
|
||||
Wp1 = 160e3/nyqistrate;
|
||||
Ws1 = 200e3/nyqistrate;
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
[n, Wc] = buttord(Wp1, Ws1, Rp, Rs);
|
||||
[b] = fir1(n, Wc);
|
||||
|
||||
x = 1:22;
|
||||
y = filter(b,1,x);
|
||||
y = y(2:2:end);
|
||||
|
||||
phase_channels = 2;
|
||||
b_poly = buffer(b, phase_channels);
|
||||
y1 = filter(b_poly(2,:), 1, x(1:2:end));
|
||||
y2 = filter(b_poly(1,:), 1, x(2:2:end));
|
||||
|
||||
y_poly = y1 + y2;
|
||||
|
||||
y_err = y - y_poly;
|
||||
|
||||
print_ppf_filter_coef(b, phase_channels);
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
function [] = print_fir_filter_coef(b);
|
||||
|
||||
[R, C] = size(b);
|
||||
|
||||
fprintf(stdout, "#define COEFFS %u\n", C);
|
||||
|
||||
fprintf(stdout, "static const float b[COEFFS] = {");
|
||||
for c = 1:C,
|
||||
fprintf(stdout, "%.10g, ", b(c));
|
||||
end
|
||||
fprintf(stdout, "};\n");
|
||||
|
||||
fprintf(stdout, "#undef COEFFS\n");
|
||||
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
function [] = print_iir_filter_coef(b, a);
|
||||
|
||||
[s, g] = tf2sos(b, a);
|
||||
|
||||
[R, C] = size(s);
|
||||
|
||||
fprintf(stdout, "#define GAIN %.10g\n", g);
|
||||
fprintf(stdout, "#define SECTIONS %u\n", R);
|
||||
|
||||
fprintf(stdout, "static const float b[3*SECTIONS] = {");
|
||||
for r = 1:R,
|
||||
for c = 1:C/2,
|
||||
fprintf(stdout, "%.10g, ", s(r,c));
|
||||
end
|
||||
end
|
||||
fprintf(stdout, "};\n");
|
||||
|
||||
fprintf(stdout, "static const float a[3*SECTIONS] = {");
|
||||
for r = 1:R,
|
||||
for c = C/2+1:C,
|
||||
fprintf(stdout, "%.10g, ", s(r,c));
|
||||
end
|
||||
end
|
||||
fprintf(stdout, "};\n");
|
||||
|
||||
fprintf(stdout, "#undef SECTIONS\n");
|
||||
fprintf(stdout, "#undef GAIN\n");
|
||||
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
function [] = print_ppf_filter_coef(b, phase_channels);
|
||||
|
||||
b_poly = buffer(b, phase_channels);
|
||||
|
||||
[R, C] = size(b_poly);
|
||||
|
||||
fprintf(stdout, "#define PHASES %u\n", R);
|
||||
fprintf(stdout, "#define COEFFS %u\n", C);
|
||||
|
||||
fprintf(stdout, "static const float b[PHASES][COEFFS] = {");
|
||||
for r = 1:R,
|
||||
fprintf("\n\t{");
|
||||
for c = 1:C,
|
||||
fprintf(stdout, "%.10g, ", b_poly(r,c));
|
||||
end
|
||||
fprintf("},");
|
||||
end
|
||||
fprintf(stdout, "};\n");
|
||||
|
||||
fprintf(stdout, "#undef COEFFS\n");
|
||||
fprintf(stdout, "#undef PHASES\n");
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
clear all;
|
||||
|
||||
Fs = 1600e3; % samplerate
|
||||
t = 0:Fs-1; % seconds
|
||||
t = t./Fs;
|
||||
f = -Fs/2:Fs/2-1;
|
||||
|
||||
fid = fopen("../samples.bin");
|
||||
samples = fread(fid, size=Fs*2, precision="uint8"); % *2: i,q - samples interleaved
|
||||
fclose(fid);
|
||||
|
||||
samples = samples .- 127;
|
||||
signal = samples(1:2:end) .+ samples(2:2:end) .* j;
|
||||
|
||||
Wp1 = 160e3/(Fs/2);
|
||||
Ws1 = 200e3/(Fs/2);
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
[n, Wc] = buttord(Wp1, Ws1, Rp, Rs);
|
||||
[b] = fir1(n, Wc);
|
||||
|
||||
b_notch = [1,-1];
|
||||
a_notch = [1 , -0.98];
|
||||
%freqz(b_notch, a_notch);
|
||||
%signal = filtfilt(b_notch, a_notch, signal); % filter dc offset
|
||||
|
||||
filtered_signal = filter(b, 1, signal); % low-pass
|
||||
|
||||
conj_filtered_signal = [conj(filtered_signal(2:end)); 0];
|
||||
demodulated_signal = arg(filtered_signal .* conj_filtered_signal)/pi;
|
||||
demodulated_signal2 = demodulated_signal.^2;
|
||||
|
||||
|
||||
fid = fopen("../demod.bin", "w");
|
||||
samples = fwrite(fid, demodulated_signal./max(abs(demodulated_signal)).*32767, precision="int16");
|
||||
fclose(fid);
|
||||
|
||||
|
||||
%%%%%%%%%
|
||||
|
||||
|
||||
Ws1 = 90e3/(Fs/2);
|
||||
Wp1 = 98e3/(Fs/2);
|
||||
Wp2 = 102e3/(Fs/2);
|
||||
Ws2 = 110e3/(Fs/2);
|
||||
Rp = 1;
|
||||
Rs = 40;
|
||||
|
||||
[n, Wc] = cheb1ord([Wp1, Wp2], [Ws1, Ws2], Rp, Rs);
|
||||
[b, a] = cheby1(n, Rp, Wc);
|
||||
|
||||
takt = filter(b,a,demodulated_signal2); % band-pass
|
||||
|
||||
fft_signal = fft(signal);
|
||||
fft_filtered_signal = fft(filtered_signal);
|
||||
fft_demodulated_signal = fft(demodulated_signal);
|
||||
fft_demodulated_signal2 = fft(demodulated_signal2);
|
||||
fft_takt = fft(takt);
|
||||
|
||||
u = abs(fftshift(fft_signal));
|
||||
v = abs(fftshift(fft_filtered_signal));
|
||||
w = abs(fftshift(fft_demodulated_signal));
|
||||
x = abs(fftshift(fft_demodulated_signal2));
|
||||
y = abs(fftshift(fft_takt));
|
||||
|
||||
plot(f, [u, v]);
|
||||
title('Amplitude Spectrum')
|
||||
xlabel('f (Hz)')
|
||||
ylabel('|signal|')
|
||||
grid on;
|
||||
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
#ifndef FIR_H
|
||||
#define FIR_H
|
||||
|
||||
/*-
|
||||
* Copyright (c) 2017 <xael.south@yandex.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Floating and fixed point implementations of Finite Response Filter.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <fixedptc/fixedptc.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
const size_t length;
|
||||
const float *const b;
|
||||
|
||||
size_t i;
|
||||
float *hist;
|
||||
} FIRF_FILTER;
|
||||
|
||||
float firf(float sample, FIRF_FILTER *filter);
|
||||
|
||||
float firf(float sample, FIRF_FILTER *filter)
|
||||
{
|
||||
const float *b = filter->b;
|
||||
float *hist = &filter->hist[filter->i++];
|
||||
size_t i;
|
||||
|
||||
*hist = sample;
|
||||
|
||||
sample = 0; // will be "y"
|
||||
|
||||
for (i = filter->i; i--;)
|
||||
{
|
||||
sample += *b++ * *hist--;
|
||||
}
|
||||
|
||||
hist = &filter->hist[filter->length-1];
|
||||
for (i = filter->length; i-- > filter->i;)
|
||||
{
|
||||
sample += *b++ * *hist--;
|
||||
}
|
||||
|
||||
if (filter->i >= filter->length) filter->i = 0;
|
||||
|
||||
return sample;
|
||||
}
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
const size_t length;
|
||||
const fixedpt *const b;
|
||||
|
||||
size_t i;
|
||||
fixedpt *hist;
|
||||
} FIRFP_FILTER;
|
||||
|
||||
fixedpt firfp(fixedpt sample, FIRFP_FILTER *filter);
|
||||
|
||||
fixedpt firfp(fixedpt sample, FIRFP_FILTER *filter)
|
||||
{
|
||||
const fixedpt *b = filter->b;
|
||||
fixedpt *hist = &filter->hist[filter->i++];
|
||||
size_t i;
|
||||
|
||||
*hist = sample;
|
||||
|
||||
sample = 0; // will be "y"
|
||||
|
||||
for (i = filter->i; i--;)
|
||||
{
|
||||
sample = fixedpt_add(sample, fixedpt_mul((*b++), (*hist--)));
|
||||
}
|
||||
|
||||
hist = &filter->hist[filter->length-1];
|
||||
for (i = filter->length; i-- > filter->i;)
|
||||
{
|
||||
sample = fixedpt_add(sample, fixedpt_mul((*b++), (*hist--)));
|
||||
}
|
||||
|
||||
if (filter->i >= filter->length) filter->i = 0;
|
||||
|
||||
return sample;
|
||||
}
|
||||
|
||||
#endif /* FIR_H */
|
||||
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
#ifndef IIR_H
|
||||
#define IIR_H
|
||||
|
||||
/*-
|
||||
* Copyright (c) 2017 <xael.south@yandex.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Implementation of Infinite Response Filter.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
const size_t sections;
|
||||
|
||||
const float gain;
|
||||
const float *const b; // 3 coeff
|
||||
const float *const a; // 3 coeff, first of these is assumed to be 1
|
||||
|
||||
float *hist; // 3 taps
|
||||
} IIRF_FILTER;
|
||||
|
||||
float iirf(float sample, IIRF_FILTER *filter);
|
||||
|
||||
float iirf(float sample, IIRF_FILTER *filter)
|
||||
{
|
||||
float *hist;
|
||||
const float *a, *b;
|
||||
size_t i;
|
||||
|
||||
// sample will be "y"
|
||||
|
||||
for (i = 0; i < filter->sections; i++)
|
||||
{
|
||||
a = filter->a + 3 * i;
|
||||
b = filter->b + 3 * i;
|
||||
hist = filter->hist + 3 * i;
|
||||
|
||||
#if 0
|
||||
hist[0] = -sample;
|
||||
hist[0] = -(a[0]*hist[0] + a[1]*hist[1] + a[2]*hist[2]);
|
||||
#else
|
||||
hist[0] = sample - (a[1]*hist[1] + a[2]*hist[2]);
|
||||
#endif
|
||||
sample = b[0]*hist[0] + b[1]*hist[1] + b[2]*hist[2];
|
||||
hist[2] = hist[1];
|
||||
hist[1] = hist[0];
|
||||
}
|
||||
|
||||
sample *= filter->gain;
|
||||
|
||||
return sample;
|
||||
}
|
||||
|
||||
#endif /* IIR_H */
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
354515b14610968c922f4812eaf42c2218329a67 fixedptc-andromeda
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
all: test verify_32 verify_64
|
||||
true
|
||||
|
||||
test: test.c fixedptc.h
|
||||
gcc -o test -O3 -Wall test.c
|
||||
|
||||
verify_32: verify.c fixedptc.h
|
||||
gcc -o verify_32 -O3 -Wall -DFIXEDPT_BITS=32 -lm verify.c
|
||||
|
||||
verify_64: verify.c fixedptc.h
|
||||
gcc -o verify_64 -O3 -Wall -DFIXEDPT_BITS=64 -lm verify.c
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
fixedptc library - a simple fixed point math header library for C.
|
||||
Copyright (c) 2010-2012. Ivan Voras <ivoras@freebsd.org>
|
||||
Released under the BSDL.
|
||||
|
||||
fixedptc is intended to be simple to use and integrate in other simple
|
||||
programs, thus is it implemented as a C header library. However, as
|
||||
functions in this mode of operation are all inlined, it can result in a
|
||||
significant increase in code size for the final executable. If the complex
|
||||
functions are used often in the end-program, the library should be
|
||||
refactored into a "normal" linkable object library.
|
||||
|
|
@ -0,0 +1,412 @@
|
|||
#ifndef _FIXEDPTC_H_
|
||||
#define _FIXEDPTC_H_
|
||||
|
||||
/*
|
||||
* fixedptc.h is a 32-bit or 64-bit fixed point numeric library.
|
||||
*
|
||||
* The symbol FIXEDPT_BITS, if defined before this library header file
|
||||
* is included, determines the number of bits in the data type (its "width").
|
||||
* The default width is 32-bit (FIXEDPT_BITS=32) and it can be used
|
||||
* on any recent C99 compiler. The 64-bit precision (FIXEDPT_BITS=64) is
|
||||
* available on compilers which implement 128-bit "long long" types. This
|
||||
* precision has been tested on GCC 4.2+.
|
||||
*
|
||||
* The FIXEDPT_WBITS symbols governs how many bits are dedicated to the
|
||||
* "whole" part of the number (to the left of the decimal point). The larger
|
||||
* this width is, the larger the numbers which can be stored in the fixedpt
|
||||
* number. The rest of the bits (available in the FIXEDPT_FBITS symbol) are
|
||||
* dedicated to the fraction part of the number (to the right of the decimal
|
||||
* point).
|
||||
*
|
||||
* Since the number of bits in both cases is relatively low, many complex
|
||||
* functions (more complex than div & mul) take a large hit on the precision
|
||||
* of the end result because errors in precision accumulate.
|
||||
* This loss of precision can be lessened by increasing the number of
|
||||
* bits dedicated to the fraction part, but at the loss of range.
|
||||
*
|
||||
* Adventurous users might utilize this library to build two data types:
|
||||
* one which has the range, and one which has the precision, and carefully
|
||||
* convert between them (including adding two number of each type to produce
|
||||
* a simulated type with a larger range and precision).
|
||||
*
|
||||
* The ideas and algorithms have been cherry-picked from a large number
|
||||
* of previous implementations available on the Internet.
|
||||
* Tim Hartrick has contributed cleanup and 64-bit support patches.
|
||||
*
|
||||
* == Special notes for the 32-bit precision ==
|
||||
* Signed 32-bit fixed point numeric library for the 24.8 format.
|
||||
* The specific limits are -8388608.999... to 8388607.999... and the
|
||||
* most precise number is 0.00390625. In practice, you should not count
|
||||
* on working with numbers larger than a million or to the precision
|
||||
* of more than 2 decimal places. Make peace with the fact that PI
|
||||
* is 3.14 here. :)
|
||||
*/
|
||||
|
||||
/*-
|
||||
* Copyright (c) 2010-2012 Ivan Voras <ivoras@freebsd.org>
|
||||
* Copyright (c) 2012 Tim Hartrick <tim@edgecast.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef FIXEDPT_BITS
|
||||
#define FIXEDPT_BITS 32
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#if FIXEDPT_BITS == 32
|
||||
typedef int32_t fixedpt;
|
||||
typedef int64_t fixedptd;
|
||||
typedef uint32_t fixedptu;
|
||||
typedef uint64_t fixedptud;
|
||||
#elif FIXEDPT_BITS == 64
|
||||
typedef int64_t fixedpt;
|
||||
typedef __int128_t fixedptd;
|
||||
typedef uint64_t fixedptu;
|
||||
typedef __uint128_t fixedptud;
|
||||
#else
|
||||
#error "FIXEDPT_BITS must be equal to 32 or 64"
|
||||
#endif
|
||||
|
||||
#ifndef FIXEDPT_WBITS
|
||||
#define FIXEDPT_WBITS 24
|
||||
#endif
|
||||
|
||||
#if FIXEDPT_WBITS >= FIXEDPT_BITS
|
||||
#error "FIXEDPT_WBITS must be less than or equal to FIXEDPT_BITS"
|
||||
#endif
|
||||
|
||||
#define FIXEDPT_VCSID "$Id$"
|
||||
|
||||
#define FIXEDPT_FBITS (FIXEDPT_BITS - FIXEDPT_WBITS)
|
||||
#define FIXEDPT_FMASK (((fixedpt)1 << FIXEDPT_FBITS) - 1)
|
||||
|
||||
#define fixedpt_rconst(R) ((fixedpt)((R) * FIXEDPT_ONE + ((R) >= 0 ? 0.5 : -0.5)))
|
||||
#define fixedpt_fromint(I) ((fixedptd)(I) << FIXEDPT_FBITS)
|
||||
#define fixedpt_toint(F) ((F) >> FIXEDPT_FBITS)
|
||||
#define fixedpt_add(A,B) ((A) + (B))
|
||||
#define fixedpt_sub(A,B) ((A) - (B))
|
||||
#define fixedpt_xmul(A,B) \
|
||||
((fixedpt)(((fixedptd)(A) * (fixedptd)(B)) >> FIXEDPT_FBITS))
|
||||
#define fixedpt_xdiv(A,B) \
|
||||
((fixedpt)(((fixedptd)(A) << FIXEDPT_FBITS) / (fixedptd)(B)))
|
||||
#define fixedpt_fracpart(A) ((fixedpt)(A) & FIXEDPT_FMASK)
|
||||
|
||||
#define FIXEDPT_ONE ((fixedpt)((fixedpt)1 << FIXEDPT_FBITS))
|
||||
#define FIXEDPT_ONE_HALF (FIXEDPT_ONE >> 1)
|
||||
#define FIXEDPT_TWO (FIXEDPT_ONE + FIXEDPT_ONE)
|
||||
#define FIXEDPT_PI fixedpt_rconst(3.14159265358979323846)
|
||||
#define FIXEDPT_TWO_PI fixedpt_rconst(2 * 3.14159265358979323846)
|
||||
#define FIXEDPT_HALF_PI fixedpt_rconst(3.14159265358979323846 / 2)
|
||||
#define FIXEDPT_E fixedpt_rconst(2.7182818284590452354)
|
||||
|
||||
#define fixedpt_abs(A) ((A) < 0 ? -(A) : (A))
|
||||
|
||||
/* fixedpt is meant to be usable in environments without floating point support
|
||||
* (e.g. microcontrollers, kernels), so we can't use floating point types directly.
|
||||
* Putting them only in macros will effectively make them optional. */
|
||||
#define fixedpt_tofloat(T) ((float) ((T)*((float)(1)/(float)(1 << FIXEDPT_FBITS))))
|
||||
|
||||
|
||||
/* Multiplies two fixedpt numbers, returns the result. */
|
||||
static inline fixedpt
|
||||
fixedpt_mul(fixedpt A, fixedpt B)
|
||||
{
|
||||
return (((fixedptd)A * (fixedptd)B) >> FIXEDPT_FBITS);
|
||||
}
|
||||
|
||||
|
||||
/* Divides two fixedpt numbers, returns the result. */
|
||||
static inline fixedpt
|
||||
fixedpt_div(fixedpt A, fixedpt B)
|
||||
{
|
||||
return (((fixedptd)A << FIXEDPT_FBITS) / (fixedptd)B);
|
||||
}
|
||||
|
||||
/*
|
||||
* Note: adding and substracting fixedpt numbers can be done by using
|
||||
* the regular integer operators + and -.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Convert the given fixedpt number to a decimal string.
|
||||
* The max_dec argument specifies how many decimal digits to the right
|
||||
* of the decimal point to generate. If set to -1, the "default" number
|
||||
* of decimal digits will be used (2 for 32-bit fixedpt width, 10 for
|
||||
* 64-bit fixedpt width); If set to -2, "all" of the digits will
|
||||
* be returned, meaning there will be invalid, bogus digits outside the
|
||||
* specified precisions.
|
||||
*/
|
||||
static inline void
|
||||
fixedpt_str(fixedpt A, char *str, int max_dec)
|
||||
{
|
||||
int ndec = 0, slen = 0;
|
||||
char tmp[12] = {0};
|
||||
fixedptud fr, ip;
|
||||
const fixedptud one = (fixedptud)1 << FIXEDPT_BITS;
|
||||
const fixedptud mask = one - 1;
|
||||
|
||||
if (max_dec == -1)
|
||||
#if FIXEDPT_BITS == 32
|
||||
#if FIXEDPT_WBITS > 16
|
||||
max_dec = 2;
|
||||
#else
|
||||
max_dec = 4;
|
||||
#endif
|
||||
#elif FIXEDPT_BITS == 64
|
||||
max_dec = 10;
|
||||
#else
|
||||
#error Invalid width
|
||||
#endif
|
||||
else if (max_dec == -2)
|
||||
max_dec = 15;
|
||||
|
||||
if (A < 0) {
|
||||
str[slen++] = '-';
|
||||
A *= -1;
|
||||
}
|
||||
|
||||
ip = fixedpt_toint(A);
|
||||
do {
|
||||
tmp[ndec++] = '0' + ip % 10;
|
||||
ip /= 10;
|
||||
} while (ip != 0);
|
||||
|
||||
while (ndec > 0)
|
||||
str[slen++] = tmp[--ndec];
|
||||
str[slen++] = '.';
|
||||
|
||||
fr = (fixedpt_fracpart(A) << FIXEDPT_WBITS) & mask;
|
||||
do {
|
||||
fr = (fr & mask) * 10;
|
||||
|
||||
str[slen++] = '0' + (fr >> FIXEDPT_BITS) % 10;
|
||||
ndec++;
|
||||
} while (fr != 0 && ndec < max_dec);
|
||||
|
||||
if (ndec > 1 && str[slen-1] == '0')
|
||||
str[slen-1] = '\0'; /* cut off trailing 0 */
|
||||
else
|
||||
str[slen] = '\0';
|
||||
}
|
||||
|
||||
|
||||
/* Converts the given fixedpt number into a string, using a static
|
||||
* (non-threadsafe) string buffer */
|
||||
static inline char*
|
||||
fixedpt_cstr(const fixedpt A, const int max_dec)
|
||||
{
|
||||
static char str[25];
|
||||
|
||||
fixedpt_str(A, str, max_dec);
|
||||
return (str);
|
||||
}
|
||||
|
||||
|
||||
/* Returns the square root of the given number, or -1 in case of error */
|
||||
static inline fixedpt
|
||||
fixedpt_sqrt(fixedpt A)
|
||||
{
|
||||
int invert = 0;
|
||||
int iter = FIXEDPT_FBITS;
|
||||
int l, i;
|
||||
|
||||
if (A < 0)
|
||||
return (-1);
|
||||
if (A == 0 || A == FIXEDPT_ONE)
|
||||
return (A);
|
||||
if (A < FIXEDPT_ONE && A > 6) {
|
||||
invert = 1;
|
||||
A = fixedpt_div(FIXEDPT_ONE, A);
|
||||
}
|
||||
if (A > FIXEDPT_ONE) {
|
||||
int s = A;
|
||||
|
||||
iter = 0;
|
||||
while (s > 0) {
|
||||
s >>= 2;
|
||||
iter++;
|
||||
}
|
||||
}
|
||||
|
||||
/* Newton's iterations */
|
||||
l = (A >> 1) + 1;
|
||||
for (i = 0; i < iter; i++)
|
||||
l = (l + fixedpt_div(A, l)) >> 1;
|
||||
if (invert)
|
||||
return (fixedpt_div(FIXEDPT_ONE, l));
|
||||
return (l);
|
||||
}
|
||||
|
||||
|
||||
/* Returns the sine of the given fixedpt number.
|
||||
* Note: the loss of precision is extraordinary! */
|
||||
static inline fixedpt
|
||||
fixedpt_sin(fixedpt fp)
|
||||
{
|
||||
int sign = 1;
|
||||
fixedpt sqr, result;
|
||||
const fixedpt SK[2] = {
|
||||
fixedpt_rconst(7.61e-03),
|
||||
fixedpt_rconst(1.6605e-01)
|
||||
};
|
||||
|
||||
fp %= 2 * FIXEDPT_PI;
|
||||
if (fp < 0)
|
||||
fp = FIXEDPT_PI * 2 + fp;
|
||||
if ((fp > FIXEDPT_HALF_PI) && (fp <= FIXEDPT_PI))
|
||||
fp = FIXEDPT_PI - fp;
|
||||
else if ((fp > FIXEDPT_PI) && (fp <= (FIXEDPT_PI + FIXEDPT_HALF_PI))) {
|
||||
fp = fp - FIXEDPT_PI;
|
||||
sign = -1;
|
||||
} else if (fp > (FIXEDPT_PI + FIXEDPT_HALF_PI)) {
|
||||
fp = (FIXEDPT_PI << 1) - fp;
|
||||
sign = -1;
|
||||
}
|
||||
sqr = fixedpt_mul(fp, fp);
|
||||
result = SK[0];
|
||||
result = fixedpt_mul(result, sqr);
|
||||
result -= SK[1];
|
||||
result = fixedpt_mul(result, sqr);
|
||||
result += FIXEDPT_ONE;
|
||||
result = fixedpt_mul(result, fp);
|
||||
return sign * result;
|
||||
}
|
||||
|
||||
|
||||
/* Returns the cosine of the given fixedpt number */
|
||||
static inline fixedpt
|
||||
fixedpt_cos(fixedpt A)
|
||||
{
|
||||
return (fixedpt_sin(FIXEDPT_HALF_PI - A));
|
||||
}
|
||||
|
||||
|
||||
/* Returns the tangens of the given fixedpt number */
|
||||
static inline fixedpt
|
||||
fixedpt_tan(fixedpt A)
|
||||
{
|
||||
return fixedpt_div(fixedpt_sin(A), fixedpt_cos(A));
|
||||
}
|
||||
|
||||
|
||||
/* Returns the value exp(x), i.e. e^x of the given fixedpt number. */
|
||||
static inline fixedpt
|
||||
fixedpt_exp(fixedpt fp)
|
||||
{
|
||||
fixedpt xabs, k, z, R, xp;
|
||||
const fixedpt LN2 = fixedpt_rconst(0.69314718055994530942);
|
||||
const fixedpt LN2_INV = fixedpt_rconst(1.4426950408889634074);
|
||||
const fixedpt EXP_P[5] = {
|
||||
fixedpt_rconst(1.66666666666666019037e-01),
|
||||
fixedpt_rconst(-2.77777777770155933842e-03),
|
||||
fixedpt_rconst(6.61375632143793436117e-05),
|
||||
fixedpt_rconst(-1.65339022054652515390e-06),
|
||||
fixedpt_rconst(4.13813679705723846039e-08),
|
||||
};
|
||||
|
||||
if (fp == 0)
|
||||
return (FIXEDPT_ONE);
|
||||
xabs = fixedpt_abs(fp);
|
||||
k = fixedpt_mul(xabs, LN2_INV);
|
||||
k += FIXEDPT_ONE_HALF;
|
||||
k &= ~FIXEDPT_FMASK;
|
||||
if (fp < 0)
|
||||
k = -k;
|
||||
fp -= fixedpt_mul(k, LN2);
|
||||
z = fixedpt_mul(fp, fp);
|
||||
/* Taylor */
|
||||
R = FIXEDPT_TWO +
|
||||
fixedpt_mul(z, EXP_P[0] + fixedpt_mul(z, EXP_P[1] +
|
||||
fixedpt_mul(z, EXP_P[2] + fixedpt_mul(z, EXP_P[3] +
|
||||
fixedpt_mul(z, EXP_P[4])))));
|
||||
xp = FIXEDPT_ONE + fixedpt_div(fixedpt_mul(fp, FIXEDPT_TWO), R - fp);
|
||||
if (k < 0)
|
||||
k = FIXEDPT_ONE >> (-k >> FIXEDPT_FBITS);
|
||||
else
|
||||
k = FIXEDPT_ONE << (k >> FIXEDPT_FBITS);
|
||||
return (fixedpt_mul(k, xp));
|
||||
}
|
||||
|
||||
|
||||
/* Returns the natural logarithm of the given fixedpt number. */
|
||||
static inline fixedpt
|
||||
fixedpt_ln(fixedpt x)
|
||||
{
|
||||
fixedpt log2, xi;
|
||||
fixedpt f, s, z, w, R;
|
||||
const fixedpt LN2 = fixedpt_rconst(0.69314718055994530942);
|
||||
const fixedpt LG[7] = {
|
||||
fixedpt_rconst(6.666666666666735130e-01),
|
||||
fixedpt_rconst(3.999999999940941908e-01),
|
||||
fixedpt_rconst(2.857142874366239149e-01),
|
||||
fixedpt_rconst(2.222219843214978396e-01),
|
||||
fixedpt_rconst(1.818357216161805012e-01),
|
||||
fixedpt_rconst(1.531383769920937332e-01),
|
||||
fixedpt_rconst(1.479819860511658591e-01)
|
||||
};
|
||||
|
||||
if (x < 0)
|
||||
return (0);
|
||||
if (x == 0)
|
||||
return 0xffffffff;
|
||||
|
||||
log2 = 0;
|
||||
xi = x;
|
||||
while (xi > FIXEDPT_TWO) {
|
||||
xi >>= 1;
|
||||
log2++;
|
||||
}
|
||||
f = xi - FIXEDPT_ONE;
|
||||
s = fixedpt_div(f, FIXEDPT_TWO + f);
|
||||
z = fixedpt_mul(s, s);
|
||||
w = fixedpt_mul(z, z);
|
||||
R = fixedpt_mul(w, LG[1] + fixedpt_mul(w, LG[3]
|
||||
+ fixedpt_mul(w, LG[5]))) + fixedpt_mul(z, LG[0]
|
||||
+ fixedpt_mul(w, LG[2] + fixedpt_mul(w, LG[4]
|
||||
+ fixedpt_mul(w, LG[6]))));
|
||||
return (fixedpt_mul(LN2, (log2 << FIXEDPT_FBITS)) + f
|
||||
- fixedpt_mul(s, f - R));
|
||||
}
|
||||
|
||||
|
||||
/* Returns the logarithm of the given base of the given fixedpt number */
|
||||
static inline fixedpt
|
||||
fixedpt_log(fixedpt x, fixedpt base)
|
||||
{
|
||||
return (fixedpt_div(fixedpt_ln(x), fixedpt_ln(base)));
|
||||
}
|
||||
|
||||
|
||||
/* Return the power value (n^exp) of the given fixedpt numbers */
|
||||
static inline fixedpt
|
||||
fixedpt_pow(fixedpt n, fixedpt exp)
|
||||
{
|
||||
if (exp == 0)
|
||||
return (FIXEDPT_ONE);
|
||||
if (n < 0)
|
||||
return 0;
|
||||
return (fixedpt_exp(fixedpt_mul(fixedpt_ln(n), exp)));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/* This is a pure integer-only test program for fixedptc */
|
||||
|
||||
//#define FIXEDPT_WBITS 16
|
||||
|
||||
|
||||
#include "fixedptc.h"
|
||||
|
||||
void
|
||||
fixedpt_print(fixedpt A)
|
||||
{
|
||||
char num[20];
|
||||
|
||||
fixedpt_str(A, num, -2);
|
||||
puts(num);
|
||||
}
|
||||
|
||||
int main() {
|
||||
|
||||
fixedpt A, B, C;
|
||||
|
||||
printf("fixedptc library version: %s\n", FIXEDPT_VCSID);
|
||||
printf("Using %d-bit precision, %d.%d format\n\n", FIXEDPT_BITS, FIXEDPT_WBITS, FIXEDPT_FBITS);
|
||||
|
||||
printf("The most precise number: ");
|
||||
fixedpt_print(1);
|
||||
printf("The biggest number: ");
|
||||
fixedpt_print(0x7fffff00);
|
||||
printf("Here are some example numbers:\n");
|
||||
|
||||
printf("Random number: ");
|
||||
fixedpt_print(fixedpt_rconst(143.125));
|
||||
printf("PI: ");
|
||||
fixedpt_print(FIXEDPT_PI);
|
||||
printf("e: ");
|
||||
fixedpt_print(FIXEDPT_E);
|
||||
puts("");
|
||||
|
||||
A = fixedpt_rconst(2.5);
|
||||
B = fixedpt_fromint(3);
|
||||
|
||||
fixedpt_print(A);
|
||||
puts("+");
|
||||
fixedpt_print(B);
|
||||
C = fixedpt_add(A, B);
|
||||
puts("=");
|
||||
fixedpt_print(C);
|
||||
puts("");
|
||||
|
||||
fixedpt_print(A);
|
||||
puts("*");
|
||||
fixedpt_print(B);
|
||||
puts("=");
|
||||
C = fixedpt_mul(A, B);
|
||||
fixedpt_print(C);
|
||||
puts("");
|
||||
|
||||
A = fixedpt_rconst(1);
|
||||
B = fixedpt_rconst(4);
|
||||
C = fixedpt_div(A, B);
|
||||
|
||||
fixedpt_print(A);
|
||||
puts("/");
|
||||
fixedpt_print(B);
|
||||
puts("=");
|
||||
fixedpt_print(C);
|
||||
|
||||
printf("exp(1)=");
|
||||
fixedpt_print(fixedpt_exp(FIXEDPT_ONE));
|
||||
|
||||
puts("");
|
||||
puts("sqrt(pi)=");
|
||||
fixedpt_print(fixedpt_sqrt(FIXEDPT_PI));
|
||||
|
||||
puts("");
|
||||
puts("sqrt(25)=");
|
||||
fixedpt_print(fixedpt_sqrt(fixedpt_rconst(25)));
|
||||
|
||||
puts("");
|
||||
puts("sin(pi/2)=");
|
||||
fixedpt_print(fixedpt_sin(FIXEDPT_HALF_PI));
|
||||
|
||||
puts("");
|
||||
puts("sin(3.5*pi)=");
|
||||
fixedpt_print(fixedpt_sin(fixedpt_mul(fixedpt_rconst(3.5), FIXEDPT_PI)));
|
||||
|
||||
puts("");
|
||||
puts("4^3.5=");
|
||||
fixedpt_print(fixedpt_pow(fixedpt_rconst(4), fixedpt_rconst(3.5)));
|
||||
|
||||
puts("");
|
||||
puts("4^0.5=");
|
||||
fixedpt_print(fixedpt_pow(fixedpt_rconst(4), fixedpt_rconst(0.5)));
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <locale.h>
|
||||
|
||||
//#define FIXEDPT_WBITS 16
|
||||
#include "fixedptc.h"
|
||||
|
||||
/* This test program verifies the fixedpt precision, comparing it to
|
||||
* float and double precision results. */
|
||||
|
||||
static const float pi_f = 3.14159265358979323846264338;
|
||||
static const double pi_d = 3.14159265358979323846264338;
|
||||
static const fixedpt pi_x = fixedpt_rconst(3.14159265358979323846264338);
|
||||
|
||||
static const float e_f = 2.71828182845904523536028747;
|
||||
static const double e_d = 2.71828182845904523536028747;
|
||||
static const fixedpt e_x = fixedpt_rconst(2.71828182845904523536028747);
|
||||
|
||||
void
|
||||
verify_numbers()
|
||||
{
|
||||
printf("pi as string:\t3.14159265358979323846264338\n");
|
||||
printf("pi as float:\t%0.6f\n", pi_f);
|
||||
printf("pi as double:\t%0.15lf\n", pi_d);
|
||||
printf("pi as fixedpt:\t%s\n", fixedpt_cstr(pi_x, -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(pi_x, -1)) - pi_d);
|
||||
printf("pi as fixedpt converted to float: %0.6f\n", fixedpt_tofloat(pi_x));
|
||||
|
||||
printf("e as string:\t2.71828182845904523536028747\n");
|
||||
printf("e as float:\t%0.6f\n", e_f);
|
||||
printf("e as double:\t%0.15lf\n", e_d);
|
||||
printf("e as fixedpt:\t%s\n", fixedpt_cstr(e_x, -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(e_x, -1)) - e_d);
|
||||
}
|
||||
|
||||
void
|
||||
verify_trig()
|
||||
{
|
||||
printf("sin(pi) as float:\t%0.6f\n", sinf(pi_f));
|
||||
printf("sin(pi) as double:\t%0.15lf\n", sin(pi_d));
|
||||
printf("sin(pi) as fixedpt:\t%s\n", fixedpt_cstr(fixedpt_sin(pi_x), -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(fixedpt_sin(pi_x), -1)) - sin(pi_d));
|
||||
|
||||
printf("sin(e) as float:\t%0.6f\n", sinf(e_f));
|
||||
printf("sin(e) as double:\t%0.15lf\n", sin(e_d));
|
||||
printf("sin(e) as fixedpt:\t%s\n", fixedpt_cstr(fixedpt_sin(e_x), -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(fixedpt_sin(e_x), -1)) - sin(e_d));
|
||||
|
||||
printf("tan(e) as float:\t%0.6f\n", tanf(e_f));
|
||||
printf("tan(e) as double:\t%0.15lf\n", tan(e_d));
|
||||
printf("tan(e) as fixedpt:\t%s\n", fixedpt_cstr(fixedpt_tan(e_x), -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(fixedpt_tan(e_x), -1)) - tan(e_d));
|
||||
}
|
||||
|
||||
void
|
||||
verify_powers()
|
||||
{
|
||||
printf("pow(pi,3) as float:\t%0.6f\n", powf(pi_f, 3));
|
||||
printf("pow(pi,3) as double:\t%0.15f\n", pow(pi_d, 3));
|
||||
printf("pow(pi,3) as fixedpt:\t%s\n", fixedpt_cstr(fixedpt_pow(pi_x, fixedpt_rconst(3)), -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(fixedpt_pow(pi_x, fixedpt_rconst(3)), -1)) - pow(pi_d, 3));
|
||||
|
||||
printf("exp(3) as float:\t%0.6f\n", expf(3));
|
||||
printf("exp(3) as double:\t%0.15f\n", expf(3));
|
||||
printf("exp(3) as fixedpt:\t%s\n", fixedpt_cstr(fixedpt_exp(fixedpt_rconst(3)), -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(fixedpt_exp(fixedpt_rconst(3)), -1)) - exp(3));
|
||||
|
||||
printf("ln(e) as float:\t%0.6f\n", logf(e_f));
|
||||
printf("ln(e) as double:\t%0.15f\n", log(e_d));
|
||||
printf("ln(e) as fixedpt:\t%s\n", fixedpt_cstr(fixedpt_ln(e_x), -1));
|
||||
printf(" delta fixedpt-double:\t%0.10lf\n", atof(fixedpt_cstr(fixedpt_ln(e_x), -1)) - log(e_d));
|
||||
}
|
||||
|
||||
int
|
||||
main()
|
||||
{
|
||||
setlocale(LC_NUMERIC, "C");
|
||||
printf("fixedptc library version: %s\n", FIXEDPT_VCSID);
|
||||
printf("Using %d-bit precision, %d.%d format\n\n", FIXEDPT_BITS, FIXEDPT_WBITS, FIXEDPT_FBITS);
|
||||
|
||||
verify_numbers();
|
||||
printf("\n");
|
||||
verify_trig();
|
||||
printf("\n");
|
||||
verify_powers();
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
#ifndef MOVING_AVERAGE_FILTER_H
|
||||
#define MOVING_AVERAGE_FILTER_H
|
||||
|
||||
/*-
|
||||
* Copyright (c) 2017 <xael.south@yandex.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Moving average filter implementation.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
size_t i;
|
||||
int *hist;
|
||||
|
||||
const size_t length;
|
||||
int sum;
|
||||
} MAVGI_FILTER;
|
||||
|
||||
float mavgi(int sample, MAVGI_FILTER *filter);
|
||||
|
||||
float mavgi(int sample, MAVGI_FILTER *filter)
|
||||
{
|
||||
filter->sum = filter->sum - filter->hist[filter->i] + sample;
|
||||
filter->hist[filter->i++] = sample;
|
||||
if (filter->i >= filter->length) filter->i = 0;
|
||||
|
||||
return (float)filter->sum / filter->length;
|
||||
}
|
||||
|
||||
#if 0
|
||||
static int test_mavgi(void)
|
||||
{
|
||||
#define COEFFS 5
|
||||
static int hist[COEFFS];
|
||||
|
||||
static MAVGI_FILTER filter = { .length = COEFFS, .hist = hist };
|
||||
#undef COEFFS
|
||||
|
||||
for (int sample = 0; sample < 20; sample++)
|
||||
{
|
||||
printf("%f, ", mavgi(sample, &filter));
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* MOVING_AVERAGE_FILTER_H */
|
||||
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
#ifndef NET_SUPPORT_H
|
||||
#define NET_SUPPORT_H
|
||||
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <errno.h>
|
||||
|
||||
|
||||
FILE * get_net(const char *hostname, int port);
|
||||
|
||||
|
||||
FILE * get_net(const char *hostname, int port)
|
||||
{
|
||||
struct sockaddr_in address = {.sin_family = AF_INET, .sin_port = htons(port)};
|
||||
if (-1 == inet_aton(hostname, &address.sin_addr))
|
||||
{
|
||||
fprintf(stderr, "inet_aton() error\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int fd = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (fd == -1)
|
||||
{
|
||||
fprintf(stderr, "socket() error\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (connect(fd, (struct sockaddr *) &address, sizeof (address)) == -1)
|
||||
{
|
||||
fprintf(stderr, "connect() error: %s:%d\n", hostname, port);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
FILE *input = fdopen(fd, "rb");
|
||||
if (input == NULL)
|
||||
{
|
||||
fprintf(stderr, "fdopen() error\n");
|
||||
}
|
||||
|
||||
return input;
|
||||
}
|
||||
|
||||
|
||||
#endif /* NET_SUPPORT_H */
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
#ifndef PPF_H
|
||||
#define PPF_H
|
||||
|
||||
/*-
|
||||
* Copyright (c) 2017 <xael.south@yandex.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Floating and fixed point implementations of Polyphase Filter.
|
||||
*/
|
||||
|
||||
#include "fir.h"
|
||||
#include <fixedptc/fixedptc.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float sum;
|
||||
size_t phase;
|
||||
const size_t max_phase;
|
||||
FIRF_FILTER *fir;
|
||||
} PPF_FILTER;
|
||||
|
||||
float ppf(float sample, PPF_FILTER *filter);
|
||||
|
||||
float ppf(float sample, PPF_FILTER *filter)
|
||||
{
|
||||
if (filter->phase == filter->max_phase)
|
||||
{
|
||||
filter->phase = 0;
|
||||
filter->sum = 0;
|
||||
}
|
||||
|
||||
filter->sum += firf(sample, filter->fir + filter->phase);
|
||||
|
||||
filter->phase++;
|
||||
|
||||
return filter->sum;
|
||||
}
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fixedpt sum;
|
||||
size_t phase;
|
||||
const size_t max_phase;
|
||||
FIRFP_FILTER *fir;
|
||||
} PPFFP_FILTER;
|
||||
|
||||
fixedpt ppffp(fixedpt sample, PPFFP_FILTER *filter);
|
||||
|
||||
fixedpt ppffp(fixedpt sample, PPFFP_FILTER *filter)
|
||||
{
|
||||
if (filter->phase == filter->max_phase)
|
||||
{
|
||||
filter->phase = 0;
|
||||
filter->sum = 0;
|
||||
}
|
||||
|
||||
filter->sum = fixedpt_add(filter->sum, firfp(sample, filter->fir + filter->phase));
|
||||
|
||||
filter->phase++;
|
||||
|
||||
return filter->sum;
|
||||
}
|
||||
|
||||
#if 0
|
||||
static int test_ppf(void)
|
||||
{
|
||||
#define PHASES 2
|
||||
#define COEFFS 5
|
||||
static const float b[PHASES][COEFFS] =
|
||||
{
|
||||
{0.01208900045, 0.1180545517, 0.2457748215, 0.1180545517, 0.01208900045, },
|
||||
{0.04038886734, 0.2065801697, 0.2065801697, 0.04038886734, 0, },
|
||||
};
|
||||
|
||||
static float hist[PHASES][COEFFS] = {};
|
||||
|
||||
static FIRF_FILTER fir[PHASES] =
|
||||
{
|
||||
{.length = COEFFS, .b = b[1], .hist = hist[0]}, // !inverted indexing of fir!
|
||||
{.length = COEFFS, .b = b[0], .hist = hist[1]}, // !inverted indexing of fir!
|
||||
};
|
||||
|
||||
static PPF_FILTER filter =
|
||||
{
|
||||
.sum = 0, .phase = 0, .max_phase = PHASES, .fir = fir,
|
||||
};
|
||||
#undef COEFFS
|
||||
#undef PHASES
|
||||
|
||||
|
||||
for (int sample = 1, phase = 0; sample <= 22; sample++, phase ^= 1)
|
||||
{
|
||||
float x = ppf(sample, &filter);
|
||||
if (phase == 1) printf("%f, ", x);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* PPF_H */
|
||||
|
||||
Plik binarny nie jest wyświetlany.
|
|
@ -0,0 +1,516 @@
|
|||
/*-
|
||||
* Copyright (c) 2017 <xael.south@yandex.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <complex.h>
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
#include <fixedptc/fixedptc.h>
|
||||
#include "fir.h"
|
||||
#include "iir.h"
|
||||
#include "ppf.h"
|
||||
#include "moving_average_filter.h"
|
||||
#include "atan2.h"
|
||||
#include "net_support.h"
|
||||
#include "t1_c1_packet_decoder.h"
|
||||
|
||||
|
||||
#if !defined(__ANDROID__)
|
||||
#include <immintrin.h>
|
||||
#endif
|
||||
|
||||
|
||||
static float lp_1600kHz_56kHz(int sample, size_t i_or_q)
|
||||
{
|
||||
static float moving_average[2];
|
||||
|
||||
#define ALPHA 0.80259f // exp(-2.0 * M_PI * 56e3 / 1600e3)
|
||||
moving_average[i_or_q] = ALPHA * (moving_average[i_or_q] - sample) + sample;
|
||||
#undef ALPHA
|
||||
|
||||
return moving_average[i_or_q];
|
||||
}
|
||||
|
||||
|
||||
static inline float moving_average(int sample, size_t i_or_q)
|
||||
{
|
||||
#define COEFFS 12
|
||||
static int i_hist[COEFFS];
|
||||
static int q_hist[COEFFS];
|
||||
|
||||
static MAVGI_FILTER filter[2] = // i/q
|
||||
{
|
||||
{.length = COEFFS, .hist = i_hist}, // 0
|
||||
{.length = COEFFS, .hist = q_hist} // 1
|
||||
};
|
||||
#undef COEFFS
|
||||
|
||||
return mavgi(sample, &filter[i_or_q]);
|
||||
}
|
||||
|
||||
|
||||
static inline float lp_fir_butter_1600kHz_160kHz_200kHz(int sample, size_t i_or_q)
|
||||
{
|
||||
#define COEFFS 23
|
||||
static const float b[COEFFS] = {0.000140535927, 1.102280392e-05, 0.0001309279731, 0.001356012537, 0.00551787474, 0.01499414005, 0.03160167988, 0.05525973093, 0.08315031015, 0.1099887688, 0.1295143636, 0.1366692652, 0.1295143636, 0.1099887688, 0.08315031015, 0.05525973093, 0.03160167988, 0.01499414005, 0.00551787474, 0.001356012537, 0.0001309279731, 1.102280392e-05, 0.000140535927, };
|
||||
|
||||
static float i_hist[COEFFS] = {};
|
||||
static float q_hist[COEFFS] = {};
|
||||
|
||||
static FIRF_FILTER filter[2] = // i/q
|
||||
{
|
||||
{.length = COEFFS, .b = b, .hist = i_hist}, // 0
|
||||
{.length = COEFFS, .b = b, .hist = q_hist} // 1
|
||||
};
|
||||
#undef COEFFS
|
||||
|
||||
return firf(sample, &filter[i_or_q]);
|
||||
}
|
||||
|
||||
|
||||
static inline float lp_firfp_butter_1600kHz_160kHz_200kHz(int sample, size_t i_or_q)
|
||||
{
|
||||
#define COEFFS 23
|
||||
static const fixedpt b[COEFFS] = {fixedpt_rconst(0.000140535927), fixedpt_rconst(1.102280392e-05), fixedpt_rconst(0.0001309279731), fixedpt_rconst(0.001356012537), fixedpt_rconst(0.00551787474),
|
||||
fixedpt_rconst(0.01499414005), fixedpt_rconst(0.03160167988), fixedpt_rconst(0.05525973093), fixedpt_rconst(0.08315031015), fixedpt_rconst(0.1099887688),
|
||||
fixedpt_rconst(0.1295143636), fixedpt_rconst(0.1366692652), fixedpt_rconst(0.1295143636), fixedpt_rconst(0.1099887688), fixedpt_rconst(0.08315031015),
|
||||
fixedpt_rconst(0.05525973093), fixedpt_rconst(0.03160167988), fixedpt_rconst(0.01499414005), fixedpt_rconst(0.00551787474), fixedpt_rconst(0.001356012537),
|
||||
fixedpt_rconst(0.0001309279731), fixedpt_rconst(1.102280392e-05), fixedpt_rconst(0.000140535927),
|
||||
};
|
||||
static fixedpt i_hist[COEFFS] = {};
|
||||
static fixedpt q_hist[COEFFS] = {};
|
||||
|
||||
static FIRFP_FILTER filter[2] = // i/q
|
||||
{
|
||||
{.length = COEFFS, .b = b, .hist = i_hist}, // 0
|
||||
{.length = COEFFS, .b = b, .hist = q_hist} // 1
|
||||
};
|
||||
#undef COEFFS
|
||||
|
||||
return fixedpt_tofloat(firfp(fixedpt_fromint(sample), &filter[i_or_q]));
|
||||
}
|
||||
|
||||
|
||||
static inline float lp_ppf_butter_1600kHz_160kHz_200kHz(int sample, size_t i_or_q)
|
||||
{
|
||||
#define PHASES 2
|
||||
#define COEFFS 12
|
||||
static const float b[PHASES][COEFFS] =
|
||||
{
|
||||
{0.000140535927, 0.0001309279731, 0.00551787474, 0.03160167988, 0.08315031015, 0.1295143636, 0.1295143636, 0.08315031015, 0.03160167988, 0.00551787474, 0.0001309279731, 0.000140535927, },
|
||||
{1.102280392e-05, 0.001356012537, 0.01499414005, 0.05525973093, 0.1099887688, 0.1366692652, 0.1099887688, 0.05525973093, 0.01499414005, 0.001356012537, 1.102280392e-05, 0, },
|
||||
};
|
||||
|
||||
static float i_hist[PHASES][COEFFS] = {};
|
||||
static float q_hist[PHASES][COEFFS] = {};
|
||||
|
||||
static FIRF_FILTER fir[2][PHASES] =
|
||||
{
|
||||
{
|
||||
// i/q phase
|
||||
{.length = COEFFS, .b = b[1], .hist = i_hist[0]}, // 0 0
|
||||
{.length = COEFFS, .b = b[0], .hist = i_hist[1]} // 0 1
|
||||
},
|
||||
{
|
||||
// i/q phase
|
||||
{.length = COEFFS, .b = b[1], .hist = q_hist[0]}, // 1 0
|
||||
{.length = COEFFS, .b = b[0], .hist = q_hist[1]} // 1 1
|
||||
},
|
||||
};
|
||||
|
||||
static PPF_FILTER filter[2] =
|
||||
{
|
||||
{.sum = 0, .phase = 0, .max_phase = PHASES, .fir = fir[0]}, // 0 =: i
|
||||
{.sum = 0, .phase = 0, .max_phase = PHASES, .fir = fir[1]}, // 1 =: q
|
||||
};
|
||||
#undef COEFFS
|
||||
#undef PHASES
|
||||
|
||||
return ppf(sample, &filter[i_or_q]);
|
||||
}
|
||||
|
||||
|
||||
static inline float lp_ppffp_butter_1600kHz_160kHz_200kHz(int sample, size_t i_or_q)
|
||||
{
|
||||
#define PHASES 2
|
||||
#define COEFFS 12
|
||||
static const fixedpt b[PHASES][COEFFS] =
|
||||
{
|
||||
{fixedpt_rconst(0.000140535927), fixedpt_rconst(0.0001309279731), fixedpt_rconst(0.00551787474), fixedpt_rconst(0.03160167988), fixedpt_rconst(0.08315031015), fixedpt_rconst(0.1295143636), fixedpt_rconst(0.1295143636), fixedpt_rconst(0.08315031015), fixedpt_rconst(0.03160167988), fixedpt_rconst(0.00551787474), fixedpt_rconst(0.0001309279731), fixedpt_rconst(0.000140535927), },
|
||||
{fixedpt_rconst(1.102280392e-05), fixedpt_rconst(0.001356012537), fixedpt_rconst(0.01499414005), fixedpt_rconst(0.05525973093), fixedpt_rconst(0.1099887688), fixedpt_rconst(0.1366692652), fixedpt_rconst(0.1099887688), fixedpt_rconst(0.05525973093), fixedpt_rconst(0.01499414005), fixedpt_rconst(0.001356012537), fixedpt_rconst(1.102280392e-05), fixedpt_rconst(0), },
|
||||
};
|
||||
|
||||
static fixedpt i_hist[PHASES][COEFFS] = {};
|
||||
static fixedpt q_hist[PHASES][COEFFS] = {};
|
||||
|
||||
static FIRFP_FILTER fir[2][PHASES] =
|
||||
{
|
||||
{
|
||||
// i/q phase
|
||||
{.length = COEFFS, .b = b[1], .hist = i_hist[0]}, // 0 0
|
||||
{.length = COEFFS, .b = b[0], .hist = i_hist[1]} // 0 1
|
||||
},
|
||||
{
|
||||
// i/q phase
|
||||
{.length = COEFFS, .b = b[1], .hist = q_hist[0]}, // 1 0
|
||||
{.length = COEFFS, .b = b[0], .hist = q_hist[1]} // 1 1
|
||||
},
|
||||
};
|
||||
|
||||
static PPFFP_FILTER filter[2] =
|
||||
{
|
||||
{.sum = fixedpt_rconst(0), .phase = 0, .max_phase = PHASES, .fir = fir[0]}, // 0 =: i
|
||||
{.sum = fixedpt_rconst(0), .phase = 0, .max_phase = PHASES, .fir = fir[1]}, // 1 =: q
|
||||
};
|
||||
#undef COEFFS
|
||||
#undef PHASES
|
||||
|
||||
return fixedpt_tofloat(ppffp(fixedpt_fromint(sample), &filter[i_or_q]));
|
||||
}
|
||||
|
||||
|
||||
static inline float bp_iir_cheb1_800kHz_90kHz_98kHz_102kHz_110kHz(float sample)
|
||||
{
|
||||
#define GAIN 1.874981046e-06
|
||||
#define SECTIONS 3
|
||||
static const float b[3*SECTIONS] = {1, 1.999994649, 0.9999946492, 1, -1.99999482, 0.9999948196, 1, 1.703868036e-07, -1.000010531, };
|
||||
static const float a[3*SECTIONS] = {1, -1.387139203, 0.9921518712, 1, -1.403492665, 0.9845934971, 1, -1.430055639, 0.9923856172, };
|
||||
static float hist[3*SECTIONS] = {};
|
||||
|
||||
static IIRF_FILTER filter = {.sections = SECTIONS, .b = b, .a = a, .gain = GAIN, .hist = hist};
|
||||
#undef SECTIONS
|
||||
#undef GAIN
|
||||
|
||||
return iirf(sample, &filter);
|
||||
}
|
||||
|
||||
|
||||
static inline float lp_fir_butter_800kHz_100kHz_10kHz(float sample)
|
||||
{
|
||||
#define COEFFS 4
|
||||
static const float b[COEFFS] = {0.04421550009, 0.4557844999, 0.4557844999, 0.04421550009, };
|
||||
|
||||
static float hist[COEFFS];
|
||||
|
||||
static FIRF_FILTER filter = {.length = COEFFS, .b = b, .hist = hist};
|
||||
#undef COEFFS
|
||||
|
||||
return firf(sample, &filter);
|
||||
}
|
||||
|
||||
|
||||
static float rssi_filter(int sample)
|
||||
{
|
||||
static float old_sample;
|
||||
|
||||
#define ALPHA 0.6789f
|
||||
old_sample = ALPHA*sample + (1.0f - ALPHA)*old_sample;
|
||||
#undef ALPHA
|
||||
|
||||
return old_sample;
|
||||
}
|
||||
|
||||
|
||||
static inline float polar_discriminator(float i, float q)
|
||||
{
|
||||
static float complex s_last;
|
||||
const float complex s = i + q * _Complex_I;
|
||||
const float complex y = s * conj(s_last);
|
||||
|
||||
#if 0
|
||||
const float delta_phi = atan2_libm(y);
|
||||
#elif 1
|
||||
const float delta_phi = atan2_approximation(y);
|
||||
#else
|
||||
const float delta_phi = atan2_approximation2(y);
|
||||
#endif
|
||||
|
||||
s_last = s;
|
||||
|
||||
return delta_phi;
|
||||
}
|
||||
|
||||
|
||||
/** @brief Sparse Ones runs in time proportional to the number
|
||||
* of 1 bits.
|
||||
*
|
||||
* From: http://gurmeet.net/puzzles/fast-bit-counting-routines
|
||||
*/
|
||||
static inline unsigned count_set_bits_sparse_one(uint32_t n)
|
||||
{
|
||||
unsigned count = 0;
|
||||
|
||||
while (n)
|
||||
{
|
||||
count++;
|
||||
n &= (n - 1) ; // set rightmost 1 bit in n to 0
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
|
||||
static inline unsigned count_set_bits(uint32_t n)
|
||||
{
|
||||
#if defined(__i386__) || defined(__arm__)
|
||||
return __builtin_popcount(n);
|
||||
#else
|
||||
return count_set_bits_sparse_one(n);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
static inline int majority_votes_bitfilter(uint32_t unfilt_bitstream, uint32_t bits_in_unfilt_bitstream)
|
||||
{
|
||||
const unsigned ones = count_set_bits(unfilt_bitstream & bits_in_unfilt_bitstream);
|
||||
const bool odd = (ones & 1) > 0;
|
||||
const uint32_t bits_in_unfilt_bitstream_half = count_set_bits(bits_in_unfilt_bitstream)/2;
|
||||
|
||||
if (odd)
|
||||
return (ones <= bits_in_unfilt_bitstream_half) ? 0 : 1;
|
||||
|
||||
if (ones < bits_in_unfilt_bitstream_half)
|
||||
return 0;
|
||||
|
||||
if (ones > bits_in_unfilt_bitstream_half)
|
||||
return 1;
|
||||
|
||||
return unfilt_bitstream & 1;
|
||||
}
|
||||
|
||||
|
||||
typedef void (*OutFunction)(unsigned bit, unsigned rssi);
|
||||
|
||||
|
||||
static inline void to_stdout(unsigned bit, unsigned rssi)
|
||||
{
|
||||
(void)rssi;
|
||||
|
||||
const uint8_t tmp = bit;
|
||||
|
||||
fwrite(&tmp, sizeof(tmp), 1, stdout);
|
||||
}
|
||||
|
||||
|
||||
static const OutFunction out_functions[] = { to_stdout, t1_c1_packet_decoder };
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
(void)argc;
|
||||
(void)argv;
|
||||
|
||||
|
||||
// --- parameter section begin ---
|
||||
// The idea behind the variables in the section is to make parameters
|
||||
// configurable via command line.
|
||||
const unsigned CLOCK_LOCK_THRESHOLD = 2;
|
||||
|
||||
const unsigned DECIMATION_RATE = 2;
|
||||
|
||||
//#define USING_BITFILTER
|
||||
|
||||
const uint32_t ACCESS_CODE = 0b0101010101010000111101u;
|
||||
const uint32_t ACCESS_CODE_BITMASK = 0x3FFFFFu;
|
||||
const unsigned ACCESS_CODE_ERRORS = 1u; // 0 if no errors allowed
|
||||
// --- parameter section end ---
|
||||
|
||||
// Select function for output
|
||||
OutFunction out_function = out_functions[1];
|
||||
|
||||
__attribute__((__aligned__(16))) uint8_t samples[4096];
|
||||
float i = 0, q = 0;
|
||||
unsigned decimation_rate_index = 0;
|
||||
int16_t old_clock = INT16_MIN;
|
||||
uint32_t bitstream = 0;
|
||||
unsigned clock_lock = 0;
|
||||
|
||||
#if defined(USING_BITFILTER)
|
||||
uint32_t unfilt_bitstream = 0;
|
||||
uint32_t bits_in_unfilt_bitstream = 0;
|
||||
#endif
|
||||
|
||||
#if defined (__SSE4_2__)
|
||||
__attribute__((__aligned__(16))) int16_t iq_samples[sizeof(samples)];
|
||||
const __m128i dc_offset = _mm_set_epi16(-127, -127, -127, -127, -127, -127, -127, -127);
|
||||
#endif
|
||||
|
||||
//FILE *input = fopen("samples.bin", "rb");
|
||||
//FILE *input = get_net("localhost", 14423);
|
||||
FILE *input= stdin;
|
||||
|
||||
if (input == NULL)
|
||||
{
|
||||
fprintf(stderr, "opening input error\n");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
//FILE *demod_out = fopen("demod.bin", "wb");
|
||||
//FILE *demod_out2 = fopen("demod.bin", "wb");
|
||||
//FILE *clock_out = fopen("clock.bin", "wb");
|
||||
//FILE *bits_out= fopen("bits.bin", "wb");
|
||||
|
||||
while (!feof(input))
|
||||
{
|
||||
size_t read_items = fread(samples, sizeof(samples), 1, input);
|
||||
if (1 != read_items)
|
||||
{
|
||||
// End of file?..
|
||||
return 2;
|
||||
}
|
||||
|
||||
#if defined (__SSE4_2__)
|
||||
for (size_t k = 0; k < sizeof(samples)/sizeof(samples[0]); k += 8) // +2 : i and q interleaved
|
||||
{
|
||||
__m128i tmp = _mm_loadu_si128((__m128i const*)&samples[k]); // Hmmm, loading 8 byte besides of upper boundary?..
|
||||
__m128i cvt = _mm_add_epi16(_mm_cvtepu8_epi16(tmp), dc_offset);
|
||||
_mm_store_si128((__m128i *)&iq_samples[k], cvt);
|
||||
}
|
||||
#endif
|
||||
|
||||
for (size_t k = 0; k < sizeof(samples)/sizeof(samples[0]); k += 2) // +2 : i and q interleaved
|
||||
{
|
||||
#if defined (__SSE4_2__)
|
||||
const int i_unfilt = iq_samples[k];
|
||||
const int q_unfilt = iq_samples[k+1];
|
||||
#else
|
||||
const int i_unfilt = ((int)samples[k] - 127);
|
||||
const int q_unfilt = ((int)samples[k + 1] - 127);
|
||||
#endif
|
||||
|
||||
// Low-Pass-Filtering before decimation is necessary, to ensure
|
||||
// that i and q signals don't contain frequencies above new sample
|
||||
// rate.
|
||||
// The sample rate decimation is realised as sum over i and q,
|
||||
// which must not be divided by decimation factor before
|
||||
// demodulating (atan2(q,i)).
|
||||
#if 0
|
||||
i = lp_fir_butter_1600kHz_160kHz_200kHz(i_unfilt, 0);
|
||||
q = lp_fir_butter_1600kHz_160kHz_200kHz(q_unfilt, 1);
|
||||
#elif 0
|
||||
i = lp_ppf_butter_1600kHz_160kHz_200kHz(i_unfilt, 0);
|
||||
q = lp_ppf_butter_1600kHz_160kHz_200kHz(q_unfilt, 1);
|
||||
#elif 0
|
||||
i = lp_firfp_butter_1600kHz_160kHz_200kHz(i_unfilt, 0);
|
||||
q = lp_firfp_butter_1600kHz_160kHz_200kHz(q_unfilt, 1);
|
||||
#elif 0
|
||||
i = lp_ppffp_butter_1600kHz_160kHz_200kHz(i_unfilt, 0);
|
||||
q = lp_ppffp_butter_1600kHz_160kHz_200kHz(q_unfilt, 1);
|
||||
#elif 0
|
||||
i += lp_1600kHz_58kHz(i_unfilt, 0);
|
||||
q += lp_1600kHz_58kHz(q_unfilt, 1);
|
||||
#define USE_MOVING_AVERAGE
|
||||
#else
|
||||
i += moving_average(i_unfilt, 0);
|
||||
q += moving_average(q_unfilt, 1);
|
||||
#define USE_MOVING_AVERAGE
|
||||
#endif
|
||||
|
||||
++decimation_rate_index;
|
||||
if (decimation_rate_index < DECIMATION_RATE) continue;
|
||||
decimation_rate_index = 0;
|
||||
|
||||
// Demodulate.
|
||||
float delta_phi = polar_discriminator(i, q);
|
||||
|
||||
//int16_t demodulated_signal = (INT16_MAX-1)*delta_phi;
|
||||
//fwrite(&demodulated_signal, sizeof(demodulated_signal), 1, demod_out);
|
||||
|
||||
// Post-filtering to prevent bit errors because of signal jitter.
|
||||
delta_phi = lp_fir_butter_800kHz_100kHz_10kHz(delta_phi);
|
||||
//int16_t demodulated_signal = (INT16_MAX-1)*delta_phi;
|
||||
//fwrite(&demodulated_signal, sizeof(demodulated_signal), 1, demod_out2);
|
||||
|
||||
// --- clock recovery section begin ---
|
||||
// The time-2 method is implemented: push squared signal through a bandpass
|
||||
// tuned close to the symbol rate. Saturating band-pass output produces a
|
||||
// rectangular pulses with the required timing information.
|
||||
// Clock-Signal is crossing zero in half period.
|
||||
const int16_t clock = (bp_iir_cheb1_800kHz_90kHz_98kHz_102kHz_110kHz(delta_phi * delta_phi) >= 0) ? INT16_MAX : INT16_MIN;
|
||||
//fwrite(&clock, sizeof(clock), 1, clock_out);
|
||||
|
||||
unsigned bit = (delta_phi >= 0) ? (1u<<PACKET_DATABIT_SHIFT) : (0u<<PACKET_DATABIT_SHIFT);
|
||||
|
||||
#if defined(USING_BITFILTER)
|
||||
unfilt_bitstream = (unfilt_bitstream << 1) | bit;
|
||||
bits_in_unfilt_bitstream = (bits_in_unfilt_bitstream << 1) | 1;
|
||||
#endif
|
||||
|
||||
// We are using one simple filter to rssi value in order to
|
||||
// prevent unexpected "splashes" in signal power.
|
||||
float rssi = sqrtf(i*i + q*q);
|
||||
rssi = rssi_filter(rssi); // comment out, if rssi filtering is unwanted
|
||||
|
||||
if (clock > old_clock) // rising edge
|
||||
{
|
||||
clock_lock = 1;
|
||||
|
||||
#if defined(USING_BITFILTER)
|
||||
unfilt_bitstream = bit;
|
||||
bits_in_unfilt_bitstream = 1;
|
||||
#endif
|
||||
}
|
||||
else if (old_clock == clock && clock_lock < CLOCK_LOCK_THRESHOLD)
|
||||
{
|
||||
clock_lock++;
|
||||
}
|
||||
else if (clock_lock == CLOCK_LOCK_THRESHOLD) // sample data bit on CLOCK_LOCK_THRESHOLD after rose up
|
||||
{
|
||||
clock_lock++;
|
||||
|
||||
#if defined(USE_MOVING_AVERAGE)
|
||||
// If using moving average, we would habe doubles of each of i- and q- signal components.
|
||||
rssi /= DECIMATION_RATE;
|
||||
#endif
|
||||
|
||||
#if defined(USING_BITFILTER)
|
||||
// Bitfilter can be used to remove unwanted spikes in the demodulated signal.
|
||||
bit = majority_votes_bitfilter(unfilt_bitstream, bits_in_unfilt_bitstream);
|
||||
#endif
|
||||
|
||||
bitstream = (bitstream << 1) | bit;
|
||||
|
||||
if (count_set_bits((bitstream & ACCESS_CODE_BITMASK) ^ ACCESS_CODE) <= ACCESS_CODE_ERRORS)
|
||||
{
|
||||
bit |= (1u<<PACKET_PREAMBLE_DETECTED_SHIFT); // packet detected; mark the bit similar to "Access Code"-Block in GNU Radio
|
||||
}
|
||||
|
||||
//fwrite(&bit, sizeof(bit), 1, bits_out);
|
||||
out_function(bit, rssi);
|
||||
}
|
||||
old_clock = clock;
|
||||
// --- clock recovery section end ---
|
||||
|
||||
#if defined(USE_MOVING_AVERAGE)
|
||||
i = q = 0;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,586 @@
|
|||
#ifndef T1_C1_PACKET_DECODER_H
|
||||
#define T1_C1_PACKET_DECODER_H
|
||||
|
||||
/*-
|
||||
* Copyright (c) 2017 <xael.south@yandex.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#if !defined(PACKET_CAPTURE_THRESHOLD)
|
||||
#define PACKET_CAPTURE_THRESHOLD 5u
|
||||
#endif
|
||||
|
||||
#define C1_MODE_A 0b010101001100
|
||||
#define C1_MODE_B 0b010101000011
|
||||
#define C1_MODE_AB_TRAILER 0b1101
|
||||
|
||||
#define PACKET_DATABIT_SHIFT (0u)
|
||||
#define PACKET_PREAMBLE_DETECTED_SHIFT (1u)
|
||||
|
||||
#define PACKET_DATABIT_MASK (1u<<PACKET_DATABIT_SHIFT)
|
||||
#define PACKET_PREAMBLE_DETECTED_MASK (1u<<PACKET_PREAMBLE_DETECTED_SHIFT)
|
||||
|
||||
|
||||
static const uint8_t HIGH_NIBBLE_3OUTOF6[] =
|
||||
{
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x30, 0xFF, 0x10, 0x20, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0x70, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0x50, 0x60, 0xFF, 0x40, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xB0, 0xFF, 0x90, 0xA0, 0xFF, 0xFF, 0xF0, 0xFF, 0xFF, 0x80, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xD0, 0xE0, 0xFF, 0xC0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
|
||||
};
|
||||
|
||||
|
||||
static const uint8_t LOW_NIBBLE_3OUTOF6[] =
|
||||
{
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x03, 0xFF, 0x01, 0x02, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0x07, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0x05, 0x06, 0xFF, 0x04, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0x0B, 0xFF, 0x09, 0x0A, 0xFF, 0xFF, 0x0F, 0xFF, 0xFF, 0x08, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0x0D, 0x0E, 0xFF, 0x0C, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
|
||||
};
|
||||
|
||||
|
||||
static const uint16_t FULL_TLG_LENGTH_FROM_L_FIELD[] =
|
||||
{
|
||||
3, 4, 5, 6, 7, 8, 9, 10, 11, 12,
|
||||
15, 16, 17, 18, 19, 20, 21, 22, 23, 24,
|
||||
25, 26, 27, 28, 29, 30, 33, 34, 35, 36,
|
||||
37, 38, 39, 40, 41, 42, 43, 44, 45, 46,
|
||||
47, 48, 51, 52, 53, 54, 55, 56, 57, 58,
|
||||
59, 60, 61, 62, 63, 64, 65, 66, 69, 70,
|
||||
71, 72, 73, 74, 75, 76, 77, 78, 79, 80,
|
||||
81, 82, 83, 84, 87, 88, 89, 90, 91, 92,
|
||||
93, 94, 95, 96, 97, 98, 99, 100, 101, 102,
|
||||
105, 106, 107, 108, 109, 110, 111, 112, 113, 114,
|
||||
115, 116, 117, 118, 119, 120, 123, 124, 125, 126,
|
||||
127, 128, 129, 130, 131, 132, 133, 134, 135, 136,
|
||||
137, 138, 141, 142, 143, 144, 145, 146, 147, 148,
|
||||
149, 150, 151, 152, 153, 154, 155, 156, 159, 160,
|
||||
161, 162, 163, 164, 165, 166, 167, 168, 169, 170,
|
||||
171, 172, 173, 174, 177, 178, 179, 180, 181, 182,
|
||||
183, 184, 185, 186, 187, 188, 189, 190, 191, 192,
|
||||
195, 196, 197, 198, 199, 200, 201, 202, 203, 204,
|
||||
205, 206, 207, 208, 209, 210, 213, 214, 215, 216,
|
||||
217, 218, 219, 220, 221, 222, 223, 224, 225, 226,
|
||||
227, 228, 231, 232, 233, 234, 235, 236, 237, 238,
|
||||
239, 240, 241, 242, 243, 244, 245, 246, 249, 250,
|
||||
251, 252, 253, 254, 255, 256, 257, 258, 259, 260,
|
||||
261, 262, 263, 264, 267, 268, 269, 270, 271, 272,
|
||||
273, 274, 275, 276, 277, 278, 279, 280, 281, 282,
|
||||
285, 286, 287, 288, 289, 290
|
||||
};
|
||||
|
||||
|
||||
static const uint16_t CRC16_DNP_TABLE[] =
|
||||
{
|
||||
0x0000, 0x3d65, 0x7aca, 0x47af, 0xf594, 0xc8f1, 0x8f5e, 0xb23b,
|
||||
0xd64d, 0xeb28, 0xac87, 0x91e2, 0x23d9, 0x1ebc, 0x5913, 0x6476,
|
||||
0x91ff, 0xac9a, 0xeb35, 0xd650, 0x646b, 0x590e, 0x1ea1, 0x23c4,
|
||||
0x47b2, 0x7ad7, 0x3d78, 0x001d, 0xb226, 0x8f43, 0xc8ec, 0xf589,
|
||||
0x1e9b, 0x23fe, 0x6451, 0x5934, 0xeb0f, 0xd66a, 0x91c5, 0xaca0,
|
||||
0xc8d6, 0xf5b3, 0xb21c, 0x8f79, 0x3d42, 0x0027, 0x4788, 0x7aed,
|
||||
0x8f64, 0xb201, 0xf5ae, 0xc8cb, 0x7af0, 0x4795, 0x003a, 0x3d5f,
|
||||
0x5929, 0x644c, 0x23e3, 0x1e86, 0xacbd, 0x91d8, 0xd677, 0xeb12,
|
||||
0x3d36, 0x0053, 0x47fc, 0x7a99, 0xc8a2, 0xf5c7, 0xb268, 0x8f0d,
|
||||
0xeb7b, 0xd61e, 0x91b1, 0xacd4, 0x1eef, 0x238a, 0x6425, 0x5940,
|
||||
0xacc9, 0x91ac, 0xd603, 0xeb66, 0x595d, 0x6438, 0x2397, 0x1ef2,
|
||||
0x7a84, 0x47e1, 0x004e, 0x3d2b, 0x8f10, 0xb275, 0xf5da, 0xc8bf,
|
||||
0x23ad, 0x1ec8, 0x5967, 0x6402, 0xd639, 0xeb5c, 0xacf3, 0x9196,
|
||||
0xf5e0, 0xc885, 0x8f2a, 0xb24f, 0x0074, 0x3d11, 0x7abe, 0x47db,
|
||||
0xb252, 0x8f37, 0xc898, 0xf5fd, 0x47c6, 0x7aa3, 0x3d0c, 0x0069,
|
||||
0x641f, 0x597a, 0x1ed5, 0x23b0, 0x918b, 0xacee, 0xeb41, 0xd624,
|
||||
0x7a6c, 0x4709, 0x00a6, 0x3dc3, 0x8ff8, 0xb29d, 0xf532, 0xc857,
|
||||
0xac21, 0x9144, 0xd6eb, 0xeb8e, 0x59b5, 0x64d0, 0x237f, 0x1e1a,
|
||||
0xeb93, 0xd6f6, 0x9159, 0xac3c, 0x1e07, 0x2362, 0x64cd, 0x59a8,
|
||||
0x3dde, 0x00bb, 0x4714, 0x7a71, 0xc84a, 0xf52f, 0xb280, 0x8fe5,
|
||||
0x64f7, 0x5992, 0x1e3d, 0x2358, 0x9163, 0xac06, 0xeba9, 0xd6cc,
|
||||
0xb2ba, 0x8fdf, 0xc870, 0xf515, 0x472e, 0x7a4b, 0x3de4, 0x0081,
|
||||
0xf508, 0xc86d, 0x8fc2, 0xb2a7, 0x009c, 0x3df9, 0x7a56, 0x4733,
|
||||
0x2345, 0x1e20, 0x598f, 0x64ea, 0xd6d1, 0xebb4, 0xac1b, 0x917e,
|
||||
0x475a, 0x7a3f, 0x3d90, 0x00f5, 0xb2ce, 0x8fab, 0xc804, 0xf561,
|
||||
0x9117, 0xac72, 0xebdd, 0xd6b8, 0x6483, 0x59e6, 0x1e49, 0x232c,
|
||||
0xd6a5, 0xebc0, 0xac6f, 0x910a, 0x2331, 0x1e54, 0x59fb, 0x649e,
|
||||
0x00e8, 0x3d8d, 0x7a22, 0x4747, 0xf57c, 0xc819, 0x8fb6, 0xb2d3,
|
||||
0x59c1, 0x64a4, 0x230b, 0x1e6e, 0xac55, 0x9130, 0xd69f, 0xebfa,
|
||||
0x8f8c, 0xb2e9, 0xf546, 0xc823, 0x7a18, 0x477d, 0x00d2, 0x3db7,
|
||||
0xc83e, 0xf55b, 0xb2f4, 0x8f91, 0x3daa, 0x00cf, 0x4760, 0x7a05,
|
||||
0x1e73, 0x2316, 0x64b9, 0x59dc, 0xebe7, 0xd682, 0x912d, 0xac48,
|
||||
};
|
||||
|
||||
|
||||
typedef void (*t1_c1_packet_decoder_state)(unsigned bit);
|
||||
|
||||
static void idle(unsigned bit);
|
||||
static void done(unsigned bit);
|
||||
static void rx_bit(unsigned bit);
|
||||
|
||||
static void rx_high_nibble_first_lfield_bit(unsigned bit);
|
||||
static void rx_high_nibble_last_lfield_bit(unsigned bit);
|
||||
|
||||
static void rx_low_nibble_first_lfield_bit(unsigned bit);
|
||||
static void rx_low_nibble_last_lfield_bit(unsigned bit);
|
||||
|
||||
static void rx_high_nibble_first_data_bit(unsigned bit);
|
||||
static void rx_high_nibble_last_data_bit(unsigned bit);
|
||||
|
||||
static void rx_low_nibble_first_data_bit(unsigned bit);
|
||||
static void rx_low_nibble_last_data_bit(unsigned bit);
|
||||
|
||||
static void c1_rx_bit(unsigned bit);
|
||||
|
||||
static void c1_rx_first_mode_bit(unsigned bit);
|
||||
static void c1_rx_last_mode_bit(unsigned bit);
|
||||
|
||||
static void c1_rx_first_lfield_bit(unsigned bit);
|
||||
static void c1_rx_last_lfield_bit(unsigned bit);
|
||||
|
||||
static void c1_rx_first_data_bit(unsigned bit);
|
||||
static void c1_rx_last_data_bit(unsigned bit);
|
||||
|
||||
|
||||
static const t1_c1_packet_decoder_state states[] =
|
||||
{
|
||||
idle, // 0
|
||||
|
||||
rx_high_nibble_first_lfield_bit, // 1
|
||||
rx_bit, // 2
|
||||
rx_bit, // 3
|
||||
rx_bit, // 4
|
||||
rx_bit, // 5
|
||||
rx_high_nibble_last_lfield_bit, // 6
|
||||
|
||||
rx_low_nibble_first_lfield_bit, // 7
|
||||
rx_bit, // 8
|
||||
rx_bit, // 9
|
||||
rx_bit, // 10
|
||||
rx_bit, // 11
|
||||
rx_low_nibble_last_lfield_bit, // 12
|
||||
|
||||
rx_high_nibble_first_data_bit, // 13
|
||||
rx_bit, // 14
|
||||
rx_bit, // 15
|
||||
rx_bit, // 16
|
||||
rx_bit, // 17
|
||||
rx_high_nibble_last_data_bit, // 18
|
||||
|
||||
rx_low_nibble_first_data_bit, // 19
|
||||
rx_bit, // 20
|
||||
rx_bit, // 21
|
||||
rx_bit, // 22
|
||||
rx_bit, // 23
|
||||
rx_low_nibble_last_data_bit, // 24
|
||||
|
||||
done, // 25
|
||||
|
||||
c1_rx_first_mode_bit, // 26
|
||||
c1_rx_bit, // 27
|
||||
c1_rx_bit, // 28
|
||||
c1_rx_last_mode_bit, // 29
|
||||
|
||||
c1_rx_first_lfield_bit, // 30
|
||||
c1_rx_bit, // 31
|
||||
c1_rx_bit, // 32
|
||||
c1_rx_bit, // 33
|
||||
c1_rx_bit, // 34
|
||||
c1_rx_bit, // 35
|
||||
c1_rx_bit, // 36
|
||||
c1_rx_last_lfield_bit, // 37
|
||||
|
||||
c1_rx_first_data_bit, // 38
|
||||
c1_rx_bit, // 39
|
||||
c1_rx_bit, // 40
|
||||
c1_rx_bit, // 41
|
||||
c1_rx_bit, // 42
|
||||
c1_rx_bit, // 43
|
||||
c1_rx_bit, // 44
|
||||
c1_rx_last_data_bit, // 45
|
||||
|
||||
done, // 46
|
||||
};
|
||||
|
||||
|
||||
static struct
|
||||
{
|
||||
const t1_c1_packet_decoder_state *state;
|
||||
unsigned current_rssi;
|
||||
unsigned packet_rssi;
|
||||
union
|
||||
{
|
||||
unsigned flags;
|
||||
struct
|
||||
{
|
||||
unsigned err_3outof: 1;
|
||||
unsigned crc_ok: 1;
|
||||
unsigned c1_packet: 1;
|
||||
};
|
||||
};
|
||||
unsigned l;
|
||||
unsigned L;
|
||||
unsigned mode;
|
||||
unsigned byte;
|
||||
uint8_t packet[290]; // max. packet length with L- and all CRC-Fields
|
||||
char timestamp[64];
|
||||
} t1_c1_packet_decoder_work = {.state = &states[0]}; // idle
|
||||
|
||||
|
||||
static void idle(unsigned bit)
|
||||
{
|
||||
if (!(bit & PACKET_PREAMBLE_DETECTED_MASK))
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[0]; // idle
|
||||
}
|
||||
}
|
||||
|
||||
static void done(unsigned bit)
|
||||
{
|
||||
(void)bit;
|
||||
}
|
||||
|
||||
static void rx_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void rx_high_nibble_first_lfield_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
t1_c1_packet_decoder_work.packet_rssi = t1_c1_packet_decoder_work.current_rssi;
|
||||
}
|
||||
|
||||
static void rx_high_nibble_last_lfield_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
t1_c1_packet_decoder_work.mode = t1_c1_packet_decoder_work.byte;
|
||||
|
||||
t1_c1_packet_decoder_work.L = HIGH_NIBBLE_3OUTOF6[t1_c1_packet_decoder_work.byte];
|
||||
t1_c1_packet_decoder_work.flags = 0;
|
||||
}
|
||||
|
||||
static void rx_low_nibble_first_lfield_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void rx_low_nibble_last_lfield_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
t1_c1_packet_decoder_work.mode <<= 6;
|
||||
t1_c1_packet_decoder_work.mode |= t1_c1_packet_decoder_work.byte;
|
||||
|
||||
const unsigned byte = LOW_NIBBLE_3OUTOF6[t1_c1_packet_decoder_work.byte];
|
||||
|
||||
if (t1_c1_packet_decoder_work.L == 0xFFu || byte == 0xFFu)
|
||||
{
|
||||
if (t1_c1_packet_decoder_work.mode == C1_MODE_A || t1_c1_packet_decoder_work.mode == C1_MODE_B)
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[26]; // c1_rx_first_mode_bit
|
||||
}
|
||||
else
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[0]; // idle
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
t1_c1_packet_decoder_work.L |= byte;
|
||||
t1_c1_packet_decoder_work.l = 0;
|
||||
t1_c1_packet_decoder_work.packet[t1_c1_packet_decoder_work.l++] = t1_c1_packet_decoder_work.L;
|
||||
t1_c1_packet_decoder_work.L = FULL_TLG_LENGTH_FROM_L_FIELD[t1_c1_packet_decoder_work.L];
|
||||
}
|
||||
}
|
||||
|
||||
static void rx_high_nibble_first_data_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void rx_high_nibble_last_data_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
|
||||
const unsigned byte = HIGH_NIBBLE_3OUTOF6[t1_c1_packet_decoder_work.byte];
|
||||
|
||||
if (byte == 0xFFu) t1_c1_packet_decoder_work.err_3outof = 1;
|
||||
|
||||
t1_c1_packet_decoder_work.packet[t1_c1_packet_decoder_work.l] = byte;
|
||||
}
|
||||
|
||||
static void rx_low_nibble_first_data_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void rx_low_nibble_last_data_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
|
||||
const unsigned byte = LOW_NIBBLE_3OUTOF6[t1_c1_packet_decoder_work.byte];
|
||||
|
||||
if (byte == 0xFFu) t1_c1_packet_decoder_work.err_3outof = 1;
|
||||
|
||||
t1_c1_packet_decoder_work.packet[t1_c1_packet_decoder_work.l++] |= byte;
|
||||
|
||||
if (t1_c1_packet_decoder_work.l < t1_c1_packet_decoder_work.L)
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[13]; // rx_high_nibble_first_data_bit
|
||||
}
|
||||
else
|
||||
{
|
||||
time_t now;
|
||||
time(&now);
|
||||
|
||||
struct tm *timeinfo = gmtime(&now);
|
||||
strftime(t1_c1_packet_decoder_work.timestamp, sizeof(t1_c1_packet_decoder_work.timestamp), "%Y-%m-%d %H:%M:%S.000", timeinfo);
|
||||
}
|
||||
}
|
||||
|
||||
static void c1_rx_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void c1_rx_first_mode_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void c1_rx_last_mode_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
|
||||
t1_c1_packet_decoder_work.mode <<= 4;
|
||||
t1_c1_packet_decoder_work.mode |= t1_c1_packet_decoder_work.byte;
|
||||
|
||||
if (t1_c1_packet_decoder_work.byte == C1_MODE_AB_TRAILER)
|
||||
{
|
||||
t1_c1_packet_decoder_work.c1_packet = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[0]; // idle
|
||||
}
|
||||
}
|
||||
|
||||
static void c1_rx_first_lfield_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void c1_rx_last_lfield_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
|
||||
t1_c1_packet_decoder_work.L = t1_c1_packet_decoder_work.byte;
|
||||
t1_c1_packet_decoder_work.l = 0;
|
||||
t1_c1_packet_decoder_work.packet[t1_c1_packet_decoder_work.l++] = t1_c1_packet_decoder_work.L;
|
||||
t1_c1_packet_decoder_work.L = FULL_TLG_LENGTH_FROM_L_FIELD[t1_c1_packet_decoder_work.L];
|
||||
}
|
||||
|
||||
static void c1_rx_first_data_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte = (bit & PACKET_DATABIT_MASK);
|
||||
}
|
||||
|
||||
static void c1_rx_last_data_bit(unsigned bit)
|
||||
{
|
||||
t1_c1_packet_decoder_work.byte <<= 1;
|
||||
t1_c1_packet_decoder_work.byte |= (bit & PACKET_DATABIT_MASK);
|
||||
|
||||
t1_c1_packet_decoder_work.packet[t1_c1_packet_decoder_work.l++] = t1_c1_packet_decoder_work.byte;
|
||||
|
||||
if (t1_c1_packet_decoder_work.l < t1_c1_packet_decoder_work.L)
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[38]; // c1_rx_first_data_bit
|
||||
}
|
||||
else
|
||||
{
|
||||
time_t now;
|
||||
time(&now);
|
||||
|
||||
struct tm *timeinfo = gmtime(&now);
|
||||
strftime(t1_c1_packet_decoder_work.timestamp, sizeof(t1_c1_packet_decoder_work.timestamp), "%Y-%m-%d %H:%M:%S.000", timeinfo);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static uint16_t calc_crc_wmbus(const uint8_t *data, size_t datalen)
|
||||
{
|
||||
uint16_t crc = 0;
|
||||
while (datalen--) crc = CRC16_DNP_TABLE[*data++ ^ (crc >> 8)] ^ (crc << 8);
|
||||
crc = ~crc;
|
||||
return crc;
|
||||
}
|
||||
|
||||
static bool check_calc_crc_wmbus(const uint8_t *data, size_t datalen)
|
||||
{
|
||||
bool crc_ok = false;
|
||||
uint16_t crc1, crc2;
|
||||
|
||||
if (datalen >= 12)
|
||||
{
|
||||
crc1 = calc_crc_wmbus(data, 10);
|
||||
crc2 = (data[10] << 8) | (data[11]);
|
||||
data += 12;
|
||||
datalen -= 12;
|
||||
crc_ok = (crc1 == crc2);
|
||||
|
||||
while (crc_ok && datalen)
|
||||
{
|
||||
if (datalen >= 18)
|
||||
{
|
||||
crc1 = calc_crc_wmbus(data, 16);
|
||||
crc2 = (data[16] << 8) | (data[17]);
|
||||
data += 18;
|
||||
datalen -= 18;
|
||||
crc_ok = (crc1 == crc2);
|
||||
}
|
||||
else
|
||||
{
|
||||
crc1 = calc_crc_wmbus(data, datalen-2);
|
||||
crc2 = (data[datalen-2] << 8) | (data[datalen-1]);
|
||||
data += datalen;
|
||||
datalen -= datalen;
|
||||
crc_ok = (crc1 == crc2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return crc_ok;
|
||||
}
|
||||
|
||||
/** @brief Strip CRCs in place. */
|
||||
static unsigned cook_pkt(uint8_t *data, unsigned datalen)
|
||||
{
|
||||
uint8_t *dst = data;
|
||||
unsigned dstlen = 0;
|
||||
|
||||
if (datalen >= 12)
|
||||
{
|
||||
dst += 10;
|
||||
dstlen += 10;
|
||||
|
||||
data += 12;
|
||||
datalen -= 12;
|
||||
|
||||
while (datalen)
|
||||
{
|
||||
if (datalen >= 18)
|
||||
{
|
||||
memmove(dst, data, 16);
|
||||
|
||||
dst += 16;
|
||||
dstlen += 16;
|
||||
|
||||
data += 18;
|
||||
datalen -= 18;
|
||||
}
|
||||
else
|
||||
{
|
||||
memmove(dst, data, datalen-2);
|
||||
|
||||
dst += (datalen-2);
|
||||
dstlen += (datalen-2);
|
||||
|
||||
data += datalen;
|
||||
datalen -= datalen;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return dstlen;
|
||||
}
|
||||
|
||||
static inline uint32_t get_serial(const uint8_t *const packet)
|
||||
{
|
||||
uint32_t serial;
|
||||
|
||||
memcpy(&serial, &packet[4], sizeof(serial));
|
||||
|
||||
return serial;
|
||||
}
|
||||
|
||||
static void t1_c1_packet_decoder(unsigned bit, unsigned rssi)
|
||||
{
|
||||
t1_c1_packet_decoder_work.current_rssi = rssi;
|
||||
|
||||
(*t1_c1_packet_decoder_work.state++)(bit);
|
||||
|
||||
if (*t1_c1_packet_decoder_work.state == idle)
|
||||
{
|
||||
// nothing
|
||||
}
|
||||
else if (*t1_c1_packet_decoder_work.state == done)
|
||||
{
|
||||
t1_c1_packet_decoder_work.crc_ok = check_calc_crc_wmbus(t1_c1_packet_decoder_work.packet, t1_c1_packet_decoder_work.L) ? 1 : 0;
|
||||
|
||||
fprintf(stdout, "%s;%u;%u;%s;%u;%u;%08X;", t1_c1_packet_decoder_work.c1_packet ? "C1": "T1",
|
||||
t1_c1_packet_decoder_work.crc_ok,
|
||||
t1_c1_packet_decoder_work.err_3outof^1,
|
||||
t1_c1_packet_decoder_work.timestamp,
|
||||
t1_c1_packet_decoder_work.packet_rssi,
|
||||
rssi,
|
||||
get_serial(t1_c1_packet_decoder_work.packet));
|
||||
|
||||
#if 0
|
||||
fprintf(stdout, "0x");
|
||||
for (size_t l = 0; l < t1_c1_packet_decoder_work.L; l++) fprintf(stdout, "%02x", t1_c1_packet_decoder_work.packet[l]);
|
||||
fprintf(stdout, ";");
|
||||
#endif
|
||||
|
||||
#if 1
|
||||
t1_c1_packet_decoder_work.L = cook_pkt(t1_c1_packet_decoder_work.packet, t1_c1_packet_decoder_work.L);
|
||||
fprintf(stdout, "0x");
|
||||
for (size_t l = 0; l < t1_c1_packet_decoder_work.L; l++) fprintf(stdout, "%02x", t1_c1_packet_decoder_work.packet[l]);
|
||||
#endif
|
||||
|
||||
fprintf(stdout, "\n");
|
||||
fflush(stdout);
|
||||
|
||||
t1_c1_packet_decoder_work.state = &states[0]; // idle
|
||||
}
|
||||
else
|
||||
{
|
||||
// Stop receiving packet if current rssi below threshold.
|
||||
// The current packet seems to be collided with an another one.
|
||||
if (rssi < PACKET_CAPTURE_THRESHOLD)
|
||||
{
|
||||
t1_c1_packet_decoder_work.state = &states[0]; // idle
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* T1_C1_PACKET_DECODER_H */
|
||||
|
||||
Ładowanie…
Reference in New Issue