From 45556bd223f5ef932d847e9968ae5b154506f490 Mon Sep 17 00:00:00 2001 From: Mark Jessop Date: Sat, 23 Jul 2022 11:17:08 +0930 Subject: [PATCH] Rebase demodulators, update some test scripts. Needs a lot of real-world testing. --- auto_rx/autorx/__init__.py | 2 +- auto_rx/autorx/decode.py | 2 +- auto_rx/test/generate_lowsnr.py | 3 +- auto_rx/test/plot_per.py | 2 + auto_rx/test/test_demod.py | 12 +- demod/mod/demod_mod.h | 1 + demod/mod/m20mod.c | 5 + demod/mod/mXXmod.c | 1383 -------------------- demod/mod/meisei100mod.c | 110 +- demod/mod/rs41mod18.c | 1646 ++++++++++++++++++++++++ demod/mod/rs41mod34.c | 2142 +++++++++++++++++++++++++++++++ imet/imet1rs_dft.c | 10 +- mk2a/mk2a1680mod.c | 501 +++++--- 13 files changed, 4228 insertions(+), 1591 deletions(-) delete mode 100644 demod/mod/mXXmod.c create mode 100644 demod/mod/rs41mod18.c create mode 100644 demod/mod/rs41mod34.c diff --git a/auto_rx/autorx/__init__.py b/auto_rx/autorx/__init__.py index 2a67978..623ddb7 100644 --- a/auto_rx/autorx/__init__.py +++ b/auto_rx/autorx/__init__.py @@ -17,7 +17,7 @@ except ImportError: # MINOR - New sonde type support, other fairly big changes that may result in telemetry or config file incompatability issus. # PATCH - Small changes, or minor feature additions. -__version__ = "1.6.0-beta3" +__version__ = "1.6.0-beta4" # Global Variables diff --git a/auto_rx/autorx/decode.py b/auto_rx/autorx/decode.py index b4c4252..815dbd6 100644 --- a/auto_rx/autorx/decode.py +++ b/auto_rx/autorx/decode.py @@ -687,7 +687,7 @@ class SondeDecoder(object): decode_cmd += " tee decode_%s.wav |" % str(self.rtl_device_idx) # Meisei IMS-100 decoder - decode_cmd += f"./meisei100mod --json 2>/dev/null" + decode_cmd += f"./meisei100mod --json --ptu --ecc 2>/dev/null" elif self.sonde_type == "UDP": # UDP Input Mode. diff --git a/auto_rx/test/generate_lowsnr.py b/auto_rx/test/generate_lowsnr.py index 23ff41a..c62b462 100644 --- a/auto_rx/test/generate_lowsnr.py +++ b/auto_rx/test/generate_lowsnr.py @@ -45,7 +45,8 @@ SAMPLES = [ ['imet54_96k_float.bin', 4800, -10.0, 96000], # 4800 baud GMSK ['rsngp_96k_float.bin', 2400, -100.0, 96000], # RS92-NGP - wider bandwidth. ['lms6-400_96k_float.bin', 4800, -100, 96000], # LMS6, 400 MHz variant. Continuous signal. - ['mrz_96k_float.bin', 2400, -100, 96000] # MRZ Continuous signal. + ['mrz_96k_float.bin', 2400, -100, 96000], # MRZ Continuous signal. + ['m20_96k_float.bin', 9600, -15, 96000] # M20, kind of continuous signal? residual carrier when not transmitting ] diff --git a/auto_rx/test/plot_per.py b/auto_rx/test/plot_per.py index 718526d..66097a4 100644 --- a/auto_rx/test/plot_per.py +++ b/auto_rx/test/plot_per.py @@ -51,6 +51,8 @@ sonde_types = { 'LMS6-400': {'csv':'lms6-400_fsk_demod_soft_centre.txt', 'packets': 120, 'color': 'C5'}, 'MRZ': {'csv':'mrz_fsk_demod_soft_centre.txt', 'packets': 105, 'color': 'C6'}, 'iMet-54': {'csv':'imet54_fsk_demod_soft_centre.txt', 'packets': 240, 'color': 'C7'}, + 'M20': {'csv':'m20_fsk_demod_soft_centre.txt', 'packets': 120, 'color': 'C8'}, + 'iMet-4': {'csv':'imet4_iq.txt', 'packets': 218, 'color': 'C9'}, } diff --git a/auto_rx/test/test_demod.py b/auto_rx/test/test_demod.py index 63773c1..b5d21c5 100644 --- a/auto_rx/test/test_demod.py +++ b/auto_rx/test/test_demod.py @@ -275,6 +275,16 @@ processing_type = { "post_process" : "| grep aprsid | wc -l", 'files' : "./generated/m10*" }, + 'm20_fsk_demod_soft_centre': { + # Shift up to ~24 khz, and then pass into fsk_demod. + 'demod' : "| csdr convert_f_s16 | ./tsrc - - 0.500 | ../fsk_demod --cs16 -p 5 -b -10000 -u 10000 -s --stats=5 2 48000 9600 - - 2>stats.txt |", + + # Decode using rs41ecc + 'decode': "../m20mod --json --ptu -vvv --softin -i 2>/dev/null", + # Count the number of telemetry lines. + "post_process" : " | grep rawid | wc -l", + 'files' : "./generated/m20*" + }, 'dfm_fsk_demod_soft': { # cat ./generated/dfm09_96k_float_15.0dB.bin | csdr shift_addition_cc 0.25000 2>/dev/null | csdr convert_f_s16 | #./tsrc - - 1.041666 | ../fsk_demod --cs16 -b 1 -u 45000 2 100000 2500 - - 2>/dev/null | @@ -824,7 +834,7 @@ if __name__ == "__main__": #batch_modes = ['dfm_fsk_demod_soft', 'rs41_fsk_demod_soft', 'm10_fsk_demod_soft', 'rs92_fsk_demod_soft', 'rs92ngp_fsk_demod_soft', 'lms6-400_fsk_demod_soft', 'imet4_rtlfm', 'mrz_fsk_demod_soft', 'imet54_fsk_demod_soft'] - batch_modes = ['dfm_fsk_demod_soft_centre', 'rs41_fsk_demod_soft_centre', 'm10_fsk_demod_soft_centre', 'rs92_fsk_demod_soft_centre', 'rs92ngp_fsk_demod_soft_centre', 'lms6-400_fsk_demod_soft_centre', 'imet4_iq', 'mrz_fsk_demod_soft_centre', 'imet54_fsk_demod_soft_centre'] + batch_modes = ['dfm_fsk_demod_soft_centre', 'rs41_fsk_demod_soft_centre', 'm10_fsk_demod_soft_centre', 'rs92_fsk_demod_soft_centre', 'rs92ngp_fsk_demod_soft_centre', 'lms6-400_fsk_demod_soft_centre', 'imet4_iq', 'mrz_fsk_demod_soft_centre', 'imet54_fsk_demod_soft_centre', 'm20_fsk_demod_soft_centre'] if args.batch: for _mode in batch_modes: diff --git a/demod/mod/demod_mod.h b/demod/mod/demod_mod.h index 8225156..5fb4899 100644 --- a/demod/mod/demod_mod.h +++ b/demod/mod/demod_mod.h @@ -17,6 +17,7 @@ typedef unsigned char ui8_t; typedef unsigned short ui16_t; typedef unsigned int ui32_t; +typedef unsigned long long ui64_t; typedef char i8_t; typedef short i16_t; typedef int i32_t; diff --git a/demod/mod/m20mod.c b/demod/mod/m20mod.c index ad9b986..139c113 100644 --- a/demod/mod/m20mod.c +++ b/demod/mod/m20mod.c @@ -842,6 +842,11 @@ static int print_pos(gpx_t *gpx, int bcOK, int csOK) { if (gpx->jsn_freq > 0) { fprintf(stdout, ", \"freq\": %d", gpx->jsn_freq); } + + // Reference time/position + fprintf(stdout, ", \"ref_datetime\": \"%s\"", "GPS" ); // {"GPS", "UTC"} GPS-UTC=leap_sec + fprintf(stdout, ", \"ref_position\": \"%s\"", "GPS" ); // {"GPS", "MSL"} GPS=ellipsoid , MSL=geoid + #ifdef VER_JSN_STR ver_jsn = VER_JSN_STR; #endif diff --git a/demod/mod/mXXmod.c b/demod/mod/mXXmod.c deleted file mode 100644 index 139c113..0000000 --- a/demod/mod/mXXmod.c +++ /dev/null @@ -1,1383 +0,0 @@ - -/* - * mXX m18/m20 (test) - * - * (cf. mXX_20180919.c) - * sync header: correlation/matched filter - * files: mXXmod.c demod_mod.h demod_mod.c - * compile: - * gcc -c demod_mod.c - * gcc mXXmod.c demod_mod.o -lm -o mXXmod - * - * 2018-09-19 Ury: (len=0x43) ./mXX -c -vv --br 9600 mXX_20180919.wav - * 2019-11-06 Ury: (len=0x45) ./mXX -c -vv --br 9600 mXX_20191106.wav - * 2020-02-14 Ury: (len=0x45) ./mXX -c -vv --br 9600 mXX_20200214.wav - * 2020-05-11 Wien: (len=0x45) ./mXX -c -vv --br 9603 mXX_20200511.wav - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - -// optional JSON "version" -// (a) set global -// gcc -DVERSION_JSN [-I] ... -#ifdef VERSION_JSN - #include "version_jsn.h" -#endif -// or -// (b) set local compiler option, e.g. -// gcc -DVER_JSN_STR=\"0.0.2\" ... - - -#include "demod_mod.h" - - -typedef struct { - i8_t vbs; // verbose output - i8_t raw; // raw frames - i8_t crc; // CRC check output - i8_t ecc; // M10/M20: no ECC - i8_t sat; // GPS sat data - i8_t ptu; // PTU: temperature - i8_t inv; - i8_t aut; - i8_t col; // colors - i8_t jsn; // JSON output (auto_rx) - i8_t slt; // silent (only raw/json) -} option_t; - - -// ? 9600 baud M20 <-> 9616 baud M10 ? -#define BAUD_RATE 9600 // 9600..9604 // 9614..9616 - -/* -------------------------------------------------------------------------- */ - -/* -Header = Sync-Header + Sonde-Header: -1100110011001100 1010011001001100 1101010011010011 0100110101010101 0011010011001100 -uudduudduudduudd ududduuddudduudd uudududduududduu dudduudududududu dduududduudduudd (oder:) -dduudduudduudduu duduudduuduudduu ddududuudduduudd uduuddududududud uudduduudduudduu (komplement) - 0 0 0 0 0 0 0 0 1 1 - - - 0 0 0 0 1 1 0 0 1 0 0 1 0 0 1 1 1 1 1 0 0 1 0 0 0 0 0 -*/ - -#define BITS 8 -#define HEADLEN 32 // HEADLEN+HEADOFS=32 <= strlen(header) -#define HEADOFS 0 - // Sync-Header (raw) // Sonde-Header (bits) -//char head[] = "11001100110011001010011001001100"; //"0110010010011111"; // M10: 64 9F , M2K2: 64 8F - //"0111011010011111"; // M10: 76 9F , w/ aux-data - //"0110010001001001"; // M10-dop: 64 49 09 - //"0110010010101111"; // M10+: 64 AF w/ gtop-GPS -static char rawheader[] = "10011001100110010100110010011001"; - -#define FRAME_LEN (100+1) // 0x64+1 -#define BITFRAME_LEN (FRAME_LEN*BITS) - -#define AUX_LEN 20 -#define BITAUX_LEN (AUX_LEN*BITS) - - -#define t_M2K2 0x8F -#define t_M10 0x9F -#define t_M10plus 0xAF -#define t_M20 0x20 - -typedef struct { - ui32_t gps_cnt; - ui8_t cnt; - ui8_t _diffcnt; - int week; int tow_ms; int gpssec; - int jahr; int monat; int tag; - int wday; - int std; int min; float sek; - double lat; double lon; double alt; - double vH; double vD; double vV; - double vx; double vy; double vD2; - float T; float RH; float TH; float P; - ui8_t numSV; - ui8_t utc_ofs; - char SN[12+4]; - ui8_t SNraw[3]; - ui8_t frame_bytes[FRAME_LEN+AUX_LEN+4]; - char frame_bits[BITFRAME_LEN+BITAUX_LEN+8]; - int auxlen; // ? 0 .. 0x57-0x45 - int jsn_freq; // freq/kHz (SDR) - option_t option; - ui8_t type; -} gpx_t; - - -/* -------------------------------------------------------------------------- */ -#define SECONDS_IN_WEEK (604800.0) // 7*86400 -/* - * Convert GPS Week and Seconds to Modified Julian Day. - * - Adapted from sci.astro FAQ. - * - Ignores UTC leap seconds. - */ -static void Gps2Date(long GpsWeek, long GpsSeconds, int *Year, int *Month, int *Day) { - - long GpsDays, Mjd; - long J, C, Y, M; - - GpsDays = GpsWeek * 7 + (GpsSeconds / 86400); - Mjd = 44244 + GpsDays; - - J = Mjd + 2468570; - C = 4 * J / 146097; - J = J - (146097 * C + 3) / 4; - Y = 4000 * (J + 1) / 1461001; - J = J - 1461 * Y / 4 + 31; - M = 80 * J / 2447; - *Day = J - 2447 * M / 80; - J = M / 11; - *Month = M + 2 - (12 * J); - *Year = 100 * (C - 49) + Y + J; -} -/* -------------------------------------------------------------------------- */ - -static int bits2bytes(char *bitstr, ui8_t *bytes) { - int i, bit, d, byteval; - int bitpos, bytepos; - - bitpos = 0; - bytepos = 0; - - while (bytepos < FRAME_LEN+AUX_LEN) { - - byteval = 0; - d = 1; - for (i = 0; i < BITS; i++) { - //bit=*(bitstr+bitpos+i); /* little endian */ - bit=*(bitstr+bitpos+7-i); /* big endian */ - // bit == 'x' ? - if (bit == '1') byteval += d; - else /*if ((bit == '0') || (bit == 'x'))*/ byteval += 0; - d <<= 1; - } - bitpos += BITS; - bytes[bytepos++] = byteval & 0xFF; - - } - - //while (bytepos < FRAME_LEN+AUX_LEN) bytes[bytepos++] = 0; - - return 0; -} - -/* -------------------------------------------------------------------------- */ - -/* -M20 - -GPS data: Big Endian -PTU/ADC data: little endian - -frame[0x0] = framelen // (0x43,) 0x45 -frame[0x1] = 0x20 (type M20) - -frame[0x02..0x18]: most important data at beginning (incl. counter + M10check) -frame[0x02..0x03]: ADC RH (incl.555) -frame[0x04..0x05]: ADC Temperatur , frame[0x46]: scale/range ? -frame[0x06..0x07]: ADC RH-Temperature range: 0:0..4095 , 1:4096..8191 , 2:8192..12287 -frame[0x08..0x0A]: GPS altitude -frame[0x0B..0x0E]: GPS hor.Vel. (velE,velN) -frame[0x0F..0x11]: GPS TOW -frame[0x15]: counter -frame[0x16..0x17]: block check - -frame[0x18..0x19]: GPS ver.Vel. (velU) -frame[0x1A..0x1B]: GPS week -frame[0x1C..0x1F]: GPS latitude -frame[0x20..0x23]: GPS longitude - -frame[0x44..0x45]: frame check -*/ - -#define stdFLEN 0x45 // pos[0]=0x45 // M20: 0x45 (0x43) M10: 0x64 -#define pos_GPSTOW 0x0F // 3 byte -#define pos_GPSlat 0x1C // 4 byte -#define pos_GPSlon 0x20 // 4 byte -#define pos_GPSalt 0x08 // 3 byte -//#define pos_GPSsats 0xXX // 1 byte -//#define pos_GPSutc 0xXX // 1 byte -#define pos_GPSweek 0x1A // 2 byte -//Velocity East-North-Up (ENU) -#define pos_GPSvE 0x0B // 2 byte -#define pos_GPSvN 0x0D // 2 byte -#define pos_GPSvU 0x18 // 2 byte -#define pos_SN 0x12 // 3 byte -#define pos_CNT 0x15 // 1 byte -#define pos_BlkChk 0x16 // 2 byte -#define pos_Check (stdFLEN-1) // 2 byte - -#define len_BlkChk 0x16 // frame[0x02..0x17] , incl. chk16 - - -#define ANSI_COLOR_RED "\x1b[31m" -#define ANSI_COLOR_GREEN "\x1b[32m" -#define ANSI_COLOR_YELLOW "\x1b[33m" -#define ANSI_COLOR_BLUE "\x1b[34m" -#define ANSI_COLOR_MAGENTA "\x1b[35m" -#define ANSI_COLOR_CYAN "\x1b[36m" -#define ANSI_COLOR_RESET "\x1b[0m" - -#define XTERM_COLOR_BROWN "\x1b[38;5;94m" // 38;5;{0..255}m - -#define col_Mtype "\x1b[38;5;250m" // 1 byte -#define col_GPSweek "\x1b[38;5;20m" // 2 byte -#define col_GPSTOW "\x1b[38;5;27m" // 3 byte -#define col_GPSdate "\x1b[38;5;94m" //111 -#define col_GPSlat "\x1b[38;5;34m" // 4 byte -#define col_GPSlon "\x1b[38;5;70m" // 4 byte -#define col_GPSalt "\x1b[38;5;82m" // 3 byte -#define col_GPSvel "\x1b[38;5;36m" // 6 byte -#define col_SN "\x1b[38;5;58m" // 3 byte -#define col_CNT "\x1b[38;5;172m" // 1 byte -#define col_Check "\x1b[38;5;11m" // 2 byte -#define col_TXT "\x1b[38;5;244m" -#define col_FRTXT "\x1b[38;5;244m" -#define col_CSok "\x1b[38;5;2m" -#define col_CSoo "\x1b[38;5;220m" -#define col_CSno "\x1b[38;5;1m" -#define col_CNST "\x1b[38;5;58m" // 3 byte - -/* -$ for code in {0..255} -> do echo -e "\e[38;5;${code}m"'\\e[38;5;'"$code"m"\e[0m" -> done -*/ - -static int get_GPSweek(gpx_t *gpx) { - int i; - unsigned byte; - ui8_t gpsweek_bytes[2]; - int gpsweek; - - //gpx->numSV = gpx->frame_bytes[pos_GPSsats]; - //gpx->utc_ofs = gpx->frame_bytes[pos_GPSutc]; - - for (i = 0; i < 2; i++) { - byte = gpx->frame_bytes[pos_GPSweek + i]; - gpsweek_bytes[i] = byte; - } - - gpsweek = (gpsweek_bytes[0] << 8) + gpsweek_bytes[1]; - - if (gpsweek > 4000) return -1; - - // Trimble Copernicus II WNRO (AirPrime XM1110 OK) - if (gpsweek < 1304 /*2005-01-02*/ ) gpsweek += 1024; - - gpx->week = gpsweek; - - return 0; -} - -//char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; -static char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; - -static int get_GPStime(gpx_t *gpx) { - int i, ret = 0; - unsigned byte; - ui8_t gpstime_bytes[4]; - int gpstime, day; - int ms; - double sec_gps0 = 0.0; - - for (i = 0; i < 3; i++) { - byte = gpx->frame_bytes[pos_GPSTOW + i]; - gpstime_bytes[i] = byte; - } - - gpstime = 0; - for (i = 0; i < 3; i++) { - gpstime |= gpstime_bytes[i] << (8*(2-i)); - } - - gpx->tow_ms = gpstime*1000; - ms = 0;//gpstime % 1000; - //gpstime /= 1000; - gpx->gpssec = gpstime; - - day = gpstime / (24 * 3600); - if ((day < 0) || (day > 6)) return -1; - - gpstime %= (24*3600); - - gpx->wday = day; - gpx->std = gpstime/3600; - gpx->min = (gpstime%3600)/60; - gpx->sek = gpstime%60 + ms/1000.0; - - - ret = get_GPSweek(gpx); - if (ret) return ret; - - sec_gps0 = (double)gpx->week*SECONDS_IN_WEEK + gpx->tow_ms/1e3; - gpx->gps_cnt = (ui32_t)(sec_gps0+0.5); - gpx->cnt = gpx->frame_bytes[pos_CNT]; - gpx->_diffcnt = (ui8_t)(gpx->gps_cnt - gpx->cnt); - - return 0; -} - -//static double B60B60 = (1<<30)/90.0; // 2^32/360 = 2^30/90 = 0xB60B60.711x // M10 -static double B60B60 = 1e6; // M20 - -static int get_GPSlat(gpx_t *gpx) { - int i; - unsigned byte; - ui8_t gpslat_bytes[4]; - int gpslat; - double lat; - - for (i = 0; i < 4; i++) { - byte = gpx->frame_bytes[pos_GPSlat + i]; - gpslat_bytes[i] = byte; - } - - gpslat = 0; - for (i = 0; i < 4; i++) { - gpslat |= gpslat_bytes[i] << (8*(3-i)); - } - lat = gpslat / B60B60; - gpx->lat = lat; - - return 0; -} - -static int get_GPSlon(gpx_t *gpx) { - int i; - unsigned byte; - ui8_t gpslon_bytes[4]; - int gpslon; - double lon; - - for (i = 0; i < 4; i++) { - byte = gpx->frame_bytes[pos_GPSlon + i]; - gpslon_bytes[i] = byte; - } - - gpslon = 0; - for (i = 0; i < 4; i++) { - gpslon |= gpslon_bytes[i] << (8*(3-i)); - } - lon = gpslon / B60B60; - gpx->lon = lon; - - return 0; -} - -static int get_GPSalt(gpx_t *gpx) { // 24 bit - int i; - unsigned byte; - ui8_t gpsalt_bytes[4]; - int gpsalt; - double alt; - - for (i = 0; i < 3; i++) { - byte = gpx->frame_bytes[pos_GPSalt + i]; - gpsalt_bytes[i] = byte; - } - - gpsalt = 0; - for (i = 0; i < 3; i++) { - gpsalt |= gpsalt_bytes[i] << (8*(2-i)); - } - alt = gpsalt / 100.0; - gpx->alt = alt; - - return 0; -} - -static int get_GPSvel(gpx_t *gpx) { - int i; - unsigned byte; - ui8_t gpsVel_bytes[2]; - short vel16; - double vx, vy, dir, alpha; - - for (i = 0; i < 2; i++) { - byte = gpx->frame_bytes[pos_GPSvE + i]; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; - vx = vel16 / 1e2; // ost - - for (i = 0; i < 2; i++) { - byte = gpx->frame_bytes[pos_GPSvN + i]; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; - vy= vel16 / 1e2; // nord - - gpx->vx = vx; - gpx->vy = vy; - gpx->vH = sqrt(vx*vx+vy*vy); -///* - alpha = atan2(vy, vx)*180/M_PI; // ComplexPlane (von x-Achse nach links) - GeoMeteo (von y-Achse nach rechts) - dir = 90-alpha; // z=x+iy= -> i*conj(z)=y+ix=re(i(pi/2-t)), Achsen und Drehsinn vertauscht - if (dir < 0) dir += 360; // atan2(y,x)=atan(y/x)=pi/2-atan(x/y) , atan(1/t) = pi/2 - atan(t) - gpx->vD2 = dir; -//*/ - dir = atan2(vx, vy) * 180 / M_PI; - if (dir < 0) dir += 360; - gpx->vD = dir; - - for (i = 0; i < 2; i++) { - byte = gpx->frame_bytes[pos_GPSvU + i]; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; - gpx->vV = vel16 / 1e2; - - return 0; -} - -static int get_SN(gpx_t *gpx) { - int i; - ui8_t b0 = gpx->frame_bytes[pos_SN]; //0x12 - ui32_t s2 = (gpx->frame_bytes[pos_SN+2]<<8) | gpx->frame_bytes[pos_SN+1]; - ui8_t ym = b0 & 0x7F; // #{0x0,..,0x77}=120=10*12 - ui8_t y = ym / 12; - ui8_t m = (ym % 12)+1; // there is b0=0x69<0x80 from 2018-09-19 ... - ui32_t sn_val = 0; - - for (i = 0; i < 11; i++) gpx->SN[i] = ' '; gpx->SN[11] = '\0'; - for (i = 12; i < 15; i++) gpx->SN[i] = '\0'; gpx->SN[15] = '\0'; - - for (i = 0; i < 3; i++) { - gpx->SNraw[i] = gpx->frame_bytes[pos_SN + i]; - } - sn_val = (gpx->SNraw[0]<<16) | (gpx->SNraw[1]<<8) | gpx->SNraw[2]; - - sprintf(gpx->SN, "%u%02u", y, m); // more samples needed - sprintf(gpx->SN+3, "-%u-", (s2&0x3)+2); // (b0>>7)+1? (s2&0x3)+2? - sprintf(gpx->SN+6, "%u", (s2>>(2+13))&0x1); // ?(s2>>(2+13))&0x1 ?? (s2&0x3)? - sprintf(gpx->SN+7, "%04u", (s2>>2)&0x1FFF); - - - if (sn_val == 0) - { // get_GPStime(gpx); - // replace SN: 001-2-00000 -> 000-0-00000-[_diffcnt] - sprintf(gpx->SN, "%s", "000-0-00000"); - sprintf(gpx->SN+11, "-%03u", gpx->_diffcnt & 0xFF); - } - - return 0; -} - -/* -------------------------------------------------------------------------- */ -/* -g : F^n -> F^16 // checksum, linear -g(m||b) = f(g(m),b) - -// update checksum -f : F^16 x F^8 -> F^16 linear - -010100001000000101000000 -001010000100000010100000 -000101000010000001010000 -000010100001000000101000 -000001010000100000010100 -100000100000010000001010 -000000011010100000000100 -100000000101010000000010 -000000001000000000000000 -000000000100000000000000 -000000000010000000000000 -000000000001000000000000 -000000000000100000000000 -000000000000010000000000 -000000000000001000000000 -000000000000000100000000 -*/ - -static int update_checkM10(int c, ui8_t b) { - int c0, c1, t, t6, t7, s; - - c1 = c & 0xFF; - - // B - b = (b >> 1) | ((b & 1) << 7); - b ^= (b >> 2) & 0xFF; - - // A1 - t6 = ( c & 1) ^ ((c>>2) & 1) ^ ((c>>4) & 1); - t7 = ((c>>1) & 1) ^ ((c>>3) & 1) ^ ((c>>5) & 1); - t = (c & 0x3F) | (t6 << 6) | (t7 << 7); - - // A2 - s = (c >> 7) & 0xFF; - s ^= (s >> 2) & 0xFF; - - - c0 = b ^ t ^ s; - - return ((c1<<8) | c0) & 0xFFFF; -} - -static int checkM10(ui8_t *msg, int len) { - int i, cs; // msg[0] = len+1 - - cs = 0; - for (i = 0; i < len; i++) { - cs = update_checkM10(cs, msg[i]); - } - - return cs & 0xFFFF; -} -// checkM10(frame, frame[0]-1) = blk_checkM10(frame[0], frame+1) -static int blk_checkM10(int len, ui8_t *msg) { - int i, cs; - ui8_t pre = len & 0xFF; // len(block+chk16) - cs = 0; - - cs = update_checkM10(cs, pre); - - for (i = 0; i < len-2; i++) { - cs = update_checkM10(cs, msg[i]); - } - - return cs & 0xFFFF; -} - -/* -------------------------------------------------------------------------- */ - -static float get_Temp(gpx_t *gpx) { -// NTC-Thermistor Shibaura PB5-41E ? -// T00 = 273.15 + 0.0 , R00 = 15e3 -// T25 = 273.15 + 25.0 , R25 = 5.369e3 -// B00 = 3450.0 Kelvin // 0C..100C, poor fit low temps -// [ T/C , R/1e3 ] ( [P__-43]/2.0 ): -// [ -50.0 , 204.0 ] -// [ -45.0 , 150.7 ] -// [ -40.0 , 112.6 ] -// [ -35.0 , 84.90 ] -// [ -30.0 , 64.65 ] -// [ -25.0 , 49.66 ] -// [ -20.0 , 38.48 ] -// [ -15.0 , 30.06 ] -// [ -10.0 , 23.67 ] -// [ -5.0 , 18.78 ] -// [ 0.0 , 15.00 ] -// [ 5.0 , 12.06 ] -// [ 10.0 , 9.765 ] -// [ 15.0 , 7.955 ] -// [ 20.0 , 6.515 ] -// [ 25.0 , 5.370 ] -// [ 30.0 , 4.448 ] -// [ 35.0 , 3.704 ] -// [ 40.0 , 3.100 ] -// -> Steinhart-Hart coefficients (polyfit): - float p0 = 1.07303516e-03, - p1 = 2.41296733e-04, - p2 = 2.26744154e-06, - p3 = 6.52855181e-08; -// T/K = 1/( p0 + p1*ln(R) + p2*ln(R)^2 + p3*ln(R)^3 ) - - // range/scale 0, 1, 2: // M10-pcb - float Rs[3] = { 12.1e3 , 36.5e3 , 475.0e3 }; // bias/series - float Rp[3] = { 1e20 , 330.0e3 , 2000.0e3 }; // parallel, Rp[0]=inf - - ui8_t scT = 0; // {0,1,2}, range/scale voltage divider - ui16_t ADC_RT; // ADC12 - //ui16_t Tcal[2]; - - float x, R; - float T = 0; // T/Kelvin - - ADC_RT = (gpx->frame_bytes[0x5] << 8) | gpx->frame_bytes[0x4]; - - //ui8_t sc = gpx->frame_bytes[0x32] & 3; // (frame[0x32]<<8)|frame[0x31] - // frame[0x31..0x32], frame[0x32]: 0x9=0b1001:0, 0xA=0b1010:1, 0x8=0b1000:2 - // ? Temp-Calibration depending on range ? - // - // range: 0:0..4095 , 1:4096..8191 , 2:8192..12287 - /* - if (sc == 0x1) { scT = 0; } - else if (sc == 0x2) { scT = 1; ADC_RT -= 4096; } - else if (sc == 0x0) { scT = 2; ADC_RT -= 8192; } - else: // sc == 0x3 // test only range below: - */ - // range, i.e. (ADC_RT>>12)&3 - if (ADC_RT > 8191) { scT = 2; ADC_RT -= 8192; } - else if (ADC_RT > 4095) { scT = 1; ADC_RT -= 4096; } - else { scT = 0; } // also if (ADC_RT>>12)&3 == 3 - - // ADC12 , 4096 = 1<<12, max: 4095 - x = (4095.0-ADC_RT)/ADC_RT; // (Vcc-Vout)/Vout = Vcc/Vout - 1 - R = Rs[scT] /( x - Rs[scT]/Rp[scT] ); - - if (R > 0) T = 1/( p0 + p1*log(R) + p2*log(R)*log(R) + p3*log(R)*log(R)*log(R) ); - - return T - 273.15; // Celsius -} - -static float get_Tntc2(gpx_t *gpx) { - // SMD ntc , RH-Temperature - float Rs = 22.1e3; // P5.6=Vcc - float R25 = 2.2e3;// 0.119e3; //2.2e3; - float b = 3650.0; // B/Kelvin - float T25 = 25.0 + 273.15; // T0=25C, R0=R25=5k - // -> Steinhart-Hart coefficients (polyfit): - float p0 = 4.42606809e-03, - p1 = -6.58184309e-04, - p2 = 8.95735557e-05, - p3 = -2.84347503e-06; - float T = 0.0; // T/Kelvin - ui16_t ADC_ntc0; // M10: ADC12 P6.4(A4) - float x, R; - - ADC_ntc0 = (gpx->frame_bytes[0x07] << 8) | gpx->frame_bytes[0x06]; // M10: 0x40,0x3F - x = (4095.0 - ADC_ntc0)/ADC_ntc0; // (Vcc-Vout)/Vout - R = Rs / x; - if (R > 0) T = 1/(1/T25 + 1/b * log(R/R25)); - //if (R > 0) T = 1/( p0 + p1*log(R) + p2*log(R)*log(R) + p3*log(R)*log(R)*log(R) ); - - return T - 273.15; -} - -static float get_RHraw(gpx_t *gpx) { - float _rh = -1.0; - float _RH = -1.0; - ui16_t ADC_rh; - - ADC_rh = (gpx->frame_bytes[0x03] << 8) | gpx->frame_bytes[0x02]; - _rh = ADC_rh / (float)(1<<15); - - _RH = -1.0; - if (_rh < 1.05) _RH = _rh*100.0; - - // Transfer function ? - // Calibration ? - // (Hyland and Wexler) Tntc2 (T_RH) <-> Tmain ? - - return _RH; -} - -static float get_RH(gpx_t *gpx) { -// from DF9DQ, -// https://github.com/einergehtnochrein/ra-firmware -// - float TU = get_Tntc2(gpx); - float RH = -1.0f; - float x; - - ui16_t humval = (gpx->frame_bytes[0x03] << 8) | gpx->frame_bytes[0x02]; - ui16_t rh_cal = (gpx->frame_bytes[0x30] << 8) | gpx->frame_bytes[0x2F]; - - float humidityCalibration = 6.4e8f / (rh_cal + 80000.0f); - - x = (humval + 80000.0f) * humidityCalibration * (1.0f - 5.8e-4f * (TU-25.0f)); - x = 4.16e9f / x; - x = 10.087f*x*x*x - 211.62f*x*x + 1388.2f*x - 2797.0f; - - RH = -1.0f; - if (humval < 48000) - { - RH = x; - if (RH < 0.0f ) RH = 0.0f; - if (RH > 100.0f) RH = 100.0f; - } - - // (Hyland and Wexler) Tntc2 (T_RH) <-> Tmain ? - - return RH; -} - -static float get_P(gpx_t *gpx) { -// cf. DF9DQ -// - float hPa = 0.0f; - ui16_t val = (gpx->frame_bytes[0x25] << 8) | gpx->frame_bytes[0x24]; - - if (val > 0) { - hPa = val/16.0f; - } - - return hPa; -} - -/* -------------------------------------------------------------------------- */ - -static int print_pos(gpx_t *gpx, int bcOK, int csOK) { - int err, err2; - - if (1 || gpx->type == t_M20) - { - err = 0; - err |= get_GPStime(gpx); // incl. get_GPSweek(gpx) - err |= get_GPSlat(gpx); - err |= get_GPSlon(gpx); - err |= get_GPSalt(gpx); - err2 = get_GPSvel(gpx); - } - else err = 0xFF; - - if (!err) { - - Gps2Date(gpx->week, gpx->gpssec, &gpx->jahr, &gpx->monat, &gpx->tag); - get_SN(gpx); - - if (gpx->option.ptu && csOK) { - gpx->T = get_Temp(gpx); // temperature - gpx->TH = get_Tntc2(gpx); // rel. humidity sensor temperature - gpx->RH = get_RH(gpx); // relative humidity - gpx->P = get_P(gpx); // (optional) pressure - } - - if ( !gpx->option.slt ) - { - if (gpx->option.col) { - fprintf(stdout, col_TXT); - if (gpx->option.vbs >= 3) { - fprintf(stdout, "[%3d]", gpx->frame_bytes[pos_CNT]); - fprintf(stdout, " (W "col_GPSweek"%d"col_TXT") ", gpx->week); - } - fprintf(stdout, col_GPSTOW"%s"col_TXT" ", weekday[gpx->wday]); - fprintf(stdout, col_GPSdate"%04d-%02d-%02d"col_TXT" "col_GPSTOW"%02d:%02d:%06.3f"col_TXT" ", - gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek); - fprintf(stdout, " lat: "col_GPSlat"%.5f"col_TXT" ", gpx->lat); - fprintf(stdout, " lon: "col_GPSlon"%.5f"col_TXT" ", gpx->lon); - fprintf(stdout, " alt: "col_GPSalt"%.2f"col_TXT" ", gpx->alt); - if (!err2) { - fprintf(stdout, " vH: "col_GPSvel"%.1f"col_TXT" D: "col_GPSvel"%.1f"col_TXT" vV: "col_GPSvel"%.1f"col_TXT" ", gpx->vH, gpx->vD, gpx->vV); - } - if (gpx->option.vbs >= 2 && (bcOK || csOK)) { // SN - fprintf(stdout, " SN: "col_SN"%s"col_TXT, gpx->SN); - } - if (gpx->option.vbs >= 2) { - fprintf(stdout, " # "); - if (bcOK > 0) fprintf(stdout, " "col_CSok"(ok)"col_TXT); - else if (bcOK < 0) fprintf(stdout, " "col_CSoo"(oo)"col_TXT); - else fprintf(stdout, " "col_CSno"(no)"col_TXT); - // - if (csOK) fprintf(stdout, " "col_CSok"[OK]"col_TXT); - else fprintf(stdout, " "col_CSno"[NO]"col_TXT); - } - if (gpx->option.ptu && csOK) { - fprintf(stdout, " "); - if (gpx->T > -273.0f) fprintf(stdout, " T:%.1fC", gpx->T); - if (gpx->RH > -0.5f) fprintf(stdout, " RH=%.0f%%", gpx->RH); - if (gpx->TH > -273.0f) fprintf(stdout, " TH:%.1fC", gpx->TH); - if (gpx->P > 0.0f) { - if (gpx->P < 100.0f) fprintf(stdout, " P=%.2fhPa ", gpx->P); - else fprintf(stdout, " P=%.1fhPa ", gpx->P); - } - } - fprintf(stdout, ANSI_COLOR_RESET""); - } - else { - if (gpx->option.vbs >= 3) { - fprintf(stdout, "[%3d]", gpx->frame_bytes[pos_CNT]); - fprintf(stdout, " (W %d) ", gpx->week); - } - fprintf(stdout, "%s ", weekday[gpx->wday]); - fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f ", - gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek); - fprintf(stdout, " lat: %.5f ", gpx->lat); - fprintf(stdout, " lon: %.5f ", gpx->lon); - fprintf(stdout, " alt: %.2f ", gpx->alt); - if (!err2) { - fprintf(stdout, " vH: %.1f D: %.1f vV: %.1f ", gpx->vH, gpx->vD, gpx->vV); - } - if (gpx->option.vbs >= 2 && (bcOK || csOK)) { // SN - fprintf(stdout, " SN: %s", gpx->SN); - } - if (gpx->option.vbs >= 2) { - fprintf(stdout, " # "); - //if (bcOK) fprintf(stdout, " (ok)"); else fprintf(stdout, " (no)"); - if (bcOK > 0) fprintf(stdout, " (ok)"); - else if (bcOK < 0) fprintf(stdout, " (oo)"); - else fprintf(stdout, " (no)"); - // - if (csOK) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - } - if (gpx->option.ptu && csOK) { - fprintf(stdout, " "); - if (gpx->T > -273.0f) fprintf(stdout, " T:%.1fC", gpx->T); - if (gpx->RH > -0.5f) fprintf(stdout, " RH=%.0f%%", gpx->RH); - if (gpx->TH > -273.0f) fprintf(stdout, " TH:%.1fC", gpx->TH); - if (gpx->P > 0.0f) { - if (gpx->P < 100.0f) fprintf(stdout, " P=%.2fhPa ", gpx->P); - else fprintf(stdout, " P=%.1fhPa ", gpx->P); - } - } - } - fprintf(stdout, "\n"); - } - - - if (gpx->option.jsn) { - // Print out telemetry data as JSON - if (csOK) { - char *ver_jsn = NULL; - int j; - char sn_id[4+12+4] = "M20-"; - - strncpy(sn_id+4, gpx->SN, 12+4); - sn_id[15+4] = '\0'; - - fprintf(stdout, "{ \"type\": \"%s\"", "M20"); - fprintf(stdout, ", \"frame\": %lu, ", (unsigned long)gpx->gps_cnt); // sec_gps0+0.5 - fprintf(stdout, "\"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f", - sn_id, gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vV); - if (gpx->option.ptu) { // temperature - if (gpx->T > -273.0f) fprintf(stdout, ", \"temp\": %.1f", gpx->T ); - if (gpx->RH > -0.5f) fprintf(stdout, ", \"humidity\": %.1f", gpx->RH ); - if (gpx->P > 0.0f) fprintf(stdout, ", \"pressure\": %.2f", gpx->P ); - } - fprintf(stdout, ", \"rawid\": \"M20_%02X%02X%02X\"", gpx->frame_bytes[pos_SN], gpx->frame_bytes[pos_SN+1], gpx->frame_bytes[pos_SN+2]); // gpx->type - fprintf(stdout, ", \"subtype\": \"0x%02X\"", gpx->type); - if (gpx->jsn_freq > 0) { - fprintf(stdout, ", \"freq\": %d", gpx->jsn_freq); - } - - // Reference time/position - fprintf(stdout, ", \"ref_datetime\": \"%s\"", "GPS" ); // {"GPS", "UTC"} GPS-UTC=leap_sec - fprintf(stdout, ", \"ref_position\": \"%s\"", "GPS" ); // {"GPS", "MSL"} GPS=ellipsoid , MSL=geoid - - #ifdef VER_JSN_STR - ver_jsn = VER_JSN_STR; - #endif - if (ver_jsn && *ver_jsn != '\0') fprintf(stdout, ", \"version\": \"%s\"", ver_jsn); - fprintf(stdout, " }\n"); - fprintf(stdout, "\n"); - } - } - - } - - return err; -} - -static int print_frame(gpx_t *gpx, int pos, int b2B) { - int i; - ui8_t byte; - int cs1, cs2; - int bc1, bc2, bc; - int flen = stdFLEN; // stdFLEN=0x64, auxFLEN=0x76; M20:0x45 ? - - if (b2B) { - bits2bytes(gpx->frame_bits, gpx->frame_bytes); - } - flen = gpx->frame_bytes[0]; - if (flen == stdFLEN) gpx->auxlen = 0; - else { - gpx->auxlen = flen - stdFLEN; - //if (gpx->auxlen < 0 || gpx->auxlen > AUX_LEN) gpx->auxlen = 0; // 0x43,0x45 - } - - cs1 = (gpx->frame_bytes[pos_Check+gpx->auxlen] << 8) | gpx->frame_bytes[pos_Check+gpx->auxlen+1]; - cs2 = checkM10(gpx->frame_bytes, pos_Check+gpx->auxlen); - - bc1 = (gpx->frame_bytes[pos_BlkChk] << 8) | gpx->frame_bytes[pos_BlkChk+1]; - bc2 = blk_checkM10(len_BlkChk, gpx->frame_bytes+2); // len(essentialBlock+chk16) = 0x16 - if (bc1 == bc2) bc = 1; - else if (bc1 == 0) bc = -1; - else bc = 0; - - switch (gpx->frame_bytes[1]) { - case 0x8F: gpx->type = t_M2K2; break; - case 0x9F: gpx->type = t_M10; break; - case 0xAF: gpx->type = t_M10plus; break; - case 0x20: gpx->type = t_M20; break; - default : gpx->type = t_M10; - } - - if (gpx->option.raw) { - - if (gpx->option.col /* && gpx->frame_bytes[1] != 0x49 */) { - fprintf(stdout, col_FRTXT); - for (i = 0; i < flen+1; i++) { - byte = gpx->frame_bytes[i]; - if (i == 1) fprintf(stdout, col_Mtype); - if ((i >= pos_GPSTOW) && (i < pos_GPSTOW+3)) fprintf(stdout, col_GPSTOW); - if ((i >= pos_GPSlat) && (i < pos_GPSlat+4)) fprintf(stdout, col_GPSlat); - if ((i >= pos_GPSlon) && (i < pos_GPSlon+4)) fprintf(stdout, col_GPSlon); - if ((i >= pos_GPSalt) && (i < pos_GPSalt+3)) fprintf(stdout, col_GPSalt); - if ((i >= pos_GPSweek) && (i < pos_GPSweek+2)) fprintf(stdout, col_GPSweek); - if ((i >= pos_GPSvE) && (i < pos_GPSvE+2)) fprintf(stdout, col_GPSvel); - if ((i >= pos_GPSvN) && (i < pos_GPSvN+2)) fprintf(stdout, col_GPSvel); - if ((i >= pos_GPSvU) && (i < pos_GPSvU+2)) fprintf(stdout, col_GPSvel); - if ((i >= pos_SN) && (i < pos_SN+3)) fprintf(stdout, col_SN); - if (i == pos_CNT) fprintf(stdout, col_CNT); - if ((i >= pos_BlkChk) && (i < pos_BlkChk+2)) fprintf(stdout, col_Check); - if ((i >= pos_Check+gpx->auxlen) && (i < pos_Check+gpx->auxlen+2)) fprintf(stdout, col_Check); - fprintf(stdout, "%02x", byte); - fprintf(stdout, col_FRTXT); - } - if (gpx->option.vbs) { - fprintf(stdout, " # "col_Check"%04x"col_FRTXT, cs2); - if (bc > 0) fprintf(stdout, " "col_CSok"(ok)"col_TXT); - else if (bc < 0) fprintf(stdout, " "col_CSoo"(oo)"col_TXT); - else fprintf(stdout, " "col_CSno"(no)"col_TXT); - if (cs1 == cs2) fprintf(stdout, " "col_CSok"[OK]"col_TXT); - else fprintf(stdout, " "col_CSno"[NO]"col_TXT); - } - fprintf(stdout, ANSI_COLOR_RESET"\n"); - } - else { - for (i = 0; i < flen+1; i++) { - byte = gpx->frame_bytes[i]; - fprintf(stdout, "%02x", byte); - } - if (gpx->option.vbs) { - fprintf(stdout, " # %04x", cs2); - if (bc > 0) fprintf(stdout, " (ok)"); - else if (bc < 0) fprintf(stdout, " (oo)"); - else fprintf(stdout, " (no)"); - if (cs1 == cs2) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - } - fprintf(stdout, "\n"); - } - if (gpx->option.slt /*&& gpx->option.jsn*/) { - print_pos(gpx, bc, cs1 == cs2); - } - } - /* - else if (gpx->frame_bytes[1] == 0x49) { - if (gpx->option.vbs == 3) { - for (i = 0; i < FRAME_LEN+gpx->auxlen; i++) { - byte = gpx->frame_bytes[i]; - fprintf(stdout, "%02x", byte); - } - fprintf(stdout, "\n"); - } - } - */ - else print_pos(gpx, bc, cs1 == cs2); - - return (gpx->frame_bytes[0]<<8)|gpx->frame_bytes[1]; -} - -/* -------------------------------------------------------------------------- */ - - -int main(int argc, char **argv) { - - //int option_res = 0; // genauere Bitmessung - int option_min = 0; - int option_iq = 0; - int option_iqdc = 0; - int option_lp = 0; - int option_dc = 0; - int option_noLUT = 0; - int option_softin = 0; - int option_pcmraw = 0; - int wavloaded = 0; - int sel_wavch = 0; // audio channel: left - int spike = 0; - int rawhex = 0; - int cfreq = -1; - - float baudrate = -1; - - FILE *fp = NULL; - char *fpname = NULL; - - int k; - - int bit, bit0; - int bitpos = 0; - int bitQ; - int pos; - hsbit_t hsbit, hsbit1; - - //int headerlen = 0; - - int header_found = 0; - - float thres = 0.76; - float _mv = 0.0; - - float lpIQ_bw = 24e3; - - int symlen = 2; - int bitofs = 0; // 0 .. +2 - int shift = 0; - - pcm_t pcm = {0}; - dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); - - hdb_t hdb = {0}; - - gpx_t gpx = {0}; - - -#ifdef CYGWIN - _setmode(fileno(stdin), _O_BINARY); // _setmode(_fileno(stdin), _O_BINARY); -#endif - setbuf(stdout, NULL); - - - fpname = argv[0]; - ++argv; - while ((*argv) && (!wavloaded)) { - if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { - fprintf(stderr, "%s [options] audio.wav\n", fpname); - fprintf(stderr, " options:\n"); - //fprintf(stderr, " -v, --verbose\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -c, --color\n"); - return 0; - } - else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { - gpx.option.vbs = 1; - } - else if ( (strcmp(*argv, "-vv" ) == 0) ) gpx.option.vbs = 2; - else if ( (strcmp(*argv, "-vvv") == 0) ) gpx.option.vbs = 3; - else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { - gpx.option.raw = 1; - } - else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { - gpx.option.inv = 1; // nicht noetig - } - else if ( (strcmp(*argv, "-c") == 0) || (strcmp(*argv, "--color") == 0) ) { - gpx.option.col = 1; - } - else if ( (strcmp(*argv, "--br") == 0) ) { - ++argv; - if (*argv) { - baudrate = atof(*argv); - if (baudrate < 9000 || baudrate > 10000) baudrate = BAUD_RATE; // default: M20:9600, M10:9615 - } - else return -1; - } - //else if (strcmp(*argv, "--res") == 0) { option_res = 1; } - else if ( (strcmp(*argv, "--ptu") == 0) ) { - gpx.option.ptu = 1; - } - else if ( (strcmp(*argv, "--spike") == 0) ) { - spike = 1; - } - else if (strcmp(*argv, "--ch2") == 0) { sel_wavch = 1; } // right channel (default: 0=left) - else if (strcmp(*argv, "--softin") == 0) { option_softin = 1; } // float32 soft input - else if (strcmp(*argv, "--silent") == 0) { gpx.option.slt = 1; } - else if (strcmp(*argv, "--ths") == 0) { - ++argv; - if (*argv) { - thres = atof(*argv); - } - else return -1; - } - else if ( (strcmp(*argv, "-d") == 0) ) { - ++argv; - if (*argv) { - shift = atoi(*argv); - if (shift > 4) shift = 4; - if (shift < -4) shift = -4; - } - else return -1; - } - else if (strcmp(*argv, "--iq0") == 0) { option_iq = 1; } // differential/FM-demod - else if (strcmp(*argv, "--iq2") == 0) { option_iq = 2; } - else if (strcmp(*argv, "--iq3") == 0) { option_iq = 3; } // iq2==iq3 - else if (strcmp(*argv, "--iqdc") == 0) { option_iqdc = 1; } // iq-dc removal (iq0,2,3) - else if (strcmp(*argv, "--IQ") == 0) { // fq baseband -> IF (rotate from and decimate) - double fq = 0.0; // --IQ , -0.5 < fq < 0.5 - ++argv; - if (*argv) fq = atof(*argv); - else return -1; - if (fq < -0.5) fq = -0.5; - if (fq > 0.5) fq = 0.5; - dsp.xlt_fq = -fq; // S(t) -> S(t)*exp(-f*2pi*I*t) - option_iq = 5; - } - else if (strcmp(*argv, "--lpIQ") == 0) { option_lp |= LP_IQ; } // IQ/IF lowpass - else if (strcmp(*argv, "--lpbw") == 0) { // IQ lowpass BW / kHz - double bw = 0.0; - ++argv; - if (*argv) bw = atof(*argv); - else return -1; - if (bw > 4.6 && bw < 48.0) lpIQ_bw = bw*1e3; - option_lp |= LP_IQ; - } - else if (strcmp(*argv, "--lpFM") == 0) { option_lp |= LP_FM; } // FM lowpass - else if (strcmp(*argv, "--dc") == 0) { option_dc = 1; } - else if (strcmp(*argv, "--noLUT") == 0) { option_noLUT = 1; } - else if (strcmp(*argv, "--min") == 0) { - option_min = 1; - } - else if (strcmp(*argv, "--json") == 0) { gpx.option.jsn = 1; } - else if (strcmp(*argv, "--jsn_cfq") == 0) { - int frq = -1; // center frequency / Hz - ++argv; - if (*argv) frq = atoi(*argv); else return -1; - if (frq < 300000000) frq = -1; - cfreq = frq; - } - else if (strcmp(*argv, "--rawhex") == 0) { rawhex = 2; } // raw hex input - else if (strcmp(*argv, "-") == 0) { - int sample_rate = 0, bits_sample = 0, channels = 0; - ++argv; - if (*argv) sample_rate = atoi(*argv); else return -1; - ++argv; - if (*argv) bits_sample = atoi(*argv); else return -1; - channels = 2; - if (sample_rate < 1 || (bits_sample != 8 && bits_sample != 16 && bits_sample != 32)) { - fprintf(stderr, "- \n"); - return -1; - } - pcm.sr = sample_rate; - pcm.bps = bits_sample; - pcm.nch = channels; - option_pcmraw = 1; - } - else { - fp = fopen(*argv, "rb"); - if (fp == NULL) { - fprintf(stderr, "error: open %s\n", *argv); - return -1; - } - wavloaded = 1; - } - ++argv; - } - if (!wavloaded) fp = stdin; - - if (option_iq == 5 && option_dc) option_lp |= LP_FM; - - // LUT faster for decM, however frequency correction after decimation - // LUT recommonded if decM > 2 - // - if (option_noLUT && option_iq == 5) dsp.opt_nolut = 1; else dsp.opt_nolut = 0; - - - if (gpx.option.raw && gpx.option.jsn) gpx.option.slt = 1; - - if (cfreq > 0) gpx.jsn_freq = (cfreq+500)/1000; - - - #ifdef EXT_FSK - if (!option_softin) { - option_softin = 1; - fprintf(stderr, "reading float32 soft symbols\n"); - } - #endif - - if (!rawhex) { - if (!option_softin) { - - if (option_iq == 0 && option_pcmraw) { - fclose(fp); - fprintf(stderr, "error: raw data not IQ\n"); - return -1; - } - if (option_iq) sel_wavch = 0; - - pcm.sel_ch = sel_wavch; - if (option_pcmraw == 0) { - k = read_wav_header(&pcm, fp); - if ( k < 0 ) { - fclose(fp); - fprintf(stderr, "error: wav header\n"); - return -1; - } - } - - if (cfreq > 0) { - int fq_kHz = (cfreq - dsp.xlt_fq*pcm.sr + 500)/1e3; - gpx.jsn_freq = fq_kHz; - } - - // m10: BT>1?, h=1.2 ? - symlen = 2; - - // init dsp - // - dsp.fp = fp; - dsp.sr = pcm.sr; - dsp.bps = pcm.bps; - dsp.nch = pcm.nch; - dsp.ch = pcm.sel_ch; - dsp.br = (float)BAUD_RATE; - dsp.sps = (float)dsp.sr/dsp.br; - dsp.symlen = symlen; - dsp.symhd = 1; // M10!header - dsp._spb = dsp.sps*symlen; - dsp.hdr = rawheader; - dsp.hdrlen = strlen(rawheader); - dsp.BT = 1.8; // bw/time (ISI) // 1.0..2.0 // M20 ? - dsp.h = 0.9; // 1.2 modulation index // M20 ? - dsp.opt_iq = option_iq; - dsp.opt_iqdc = option_iqdc; - dsp.opt_lp = option_lp; - dsp.lpIQ_bw = lpIQ_bw; //24e3; // IF lowpass bandwidth - dsp.lpFM_bw = 10e3; // FM audio lowpass - dsp.opt_dc = option_dc; - dsp.opt_IFmin = option_min; - - if ( dsp.sps < 8 ) { - fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); - } - - if (baudrate > 0) { - dsp.br = (float)baudrate; - dsp.sps = (float)dsp.sr/dsp.br; - fprintf(stderr, "sps corr: %.4f\n", dsp.sps); - } - - //headerlen = dsp.hdrlen; - - - k = init_buffers(&dsp); - if ( k < 0 ) { - fprintf(stderr, "error: init buffers\n"); - return -1; - } - - bitofs += shift; - } - else { - // init circular header bit buffer - hdb.hdr = rawheader; - hdb.len = strlen(rawheader); - //hdb.thb = 1.0 - 3.1/(float)hdb.len; // 1.0-max_bit_errors/hdrlen - hdb.bufpos = -1; - hdb.buf = NULL; - /* - calloc(hdb.len, sizeof(char)); - if (hdb.buf == NULL) { - fprintf(stderr, "error: malloc\n"); - return -1; - } - */ - hdb.ths = 0.8; // caution 0.7: false positive / offset - hdb.sbuf = calloc(hdb.len, sizeof(float)); - if (hdb.sbuf == NULL) { - fprintf(stderr, "error: malloc\n"); - return -1; - } - } - - - while ( 1 ) - { - if (option_softin) { - header_found = find_softbinhead(fp, &hdb, &_mv); - } - else { // FM-audio: - header_found = find_header(&dsp, thres, 2, bitofs, dsp.opt_dc); // optional 2nd pass: dc=0 - _mv = dsp.mv; - } - - if (header_found == EOF) break; - - // mv == correlation score - if (_mv*(0.5-gpx.option.inv) < 0) { - gpx.option.inv ^= 0x1; // M10: irrelevant - } - - if (header_found) { - - bitpos = 0; - pos = 0; - pos /= 2; - bit0 = '0'; // oder: _mv[j] > 0 - - while ( pos < BITFRAME_LEN+BITAUX_LEN ) { - - if (option_softin) { - float s1 = 0.0; - float s2 = 0.0; - float s = 0.0; - bitQ = f32soft_read(fp, &s1); - if (bitQ != EOF) { - bitQ = f32soft_read(fp, &s2); - if (bitQ != EOF) { - s = s2-s1; // integrate both symbols // only 2nd Manchester symbol: s2 - bit = (s>=0.0); // no soft decoding - } - } - } - else { - float bl = -1; - if (option_iq >= 2) spike = 0; - if (option_iq > 2) bl = 4.0; - //bitQ = read_slbit(&dsp, &bit, 0, bitofs, bitpos, bl, spike); // symlen=2 - bitQ = read_softbit2p(&dsp, &hsbit, 0, bitofs, bitpos, bl, spike, &hsbit1); // symlen=2 - bit = hsbit.hb; - } - if ( bitQ == EOF ) { break; } - - gpx.frame_bits[pos] = 0x31 ^ (bit0 ^ bit); - pos++; - bit0 = bit; - bitpos += 1; - } - gpx.frame_bits[pos] = '\0'; - print_frame(&gpx, pos, 1); - if (pos < BITFRAME_LEN) break; - - header_found = 0; - - // bis Ende der Sekunde vorspulen; allerdings Doppel-Frame alle 10 sek - if (gpx.option.vbs < 3) { // && (regulare frame) // print_frame-return? - while ( bitpos < 5*BITFRAME_LEN ) { - if (option_softin) { - float s = 0.0; - bitQ = f32soft_read(fp, &s); - } - else { - bitQ = read_slbit(&dsp, &bit, 0, bitofs, bitpos, -1, 0); // symlen=2 - } - if (bitQ == EOF) break; - bitpos++; - } - } - - pos = 0; - } - } - - if (!option_softin) free_buffers(&dsp); - else { - if (hdb.buf) { free(hdb.buf); hdb.buf = NULL; } - } - } - else //if (rawhex) - { - char buffer_rawhex[2*(FRAME_LEN+AUX_LEN)+12]; - char *pbuf = NULL, *buf_sp = NULL; - ui8_t frmbyte; - int frameofs = 0, len, i; - - while (1 > 0) { - - memset(buffer_rawhex, 2*(FRAME_LEN+AUX_LEN)+12, 0); - pbuf = fgets(buffer_rawhex, 2*(FRAME_LEN+AUX_LEN)+12, fp); - if (pbuf == NULL) break; - buffer_rawhex[2*(FRAME_LEN+AUX_LEN)] = '\0'; - buf_sp = strchr(buffer_rawhex, ' '); - if (buf_sp != NULL && buf_sp-buffer_rawhex < 2*(FRAME_LEN+AUX_LEN)) { - buffer_rawhex[buf_sp-buffer_rawhex] = '\0'; - for (i = buf_sp-buffer_rawhex+1; i < 2*(FRAME_LEN+AUX_LEN); i++) buffer_rawhex[i] = '\0'; - } - len = strlen(buffer_rawhex) / 2; - if (len > pos_GPSweek+2) { - for (i = 0; i < len; i++) { //%2x SCNx8=%hhx(inttypes.h) - sscanf(buffer_rawhex+2*i, "%2hhx", &frmbyte); - // wenn ohne %hhx: sscanf(buffer_rawhex+rawhex*i, "%2x", &byte); frame[frameofs+i] = (ui8_t)byte; - gpx.frame_bytes[frameofs+i] = frmbyte; - } - print_frame(&gpx, len*8, 0); - } - } - } - - fclose(fp); - - return 0; -} - diff --git a/demod/mod/meisei100mod.c b/demod/mod/meisei100mod.c index a23c090..5a4d64e 100644 --- a/demod/mod/meisei100mod.c +++ b/demod/mod/meisei100mod.c @@ -58,6 +58,8 @@ Variante 2 (iMS-100 ?) 0x03..0x04 16 bit 0.5s-counter, count%2=0: 0x07..0x0A 32 bit cfg[cnt%64] (float32); cfg[0,16,32,48]=SN 0x11..0x12 30xx, xx=C1(ims100?),A2(rs11?) +0x13..0x14 16 bit temperature, main sensor, raw +0x15..0x16 16 bit humidity, raw 0x17..0x18 16 bit time ms yyxx, 00.000-59.000 0x19..0x1A 16 bit time hh:mm 0x1B..0x1D HEADER 0xFB6230 @@ -138,10 +140,13 @@ typedef struct { int std; int min; float sek; double lat; double lon; double alt; double vH; double vD; double vV; + ui16_t f_ref; + float T; float RH; char frame_rawbits[RAWBITFRAME_LEN+10]; ui8_t frame_bits[BITFRAME_LEN+10]; ui32_t ecc; float cfg[64]; + ui64_t cfg_valid; ui32_t _sn; float sn; // 0 mod 16 float fq; // 15 mod 64 @@ -245,6 +250,33 @@ static int get_w16(ui8_t *subframe_bits, int j) { /* -------------------------------------------------------------------------- */ +static int sanity_check_ims100_config_temperature(gpx_t *gpx) { + int result = 1; + float R_old = 0; + float T_old = INFINITY; + int i; + + // All resistance values in the R-T interpolation table must be positive and monotonically increasing + for (i = 0; i < 12; i++) { + if (gpx->cfg[33+i] <= R_old) { + result = 0; + } + R_old = gpx->cfg[33+i]; + } + + // All temperature values in the R-T interpolation table must be monotonically decreasing + for (i = 0; i < 12; i++) { + if (gpx->cfg[17+i] >= T_old) { + result = 0; + } + T_old = gpx->cfg[17+i]; + } + + return result; +} + +/* -------------------------------------------------------------------------- */ + int main(int argc, char **argv) { @@ -253,6 +285,7 @@ int main(int argc, char **argv) { option_inv = 0, option_ecc = 0, // BCH(63,51) option_jsn = 0; // JSON output (auto_rx) + int option_ptu = 0; int option_min = 0; int option_iq = 0; int option_iqdc = 0; @@ -355,6 +388,9 @@ int main(int argc, char **argv) { option1 = 1; } else if (strcmp(*argv, "--ecc") == 0) { option_ecc = 1; } + else if (strcmp(*argv, "--ptu") == 0) { + option_ptu = 1; + } else if ( (strcmp(*argv, "-v") == 0) ) { option_verbose = 1; } else if ( (strcmp(*argv, "--br") == 0) ) { ++argv; @@ -566,7 +602,8 @@ int main(int argc, char **argv) { } gpx.sn = -1; - + gpx.RH = NAN; + gpx.T = NAN; while ( 1 ) { @@ -831,6 +868,8 @@ int main(int argc, char **argv) { jmpIMS: if (reset_gpx) { memset(&gpx, sizeof(gpx), 0); + gpx.RH = NAN; + gpx.T = NAN; sn = -1; freq = -1; reset_gpx = 0; @@ -864,11 +903,20 @@ int main(int argc, char **argv) { if (err_frm == 0 && block_err[0] < 2 && block_err[1] < 2) { gpx.cfg[counter%64] = *fcfg; + gpx.cfg_valid |= 1uLL << (counter%64); // (main?) SN if (counter % 0x10 == 0) { sn = *fcfg; gpx.sn = sn; gpx._sn = w32; } // freq if (counter % 64 == 15) { freq = 400e3+(*fcfg)*100.0; gpx.fq = freq; } + + //PTU: Save reference frequency (sent in both even and odd frames) + if (counter % 4 == 0) { + gpx.f_ref = bits2val(subframe_bits+HEADLEN+0*46+17, 16); + } + if (counter % 4 == 3) { + gpx.f_ref = bits2val(subframe_bits+HEADLEN+3*46, 16); + } } if (counter % 2 == 0) { @@ -884,6 +932,58 @@ int main(int argc, char **argv) { printf(" "); printf("%02d:%02d:%06.3f ", gpx.std, gpx.min, gpx.sek); printf(" "); + + if (option_ptu) { + gpx.T = NAN; + gpx.RH = NAN; + if (gpx.f_ref != 0) { // must know the reference frequency + int T_cfg = ((gpx.cfg_valid & 0x01E01FFE1FFE0000LL) == 0x01E01FFE1FFE0000LL); // cfg[56:53,44:33,28:17] + int U_cfg = ((gpx.cfg_valid & 0x001E000000000000LL) == 0x001E000000000000LL); // cfg[52:49] + // Necessary parameters must exist and their values must ´meet the requirements + if (T_cfg && sanity_check_ims100_config_temperature(&gpx)) { + ui16_t t_raw = bits2val(subframe_bits+HEADLEN+2*46+17, 16); + float f = ((float)t_raw / (float)gpx.f_ref) * 4.0f; + if (f > 1.0f) { + // Use config coefficients to transform measured frequency to absolute resistance (kOhms) + f = 1.0f / (f - 1.0f); + float R = gpx.cfg[53] + gpx.cfg[54]*f + gpx.cfg[55]*f*f + gpx.cfg[56]*f*f*f; + // iMS-100 sends known resistance (cfg[44:33]) for 12 temperature sampling points + // (cfg[28:17]). Actual temperature is found by interpolating in one of these + // 11 intervals. + if (R <= gpx.cfg[33]) { // R below min value? + gpx.T = gpx.cfg[17]; // --> Set T = highest temperature + } else if (R >= gpx.cfg[44]) { // R above max value? + gpx.T = gpx.cfg[28]; // --> Set T = lowest temperature + } else { + // We now know that R is inside the interpolation range. Sampling points are + // ordered by increasing resistance (decreasing temperature). + // (We have verified this in the sanity check above.) + // Search for the interval that contains R, then interpolate linearly + // (using log(R)). + for (j = 0; j < 11; j++) { + if (R < gpx.cfg[34+j]) { + f = (logf(R) - logf(gpx.cfg[33+j])) / (logf(gpx.cfg[34+j]) - logf(gpx.cfg[33+j])); + gpx.T = gpx.cfg[17+j] - f*(gpx.cfg[17+j] - gpx.cfg[18+j]); + break; + } + } + } + } + if (!isnan(gpx.T)) printf("T=%.1fC ", gpx.T); + else T_cfg = 0; + } + if (U_cfg) { + ui16_t u_raw = bits2val(subframe_bits+HEADLEN+3*46, 16); + float f = ((float)u_raw / (float)gpx.f_ref) * 4.0f; + gpx.RH = gpx.cfg[49] + gpx.cfg[50]*f + gpx.cfg[51]*f*f + gpx.cfg[52]*f*f*f; + // Limit to 0...100% + gpx.RH = fmaxf(gpx.RH, 0.0f); + gpx.RH = fminf(gpx.RH, 100.0f); + printf("RH=%.0f%% ", gpx.RH); + } + if (T_cfg || U_cfg) printf(" "); + } + } } } @@ -997,6 +1097,14 @@ int main(int argc, char **argv) { if (gpx.frm1_valid && (gpx.frm1_count == gpx.frm0_count + 1)) { if (gpx.vV_valid) printf(", \"vel_v\": %.5f", gpx.vV ); } + if (option_ptu) { + if (!isnan(gpx.T)) { + fprintf(stdout, ", \"temp\": %.1f", gpx.T ); + } + if (!isnan(gpx.RH)) { + fprintf(stdout, ", \"humidity\": %.1f", gpx.RH ); + } + } printf(", \"subtype\": \"IMS100\""); if (gpx.jsn_freq > 0) { // not gpx.fq, because gpx.sn not in every frame printf(", \"freq\": %d", gpx.jsn_freq); diff --git a/demod/mod/rs41mod18.c b/demod/mod/rs41mod18.c new file mode 100644 index 0000000..6e0e3f6 --- /dev/null +++ b/demod/mod/rs41mod18.c @@ -0,0 +1,1646 @@ + +/* + * rs41 + * sync header: correlation/matched filter + * files: rs41mod.c bch_ecc_mod.c demod_mod.c demod_mod.h + * compile, either (a) or (b): + * (a) + * gcc -c demod_mod.c + * gcc -DINCLUDESTATIC rs41mod.c demod_mod.o -lm -o rs41mod + * (b) + * gcc -c demod_mod.c + * gcc -c bch_ecc_mod.c + * gcc rs41mod.c demod_mod.o bch_ecc_mod.o -lm -o rs41mod + * + * author: zilog80 + */ + +#include +#include +#include +#include + +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif + +//typedef unsigned char ui8_t; +//typedef unsigned short ui16_t; +//typedef unsigned int ui32_t; +//typedef short i16_t; +//typedef int i32_t; + +#include "demod_mod.h" + +//#define INCLUDESTATIC 1 +#ifdef INCLUDESTATIC + #include "bch_ecc_mod.c" +#else + #include "bch_ecc_mod.h" +#endif + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t aut; + i8_t jsn; // JSON output (auto_rx) +} option_t; + +typedef struct { + int typ; + int msglen; + int msgpos; + int parpos; + int hdrlen; + int frmlen; +} rscfg_t; + +static rscfg_t cfg_rs41 = { 41, (320-56)/2, 56, 8, 8, 320}; // const: msgpos, parpos + + +#define NDATA_LEN 320 // std framelen 320 +#define XDATA_LEN 198 +#define FRAME_LEN (NDATA_LEN+XDATA_LEN) // max framelen 518 +/* +ui8_t //xframe[FRAME_LEN] = { 0x10, 0xB6, 0xCA, 0x11, 0x22, 0x96, 0x12, 0xF8}, = xorbyte( frame) + frame[FRAME_LEN] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; // = xorbyte(xframe) +*/ +typedef struct { + int out; + int frnr; + char id[9]; + ui8_t numSV; + int week; int tow_ms; int gpssec; + int jahr; int monat; int tag; + int wday; + int std; int min; float sek; + double lat; double lon; double alt; + double vH; double vD; double vV; + float T; float RH; + ui32_t crc; + ui8_t frame[FRAME_LEN]; + ui8_t calibytes[51*16]; + ui8_t calfrchk[51]; + float ptu_Rf1; // ref-resistor f1 (750 Ohm) + float ptu_Rf2; // ref-resistor f2 (1100 Ohm) + float ptu_co1[3]; // { -243.911 , 0.187654 , 8.2e-06 } + float ptu_calT1[3]; // calibration T1 + float ptu_co2[3]; // { -243.911 , 0.187654 , 8.2e-06 } + float ptu_calT2[3]; // calibration T2-Hum + float ptu_calH[2]; // calibration Hum + ui32_t freq; // freq/kHz + ui16_t conf_fw; // firmware + ui16_t conf_kt; // kill timer (sec) + ui16_t conf_bt; // burst timer (sec) + ui8_t conf_bk; // burst kill + ui16_t conf_cd; // kill countdown (sec) (kt or bt) + char rstyp[9]; // RS41-SG, RS41-SGP + int aux; + char xdata[XDATA_LEN+16]; // xdata: aux_str1#aux_str2 ... + option_t option; + RS_t RS; +} gpx_t; + + +#define BITS 8 +#define HEADLEN 64 +#define FRAMESTART ((HEADLEN)/BITS) + +/* 10 B6 CA 11 22 96 12 F8 */ +static char rs41_header[] = "0000100001101101010100111000100001000100011010010100100000011111"; + +static ui8_t rs41_header_bytes[8] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; + +#define MASK_LEN 64 +static ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, + 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, + 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, + 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, + 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, + 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, + 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, + 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; +/* LFSR: ab i=8 (mod 64): + * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] + * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 + * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 + */ +/* + frame[pos] = xframe[pos] ^ mask[pos % MASK_LEN]; +*/ + +/* ------------------------------------------------------------------------------------ */ + +#define BAUD_RATE 4800 + +/* ------------------------------------------------------------------------------------ */ +/* + * Convert GPS Week and Seconds to Modified Julian Day. + * - Adapted from sci.astro FAQ. + * - Ignores UTC leap seconds. + */ +// in : week, gpssec +// out: jahr, monat, tag +static void Gps2Date(gpx_t *gpx) { + long GpsDays, Mjd; + long J, C, Y, M; + + GpsDays = gpx->week * 7 + (gpx->gpssec / 86400); + Mjd = 44244 + GpsDays; + + J = Mjd + 2468570; + C = 4 * J / 146097; + J = J - (146097 * C + 3) / 4; + Y = 4000 * (J + 1) / 1461001; + J = J - 1461 * Y / 4 + 31; + M = 80 * J / 2447; + gpx->tag = J - 2447 * M / 80; + J = M / 11; + gpx->monat = M + 2 - (12 * J); + gpx->jahr = 100 * (C - 49) + Y + J; +} +/* ------------------------------------------------------------------------------------ */ + +static int bits2byte(char bits[]) { + int i, byteval=0, d=1; + for (i = 0; i < 8; i++) { // little endian + /* for (i = 7; i >= 0; i--) { // big endian */ + if (bits[i] == 1) byteval += d; + else if (bits[i] == 0) byteval += 0; + else return 0x100; + d <<= 1; + } + return byteval; +} + +/* ------------------------------------------------------------------------------------ */ + +static ui32_t u4(ui8_t *bytes) { // 32bit unsigned int + ui32_t val = 0; + memcpy(&val, bytes, 4); + return val; +} + +static ui32_t u3(ui8_t *bytes) { // 24bit unsigned int + int val24 = 0; + val24 = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); + // = memcpy(&val, bytes, 3), val &= 0x00FFFFFF; + return val24; +} + +static int i3(ui8_t *bytes) { // 24bit signed int + int val = 0, + val24 = 0; + val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); + val24 = val & 0xFFFFFF; if (val24 & 0x800000) val24 = val24 - 0x1000000; + return val24; +} + +static ui32_t u2(ui8_t *bytes) { // 16bit unsigned int + return bytes[0] | (bytes[1]<<8); +} + +/* +double r8(ui8_t *bytes) { + double val = 0; + memcpy(&val, bytes, 8); + return val; +} + +float r4(ui8_t *bytes) { + float val = 0; + memcpy(&val, bytes, 4); + return val; +} +*/ + +static int crc16(gpx_t *gpx, int start, int len) { + int crc16poly = 0x1021; + int rem = 0xFFFF, i, j; + int byte; + + if (start+len+2 > FRAME_LEN) return -1; + + for (i = 0; i < len; i++) { + byte = gpx->frame[start+i]; + rem = rem ^ (byte << 8); + for (j = 0; j < 8; j++) { + if (rem & 0x8000) { + rem = (rem << 1) ^ crc16poly; + } + else { + rem = (rem << 1); + } + rem &= 0xFFFF; + } + } + return rem; +} + +static int check_CRC(gpx_t *gpx, ui32_t pos, ui32_t pck) { + ui32_t crclen = 0, + crcdat = 0; + // check only pck_type (variable len pcks 0x76, 0x7E) + if (((pck>>8) & 0xFF) != gpx->frame[pos]) return -1; + crclen = gpx->frame[pos+1]; + if (pos + crclen + 4 > FRAME_LEN) return -1; + crcdat = u2(gpx->frame+pos+2+crclen); + if ( crcdat != crc16(gpx, pos+2, crclen) ) { + return 1; // CRC NO + } + else return 0; // CRC OK +} + + +/* +GPS chip: ublox UBX-G6010-ST + + Pos: SubHeader, 1+1 byte (ID+LEN) +0x039: 7928 FrameNumber+SondeID + +(0x050: 0732 CalFrames 0x00..0x32) +0x065: 7A2A PTU +0x093: 7C1E GPS1: RXM-RAW (0x02 0x10) Week, TOW, Sats +0x0B5: 7D59 GPS2: RXM-RAW (0x02 0x10) pseudorange, doppler +0x112: 7B15 GPS3: NAV-SOL (0x01 0x06) ECEF-POS, ECEF-VEL +0x12B: 7611 00 +0x12B: 7Exx AUX-xdata +*/ + +#define crc_FRAME (1<<0) +#define xor_FRAME 0x1713 // ^0x6E3B=0x7928 +#define pck_FRAME 0x7928 +#define pos_FRAME 0x039 +#define pos_FrameNb 0x03B // 2 byte +#define pos_SondeID 0x03D // 8 byte +#define pos_CalData 0x052 // 1 byte, counter 0x00..0x32 +#define pos_Calfreq 0x055 // 2 byte, calfr 0x00 +#define pos_Calburst 0x05E // 1 byte, calfr 0x02 +// ? #define pos_Caltimer 0x05A // 2 byte, calfr 0x02 ? +#define pos_CalRSTyp 0x05B // 8 byte, calfr 0x21 (+2 byte in 0x22?) + // weitere chars in calfr 0x22/0x23; weitere ID + +#define crc_PTU (1<<1) +#define xor_PTU 0xE388 // ^0x99A2=0x0x7A2A +#define pck_PTU 0x7A2A // PTU +#define pos_PTU 0x065 + +#define crc_GPS1 (1<<2) +#define xor_GPS1 0x9667 // ^0xEA79=0x7C1E +#define pck_GPS1 0x7C1E // RXM-RAW (0x02 0x10) +#define pos_GPS1 0x093 +#define pos_GPSweek 0x095 // 2 byte +#define pos_GPSiTOW 0x097 // 4 byte +#define pos_satsN 0x09B // 12x2 byte (1: SV, 1: quality,strength) + +#define crc_GPS2 (1<<3) +#define xor_GPS2 0xD7AD // ^0xAAF4=0x7D59 +#define pck_GPS2 0x7D59 // RXM-RAW (0x02 0x10) +#define pos_GPS2 0x0B5 +#define pos_minPR 0x0B7 // 4 byte +#define pos_FF 0x0BB // 1 byte +#define pos_dataSats 0x0BC // 12x(4+3) byte (4: pseudorange, 3: doppler) + +#define crc_GPS3 (1<<4) +#define xor_GPS3 0xB9FF // ^0xC2EA=0x7B15 +#define pck_GPS3 0x7B15 // NAV-SOL (0x01 0x06) +#define pos_GPS3 0x112 +#define pos_GPSecefX 0x114 // 4 byte +#define pos_GPSecefY 0x118 // 4 byte +#define pos_GPSecefZ 0x11C // 4 byte +#define pos_GPSecefV 0x120 // 3*2 byte +#define pos_numSats 0x126 // 1 byte +#define pos_sAcc 0x127 // 1 byte +#define pos_pDOP 0x128 // 1 byte + +#define crc_AUX (1<<5) +#define pck_AUX 0x7E00 // LEN variable +#define pos_AUX 0x12B + +#define crc_ZERO (1<<6) // LEN variable +#define pck_ZERO 0x7600 +#define pck_ZEROstd 0x7611 // NDATA std-frm, no aux +#define pos_ZEROstd 0x12B // pos_AUX(0) + +#define pck_ENCRYPTED 0x8000 // Packet type for an Encrypted payload + +/* + frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) + frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) +*/ +static int frametype(gpx_t *gpx) { // -4..+4: 0xF0 -> -4 , 0x0F -> +4 + int i; + ui8_t b = gpx->frame[pos_FRAME-1]; + int ft = 0; + for (i = 0; i < 4; i++) { + ft += ((b>>i)&1) - ((b>>(i+4))&1); + } + return ft; +} + +static int get_FrameNb(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t frnr_bytes[2]; + int frnr; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_FrameNb + i]; + frnr_bytes[i] = byte; + } + + frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); + gpx->frnr = frnr; + + return 0; +} + +static int get_SondeID(gpx_t *gpx, int crc) { + int i; + unsigned byte; + char sondeid_bytes[9]; + + if (crc == 0) { + for (i = 0; i < 8; i++) { + byte = gpx->frame[pos_SondeID + i]; + //if ((byte < 0x20) || (byte > 0x7E)) return -1; + sondeid_bytes[i] = byte; + } + sondeid_bytes[8] = '\0'; + if ( strncmp(gpx->id, sondeid_bytes, 8) != 0 ) { + //for (i = 0; i < 51; i++) gpx->calfrchk[i] = 0; + memset(gpx->calfrchk, 0, 51); + // reset conf data + memset(gpx->rstyp, 0, 9); + gpx->freq = 0; + gpx->conf_fw = 0; + gpx->conf_bt = 0; + gpx->conf_bk = 0; + gpx->conf_cd = -1; + gpx->conf_kt = -1; + // don't reset gpx->frame[] ! + // gpx->T = -273.15; + // gpx->RH = -1.0; + // new ID: + memcpy(gpx->id, sondeid_bytes, 8); + gpx->id[8] = '\0'; + } + } + + return 0; +} + +static int get_FrameConf(gpx_t *gpx) { + int crc, err; + ui8_t calfr; + int i; + + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + if (crc) gpx->crc |= crc_FRAME; + + err = crc; + err |= get_SondeID(gpx, crc); + err |= get_FrameNb(gpx); + + if (crc == 0) { + calfr = gpx->frame[pos_CalData]; + if (gpx->calfrchk[calfr] == 0) // const? + { // 0x32 not constant + for (i = 0; i < 16; i++) { + gpx->calibytes[calfr*16 + i] = gpx->frame[pos_CalData+1+i]; + } + gpx->calfrchk[calfr] = 1; + } + } + + return err; +} + +static int get_CalData(gpx_t *gpx) { + + memcpy(&(gpx->ptu_Rf1), gpx->calibytes+61, 4); // 0x03*0x10+13 + memcpy(&(gpx->ptu_Rf2), gpx->calibytes+65, 4); // 0x04*0x10+ 1 + + memcpy(gpx->ptu_co1+0, gpx->calibytes+77, 4); // 0x04*0x10+13 + memcpy(gpx->ptu_co1+1, gpx->calibytes+81, 4); // 0x05*0x10+ 1 + memcpy(gpx->ptu_co1+2, gpx->calibytes+85, 4); // 0x05*0x10+ 5 + + memcpy(gpx->ptu_calT1+0, gpx->calibytes+89, 4); // 0x05*0x10+ 9 + memcpy(gpx->ptu_calT1+1, gpx->calibytes+93, 4); // 0x05*0x10+13 + memcpy(gpx->ptu_calT1+2, gpx->calibytes+97, 4); // 0x06*0x10+ 1 + + memcpy(gpx->ptu_calH+0, gpx->calibytes+117, 4); // 0x07*0x10+ 5 + memcpy(gpx->ptu_calH+1, gpx->calibytes+121, 4); // 0x07*0x10+ 9 + + memcpy(gpx->ptu_co2+0, gpx->calibytes+293, 4); // 0x12*0x10+ 5 + memcpy(gpx->ptu_co2+1, gpx->calibytes+297, 4); // 0x12*0x10+ 9 + memcpy(gpx->ptu_co2+2, gpx->calibytes+301, 4); // 0x12*0x10+13 + + memcpy(gpx->ptu_calT2+0, gpx->calibytes+305, 4); // 0x13*0x10+ 1 + memcpy(gpx->ptu_calT2+1, gpx->calibytes+309, 4); // 0x13*0x10+ 5 + memcpy(gpx->ptu_calT2+2, gpx->calibytes+313, 4); // 0x13*0x10+ 9 + + return 0; +} + +/* +static float get_Tc0(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2) { + // y = (f - f1) / (f2 - f1); + // y1 = (f - f1) / f2; // = (1 - f1/f2)*y + float a = 3.9083e-3, // Pt1000 platinum resistance + b = -5.775e-7, + c = -4.183e-12; // below 0C, else C=0 + float *cal = gpx->ptu_calT1; + float Rb = (f1*gpx->ptu_Rf2-f2*gpx->ptu_Rf1)/(f2-f1), // ofs + Ra = f * (gpx->ptu_Rf2-gpx->ptu_Rf1)/(f2-f1) - Rb, + raw = Ra/1000.0, + g_r = 0.8024*cal[0] + 0.0176, // empirisch + r_o = 0.0705*cal[1] + 0.0011, // empirisch + r = raw * g_r + r_o, + t = (-a + sqrt(a*a + 4*b*(r-1)))/(2*b); // t>0: c=0 + // R/R0 = 1 + at + bt^2 + c(t-100)t^3 , R0 = 1000 Ohm, t/Celsius + return t; +} +*/ +// T_RH-sensor +static float get_TH(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2) { + float *p = gpx->ptu_co2; + float *c = gpx->ptu_calT2; + float g = (float)(f2-f1)/(gpx->ptu_Rf2-gpx->ptu_Rf1), // gain + Rb = (f1*gpx->ptu_Rf2-f2*gpx->ptu_Rf1)/(float)(f2-f1), // ofs + Rc = f/g - Rb, + //R = (Rc + c[1]) * c[0], + //T = p[0] + p[1]*R + p[2]*R*R; + R = Rc * c[0], + T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); + return T; +} +// T-sensor, platinum resistor +static float get_Tc(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2) { + float *p = gpx->ptu_co1; + float *c = gpx->ptu_calT1; + float g = (float)(f2-f1)/(gpx->ptu_Rf2-gpx->ptu_Rf1), // gain + Rb = (f1*gpx->ptu_Rf2-f2*gpx->ptu_Rf1)/(float)(f2-f1), // ofs + Rc = f/g - Rb, + //R = (Rc + c[1]) * c[0], + //T = p[0] + p[1]*R + p[2]*R*R; + R = Rc * c[0], + T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); + return T; +} + +// rel.hum., capacitor +// (data:) ftp://ftp-cdc.dwd.de/pub/CDC/observations_germany/radiosondes/ +// (diffAlt: Ellipsoid-Geoid) +static float get_RH(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2, float T) { + float a0 = 7.5; // empirical + float a1 = 350.0/gpx->ptu_calH[0]; // empirical + float fh = (f-f1)/(float)(f2-f1); + float rh = 100.0 * ( a1*fh - a0 ); + float T0 = 0.0, T1 = -25.0; // T/C + rh += T0 - T/5.5; // empir. temperature compensation + if (T < T1) rh *= 1.0 + (T1-T)/90.0; // empir. temperature compensation + if (rh < 0.0) rh = 0.0; + if (rh > 100.0) rh = 100.0; + if (T < -273.0) rh = -1.0; + return rh; +} + +static int get_PTU(gpx_t *gpx) { + int err=0, i; + int bR, bc1, bT1, + bc2, bT2; + int bH; + ui32_t meas[12]; + float Tc = -273.15; + float TH = -273.15; + float RH = -1.0; + + get_CalData(gpx); + + err = check_CRC(gpx, pos_PTU, pck_PTU); + if (err) gpx->crc |= crc_PTU; + + if (err == 0) + { + + for (i = 0; i < 12; i++) { + meas[i] = u3(gpx->frame+pos_PTU+2+3*i); + } + + bR = gpx->calfrchk[0x03] && gpx->calfrchk[0x04]; + bc1 = gpx->calfrchk[0x04] && gpx->calfrchk[0x05]; + bT1 = gpx->calfrchk[0x05] && gpx->calfrchk[0x06]; + bc2 = gpx->calfrchk[0x12] && gpx->calfrchk[0x13]; + bT2 = gpx->calfrchk[0x13]; + bH = gpx->calfrchk[0x07]; + + if (bR && bc1 && bT1) { + Tc = get_Tc(gpx, meas[0], meas[1], meas[2]); + //Tc0 = get_Tc0(gpx, meas[0], meas[1], meas[2]); + } + gpx->T = Tc; + + if (bR && bc2 && bT2) { + TH = get_TH(gpx, meas[6], meas[7], meas[8]); + } + + if (bH) { + RH = get_RH(gpx, meas[3], meas[4], meas[5], Tc); // TH, TH-Tc (sensorT - T) + } + gpx->RH = RH; + + + if (gpx->option.vbs == 4 && (gpx->crc & (crc_PTU | crc_GPS3))==0) + { + printf(" h: %8.2f # ", gpx->alt); // crc_GPS3 ? + + printf("1: %8d %8d %8d", meas[0], meas[1], meas[2]); + printf(" # "); + printf("2: %8d %8d %8d", meas[3], meas[4], meas[5]); + printf(" # "); + printf("3: %8d %8d %8d", meas[6], meas[7], meas[8]); + printf(" # "); + + //if (Tc > -273.0 && RH > -0.5) + { + printf(" "); + printf(" Tc:%.2f ", Tc); + printf(" RH:%.1f ", RH); + printf(" TH:%.2f ", TH); + } + printf("\n"); + + //if (gpx->alt > -400.0) + { + printf(" %9.2f ; %6.1f ; %6.1f ", gpx->alt, gpx->ptu_Rf1, gpx->ptu_Rf2); + printf("; %10.6f ; %10.6f ; %10.6f ", gpx->ptu_calT1[0], gpx->ptu_calT1[1], gpx->ptu_calT1[2]); + //printf("; %8d ; %8d ; %8d ", meas[0], meas[1], meas[2]); + printf("; %10.6f ; %10.6f ", gpx->ptu_calH[0], gpx->ptu_calH[1]); + //printf("; %8d ; %8d ; %8d ", meas[3], meas[4], meas[5]); + printf("; %10.6f ; %10.6f ; %10.6f ", gpx->ptu_calT2[0], gpx->ptu_calT2[1], gpx->ptu_calT2[2]); + //printf("; %8d ; %8d ; %8d" , meas[6], meas[7], meas[8]); + printf("\n"); + } + } + + } + + return err; +} + + +const double c = 299.792458e6; +const double L1 = 1575.42e6; + +static int get_SatData(gpx_t *gpx) { + int i, n; + int sv; + ui32_t minPR; + int numSV; + double pDOP, sAcc; + int err = 0; + + if ( ((gpx->frame[pos_GPS1]<<8) | gpx->frame[pos_GPS1+1]) != pck_GPS1 ) return -1; + if ( ((gpx->frame[pos_GPS2]<<8) | gpx->frame[pos_GPS2+1]) != pck_GPS2 ) return -2; + if ( ((gpx->frame[pos_GPS3]<<8) | gpx->frame[pos_GPS3+1]) != pck_GPS3 ) return -3; + + err = get_FrameConf(gpx); + + if (!err) { + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, "(%s) ", gpx->id); + fprintf(stdout, "\n"); + } + + fprintf(stdout, "iTOW: 0x%08X", u4(gpx->frame+pos_GPSiTOW)); + fprintf(stdout, " week: 0x%04X", u2(gpx->frame+pos_GPSweek)); + fprintf(stdout, "\n"); + minPR = u4(gpx->frame+pos_minPR); + fprintf(stdout, "minPR: %d", minPR); + fprintf(stdout, "\n"); + + for (i = 0; i < 12; i++) { + n = i*7; + sv = gpx->frame[pos_satsN+2*i]; + if (sv == 0xFF) break; + fprintf(stdout, " SV: %2d ", sv); + //fprintf(stdout, " (%02x) ", gpx->frame[pos_satsN+2*i+1]); + fprintf(stdout, "# "); + fprintf(stdout, "prMes: %.1f", u4(gpx->frame+pos_dataSats+n)/100.0 + minPR); + fprintf(stdout, " "); + fprintf(stdout, "doMes: %.1f", -i3(gpx->frame+pos_dataSats+n+4)/100.0*L1/c); + fprintf(stdout, "\n"); + } + + fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", + (i32_t)u4(gpx->frame+pos_GPSecefX), + (i32_t)u4(gpx->frame+pos_GPSecefY), + (i32_t)u4(gpx->frame+pos_GPSecefZ)); + fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", + (i16_t)u2(gpx->frame+pos_GPSecefV+0), + (i16_t)u2(gpx->frame+pos_GPSecefV+2), + (i16_t)u2(gpx->frame+pos_GPSecefV+4)); + + numSV = gpx->frame[pos_numSats]; + sAcc = gpx->frame[pos_sAcc]/10.0; if (gpx->frame[pos_sAcc] == 0xFF) sAcc = -1.0; + pDOP = gpx->frame[pos_pDOP]/10.0; if (gpx->frame[pos_pDOP] == 0xFF) pDOP = -1.0; + fprintf(stdout, "numSatsFix: %2d sAcc: %.1f pDOP: %.1f\n", numSV, sAcc, pDOP); + + + fprintf(stdout, "CRC: "); + fprintf(stdout, " %04X", pck_GPS1); + if (check_CRC(gpx, pos_GPS1, pck_GPS1)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS1, pck_GPS1)); + fprintf(stdout, " %04X", pck_GPS2); + if (check_CRC(gpx, pos_GPS2, pck_GPS2)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS2, pck_GPS2)); + fprintf(stdout, " %04X", pck_GPS3); + if (check_CRC(gpx, pos_GPS3, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS3, pck_GPS3)); + + fprintf(stdout, "\n"); + fprintf(stdout, "\n"); + + return 0; +} + +static int get_GPSweek(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpsweek_bytes[2]; + int gpsweek; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_GPSweek + i]; + gpsweek_bytes[i] = byte; + } + + gpsweek = gpsweek_bytes[0] + (gpsweek_bytes[1] << 8); + //if (gpsweek < 0) { gpx->week = -1; return -1; } // (short int) + gpx->week = gpsweek; + + return 0; +} + +//char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; +static char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; + +static int get_GPStime(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpstime_bytes[4]; + int gpstime = 0, // 32bit + day; + int ms; + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSiTOW + i]; + gpstime_bytes[i] = byte; + } + + memcpy(&gpstime, gpstime_bytes, 4); + + gpx->tow_ms = gpstime; + ms = gpstime % 1000; + gpstime /= 1000; + gpx->gpssec = gpstime; + + day = (gpstime / (24 * 3600)) % 7; + //if ((day < 0) || (day > 6)) return -1; // besser CRC-check + + gpstime %= (24*3600); + + gpx->wday = day; + gpx->std = gpstime / 3600; + gpx->min = (gpstime % 3600) / 60; + gpx->sek = gpstime % 60 + ms/1000.0; + + return 0; +} + +static int get_GPS1(gpx_t *gpx) { + int err=0; + + // gpx->frame[pos_GPS1+1] != (pck_GPS1 & 0xFF) ? + err = check_CRC(gpx, pos_GPS1, pck_GPS1); + if (err) { + gpx->crc |= crc_GPS1; + // reset GPS1-data (json) + gpx->jahr = 0; gpx->monat = 0; gpx->tag = 0; + gpx->std = 0; gpx->min = 0; gpx->sek = 0.0; + return -1; + } + + err |= get_GPSweek(gpx); // no plausibility-check + err |= get_GPStime(gpx); // no plausibility-check + + return err; +} + +static int get_GPS2(gpx_t *gpx) { + int err=0; + + // gpx->frame[pos_GPS2+1] != (pck_GPS2 & 0xFF) ? + err = check_CRC(gpx, pos_GPS2, pck_GPS2); + if (err) gpx->crc |= crc_GPS2; + + return err; +} + +#define EARTH_a 6378137.0 +#define EARTH_b 6356752.31424518 +#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) + +const +double a = EARTH_a, + b = EARTH_b, + a_b = EARTH_a2_b2, + e2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), + ee2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); + +static void ecef2elli(double X[], double *lat, double *lon, double *alt) { + double phi, lam, R, p, t; + + lam = atan2( X[1] , X[0] ); + + p = sqrt( X[0]*X[0] + X[1]*X[1] ); + t = atan2( X[2]*a , p*b ); + + phi = atan2( X[2] + ee2 * b * sin(t)*sin(t)*sin(t) , + p - e2 * a * cos(t)*cos(t)*cos(t) ); + + R = a / sqrt( 1 - e2*sin(phi)*sin(phi) ); + *alt = p / cos(phi) - R; + + *lat = phi*180/M_PI; + *lon = lam*180/M_PI; +} + +static int get_GPSkoord(gpx_t *gpx) { + int i, k; + unsigned byte; + ui8_t XYZ_bytes[4]; + int XYZ; // 32bit + double X[3], lat, lon, alt; + ui8_t gpsVel_bytes[2]; + short vel16; // 16bit + double V[3]; + double phi, lam, dir; + double vN; double vE; double vU; + + + for (k = 0; k < 3; k++) { + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSecefX + 4*k + i]; + XYZ_bytes[i] = byte; + } + memcpy(&XYZ, XYZ_bytes, 4); + X[k] = XYZ / 100.0; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_GPSecefV + 2*k + i]; + gpsVel_bytes[i] = byte; + } + vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8; + V[k] = vel16 / 100.0; + + } + + + // ECEF-Position + ecef2elli(X, &lat, &lon, &alt); + gpx->lat = lat; + gpx->lon = lon; + gpx->alt = alt; + if ((alt < -1000) || (alt > 80000)) return -3; // plausibility-check: altitude, if ecef=(0,0,0) + + + // ECEF-Velocities + // ECEF-Vel -> NorthEastUp + phi = lat*M_PI/180.0; + lam = lon*M_PI/180.0; + vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); + vE = -V[0]*sin(lam) + V[1]*cos(lam); + vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); + + // NEU -> HorDirVer + gpx->vH = sqrt(vN*vN+vE*vE); +/* + double alpha; + alpha = atan2(gpx->vN, gpx->vE)*180/M_PI; // ComplexPlane (von x-Achse nach links) - GeoMeteo (von y-Achse nach rechts) + dir = 90-alpha; // z=x+iy= -> i*conj(z)=y+ix=re(i(pi/2-t)), Achsen und Drehsinn vertauscht + if (dir < 0) dir += 360; // atan2(y,x)=atan(y/x)=pi/2-atan(x/y) , atan(1/t) = pi/2 - atan(t) + gpx->vD2 = dir; +*/ + dir = atan2(vE, vN) * 180 / M_PI; + if (dir < 0) dir += 360; + gpx->vD = dir; + + gpx->vV = vU; + + gpx->numSV = gpx->frame[pos_numSats]; + + return 0; +} + +static int get_GPS3(gpx_t *gpx) { + int err=0; + + // gpx->frame[pos_GPS3+1] != (pck_GPS3 & 0xFF) ? + err = check_CRC(gpx, pos_GPS3, pck_GPS3); + if (err) { + gpx->crc |= crc_GPS3; + // reset GPS3-data (json) + gpx->lat = 0.0; gpx->lon = 0.0; gpx->alt = 0.0; + gpx->vH = 0.0; gpx->vD = 0.0; gpx->vV = 0.0; + gpx->numSV = 0; + return -1; + } + + err |= get_GPSkoord(gpx); // plausibility-check: altitude, if ecef=(0,0,0) + + return err; +} + +static int get_Aux(gpx_t *gpx) { +// +// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide +// + int auxlen, auxcrc, count7E, pos7E; + int i, n; + + n = 0; + count7E = 0; + pos7E = pos_AUX; + gpx->xdata[0] = '\0'; + + if (frametype(gpx) <= 0) // pos7E == pos7611, 0x7E^0x76=0x08 ... + { + // 7Exx: xdata + while ( pos7E < FRAME_LEN && gpx->frame[pos7E] == 0x7E ) { + + auxlen = gpx->frame[pos7E+1]; + auxcrc = gpx->frame[pos7E+2+auxlen] | (gpx->frame[pos7E+2+auxlen+1]<<8); + + if ( auxcrc == crc16(gpx, pos7E+2, auxlen) ) { + if (count7E == 0) fprintf(stdout, "\n # xdata = "); + else { fprintf(stdout, " # "); gpx->xdata[n++] = '#'; } // aux separator + + //fprintf(stdout, " # %02x : ", gpx->frame[pos7E+2]); + for (i = 1; i < auxlen; i++) { + ui8_t c = gpx->frame[pos7E+2+i]; // (char) or better < 0x7F + if (c > 0x1E && c < 0x7F) { // ASCII-only + fprintf(stdout, "%c", c); + gpx->xdata[n++] = c; + } + } + count7E++; + pos7E += 2+auxlen+2; + } + else { + pos7E = FRAME_LEN; + gpx->crc |= crc_AUX; + } + } + } + gpx->xdata[n] = '\0'; + + i = check_CRC(gpx, pos7E, pck_ZERO); // 0x76xx: 00-padding block + if (i) gpx->crc |= crc_ZERO; + + return count7E; +} + +static int get_Calconf(gpx_t *gpx, int out) { + int i; + unsigned byte; + ui8_t calfr = 0; + ui16_t fw = 0; + int freq = 0, f0 = 0, f1 = 0; + char sondetyp[9]; + int err = 0; + + byte = gpx->frame[pos_CalData]; + calfr = byte; + err = check_CRC(gpx, pos_FRAME, pck_FRAME); + + if (gpx->option.vbs == 3) { + fprintf(stdout, "\n"); // fflush(stdout); + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, " 0x%02x: ", calfr); + for (i = 0; i < 16; i++) { + byte = gpx->frame[pos_CalData+1+i]; + fprintf(stdout, "%02x ", byte); + } + if (err == 0) fprintf(stdout, "[OK]"); + else fprintf(stdout, "[NO]"); + fprintf(stdout, " "); + } + + if (err == 0) + { + if (calfr == 0x00) { + byte = gpx->frame[pos_Calfreq] & 0xC0; // erstmal nur oberste beiden bits + f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 + byte = gpx->frame[pos_Calfreq+1]; + f1 = 40 * byte; + freq = 400000 + f1+f0; // kHz; + if (out && gpx->option.vbs) fprintf(stdout, ": fq %d ", freq); + gpx->freq = freq; + } + + if (calfr == 0x01) { + fw = gpx->frame[pos_CalData+6] | (gpx->frame[pos_CalData+7]<<8); + if (out && gpx->option.vbs) fprintf(stdout, ": fw 0x%04x ", fw); + gpx->conf_fw = fw; + } + + if (calfr == 0x02) { // 0x5E, 0x5A..0x5B + ui8_t bk = gpx->frame[pos_Calburst]; // fw >= 0x4ef5, burst-killtimer in 0x31 relevant + ui16_t kt = gpx->frame[pos_CalData+8] + (gpx->frame[pos_CalData+9] << 8); // killtimer (short?) + if (out && gpx->option.vbs) fprintf(stdout, ": BK %02X ", bk); + if (out && gpx->option.vbs && kt != 0xFFFF ) fprintf(stdout, ": kt %.1fmin ", kt/60.0); + gpx->conf_bk = bk; + gpx->conf_kt = kt; + } + + if (calfr == 0x31) { // 0x59..0x5A + ui16_t bt = gpx->frame[pos_CalData+7] + (gpx->frame[pos_CalData+8] << 8); // burst timer (short?) + // fw >= 0x4ef5: default=[88 77]=0x7788sec=510min + if (out && bt != 0x0000 && + (gpx->option.vbs == 3 || gpx->option.vbs && gpx->conf_bk) + ) fprintf(stdout, ": bt %.1fmin ", bt/60.0); + gpx->conf_bt = bt; + } + + if (calfr == 0x32) { + ui16_t cd = gpx->frame[pos_CalData+1] + (gpx->frame[pos_CalData+2] << 8); // countdown (bt or kt) (short?) + if (out && cd != 0xFFFF && + (gpx->option.vbs == 3 || gpx->option.vbs && (gpx->conf_bk || gpx->conf_kt != 0xFFFF)) + ) fprintf(stdout, ": cd %.1fmin ", cd/60.0); + gpx->conf_cd = cd; + } + + if (calfr == 0x21) { // ... eventuell noch 2 bytes in 0x22 + for (i = 0; i < 9; i++) sondetyp[i] = 0; + for (i = 0; i < 8; i++) { + byte = gpx->frame[pos_CalRSTyp + i]; + if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte; + else if (byte == 0x00) sondetyp[i] = '\0'; + } + if (out && gpx->option.vbs) fprintf(stdout, ": %s ", sondetyp); + strcpy(gpx->rstyp, sondetyp); + } + } + + return 0; +} + +/* ------------------------------------------------------------------------------------ */ + +#define rs_N 255 +#define rs_R 24 +#define rs_K (rs_N-rs_R) + +static int rs41_ecc(gpx_t *gpx, int frmlen) { +// richtige framelen wichtig fuer 0-padding + + int i, leak, ret = 0; + int errors1, errors2; + ui8_t cw1[rs_N], cw2[rs_N]; + ui8_t err_pos1[rs_R], err_pos2[rs_R], + err_val1[rs_R], err_val2[rs_R]; + + memset(cw1, 0, rs_N); + memset(cw2, 0, rs_N); + + if (frmlen > FRAME_LEN) frmlen = FRAME_LEN; + //cfg_rs41.frmlen = frmlen; + //cfg_rs41.msglen = (frmlen-56)/2; // msgpos=56; + leak = frmlen % 2; + + for (i = frmlen; i < FRAME_LEN; i++) gpx->frame[i] = 0; // FRAME_LEN-HDR = 510 = 2*255 + + + for (i = 0; i < rs_R; i++) cw1[i] = gpx->frame[cfg_rs41.parpos+i ]; + for (i = 0; i < rs_R; i++) cw2[i] = gpx->frame[cfg_rs41.parpos+i+rs_R]; + for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; + for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; + + errors1 = rs_decode(&gpx->RS, cw1, err_pos1, err_val1); + errors2 = rs_decode(&gpx->RS, cw2, err_pos2, err_val2); + + + if (gpx->option.ecc == 2 && (errors1 < 0 || errors2 < 0)) + { // 2nd pass + gpx->frame[pos_FRAME] = (pck_FRAME>>8)&0xFF; gpx->frame[pos_FRAME+1] = pck_FRAME&0xFF; + gpx->frame[pos_PTU] = (pck_PTU >>8)&0xFF; gpx->frame[pos_PTU +1] = pck_PTU &0xFF; + gpx->frame[pos_GPS1] = (pck_GPS1 >>8)&0xFF; gpx->frame[pos_GPS1 +1] = pck_GPS1 &0xFF; + gpx->frame[pos_GPS2] = (pck_GPS2 >>8)&0xFF; gpx->frame[pos_GPS2 +1] = pck_GPS2 &0xFF; + gpx->frame[pos_GPS3] = (pck_GPS3 >>8)&0xFF; gpx->frame[pos_GPS3 +1] = pck_GPS3 &0xFF; + // AUX-frames mit vielen Fehlern besser mit 00 auffuellen + // std-O3-AUX-frame: NDATA+7 + if (frametype(gpx) < -2) { // ft >= 0: NDATA_LEN , ft < 0: FRAME_LEN + for (i = NDATA_LEN + 7; i < FRAME_LEN-2; i++) gpx->frame[i] = 0; + } + else { // std-frm (len=320): std_ZERO-frame (7611 00..00 ECC7) + for (i = NDATA_LEN; i < FRAME_LEN; i++) gpx->frame[i] = 0; + gpx->frame[pos_ZEROstd ] = 0x76; // pck_ZEROstd + gpx->frame[pos_ZEROstd+1] = 0x11; // pck_ZEROstd + for (i = pos_ZEROstd+2; i < NDATA_LEN-2; i++) gpx->frame[i] = 0; + gpx->frame[NDATA_LEN-2] = 0xEC; // crc(pck_ZEROstd) + gpx->frame[NDATA_LEN-1] = 0xC7; // crc(pck_ZEROstd) + } + for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; + for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; + errors1 = rs_decode(&gpx->RS, cw1, err_pos1, err_val1); + errors2 = rs_decode(&gpx->RS, cw2, err_pos2, err_val2); + } + + + // Wenn Fehler im 00-padding korrigiert wurden, + // war entweder der frame zu kurz, oder + // Fehler wurden falsch korrigiert; + // allerdings ist bei t=12 die Wahrscheinlichkeit, + // dass falsch korrigiert wurde mit 1/t! sehr gering. + + // check CRC32 + // CRC32 OK: + //for (i = 0; i < cfg_rs41.hdrlen; i++) frame[i] = data[i]; + for (i = 0; i < rs_R; i++) { + gpx->frame[cfg_rs41.parpos+ i] = cw1[i]; + gpx->frame[cfg_rs41.parpos+rs_R+i] = cw2[i]; + } + for (i = 0; i < rs_K; i++) { // cfg_rs41.msglen <= rs_K + gpx->frame[cfg_rs41.msgpos+ 2*i] = cw1[rs_R+i]; + gpx->frame[cfg_rs41.msgpos+1+2*i] = cw2[rs_R+i]; + } + if (leak) { + gpx->frame[cfg_rs41.msgpos+2*i] = cw1[rs_R+i]; + } + + + ret = errors1 + errors2; + if (errors1 < 0 || errors2 < 0) { + ret = 0; + if (errors1 < 0) ret |= 0x1; + if (errors2 < 0) ret |= 0x2; + ret = -ret; + } + + return ret; +} + +/* ------------------------------------------------------------------------------------ */ + + +static int print_position(gpx_t *gpx, int ec) { + int i; + int err, err0, err1, err2, err3; + int output, out_mask; + int encrypted = 0; + + gpx->out = 0; + gpx->aux = 0; + + // Quick check for an encrypted packet (RS41-SGM) + // These sondes have a type 0x80 packet in place of the regular PTU packet. + if (check_CRC(gpx, pos_PTU, pck_ENCRYPTED)==0) { // frame[pos_PTU] == pck_ENCRYPTED>>8 + encrypted = 1; // and CRC-OK + // Continue with the rest of the extraction + } + + err = get_FrameConf(gpx); + + err1 = get_GPS1(gpx); + err2 = get_GPS2(gpx); + err3 = get_GPS3(gpx); + + err0 = get_PTU(gpx); + + out_mask = crc_FRAME|crc_GPS1|crc_GPS3; + output = ((gpx->crc & out_mask) != out_mask); // (!err || !err1 || !err3); + + if (output) { + + gpx->out = 1; // cf. gpx->crc + + if (!err && gpx->option.aut && gpx->option.vbs == 3) fprintf(stdout, "<%c> ", gpx->option.inv?'-':'+'); + + if (!err) { + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, "(%s) ", gpx->id); + } + if (encrypted) { // e.g. 0x80A7-pck + fprintf(stdout, " (RS41-SGM: %02X%02X) ", gpx->frame[pos_PTU], gpx->frame[pos_PTU+1]); + } + if (!err1) { + Gps2Date(gpx); + fprintf(stdout, "%s ", weekday[gpx->wday]); + fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", + gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek); + if (gpx->option.vbs == 3) fprintf(stdout, " (W %d)", gpx->week); + } + if (!err3) { + fprintf(stdout, " "); + fprintf(stdout, " lat: %.5f ", gpx->lat); + fprintf(stdout, " lon: %.5f ", gpx->lon); + fprintf(stdout, " alt: %.2f ", gpx->alt); + //if (gpx->option.vbs) + { + //fprintf(stdout, " (%.1f %.1f %.1f) ", gpx->vN, gpx->vE, gpx->vU); + fprintf(stdout," vH: %4.1f D: %5.1f vV: %3.1f ", gpx->vH, gpx->vD, gpx->vV); + if (gpx->option.vbs == 3) fprintf(stdout," sats: %02d ", gpx->numSV); + } + } + if (gpx->option.ptu && !err0) { + fprintf(stderr, " "); + if (gpx->T > -273.0) fprintf(stderr, " T=%.1fC ", gpx->T); + if (gpx->RH > -0.5) fprintf(stderr, " RH=%.0f%% ", gpx->RH); + } + + + if (gpx->option.crc) { + fprintf(stdout, " # "); + if (gpx->option.ecc && ec >= 0 && (gpx->crc & 0x1F) != 0) { + int pos, blk, len, crc; // unexpected blocks + int flen = NDATA_LEN; + if (frametype(gpx) < 0) flen += XDATA_LEN; + pos = pos_FRAME; + while (pos < flen-1) { // e.g. + blk = gpx->frame[pos]; // 0x80xx: encrypted block + len = gpx->frame[pos+1]; // 0x76xx: 00-padding block + crc = check_CRC(gpx, pos, blk<<8); + fprintf(stdout, " %02X%02X", gpx->frame[pos], gpx->frame[pos+1]); + fprintf(stdout, "[%d]", crc&1); + pos = pos+2+len+2; + } + } + else { + fprintf(stdout, "["); + for (i=0; i<5; i++) fprintf(stdout, "%d", (gpx->crc>>i)&1); + fprintf(stdout, "]"); + } + if (gpx->option.ecc == 2) { + if (ec > 0) fprintf(stdout, " (%d)", ec); + if (ec < 0) { + if (ec == -1) fprintf(stdout, " (-+)"); + else if (ec == -2) fprintf(stdout, " (+-)"); + else /*ec == -3*/ fprintf(stdout, " (--)"); + } + } + } + + get_Calconf(gpx, output); + + if (gpx->option.vbs > 1 || gpx->option.jsn) { + gpx->aux = get_Aux(gpx); + //if (gpx->aux) fprintf(stdout, "\n%d: %s", gpx->aux, gpx->xdata); + } + + fprintf(stdout, "\n"); // fflush(stdout); + + + if (gpx->option.jsn) { + // Print out telemetry data as JSON + if ((!err && !err1 && !err3) || (!err && encrypted)) { // frame-nb/id && gps-time && gps-position (crc-)ok; 3 CRCs, RS not needed + // eigentlich GPS, d.h. UTC = GPS - 18sec (ab 1.1.2017) + fprintf(stdout, "{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f, \"sats\": %d, \"bt\": %d", + gpx->frnr, gpx->id, gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vV, gpx->numSV, gpx->conf_cd ); + if (gpx->option.ptu && !err0 && gpx->T > -273.0) { + fprintf(stdout, ", \"temp\": %.1f", gpx->T ); + } + if (gpx->option.ptu && !err0 && gpx->RH > -0.5) { + fprintf(stdout, ", \"humidity\": %.1f", gpx->RH ); + } + if (gpx->aux) { // <=> gpx->xdata[0]!='\0' + fprintf(stdout, ", \"aux\": \"%s\"", gpx->xdata ); + } + if (encrypted){ + fprintf(stderr, ", \"encrypted\": true"); + } + fprintf(stdout, " }\n"); + fprintf(stdout, "\n"); + } + } + + } + + err |= err1 | err3; + + return err; +} + +static void print_frame(gpx_t *gpx, int len) { + int i, ec = 0, ft; + + gpx->crc = 0; + + // len < NDATA_LEN: EOF + if (len < pos_GPS1) { // else: try prev.frame + for (i = len; i < FRAME_LEN; i++) gpx->frame[i] = 0; + } + + //frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) + //frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) + ft = frametype(gpx); + if (ft >= 0) len = NDATA_LEN; // ft >= 0: NDATA_LEN (default) + else len = FRAME_LEN; // ft < 0: FRAME_LEN (aux) + + + if (gpx->option.ecc) { + ec = rs41_ecc(gpx, len); + } + + + if (gpx->option.raw) { + for (i = 0; i < len; i++) { + fprintf(stdout, "%02x", gpx->frame[i]); + } + if (gpx->option.ecc) { + if (ec >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (gpx->option.ecc == 2) { + if (ec > 0) fprintf(stdout, " (%d)", ec); + if (ec < 0) { + if (ec == -1) fprintf(stdout, " (-+)"); + else if (ec == -2) fprintf(stdout, " (+-)"); + else /*ec == -3*/ fprintf(stdout, " (--)"); + } + } + } + fprintf(stdout, "\n"); + } + else if (gpx->option.sat) { + get_SatData(gpx); + } + else { + print_position(gpx, ec); + } +} + +/* -------------------------------------------------------------------------- */ + + +// header bit buffer +typedef struct { + char *hdr; + char *buf; + char len; + int bufpos; + float ths; +} hdb_t; + +static float cmp_hdb(hdb_t *hdb) { // bit-errors? + int i, j; + int headlen = hdb->len; + int berrs1 = 0, berrs2 = 0; + + i = 0; + j = hdb->bufpos; + while (i < headlen) { + if (j < 0) j = headlen-1; + if (hdb->buf[j] != hdb->hdr[headlen-1-i]) berrs1 += 1; + j--; + i++; + } + + i = 0; + j = hdb->bufpos; + while (i < headlen) { + if (j < 0) j = headlen-1; + if ((hdb->buf[j]^0x01) != hdb->hdr[headlen-1-i]) berrs2 += 1; + j--; + i++; + } + + if (berrs2 < berrs1) return (-headlen+berrs2)/(float)headlen; + else return ( headlen-berrs1)/(float)headlen; + + return 0; +} + +static int find_binhead(FILE *fp, hdb_t *hdb, float *score) { + int bit; + int headlen = hdb->len; + float mv; + + //*score = 0.0; + + while ( (bit = fgetc(fp)) != EOF ) + { + bit &= 1; + + hdb->bufpos = (hdb->bufpos+1) % headlen; + hdb->buf[hdb->bufpos] = 0x30 | bit; // Ascii + + mv = cmp_hdb(hdb); + if ( fabs(mv) > hdb->ths ) { + *score = mv; + return 1; + } + } + + return EOF; +} + + +int main(int argc, char *argv[]) { + + //int option_inv = 0; // invertiert Signal + int option_iq = 0; + int option_ofs = 0; + int option_bin = 0; + int wavloaded = 0; + int sel_wavch = 0; // audio channel: left + int rawhex = 0, xorhex = 0; + + FILE *fp; + char *fpname = NULL; + + int k; + + char bitbuf[8]; + int bitpos = 0, + b8pos = 0, + byte_count = FRAMESTART; + int bit, byte; + int bitQ; + + int header_found = 0; + + float thres = 0.7; // dsp.mv threshold + float _mv = 0.0; + + int symlen = 1; + int bitofs = 2; // +0 .. +3 + int shift = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + gpx_t gpx = {0}; + + hdb_t hdb = {0}; + + +#ifdef CYGWIN + _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) +#endif + setbuf(stdout, NULL); + + + fpname = argv[0]; + ++argv; + while ((*argv) && (!wavloaded)) { + if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { + fprintf(stderr, "%s [options] audio.wav\n", fpname); + fprintf(stderr, " options:\n"); + fprintf(stderr, " -v, -vx, -vv (info, aux, info/conf)\n"); + fprintf(stderr, " -r, --raw\n"); + fprintf(stderr, " -i, --invert\n"); + fprintf(stderr, " --crc (check CRC)\n"); + fprintf(stderr, " --ecc2 (Reed-Solomon 2-pass)\n"); + fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); + fprintf(stderr, " --iq0,2,3 (IQ data)\n"); + return 0; + } + else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { + gpx.option.vbs = 1; + } + else if (strcmp(*argv, "-vx") == 0) { gpx.option.vbs = 2; } + else if (strcmp(*argv, "-vv") == 0) { gpx.option.vbs = 3; } + else if (strcmp(*argv, "-vvv") == 0) { gpx.option.vbs = 4; } + else if (strcmp(*argv, "--crc") == 0) { gpx.option.crc = 1; } + else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { + gpx.option.raw = 1; + } + else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { + gpx.option.inv = 1; + } + else if (strcmp(*argv, "--ecc" ) == 0) { gpx.option.ecc = 1; } + else if (strcmp(*argv, "--ecc2") == 0) { gpx.option.ecc = 2; } + else if (strcmp(*argv, "--sat") == 0) { gpx.option.sat = 1; } + else if (strcmp(*argv, "--ptu") == 0) { gpx.option.ptu = 1; } + else if (strcmp(*argv, "--json") == 0) { + gpx.option.jsn = 1; + gpx.option.ecc = 2; + gpx.option.crc = 1; + } + else if (strcmp(*argv, "--ch2") == 0) { sel_wavch = 1; } // right channel (default: 0=left) + else if ( (strcmp(*argv, "--auto") == 0) ) { gpx.option.aut = 1; } + else if (strcmp(*argv, "--bin") == 0) { option_bin = 1; } // bit/byte binary input + else if (strcmp(*argv, "--ths") == 0) { + ++argv; + if (*argv) { + thres = atof(*argv); + } + else return -1; + } + else if ( (strcmp(*argv, "-d") == 0) ) { + ++argv; + if (*argv) { + shift = atoi(*argv); + if (shift > 4) shift = 4; + if (shift < -4) shift = -4; + } + else return -1; + } + else if (strcmp(*argv, "--iq0") == 0) { option_iq = 1; } // differential/FM-demod + else if (strcmp(*argv, "--iq2") == 0) { option_iq = 2; } + else if (strcmp(*argv, "--iq3") == 0) { option_iq = 3; } // iq2==iq3 + else if (strcmp(*argv, "--ofs") == 0) { option_ofs = 1; } + else if (strcmp(*argv, "--rawhex") == 0) { rawhex = 2; } // raw hex input + else if (strcmp(*argv, "--xorhex") == 0) { rawhex = 2; xorhex = 1; } // raw xor input + else { + fp = fopen(*argv, "rb"); + if (fp == NULL) { + fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv); + return -1; + } + wavloaded = 1; + } + ++argv; + } + if (!wavloaded) fp = stdin; + + + if (gpx.option.ecc < 2) gpx.option.ecc = 1; // turn off for ber-measurement + + if (gpx.option.ecc) { + rs_init_RS255(&gpx.RS); // RS, GF + } + + // init gpx + memcpy(gpx.frame, rs41_header_bytes, sizeof(rs41_header_bytes)); // 8 header bytes + + + if (!rawhex) { + + if (!option_bin) { + + if (option_iq) sel_wavch = 0; + + pcm.sel_ch = sel_wavch; + k = read_wav_header(&pcm, fp); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + + // rs41: BT=0.5, h=0.8,1.0 ? + symlen = 1; + + // init dsp + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = symlen; + dsp._spb = dsp.sps*symlen; + dsp.hdr = rs41_header; + dsp.hdrlen = strlen(rs41_header); + dsp.BT = 0.5; // bw/time (ISI) // 0.3..0.5 + dsp.h = 0.6; //0.7; // 0.7..0.8? modulation index abzgl. BT + dsp.opt_iq = option_iq; + + if ( dsp.sps < 8 ) { + fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); + } + } + else { + // init circular header bit buffer + hdb.hdr = rs41_header; + hdb.len = strlen(rs41_header); + hdb.ths = 1.0 - 3.1/(float)hdb.len; // 1.0-max_bit_errors/hdrlen + hdb.bufpos = -1; + hdb.buf = calloc(hdb.len, sizeof(char)); + if (hdb.buf == NULL) { + fprintf(stderr, "error: malloc\n"); + return -1; + } + } + + + k = init_buffers(&dsp); // BT=0.5 (IQ-Int: BT > 0.5 ?) + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return -1; + }; + + //if (option_iq >= 2) bitofs += 1; // FM: +1 , IQ: +2 + bitofs += shift; + + while ( 1 ) + { + if (option_bin) { + header_found = find_binhead(fp, &hdb, &_mv); + } + else { + header_found = find_header(&dsp, thres, 3, bitofs, 0); + _mv = dsp.mv; + } + if (header_found == EOF) break; + + // mv == correlation score + if (_mv *(0.5-gpx.option.inv) < 0) { + if (gpx.option.aut == 0) header_found = 0; + else gpx.option.inv ^= 0x1; + } + + if (header_found) + { + byte_count = FRAMESTART; + bitpos = 0; // byte_count*8-HEADLEN + b8pos = 0; + + while ( byte_count < FRAME_LEN ) + { + if (option_bin) { + bitQ = fgetc(fp); + if (bitQ != EOF) bit = bitQ & 0x1; + } + else { + if (option_iq >= 2) { + float bl = -1; + if (option_iq > 2) bl = 1.0; + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, bl, 0); + } + else { + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, -1, 0); + } + } + if ( bitQ == EOF ) break; // liest 2x EOF + + if (gpx.option.inv) bit ^= 1; + + bitpos += 1; + bitbuf[b8pos] = bit; + b8pos++; + if (b8pos == BITS) { + b8pos = 0; + byte = bits2byte(bitbuf); + gpx.frame[byte_count] = byte ^ mask[byte_count % MASK_LEN]; + byte_count++; + } + } + + print_frame(&gpx, byte_count); + byte_count = FRAMESTART; + header_found = 0; + } + } + + if (!option_bin) free_buffers(&dsp); + else { + if (hdb.buf) { free(hdb.buf); hdb.buf = NULL; } + } + } + else //if (rawhex) + { + char buffer_rawhex[2*FRAME_LEN+12]; + char *pbuf = NULL, *buf_sp = NULL; + ui8_t frmbyte; + int frameofs = 0, len, i; + + while (1 > 0) { + + pbuf = fgets(buffer_rawhex, 2*FRAME_LEN+12, fp); + if (pbuf == NULL) break; + buffer_rawhex[2*FRAME_LEN] = '\0'; + buf_sp = strchr(buffer_rawhex, ' '); + if (buf_sp != NULL && buf_sp-buffer_rawhex < 2*FRAME_LEN) { + buffer_rawhex[buf_sp-buffer_rawhex] = '\0'; + } + len = strlen(buffer_rawhex) / 2; + if (len > pos_SondeID+10) { + for (i = 0; i < len; i++) { //%2x SCNx8=%hhx(inttypes.h) + sscanf(buffer_rawhex+2*i, "%2hhx", &frmbyte); + // wenn ohne %hhx: sscanf(buffer_rawhex+rawhex*i, "%2x", &byte); frame[frameofs+i] = (ui8_t)byte; + if (xorhex) frmbyte ^= mask[(frameofs+i) % MASK_LEN]; + gpx.frame[frameofs+i] = frmbyte; + } + print_frame(&gpx, frameofs+len); + } + } + } + + + fclose(fp); + + return 0; +} + diff --git a/demod/mod/rs41mod34.c b/demod/mod/rs41mod34.c new file mode 100644 index 0000000..563374c --- /dev/null +++ b/demod/mod/rs41mod34.c @@ -0,0 +1,2142 @@ + +/* + * rs41 + * sync header: correlation/matched filter + * files: rs41mod.c bch_ecc_mod.c bch_ecc_mod.h demod_mod.c demod_mod.h + * compile, either (a) or (b): + * (a) + * gcc -c demod_mod.c + * gcc -DINCLUDESTATIC rs41mod.c demod_mod.o -lm -o rs41mod + * (b) + * gcc -c demod_mod.c + * gcc -c bch_ecc_mod.c + * gcc rs41mod.c demod_mod.o bch_ecc_mod.o -lm -o rs41mod + * + * author: zilog80 + */ + +#include +#include +#include +#include + +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif + +//typedef unsigned char ui8_t; +//typedef unsigned short ui16_t; +//typedef unsigned int ui32_t; +//typedef short i16_t; +//typedef int i32_t; + +#include "demod_mod.h" + +//#define INCLUDESTATIC 1 +#ifdef INCLUDESTATIC + #include "bch_ecc_mod.c" +#else + #include "bch_ecc_mod.h" +#endif + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t aut; + i8_t jsn; // JSON output (auto_rx) + i8_t slt; // silent +} option_t; + +typedef struct { + int typ; + int msglen; + int msgpos; + int parpos; + int hdrlen; + int frmlen; +} rscfg_t; + +static rscfg_t cfg_rs41 = { 41, (320-56)/2, 56, 8, 8, 320}; // const: msgpos, parpos + + +#define NDATA_LEN 320 // std framelen 320 +#define XDATA_LEN 198 +#define FRAME_LEN (NDATA_LEN+XDATA_LEN) // max framelen 518 +/* +ui8_t //xframe[FRAME_LEN] = { 0x10, 0xB6, 0xCA, 0x11, 0x22, 0x96, 0x12, 0xF8}, = xorbyte( frame) + frame[FRAME_LEN] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; // = xorbyte(xframe) +*/ + +typedef struct { + float frm_bytescore[FRAME_LEN+8]; + float ts; + float last_frnb_ts; + float last_calfrm_ts; + ui16_t last_frnb; + ui8_t last_calfrm; + int sort_idx1[FRAME_LEN]; // ui8_t[] sort_cw1_idx + int sort_idx2[FRAME_LEN]; // ui8_t[] sort_cw2_idx +} ecdat_t; + +typedef struct { + int out; + int frnr; + char id[9]; + ui8_t numSV; + int week; int tow_ms; int gpssec; + int jahr; int monat; int tag; + int wday; + int std; int min; float sek; + double lat; double lon; double alt; + double vH; double vD; double vV; + float T; float RH; + ui32_t crc; + ui8_t frame[FRAME_LEN]; + //ui8_t dfrm_shiftsgn[FRAME_LEN]; + ui8_t dfrm_bitscore[FRAME_LEN]; + ui8_t calibytes[51*16]; + ui8_t calfrchk[51]; + float ptu_Rf1; // ref-resistor f1 (750 Ohm) + float ptu_Rf2; // ref-resistor f2 (1100 Ohm) + float ptu_co1[3]; // { -243.911 , 0.187654 , 8.2e-06 } + float ptu_calT1[3]; // calibration T1 + float ptu_co2[3]; // { -243.911 , 0.187654 , 8.2e-06 } + float ptu_calT2[3]; // calibration T2-Hum + float ptu_calH[2]; // calibration Hum + ui32_t freq; // freq/kHz (RS41) + int jsn_freq; // freq/kHz (SDR) + float batt; // battery voltage (V) + ui16_t conf_fw; // firmware + ui16_t conf_kt; // kill timer (sec) + ui16_t conf_bt; // burst timer (sec) + ui16_t conf_cd; // kill countdown (sec) (kt or bt) + ui8_t conf_bk; // burst kill + char rstyp[9]; // RS41-SG, RS41-SGP + int aux; + char xdata[XDATA_LEN+16]; // xdata: aux_str1#aux_str2 ... + option_t option; + RS_t RS; + ecdat_t ecdat; +} gpx_t; + + +#define BITS 8 +#define HEADLEN 64 +#define FRAMESTART ((HEADLEN)/BITS) + +/* 10 B6 CA 11 22 96 12 F8 */ +static char rs41_header[] = "0000100001101101010100111000100001000100011010010100100000011111"; + +static ui8_t rs41_header_bytes[8] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; + +#define MASK_LEN 64 +static ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, + 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, + 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, + 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, + 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, + 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, + 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, + 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; +/* LFSR: ab i=8 (mod 64): + * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] + * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 + * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 + */ +/* + frame[pos] = xframe[pos] ^ mask[pos % MASK_LEN]; +*/ + +/* ------------------------------------------------------------------------------------ */ + +#define BAUD_RATE 4800 + +/* ------------------------------------------------------------------------------------ */ +/* + * Convert GPS Week and Seconds to Modified Julian Day. + * - Adapted from sci.astro FAQ. + * - Ignores UTC leap seconds. + */ +// in : week, gpssec +// out: jahr, monat, tag +static void Gps2Date(gpx_t *gpx) { + long GpsDays, Mjd; + long J, C, Y, M; + + GpsDays = gpx->week * 7 + (gpx->gpssec / 86400); + Mjd = 44244 + GpsDays; + + J = Mjd + 2468570; + C = 4 * J / 146097; + J = J - (146097 * C + 3) / 4; + Y = 4000 * (J + 1) / 1461001; + J = J - 1461 * Y / 4 + 31; + M = 80 * J / 2447; + gpx->tag = J - 2447 * M / 80; + J = M / 11; + gpx->monat = M + 2 - (12 * J); + gpx->jahr = 100 * (C - 49) + Y + J; +} +/* ------------------------------------------------------------------------------------ */ + +static int bits2byte(char bits[]) { + int i, byteval=0, d=1; + for (i = 0; i < 8; i++) { // little endian + /* for (i = 7; i >= 0; i--) { // big endian */ + if (bits[i] == 1) byteval += d; + else if (bits[i] == 0) byteval += 0; + else return 0x100; + d <<= 1; + } + return byteval; +} + +/* ------------------------------------------------------------------------------------ */ + +static ui32_t u4(ui8_t *bytes) { // 32bit unsigned int + ui32_t val = 0; + memcpy(&val, bytes, 4); + return val; +} + +static ui32_t u3(ui8_t *bytes) { // 24bit unsigned int + int val24 = 0; + val24 = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); + // = memcpy(&val, bytes, 3), val &= 0x00FFFFFF; + return val24; +} + +static int i3(ui8_t *bytes) { // 24bit signed int + int val = 0, + val24 = 0; + val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); + val24 = val & 0xFFFFFF; if (val24 & 0x800000) val24 = val24 - 0x1000000; + return val24; +} + +static ui32_t u2(ui8_t *bytes) { // 16bit unsigned int + return bytes[0] | (bytes[1]<<8); +} + +/* +double r8(ui8_t *bytes) { + double val = 0; + memcpy(&val, bytes, 8); + return val; +} + +float r4(ui8_t *bytes) { + float val = 0; + memcpy(&val, bytes, 4); + return val; +} +*/ + +static int crc16(gpx_t *gpx, int start, int len) { + int crc16poly = 0x1021; + int rem = 0xFFFF, i, j; + int byte; + + if (start+len+2 > FRAME_LEN) return -1; + + for (i = 0; i < len; i++) { + byte = gpx->frame[start+i]; + rem = rem ^ (byte << 8); + for (j = 0; j < 8; j++) { + if (rem & 0x8000) { + rem = (rem << 1) ^ crc16poly; + } + else { + rem = (rem << 1); + } + rem &= 0xFFFF; + } + } + return rem; +} + +static int check_CRC(gpx_t *gpx, ui32_t pos, ui32_t pck) { + ui32_t crclen = 0, + crcdat = 0; + // check only pck_type (variable len pcks 0x76, 0x7E) + if (((pck>>8) & 0xFF) != gpx->frame[pos]) return -1; + crclen = gpx->frame[pos+1]; + if (pos + crclen + 4 > FRAME_LEN) return -1; + crcdat = u2(gpx->frame+pos+2+crclen); + if ( crcdat != crc16(gpx, pos+2, crclen) ) { + return 1; // CRC NO + } + else return 0; // CRC OK +} + + +/* +GPS chip: ublox UBX-G6010-ST + + Pos: SubHeader, 1+1 byte (ID+LEN) +0x039: 7928 FrameNumber+SondeID + +(0x050: 0732 CalFrames 0x00..0x32) +0x065: 7A2A PTU +0x093: 7C1E GPS1: RXM-RAW (0x02 0x10) Week, TOW, Sats +0x0B5: 7D59 GPS2: RXM-RAW (0x02 0x10) pseudorange, doppler +0x112: 7B15 GPS3: NAV-SOL (0x01 0x06) ECEF-POS, ECEF-VEL +0x12B: 7611 00 +0x12B: 7Exx AUX-xdata +*/ + +#define crc_FRAME (1<<0) +#define xor_FRAME 0x1713 // ^0x6E3B=0x7928 +#define pck_FRAME 0x7928 +#define pos_FRAME 0x039 +#define pos_FrameNb 0x03B // 2 byte +#define pos_BattVolts 0x045 // 2 byte +#define pos_SondeID 0x03D // 8 byte +#define pos_CalData 0x052 // 1 byte, counter 0x00..0x32 +#define pos_Calfreq 0x055 // 2 byte, calfr 0x00 +#define pos_Calburst 0x05E // 1 byte, calfr 0x02 +// ? #define pos_Caltimer 0x05A // 2 byte, calfr 0x02 ? +#define pos_CalRSTyp 0x05B // 8 byte, calfr 0x21 (+2 byte in 0x22?) + // weitere chars in calfr 0x22/0x23; weitere ID + +#define crc_PTU (1<<1) +#define xor_PTU 0xE388 // ^0x99A2=0x0x7A2A +#define pck_PTU 0x7A2A // PTU +#define pos_PTU 0x065 + +#define crc_GPS1 (1<<2) +#define xor_GPS1 0x9667 // ^0xEA79=0x7C1E +#define pck_GPS1 0x7C1E // RXM-RAW (0x02 0x10) +#define pos_GPS1 0x093 +#define pos_GPSweek 0x095 // 2 byte +#define pos_GPSiTOW 0x097 // 4 byte +#define pos_satsN 0x09B // 12x2 byte (1: SV, 1: quality,strength) + +#define crc_GPS2 (1<<3) +#define xor_GPS2 0xD7AD // ^0xAAF4=0x7D59 +#define pck_GPS2 0x7D59 // RXM-RAW (0x02 0x10) +#define pos_GPS2 0x0B5 +#define pos_minPR 0x0B7 // 4 byte +#define pos_FF 0x0BB // 1 byte +#define pos_dataSats 0x0BC // 12x(4+3) byte (4: pseudorange, 3: doppler) + +#define crc_GPS3 (1<<4) +#define xor_GPS3 0xB9FF // ^0xC2EA=0x7B15 +#define pck_GPS3 0x7B15 // NAV-SOL (0x01 0x06) +#define pos_GPS3 0x112 +#define pos_GPSecefX 0x114 // 4 byte +#define pos_GPSecefY 0x118 // 4 byte +#define pos_GPSecefZ 0x11C // 4 byte +#define pos_GPSecefV 0x120 // 3*2 byte +#define pos_numSats 0x126 // 1 byte +#define pos_sAcc 0x127 // 1 byte +#define pos_pDOP 0x128 // 1 byte + +#define crc_AUX (1<<5) +#define pck_AUX 0x7E00 // LEN variable +#define pos_AUX 0x12B + +#define crc_ZERO (1<<6) // LEN variable +#define pck_ZERO 0x7600 +#define pck_ZEROstd 0x7611 // NDATA std-frm, no aux +#define pos_ZEROstd 0x12B // pos_AUX(0) + +#define pck_SGM_xTU 0x7F1B // temperature/humidity +#define pck_SGM_CRYPT 0x80A7 // Packet type for an Encrypted payload + +/* + frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) + frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) +*/ +static int frametype(gpx_t *gpx) { // -4..+4: 0xF0 -> -4 , 0x0F -> +4 + int i; + ui8_t b = gpx->frame[pos_FRAME-1]; + int ft = 0; + for (i = 0; i < 4; i++) { + ft += ((b>>i)&1) - ((b>>(i+4))&1); + } + return ft; +} + +static int get_FrameNb(gpx_t *gpx, int crc, int ofs) { + int i; + unsigned byte; + ui8_t frnr_bytes[2]; + int frnr; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_FrameNb+ofs + i]; + frnr_bytes[i] = byte; + } + + frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); + gpx->frnr = frnr; + + // crc check + if (crc == 0) { + gpx->ecdat.last_frnb = frnr; + gpx->ecdat.last_frnb_ts = gpx->ecdat.ts; + } + + return 0; +} + +static int get_BattVolts(gpx_t *gpx, int ofs) { + int i; + unsigned byte; + ui8_t batt_bytes[2]; + ui16_t batt_volts; // signed voltage? + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_BattVolts+ofs + i]; + batt_bytes[i] = byte; + } + // 2 bytes? V > 25.5 ? + batt_volts = batt_bytes[0]; // + (batt_bytes[1] << 8); + gpx->batt = batt_volts/10.0; + + return 0; +} + +static int get_SondeID(gpx_t *gpx, int crc, int ofs) { + int i; + unsigned byte; + char sondeid_bytes[9]; + + if (crc == 0) { + for (i = 0; i < 8; i++) { + byte = gpx->frame[pos_SondeID+ofs + i]; + //if ((byte < 0x20) || (byte > 0x7E)) return -1; + sondeid_bytes[i] = byte; + } + sondeid_bytes[8] = '\0'; + if ( strncmp(gpx->id, sondeid_bytes, 8) != 0 ) { + //for (i = 0; i < 51; i++) gpx->calfrchk[i] = 0; + memset(gpx->calfrchk, 0, 51); // 0x00..0x32 + // reset conf data + memset(gpx->rstyp, 0, 9); + gpx->freq = 0; + gpx->conf_fw = 0; + gpx->conf_bt = 0; + gpx->conf_bk = 0; + gpx->conf_cd = -1; + gpx->conf_kt = -1; + // don't reset gpx->frame[] ! + gpx->T = -273.15; + gpx->RH = -1.0; + // new ID: + memcpy(gpx->id, sondeid_bytes, 8); + gpx->id[8] = '\0'; + + gpx->ecdat.last_frnb = 0; + } + } + + return 0; +} + +static int get_FrameConf(gpx_t *gpx, int ofs) { + int crc, err; + ui8_t calfr; + int i; + + crc = check_CRC(gpx, pos_FRAME+ofs, pck_FRAME); + if (crc) gpx->crc |= crc_FRAME; + + err = crc; + err |= get_SondeID(gpx, crc, ofs); + err |= get_FrameNb(gpx, crc, ofs); + err |= get_BattVolts(gpx, ofs); + + if (crc == 0) { + calfr = gpx->frame[pos_CalData+ofs]; + if (gpx->calfrchk[calfr] == 0) // const? + { // 0x32 not constant + for (i = 0; i < 16; i++) { + gpx->calibytes[calfr*16 + i] = gpx->frame[pos_CalData+ofs+1+i]; + } + gpx->calfrchk[calfr] = 1; + } + + gpx->ecdat.last_calfrm = calfr; + gpx->ecdat.last_calfrm_ts = gpx->ecdat.ts; + } + + return err; +} + +static int get_CalData(gpx_t *gpx) { + + memcpy(&(gpx->ptu_Rf1), gpx->calibytes+61, 4); // 0x03*0x10+13 + memcpy(&(gpx->ptu_Rf2), gpx->calibytes+65, 4); // 0x04*0x10+ 1 + + memcpy(gpx->ptu_co1+0, gpx->calibytes+77, 4); // 0x04*0x10+13 + memcpy(gpx->ptu_co1+1, gpx->calibytes+81, 4); // 0x05*0x10+ 1 + memcpy(gpx->ptu_co1+2, gpx->calibytes+85, 4); // 0x05*0x10+ 5 + + memcpy(gpx->ptu_calT1+0, gpx->calibytes+89, 4); // 0x05*0x10+ 9 + memcpy(gpx->ptu_calT1+1, gpx->calibytes+93, 4); // 0x05*0x10+13 + memcpy(gpx->ptu_calT1+2, gpx->calibytes+97, 4); // 0x06*0x10+ 1 + + memcpy(gpx->ptu_calH+0, gpx->calibytes+117, 4); // 0x07*0x10+ 5 + memcpy(gpx->ptu_calH+1, gpx->calibytes+121, 4); // 0x07*0x10+ 9 + + memcpy(gpx->ptu_co2+0, gpx->calibytes+293, 4); // 0x12*0x10+ 5 + memcpy(gpx->ptu_co2+1, gpx->calibytes+297, 4); // 0x12*0x10+ 9 + memcpy(gpx->ptu_co2+2, gpx->calibytes+301, 4); // 0x12*0x10+13 + + memcpy(gpx->ptu_calT2+0, gpx->calibytes+305, 4); // 0x13*0x10+ 1 + memcpy(gpx->ptu_calT2+1, gpx->calibytes+309, 4); // 0x13*0x10+ 5 + memcpy(gpx->ptu_calT2+2, gpx->calibytes+313, 4); // 0x13*0x10+ 9 + + return 0; +} + +// temperature, platinum resistor +// T-sensor: gpx->ptu_co1 , gpx->ptu_calT1 +// T_RH-sensor: gpx->ptu_co2 , gpx->ptu_calT2 +static float get_T(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2, float *ptu_co, float *ptu_calT) { + float *p = ptu_co; + float *c = ptu_calT; + float g = (float)(f2-f1)/(gpx->ptu_Rf2-gpx->ptu_Rf1), // gain + Rb = (f1*gpx->ptu_Rf2-f2*gpx->ptu_Rf1)/(float)(f2-f1), // ofs + Rc = f/g - Rb, + R = Rc * c[0], + T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); + return T; // [Celsius] +} + +// rel.hum., capacitor +// (data:) ftp://ftp-cdc.dwd.de/climate_environment/CDC/observations_germany/radiosondes/ +// (diffAlt: Ellipsoid-Geoid) +static float get_RH(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2, float T) { + float a0 = 7.5; // empirical + float a1 = 350.0/gpx->ptu_calH[0]; // empirical + float fh = (f-f1)/(float)(f2-f1); + float rh = 100.0 * ( a1*fh - a0 ); + float T0 = 0.0, T1 = -25.0, T2 = -40.0; // T/C + rh += T0 - T/5.5; // empir. temperature compensation + if (T < T1) rh *= 1.0 + (T1-T)/90.0; // empir. temperature compensation + if (T < T2) rh *= 1.0 + (T2-T)/100.0; // empir. temperature compensation v0.3 + if (rh < 0.0) rh = 0.0; + if (rh > 100.0) rh = 100.0; + if (T < -273.0) rh = -1.0; + return rh; +} + +static int get_PTU(gpx_t *gpx, int ofs, int pck) { + int err=0, i; + int bR, bc1, bT1, + bc2, bT2; + int bH; + ui32_t meas[12]; + float Tc = -273.15; + float TH = -273.15; + float RH = -1.0; + + get_CalData(gpx); + + err = check_CRC(gpx, pos_PTU+ofs, pck); + if (err) gpx->crc |= crc_PTU; + + if (err == 0) + { + // 0x7A2A: 16 byte (P)TU + // 0x7F1B: 12 byte _TU + for (i = 0; i < 12; i++) { + meas[i] = u3(gpx->frame+pos_PTU+ofs+2+3*i); + } + + bR = gpx->calfrchk[0x03] && gpx->calfrchk[0x04]; + bc1 = gpx->calfrchk[0x04] && gpx->calfrchk[0x05]; + bT1 = gpx->calfrchk[0x05] && gpx->calfrchk[0x06]; + bc2 = gpx->calfrchk[0x12] && gpx->calfrchk[0x13]; + bT2 = gpx->calfrchk[0x13]; + bH = gpx->calfrchk[0x07]; + + if (bR && bc1 && bT1) { + Tc = get_T(gpx, meas[0], meas[1], meas[2], gpx->ptu_co1, gpx->ptu_calT1); + } + gpx->T = Tc; + + if (bR && bc2 && bT2) { + TH = get_T(gpx, meas[6], meas[7], meas[8], gpx->ptu_co2, gpx->ptu_calT2); + } + + if (bH) { + RH = get_RH(gpx, meas[3], meas[4], meas[5], Tc); // TH, TH-Tc (sensorT - T) + } + gpx->RH = RH; + + + if (gpx->option.vbs == 4 && (gpx->crc & (crc_PTU | crc_GPS3))==0) + { + printf(" h: %8.2f # ", gpx->alt); // crc_GPS3 ? + + printf("1: %8d %8d %8d", meas[0], meas[1], meas[2]); + printf(" # "); + printf("2: %8d %8d %8d", meas[3], meas[4], meas[5]); + printf(" # "); + printf("3: %8d %8d %8d", meas[6], meas[7], meas[8]); + printf(" # "); + + //if (Tc > -273.0 && RH > -0.5) + { + printf(" "); + printf(" Tc:%.2f ", Tc); + printf(" RH:%.1f ", RH); + printf(" TH:%.2f ", TH); + } + printf("\n"); + + //if (gpx->alt > -400.0) + { + printf(" %9.2f ; %6.1f ; %6.1f ", gpx->alt, gpx->ptu_Rf1, gpx->ptu_Rf2); + printf("; %10.6f ; %10.6f ; %10.6f ", gpx->ptu_calT1[0], gpx->ptu_calT1[1], gpx->ptu_calT1[2]); + //printf("; %8d ; %8d ; %8d ", meas[0], meas[1], meas[2]); + printf("; %10.6f ; %10.6f ", gpx->ptu_calH[0], gpx->ptu_calH[1]); + //printf("; %8d ; %8d ; %8d ", meas[3], meas[4], meas[5]); + printf("; %10.6f ; %10.6f ; %10.6f ", gpx->ptu_calT2[0], gpx->ptu_calT2[1], gpx->ptu_calT2[2]); + //printf("; %8d ; %8d ; %8d" , meas[6], meas[7], meas[8]); + printf("\n"); + } + } + + } + + return err; +} + + +static int get_GPSweek(gpx_t *gpx, int ofs) { + int i; + unsigned byte; + ui8_t gpsweek_bytes[2]; + int gpsweek; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_GPSweek+ofs + i]; + gpsweek_bytes[i] = byte; + } + + gpsweek = gpsweek_bytes[0] + (gpsweek_bytes[1] << 8); + //if (gpsweek < 0) { gpx->week = -1; return -1; } // (short int) + gpx->week = gpsweek; + + return 0; +} + +//char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; +static char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; + +static int get_GPStime(gpx_t *gpx, int ofs) { + int i; + unsigned byte; + ui8_t gpstime_bytes[4]; + int gpstime = 0, // 32bit + day; + int ms; + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSiTOW+ofs + i]; + gpstime_bytes[i] = byte; + } + + memcpy(&gpstime, gpstime_bytes, 4); + + gpx->tow_ms = gpstime; + ms = gpstime % 1000; + gpstime /= 1000; + gpx->gpssec = gpstime; + + day = (gpstime / (24 * 3600)) % 7; + //if ((day < 0) || (day > 6)) return -1; // besser CRC-check + + gpstime %= (24*3600); + + gpx->wday = day; + gpx->std = gpstime / 3600; + gpx->min = (gpstime % 3600) / 60; + gpx->sek = gpstime % 60 + ms/1000.0; + + return 0; +} + +static int get_GPS1(gpx_t *gpx, int ofs) { + int err=0; + + // gpx->frame[pos_GPS1+1] != (pck_GPS1 & 0xFF) ? + err = check_CRC(gpx, pos_GPS1+ofs, pck_GPS1); + if (err) { + gpx->crc |= crc_GPS1; + // reset GPS1-data (json) + gpx->jahr = 0; gpx->monat = 0; gpx->tag = 0; + gpx->std = 0; gpx->min = 0; gpx->sek = 0.0; + return -1; + } + + err |= get_GPSweek(gpx, ofs); // no plausibility-check + err |= get_GPStime(gpx, ofs); // no plausibility-check + + return err; +} + +static int get_GPS2(gpx_t *gpx, int ofs) { + int err=0; + + // gpx->frame[pos_GPS2+1] != (pck_GPS2 & 0xFF) ? + err = check_CRC(gpx, pos_GPS2+ofs, pck_GPS2); + if (err) gpx->crc |= crc_GPS2; + + return err; +} + +#define EARTH_a 6378137.0 +#define EARTH_b 6356752.31424518 +#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) + +const +double a = EARTH_a, + b = EARTH_b, + a_b = EARTH_a2_b2, + e2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), + ee2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); + +static void ecef2elli(double X[], double *lat, double *lon, double *alt) { + double phi, lam, R, p, t; + + lam = atan2( X[1] , X[0] ); + + p = sqrt( X[0]*X[0] + X[1]*X[1] ); + t = atan2( X[2]*a , p*b ); + + phi = atan2( X[2] + ee2 * b * sin(t)*sin(t)*sin(t) , + p - e2 * a * cos(t)*cos(t)*cos(t) ); + + R = a / sqrt( 1 - e2*sin(phi)*sin(phi) ); + *alt = p / cos(phi) - R; + + *lat = phi*180/M_PI; + *lon = lam*180/M_PI; +} + +static int get_GPSkoord(gpx_t *gpx, int ofs) { + int i, k; + unsigned byte; + ui8_t XYZ_bytes[4]; + int XYZ; // 32bit + double X[3], lat, lon, alt; + ui8_t gpsVel_bytes[2]; + short vel16; // 16bit + double V[3]; + double phi, lam, dir; + double vN; double vE; double vU; + + + for (k = 0; k < 3; k++) { + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSecefX+ofs + 4*k + i]; + XYZ_bytes[i] = byte; + } + memcpy(&XYZ, XYZ_bytes, 4); + X[k] = XYZ / 100.0; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_GPSecefV+ofs + 2*k + i]; + gpsVel_bytes[i] = byte; + } + vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8; + V[k] = vel16 / 100.0; + + } + + + // ECEF-Position + ecef2elli(X, &lat, &lon, &alt); + gpx->lat = lat; + gpx->lon = lon; + gpx->alt = alt; + if ((alt < -1000) || (alt > 80000)) return -3; // plausibility-check: altitude, if ecef=(0,0,0) + + + // ECEF-Velocities + // ECEF-Vel -> NorthEastUp + phi = lat*M_PI/180.0; + lam = lon*M_PI/180.0; + vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); + vE = -V[0]*sin(lam) + V[1]*cos(lam); + vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); + + // NEU -> HorDirVer + gpx->vH = sqrt(vN*vN+vE*vE); +/* + double alpha; + alpha = atan2(gpx->vN, gpx->vE)*180/M_PI; // ComplexPlane (von x-Achse nach links) - GeoMeteo (von y-Achse nach rechts) + dir = 90-alpha; // z=x+iy= -> i*conj(z)=y+ix=re(i(pi/2-t)), Achsen und Drehsinn vertauscht + if (dir < 0) dir += 360; // atan2(y,x)=atan(y/x)=pi/2-atan(x/y) , atan(1/t) = pi/2 - atan(t) + gpx->vD2 = dir; +*/ + dir = atan2(vE, vN) * 180 / M_PI; + if (dir < 0) dir += 360; + gpx->vD = dir; + + gpx->vV = vU; + + gpx->numSV = gpx->frame[pos_numSats+ofs]; + + return 0; +} + +static int get_GPS3(gpx_t *gpx, int ofs) { + int err=0; + + // gpx->frame[pos_GPS3+1] != (pck_GPS3 & 0xFF) ? + err = check_CRC(gpx, pos_GPS3+ofs, pck_GPS3); + if (err) { + gpx->crc |= crc_GPS3; + // reset GPS3-data (json) + gpx->lat = 0.0; gpx->lon = 0.0; gpx->alt = 0.0; + gpx->vH = 0.0; gpx->vD = 0.0; gpx->vV = 0.0; + gpx->numSV = 0; + return -1; + } + + err |= get_GPSkoord(gpx, ofs); // plausibility-check: altitude, if ecef=(0,0,0) + + return err; +} + +static int get_Aux(gpx_t *gpx, int out, int pos) { +// +// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide +// + int auxlen, auxcrc, count7E, pos7E; + int i, n; + + n = 0; + count7E = 0; + pos7E = 0; + //if (pos != pos_AUX) ; + gpx->xdata[0] = '\0'; + + if (frametype(gpx) <= 0) // pos7E == pos7611, 0x7E^0x76=0x08 ... + { + // 7Exx: xdata + while ( pos < FRAME_LEN && gpx->frame[pos] == 0x7E ) { + + auxlen = gpx->frame[pos+1]; + auxcrc = gpx->frame[pos+2+auxlen] | (gpx->frame[pos+2+auxlen+1]<<8); + + if ( auxcrc == crc16(gpx, pos+2, auxlen) ) { + if (count7E == 0) { + if (out) fprintf(stdout, "\n # xdata = "); + } + else { + if (out) fprintf(stdout, " # "); + gpx->xdata[n++] = '#'; // aux separator + } + + //fprintf(stdout, " # %02x : ", gpx->frame[pos7E+2]); + for (i = 1; i < auxlen; i++) { + ui8_t c = gpx->frame[pos+2+i]; // (char) or better < 0x7F + if (c > 0x1E && c < 0x7F) { // ASCII-only + if (out) fprintf(stdout, "%c", c); + gpx->xdata[n++] = c; + } + } + count7E++; + pos7E = pos; + pos += 2+auxlen+2; + } + else { + pos = FRAME_LEN; + gpx->crc |= crc_AUX; + } + } + } + gpx->xdata[n] = '\0'; + + i = check_CRC(gpx, pos, pck_ZERO); // 0x76xx: 00-padding block + if (i) gpx->crc |= crc_ZERO; + + return pos7E; // count7E +} + +static int get_Calconf(gpx_t *gpx, int out, int ofs) { + int i; + unsigned byte; + ui8_t calfr = 0; + ui16_t fw = 0; + int freq = 0, f0 = 0, f1 = 0; + char sondetyp[9]; + int err = 0; + + byte = gpx->frame[pos_CalData+ofs]; + calfr = byte; + err = check_CRC(gpx, pos_FRAME+ofs, pck_FRAME); + + if (out && gpx->option.vbs == 3) { + fprintf(stdout, "\n"); // fflush(stdout); + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, " 0x%02x: ", calfr); + for (i = 0; i < 16; i++) { + byte = gpx->frame[pos_CalData+ofs+1+i]; + fprintf(stdout, "%02x ", byte); + } + /* + if (err == 0) fprintf(stdout, "[OK]"); + else fprintf(stdout, "[NO]"); + */ + fprintf(stdout, " "); + } + + if (err == 0) + { + if (calfr == 0x00) { + byte = gpx->frame[pos_Calfreq+ofs] & 0xC0; // erstmal nur oberste beiden bits + f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 + byte = gpx->frame[pos_Calfreq+ofs+1]; + f1 = 40 * byte; + freq = 400000 + f1+f0; // kHz; + if (out && gpx->option.vbs) fprintf(stdout, ": fq %d ", freq); + gpx->freq = freq; + } + + if (calfr == 0x01) { + fw = gpx->frame[pos_CalData+ofs+6] | (gpx->frame[pos_CalData+ofs+7]<<8); + if (out && gpx->option.vbs) fprintf(stdout, ": fw 0x%04x ", fw); + gpx->conf_fw = fw; + } + + if (calfr == 0x02) { // 0x5E, 0x5A..0x5B + ui8_t bk = gpx->frame[pos_Calburst+ofs]; // fw >= 0x4ef5, burst-killtimer in 0x31 relevant + ui16_t kt = gpx->frame[pos_CalData+ofs+8] + (gpx->frame[pos_CalData+ofs+9] << 8); // killtimer (short?) + if (out && gpx->option.vbs) fprintf(stdout, ": BK %02X ", bk); + if (out && gpx->option.vbs && kt != 0xFFFF ) fprintf(stdout, ": kt %.1fmin ", kt/60.0); + gpx->conf_bk = bk; + gpx->conf_kt = kt; + } + + if (calfr == 0x31) { // 0x59..0x5A + ui16_t bt = gpx->frame[pos_CalData+ofs+7] + (gpx->frame[pos_CalData+ofs+8] << 8); // burst timer (short?) + // fw >= 0x4ef5: default=[88 77]=0x7788sec=510min + if (out && bt != 0x0000 && + (gpx->option.vbs == 3 || gpx->option.vbs && gpx->conf_bk) + ) fprintf(stdout, ": bt %.1fmin ", bt/60.0); + gpx->conf_bt = bt; + } + + if (calfr == 0x32) { + ui16_t cd = gpx->frame[pos_CalData+ofs+1] + (gpx->frame[pos_CalData+ofs+2] << 8); // countdown (bt or kt) (short?) + if (out && cd != 0xFFFF && + (gpx->option.vbs == 3 || gpx->option.vbs && (gpx->conf_bk || gpx->conf_kt != 0xFFFF)) + ) fprintf(stdout, ": cd %.1fmin ", cd/60.0); + gpx->conf_cd = cd; // (short/i16_t) ? + } + + if (calfr == 0x21) { // ... eventuell noch 2 bytes in 0x22 + for (i = 0; i < 9; i++) sondetyp[i] = 0; + for (i = 0; i < 8; i++) { + byte = gpx->frame[pos_CalRSTyp+ofs + i]; + if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte; + else if (byte == 0x00) sondetyp[i] = '\0'; + } + if (out && gpx->option.vbs) fprintf(stdout, ": %s ", sondetyp); + strcpy(gpx->rstyp, sondetyp); + if (out && gpx->option.vbs == 3) { // Stationsdruck QFE + float qfe1 = 0.0, qfe2 = 0.0; + memcpy(&qfe1, gpx->frame+pos_CalData+1, 4); + memcpy(&qfe2, gpx->frame+pos_CalData+5, 4); + if (qfe1 > 0.0 || qfe2 > 0.0) { + fprintf(stdout, " "); + if (qfe1 > 0.0) fprintf(stdout, "QFE1:%.1fhPa ", qfe1); + if (qfe2 > 0.0) fprintf(stdout, "QFE2:%.1fhPa ", qfe2); + } + } + } + } + + return 0; +} + +/* ------------------------------------------------------------------------------------ */ + +static int set_bytes(gpx_t *gpx, int pos, ui8_t *src, int len, int subcw, int *pset) { + int rem = 0; // cw1: rem=0 , cw2: rem=1 ( pos >= msgpos) + int i; + if (subcw == 2) rem = 1; else rem = 0; + for (i = 0; i < len; i++) { + if ( (pos+i) % 2 == rem) { + gpx->frame[pos+i] = src[i]; + *pset = pos+i; + pset++; + } + } + return 0; +} + +#define N_idx_fixed 5 +static int idx_fixed[N_idx_fixed] = { pos_FRAME, pos_PTU, pos_GPS1, pos_GPS2, pos_GPS3 }; + +static int inFixed(gpx_t *gpx, int idx, int *frmset, int setcnt) { + int j; + for (j = 0; j < N_idx_fixed; j++) { + if (idx == idx_fixed[j] || idx == idx_fixed[j]+1) return 1; + } + if (frametype(gpx) >= -2) { + if (idx >= pos_ZEROstd && idx < NDATA_LEN) return 1; + } + for (j = 0; j < setcnt; j++) { + if (idx == frmset[j]) return 1; + } + return 0; +} + +#define rs_N 255 +#define rs_R 24 +#define rs_K (rs_N-rs_R) + +static int rs41_ecc(gpx_t *gpx, int frmlen) { +// richtige framelen wichtig fuer 0-padding + + int i, j, k, leak, ret = 0; + int errors1, errors2; + ui8_t cw1[rs_N], cw2[rs_N]; + ui8_t err_pos1[rs_R], err_pos2[rs_R], + err_val1[rs_R], err_val2[rs_R]; + ui8_t era_pos[rs_R]; + ui8_t Era_max = 12; // iteration depth 2..255 (2 erasures for 1 error) + + int frmset[FRAME_LEN]; + int setcnt = 0; + + + memset(cw1, 0, rs_N); + memset(cw2, 0, rs_N); + + if (frmlen > FRAME_LEN) frmlen = FRAME_LEN; + //cfg_rs41.frmlen = frmlen; + //cfg_rs41.msglen = (frmlen-56)/2; // msgpos=56; + leak = frmlen % 2; + + for (i = frmlen; i < FRAME_LEN; i++) gpx->frame[i] = 0; // FRAME_LEN-HDR = 510 = 2*255 + + + for (i = 0; i < rs_R; i++) cw1[i] = gpx->frame[cfg_rs41.parpos+i ]; + for (i = 0; i < rs_R; i++) cw2[i] = gpx->frame[cfg_rs41.parpos+i+rs_R]; + for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; + for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; + + errors1 = rs_decode(&gpx->RS, cw1, err_pos1, err_val1); + errors2 = rs_decode(&gpx->RS, cw2, err_pos2, err_val2); + + + if (gpx->option.ecc >= 2 && (errors1 < 0 || errors2 < 0)) + { // 2nd pass: set packet-IDs + gpx->frame[pos_FRAME] = (pck_FRAME>>8)&0xFF; gpx->frame[pos_FRAME+1] = pck_FRAME&0xFF; + gpx->frame[pos_PTU] = (pck_PTU >>8)&0xFF; gpx->frame[pos_PTU +1] = pck_PTU &0xFF; + gpx->frame[pos_GPS1] = (pck_GPS1 >>8)&0xFF; gpx->frame[pos_GPS1 +1] = pck_GPS1 &0xFF; + gpx->frame[pos_GPS2] = (pck_GPS2 >>8)&0xFF; gpx->frame[pos_GPS2 +1] = pck_GPS2 &0xFF; + gpx->frame[pos_GPS3] = (pck_GPS3 >>8)&0xFF; gpx->frame[pos_GPS3 +1] = pck_GPS3 &0xFF; + // AUX-frames mit vielen Fehlern besser mit 00 auffuellen + // std-O3-AUX-frame: NDATA+7 + if (frametype(gpx) < -2) { // ft >= 0: NDATA_LEN , ft < 0: FRAME_LEN + for (i = NDATA_LEN + 7; i < FRAME_LEN-2; i++) gpx->frame[i] = 0; + } + else { // std-frm (len=320): std_ZERO-frame (7611 00..00 ECC7) + for (i = NDATA_LEN; i < FRAME_LEN; i++) gpx->frame[i] = 0; + gpx->frame[pos_ZEROstd ] = 0x76; // pck_ZEROstd + gpx->frame[pos_ZEROstd+1] = 0x11; // pck_ZEROstd + for (i = pos_ZEROstd+2; i < NDATA_LEN-2; i++) gpx->frame[i] = 0; + gpx->frame[NDATA_LEN-2] = 0xEC; // crc(pck_ZEROstd) + gpx->frame[NDATA_LEN-1] = 0xC7; // crc(pck_ZEROstd) + } + for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; + for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; + errors1 = rs_decode(&gpx->RS, cw1, err_pos1, err_val1); + errors2 = rs_decode(&gpx->RS, cw2, err_pos2, err_val2); + } + + if (gpx->option.ecc == 4) // set (probably) known bytes (if same rs41) + { + int crc = 0; + float frnb_ts = gpx->ecdat.ts - gpx->ecdat.last_frnb_ts + 0.5f; + int frnb = gpx->ecdat.last_frnb + (unsigned)frnb_ts; + float calfr_ts = gpx->ecdat.ts - gpx->ecdat.last_calfrm_ts + 0.5f; + int calfr = (gpx->ecdat.last_calfrm + (unsigned)calfr_ts) % 51; + + if (errors1 < 0) { + // chkCRC + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + if (crc) + { + if ( gpx->id[0] && strncmp(gpx->frame+pos_SondeID, gpx->id, 8) != 0 ) + { // raw: gpx->id[0]==0 + // check gpx->frame+pos_SondeID[1..7] in 0x30..0x39 + set_bytes(gpx, pos_SondeID, gpx->id, 8, 1, frmset+setcnt); + setcnt += 8/2; + } + + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + if (crc && gpx->calfrchk[calfr]) + { + // pos_CalData: 0x052 + // gpx->frame[pos_CalData] == calfr ? + if (gpx->frame[pos_CalData] != calfr) { + } + else { // probably same SondeID + //gpx->frame[pos_CalData] = calfr; + set_bytes(gpx, pos_CalData+1, gpx->calibytes+calfr*16, 16, 1, frmset+setcnt); + setcnt += 16/2; + } + } + + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + //pos_FrameNb: 0x03B=59 + if (crc && ((frnb>>8)&0xFF) != gpx->frame[pos_FrameNb+1]) { + // last valid check, last_frnb>0 ... + if (gpx->ecdat.last_frnb > 0) { + gpx->frame[pos_FrameNb+1] = (frnb>>8)&0xFF; + frmset[setcnt++] = pos_FrameNb+1; + } + } + } + + for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; + errors1 = rs_decode(&gpx->RS, cw1, err_pos1, err_val1); + } + if (errors2 < 0) { + // chkCRC + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + if (crc) + { // check gpx->frame+pos_SondeID[1..7] in 0x30..0x39 + if ( gpx->id[0] && strncmp(gpx->frame+pos_SondeID, gpx->id, 8) != 0 ) + { + set_bytes(gpx, pos_SondeID, gpx->id, 8, 2, frmset+setcnt); + setcnt += 8/2; + } + + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + if (crc && gpx->calfrchk[calfr]) { + if (gpx->frame[pos_CalData] == calfr) { + set_bytes(gpx, pos_CalData+1, gpx->calibytes+calfr*16, 16, 2, frmset+setcnt); + setcnt += 16/2; + } + } + + crc = check_CRC(gpx, pos_FRAME, pck_FRAME); + //pos_FrameNb: 0x03B=59 + if (crc && (frnb&0xFF) != gpx->frame[pos_FrameNb]) { + // last valid check, last_frnb>0 ... + if (gpx->ecdat.last_frnb > 0) { + gpx->frame[pos_FrameNb] = frnb&0xFF; + frmset[setcnt++] = pos_FrameNb; + } + } + } + + for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; + errors2 = rs_decode(&gpx->RS, cw2, err_pos2, err_val2); + } + } + + + // 3rd pass: + // 2 RS codewords interleaved: 2x12 errors can be corrected; + // CRC is good for list decoding, high rate is not; + // burst errors could affect neighboring bytes, however + // if AWGN and 24 bit-errors per frame, probability for 2 bit-errors in 1 byte is low; + // low byte-score -> erasure , low bit-score -> bit-toggle: + // - erasures: 11 + 2/2 = 12 (11 errors and 2 erasures per codeword can be corrected) + // 11 + 2 = 13: try combinations of 2 erasures with low byte-scores + // - toggle low-score bits + + if (gpx->option.ecc > 2) + { + int pos_cw = 0; + int pos_frm = 0; + + if (errors1 < 0) + { + for (i = 1; i < Era_max; i++) { + pos_frm = gpx->ecdat.sort_idx1[i]; + if (inFixed(gpx, pos_frm, frmset, setcnt)) continue; + if (pos_frm < cfg_rs41.msgpos) pos_cw = pos_frm - cfg_rs41.parpos; + else pos_cw = rs_R + (pos_frm - cfg_rs41.msgpos)/2; + if (pos_cw < 0 || pos_cw > 254) continue; + era_pos[0] = pos_cw; + for (j = 0; j < i; j++) { + pos_frm = gpx->ecdat.sort_idx1[j]; + if (inFixed(gpx, pos_frm, frmset, setcnt)) continue; + if (pos_frm < cfg_rs41.msgpos) pos_cw = pos_frm - cfg_rs41.parpos; + else pos_cw = rs_R + (pos_frm - cfg_rs41.msgpos)/2; + if (pos_cw < 0 || pos_cw > 254) continue; + era_pos[1] = pos_cw; + + //k = -1; + for (k = -1; k < j; k++) // toggle low-score bits + { + if (k >= 0) { + pos_frm = gpx->ecdat.sort_idx1[k]; + if (inFixed(gpx, pos_frm, frmset, setcnt)) continue; + else { + if (pos_frm < cfg_rs41.msgpos) pos_cw = pos_frm - cfg_rs41.parpos; + else pos_cw = rs_R + (pos_frm - cfg_rs41.msgpos)/2; + if (pos_cw < 0 || pos_cw > 254) continue; + cw1[pos_cw] ^= gpx->dfrm_bitscore[pos_frm]; + } + } + + errors1 = rs_decode_ErrEra(&gpx->RS, cw1, 2, era_pos, err_pos1, err_val1); + if (errors1 >= 0) { j = 256; i = 256; k = 256; } //break; + //else if (k >= 0) { cw1[pos_cw] ^= gpx->dfrm_bitscore[pos_frm]; } + } + } + } + } + + if (errors2 < 0) + { + for (i = 1; i < Era_max; i++) { + pos_frm = gpx->ecdat.sort_idx2[i]; + if (inFixed(gpx, pos_frm, frmset, setcnt)) continue; + if (pos_frm < cfg_rs41.msgpos) pos_cw = pos_frm - cfg_rs41.parpos - rs_R; + else pos_cw = rs_R + (pos_frm - cfg_rs41.msgpos)/2; + if (pos_cw < 0 || pos_cw > 254) continue; + era_pos[0] = pos_cw; + for (j = 0; j < i; j++) { + pos_frm = gpx->ecdat.sort_idx2[j]; + if (inFixed(gpx, pos_frm, frmset, setcnt)) continue; + if (pos_frm < cfg_rs41.msgpos) pos_cw = pos_frm - cfg_rs41.parpos - rs_R; + else pos_cw = rs_R + (pos_frm - cfg_rs41.msgpos)/2; + if (pos_cw < 0 || pos_cw > 254) continue; + era_pos[1] = pos_cw; + + //k = -1; + for (k = -1; k < j; k++) // toggle low-score bits + { + if (k >= 0) { + pos_frm = gpx->ecdat.sort_idx2[k]; + if (inFixed(gpx, pos_frm, frmset, setcnt)) continue; + else { + if (pos_frm < cfg_rs41.msgpos) pos_cw = pos_frm - cfg_rs41.parpos - rs_R; + else pos_cw = rs_R + (pos_frm - cfg_rs41.msgpos)/2; + if (pos_cw < 0 || pos_cw > 254) continue; + cw2[pos_cw] ^= gpx->dfrm_bitscore[pos_frm]; + } + } + + errors2 = rs_decode_ErrEra(&gpx->RS, cw2, 2, era_pos, err_pos2, err_val2); + if (errors2 >= 0) { j = 256; i = 256; k = 256; } //break; + //else if (k >= 0) { cw2[pos_cw] ^= gpx->dfrm_bitscore[pos_frm]; } + } + } + } + } + } + + + // Wenn Fehler im 00-padding korrigiert wurden, + // war entweder der frame zu kurz, oder + // Fehler wurden falsch korrigiert; + // allerdings ist bei t=12 die Wahrscheinlichkeit, + // dass falsch korrigiert wurde mit 1/t! sehr gering. + + // check CRC32 + // CRC32 OK: + //for (i = 0; i < cfg_rs41.hdrlen; i++) frame[i] = data[i]; + for (i = 0; i < rs_R; i++) { + gpx->frame[cfg_rs41.parpos+ i] = cw1[i]; + gpx->frame[cfg_rs41.parpos+rs_R+i] = cw2[i]; + } + for (i = 0; i < rs_K; i++) { // cfg_rs41.msglen <= rs_K + gpx->frame[cfg_rs41.msgpos+ 2*i] = cw1[rs_R+i]; + gpx->frame[cfg_rs41.msgpos+1+2*i] = cw2[rs_R+i]; + } + if (leak) { + gpx->frame[cfg_rs41.msgpos+2*i] = cw1[rs_R+i]; + } + + + ret = errors1 + errors2; + if (errors1 < 0 || errors2 < 0) { + ret = 0; + if (errors1 < 0) ret |= 0x1; + if (errors2 < 0) ret |= 0x2; + ret = -ret; + } + + return ret; +} + +/* ------------------------------------------------------------------------------------ */ + +static int prn_frm(gpx_t *gpx) { + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, "(%s) ", gpx->id); + if (gpx->option.vbs == 3) fprintf(stdout, "(%.1f V) ", gpx->batt); + fprintf(stdout, " "); + return 0; +} + +static int prn_ptu(gpx_t *gpx) { + fprintf(stdout, " "); + if (gpx->T > -273.0) fprintf(stdout, " T=%.1fC ", gpx->T); + if (gpx->RH > -0.5) fprintf(stdout, " RH=%.0f%% ", gpx->RH); + return 0; +} + +static int prn_gpstime(gpx_t *gpx) { + Gps2Date(gpx); + fprintf(stdout, "%s ", weekday[gpx->wday]); + fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", + gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek); + if (gpx->option.vbs == 3) fprintf(stdout, " (W %d)", gpx->week); + fprintf(stdout, " "); + return 0; +} + +static int prn_gpspos(gpx_t *gpx) { + //fprintf(stdout, " "); + fprintf(stdout, " lat: %.5f ", gpx->lat); + fprintf(stdout, " lon: %.5f ", gpx->lon); + fprintf(stdout, " alt: %.2f ", gpx->alt); + fprintf(stdout, " vH: %4.1f D: %5.1f vV: %3.1f ", gpx->vH, gpx->vD, gpx->vV); + if (gpx->option.vbs == 3) fprintf(stdout, " sats: %02d ", gpx->numSV); + return 0; +} + +static int prn_sat1(gpx_t *gpx, int ofs) { + + fprintf(stdout, "\n"); + + fprintf(stdout, "iTOW: 0x%08X", u4(gpx->frame+pos_GPSiTOW+ofs)); + fprintf(stdout, " week: 0x%04X", u2(gpx->frame+pos_GPSweek+ofs)); + + return 0; +} +const double c = 299.792458e6; +const double L1 = 1575.42e6; +static int prn_sat2(gpx_t *gpx, int ofs) { + int i, n; + int sv; + ui32_t minPR; + + fprintf(stdout, "\n"); + + minPR = u4(gpx->frame+pos_minPR+ofs); + fprintf(stdout, "minPR: %d", minPR); + fprintf(stdout, "\n"); + + for (i = 0; i < 12; i++) { + n = i*7; + sv = gpx->frame[pos_satsN+ofs+2*i]; + if (sv == 0xFF) break; + fprintf(stdout, " SV: %2d ", sv); + //fprintf(stdout, " (%02x) ", gpx->frame[pos_satsN+2*i+1]); + fprintf(stdout, "# "); + fprintf(stdout, "prMes: %.1f", u4(gpx->frame+pos_dataSats+ofs+n)/100.0 + minPR); + fprintf(stdout, " "); + fprintf(stdout, "doMes: %.1f", -i3(gpx->frame+pos_dataSats+ofs+n+4)/100.0*L1/c); + fprintf(stdout, "\n"); + } + + return 0; +} +static int prn_sat3(gpx_t *gpx, int ofs) { + int numSV; + double pDOP, sAcc; + + fprintf(stdout, "\n"); + + fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", + (i32_t)u4(gpx->frame+pos_GPSecefX+ofs), + (i32_t)u4(gpx->frame+pos_GPSecefY+ofs), + (i32_t)u4(gpx->frame+pos_GPSecefZ+ofs)); + fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", + (i16_t)u2(gpx->frame+pos_GPSecefV+ofs+0), + (i16_t)u2(gpx->frame+pos_GPSecefV+ofs+2), + (i16_t)u2(gpx->frame+pos_GPSecefV+ofs+4)); + + numSV = gpx->frame[pos_numSats+ofs]; + sAcc = gpx->frame[pos_sAcc+ofs]/10.0; if (gpx->frame[pos_sAcc+ofs] == 0xFF) sAcc = -1.0; + pDOP = gpx->frame[pos_pDOP+ofs]/10.0; if (gpx->frame[pos_pDOP+ofs] == 0xFF) pDOP = -1.0; + fprintf(stdout, "numSatsFix: %2d sAcc: %.1f pDOP: %.1f\n", numSV, sAcc, pDOP); + + /* + fprintf(stdout, "CRC: "); + fprintf(stdout, " %04X", pck_GPS1); + if (check_CRC(gpx, pos_GPS1+ofs, pck_GPS1)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS1, pck_GPS1)); + fprintf(stdout, " %04X", pck_GPS2); + if (check_CRC(gpx, pos_GPS2+ofs, pck_GPS2)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS2, pck_GPS2)); + fprintf(stdout, " %04X", pck_GPS3); + if (check_CRC(gpx, pos_GPS3+ofs, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS3, pck_GPS3)); + + fprintf(stdout, "\n"); + */ + return 0; +} + +static int print_position(gpx_t *gpx, int ec) { + int i; + int err, err0, err1, err2, err3; + //int output, out_mask; + int encrypted = 0; + int unexp = 0; + int out = 1; + int sat = 0; + int pos_aux = 0, cnt_aux = 0; + + //gpx->out = 0; + gpx->aux = 0; + + if (gpx->option.sat) sat = 1; + if (gpx->option.slt) out = 0; else out = 1; + + if ( ec >= 0 ) + { + int pos, blk, len, crc, pck; + int flen = NDATA_LEN; + + int ofs_cal = 0; + int frm_end = NDATA_LEN-2; + + if (frametype(gpx) < 0) flen += XDATA_LEN; + + switch (gpx->frame[pos_PTU]) { + case 0x7A: // 0x7A2A + frm_end = flen-2; + break; + case 0x7F: // 0x7F1B + frm_end = pos_ZEROstd + 0x1B-0x2A - 2; + break; + case 0x80: // 0x80A7 + frm_end = pos_PTU + 2 + 0xA7; + break; + } + + pos = pos_FRAME; + gpx->crc = 0; + + while (pos < flen-1) { + blk = gpx->frame[pos]; + len = gpx->frame[pos+1]; + crc = check_CRC(gpx, pos, blk<<8); + pck = (blk<<8) | len; + + if ( crc == 0 ) // ecc-OK -> crc-OK + { + int ofs = 0; + switch (pck) + { + case pck_FRAME: // 0x7928 + ofs = pos - pos_FRAME; + ofs_cal = ofs; + err = get_FrameConf(gpx, ofs); + if ( !err ) { + if (out || sat) prn_frm(gpx); + } + break; + + case pck_PTU: // 0x7A2A + ofs = pos - pos_PTU; + err0 = get_PTU(gpx, ofs, pck_PTU); + if ( 0 && !err0 && gpx->option.ptu ) { + prn_ptu(gpx); + } + break; + + case pck_GPS1: // 0x7C1E + ofs = pos - pos_GPS1; + err1 = get_GPS1(gpx, ofs); + if ( !err1 ) { + if (out) prn_gpstime(gpx); + if (sat) prn_sat1(gpx, ofs); + } + break; + + case pck_GPS2: // 0x7D59 + ofs = pos - pos_GPS2; + err2 = get_GPS2(gpx, ofs); + if ( !err2 ) { + if (sat) prn_sat2(gpx, ofs); + } + break; + + case pck_GPS3: // 0x7B15 + ofs = pos - pos_GPS3; + err3 = get_GPS3(gpx, ofs); + if ( !err3 ) { + if (out) prn_gpspos(gpx); + if (sat) prn_sat3(gpx, ofs); + } + break; + + case pck_SGM_xTU: // 0x7F1B + ofs = pos - pos_PTU; + err0 = get_PTU(gpx, ofs, pck); + break; + + case pck_SGM_CRYPT: // 0x80A7 + encrypted = 1; + if (out) fprintf(stdout, " [%04X] (RS41-SGM) ", pck_SGM_CRYPT); + break; + + default: + if (blk == 0x7E) { + if (pos_aux == 0) pos_aux = pos; // pos == pos_AUX ? + cnt_aux += 1; + } + if (blk == 0x76) { + // ZERO-Padding pck + } + + if (blk != 0x76 && blk != 0x7E) { + if (out) fprintf(stdout, " [%04X] ", pck); + unexp = 1; + } + } + } + else { // CRC-ERROR (ECC-OK) + fprintf(stdout, " [ERROR]\n"); + break; + } + + pos += 2+len+2; // next pck + + if ( pos > frm_end ) // end of (sub)frame + { + if (gpx->option.ptu && out && !sat && !err0 && !encrypted) { + prn_ptu(gpx); + } + + get_Calconf(gpx, out, ofs_cal); + + if (out && ec > 0 && pos > flen-1) fprintf(stdout, " (%d)", ec); + + if (pos_aux) gpx->aux = get_Aux(gpx, out && gpx->option.vbs > 1, pos_aux); + + gpx->crc = 0; + frm_end = FRAME_LEN-2; + + + if (out || sat) fprintf(stdout, "\n"); + + + if (gpx->option.jsn) { + // Print out telemetry data as JSON + if ((!err && !err1 && !err3) || (!err && encrypted)) { // frame-nb/id && gps-time && gps-position (crc-)ok; 3 CRCs, RS not needed + // eigentlich GPS, d.h. UTC = GPS - 18sec (ab 1.1.2017) + fprintf(stdout, "{ \"type\": \"%s\"", "RS41"); + fprintf(stdout, ", \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f, \"sats\": %d, \"bt\": %d, \"batt\": %.2f", + gpx->frnr, gpx->id, gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vV, gpx->numSV, gpx->conf_cd, gpx->batt ); + if (gpx->option.ptu && !err0 && gpx->T > -273.0) { + fprintf(stdout, ", \"temp\": %.1f", gpx->T ); + } + if (gpx->option.ptu && !err0 && gpx->RH > -0.5) { + fprintf(stdout, ", \"humidity\": %.1f", gpx->RH ); + } + if (gpx->aux) { // <=> gpx->xdata[0]!='\0' + fprintf(stdout, ", \"aux\": \"%s\"", gpx->xdata ); + } + if (encrypted) { + fprintf(stdout, ", \"subtype\": \"RS41-SGM\", \"encrypted\": true"); + } else { + fprintf(stdout, ", \"subtype\": \"%s\"", *gpx->rstyp ? gpx->rstyp : "RS41" ); // RS41-SG(P/M) + if (strncmp(gpx->rstyp, "RS41-SGM", 8) == 0) { + fprintf(stdout, ", \"encrypted\": false"); + } + } + if (gpx->jsn_freq > 0) { // rs41-frequency: gpx->freq + int fq_kHz = gpx->jsn_freq; + if (gpx->freq > 0) fq_kHz = gpx->freq; + fprintf(stdout, ", \"freq\": %d", fq_kHz); + } + fprintf(stdout, " }\n"); + fprintf(stdout, "\n"); + } + } + } + } + } + // else + if (ec < 0 && (out || sat /*|| gpx->option.jsn*/)) { + // + // crc-OK pcks ? + // + int pck, ofs; + int output = 0, out_mask; + + gpx->crc = 0; + out_mask = crc_FRAME|crc_GPS1|crc_GPS3; + if (gpx->option.ptu) out_mask |= crc_PTU; + + err = get_FrameConf(gpx, 0); + if (out && !err) prn_frm(gpx); + + pck = (gpx->frame[pos_PTU]<<8) | gpx->frame[pos_PTU+1]; + ofs = 0; + + if (pck < 0x8000) { + err0 = get_PTU(gpx, 0, pck); + if (pck == pck_PTU) ofs = 0; + else if (pck == pck_SGM_xTU) ofs = 0x1B-0x2A; + + err1 = get_GPS1(gpx, ofs); + err2 = get_GPS2(gpx, ofs); + err3 = get_GPS3(gpx, ofs); + + if (out) { + + if (!err1) prn_gpstime(gpx); + if (!err3) prn_gpspos(gpx); + if (!err0 && gpx->option.ptu) prn_ptu(gpx); + if (0 && !err) get_Calconf(gpx, out, 0); // only if ecc-OK + + output = ((gpx->crc & out_mask) != out_mask); + + if (output) { + fprintf(stdout, " "); + fprintf(stdout, "["); + for (i=0; i<5; i++) fprintf(stdout, "%d", (gpx->crc>>i)&1); + fprintf(stdout, "]"); + } + } + } + else if (pck == pck_SGM_CRYPT) { + if (out && !err) { + fprintf(stdout, " [%04X] (RS41-SGM) ", pck_SGM_CRYPT); + //fprintf(stdout, "[%d] ", check_CRC(gpx, pos_PTU, pck_SGM_CRYPT)); + output = 1; + } + } + + if (out && output) + { + if (ec == -1) fprintf(stdout, " (-+)"); + else if (ec == -2) fprintf(stdout, " (+-)"); + else /*ec == -3*/ fprintf(stdout, " (--)"); + + fprintf(stdout, "\n"); // fflush(stdout); + } + } + + + return 0; +} + +static void print_frame(gpx_t *gpx, int len) { + int i, j, ec = 0, ft; + int j1 = 0; + int j2 = 0; + int sort_score_idx[FRAME_LEN]; + float max_minscore = 0.0; + + gpx->crc = 0; + + // len < NDATA_LEN: EOF + if (len < pos_GPS1) { // else: try prev.frame + for (i = len; i < FRAME_LEN; i++) gpx->frame[i] = 0; + } + + //frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) + //frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) + ft = frametype(gpx); + if (ft >= 0) len = NDATA_LEN; // ft >= 0: NDATA_LEN (default) + else len = FRAME_LEN; // ft < 0: FRAME_LEN (aux) + + + for (i = FRAMESTART; i < len; i++) { + if (fabs(gpx->ecdat.frm_bytescore[i]) > max_minscore) max_minscore = fabs(gpx->ecdat.frm_bytescore[i]); + } + max_minscore = floor(max_minscore+1.5); + if (gpx->option.ecc > 2) { + for (i = 0; i < FRAMESTART; i++) gpx->ecdat.frm_bytescore[i] = max_minscore*2.0; //*sign + for (i = len; i < FRAME_LEN; i++) gpx->ecdat.frm_bytescore[i] = max_minscore; + } + for (i = 0; i < FRAME_LEN; i++) sort_score_idx[i] = i; + if (gpx->option.ecc > 2) { + for (i = 0; i < FRAME_LEN; i++) { + for (j = 0; j < FRAME_LEN-1; j++) { + if (fabs(gpx->ecdat.frm_bytescore[sort_score_idx[j+1]]) < fabs(gpx->ecdat.frm_bytescore[sort_score_idx[j]])) { + int tmp = sort_score_idx[j+1]; + sort_score_idx[j+1] = sort_score_idx[j]; + sort_score_idx[j] = tmp; + } + } + } + for (i = 0; i < FRAME_LEN; i++) gpx->ecdat.sort_idx1[i] = i; + for (i = 0; i < FRAME_LEN; i++) gpx->ecdat.sort_idx2[i] = i; + j1 = 0; + j2 = 0; + for (i = 0; i < FRAME_LEN; i++) { + if (sort_score_idx[i] >= cfg_rs41.parpos && sort_score_idx[i] < cfg_rs41.parpos+ rs_R) gpx->ecdat.sort_idx1[j1++] = sort_score_idx[i]; + else if (sort_score_idx[i] >= cfg_rs41.parpos+rs_R && sort_score_idx[i] < cfg_rs41.parpos+2*rs_R) gpx->ecdat.sort_idx2[j2++] = sort_score_idx[i]; + else if (sort_score_idx[i] >= cfg_rs41.msgpos && sort_score_idx[i] % 2 == 0) gpx->ecdat.sort_idx1[j1++] = sort_score_idx[i]; + else if (sort_score_idx[i] >= cfg_rs41.msgpos && sort_score_idx[i] % 2 == 1) gpx->ecdat.sort_idx2[j2++] = sort_score_idx[i]; + } + } + + + if (gpx->option.ecc) { + ec = rs41_ecc(gpx, len); + } + + + if (gpx->option.raw) { + for (i = 0; i < len; i++) { + fprintf(stdout, "%02x", gpx->frame[i]); + } + if (gpx->option.ecc) { + if (ec >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (gpx->option.ecc /*>= 2*/) { + if (ec > 0) fprintf(stdout, " (%d)", ec); + if (ec < 0) { + if (ec == -1) fprintf(stdout, " (-+)"); + else if (ec == -2) fprintf(stdout, " (+-)"); + else /*ec == -3*/ fprintf(stdout, " (--)"); + } + } + } + fprintf(stdout, "\n"); + } + else { + print_position(gpx, ec); + } +} + +/* -------------------------------------------------------------------------- */ + + +int main(int argc, char *argv[]) { + + //int option_inv = 0; // invertiert Signal + int option_min = 0; + int option_iq = 0; + int option_iqdc = 0; + int option_lp = 0; + int option_dc = 0; + int option_bin = 0; + int option_softin = 0; + int option_pcmraw = 0; + int wavloaded = 0; + int sel_wavch = 0; // audio channel: left + int rawhex = 0, xorhex = 0; + int cfreq = -1; + + FILE *fp; + char *fpname = NULL; + + int k; + + char bitbuf[8]; + int bitpos = 0, + b8pos = 0, + byte_count = FRAMESTART; + int bit, byte; + int bitQ; + int difbyte = 0; + hsbit_t hsbit, hsbit1; + + int header_found = 0; + + float thres = 0.7; // dsp.mv threshold + float _mv = 0.0; + + float lpIQ_bw = 7.4e3; + + int symlen = 1; + int bitofs = 2; // +0 .. +3 + int shift = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + gpx_t gpx = {0}; + + hdb_t hdb = {0}; + float softbits[BITS]; + + +#ifdef CYGWIN + _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) +#endif + setbuf(stdout, NULL); + + + fpname = argv[0]; + ++argv; + while ((*argv) && (!wavloaded)) { + if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { + fprintf(stderr, "%s [options] audio.wav\n", fpname); + fprintf(stderr, " options:\n"); + fprintf(stderr, " -v, -vx, -vv (info, aux, info/conf)\n"); + fprintf(stderr, " -r, --raw\n"); + fprintf(stderr, " -i, --invert\n"); + //fprintf(stderr, " --crc (check CRC)\n"); + //fprintf(stderr, " --ecc2 (Reed-Solomon )\n"); + fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); + fprintf(stderr, " --iq0,2,3 (IQ data)\n"); + return 0; + } + else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { + gpx.option.vbs = 1; + } + else if (strcmp(*argv, "-vx") == 0) { gpx.option.vbs = 2; } + else if (strcmp(*argv, "-vv") == 0) { gpx.option.vbs = 3; } + //else if (strcmp(*argv, "-vvv") == 0) { gpx.option.vbs = 4; } + else if (strcmp(*argv, "--crc") == 0) { gpx.option.crc = 1; } + else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { + gpx.option.raw = 1; + } + else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { + gpx.option.inv = 1; + } + else if (strcmp(*argv, "--ecc" ) == 0) { gpx.option.ecc = 1; } + else if (strcmp(*argv, "--ecc2") == 0) { gpx.option.ecc = 2; } + else if (strcmp(*argv, "--ecc3") == 0) { gpx.option.ecc = 3; } + else if (strcmp(*argv, "--ecc4") == 0) { gpx.option.ecc = 4; } + else if (strcmp(*argv, "--sat") == 0) { gpx.option.sat = 1; } + else if (strcmp(*argv, "--ptu") == 0) { gpx.option.ptu = 1; } + else if (strcmp(*argv, "--silent") == 0) { gpx.option.slt = 1; } + else if (strcmp(*argv, "--ch2") == 0) { sel_wavch = 1; } // right channel (default: 0=left) + else if (strcmp(*argv, "--auto") == 0) { gpx.option.aut = 1; } + else if (strcmp(*argv, "--bin") == 0) { option_bin = 1; } // bit/byte binary input + else if (strcmp(*argv, "--softin") == 0) { option_softin = 1; } // float32 soft input + else if (strcmp(*argv, "--ths") == 0) { + ++argv; + if (*argv) { + thres = atof(*argv); + } + else return -1; + } + else if ( (strcmp(*argv, "-d") == 0) ) { + ++argv; + if (*argv) { + shift = atoi(*argv); + if (shift > 4) shift = 4; + if (shift < -4) shift = -4; + } + else return -1; + } + else if (strcmp(*argv, "--iq0") == 0) { option_iq = 1; } // differential/FM-demod + else if (strcmp(*argv, "--iq2") == 0) { option_iq = 2; } + else if (strcmp(*argv, "--iq3") == 0) { option_iq = 3; } // iq2==iq3 + else if (strcmp(*argv, "--iqdc") == 0) { option_iqdc = 1; } // iq-dc removal (iq0,2,3) + else if (strcmp(*argv, "--IQ") == 0) { // fq baseband -> IF (rotate from and decimate) + double fq = 0.0; // --IQ , -0.5 < fq < 0.5 + ++argv; + if (*argv) fq = atof(*argv); + else return -1; + if (fq < -0.5) fq = -0.5; + if (fq > 0.5) fq = 0.5; + dsp.xlt_fq = -fq; // S(t) -> S(t)*exp(-f*2pi*I*t) + option_iq = 5; + } + else if (strcmp(*argv, "--lp") == 0) { option_lp = 1; } // IQ lowpass + else if (strcmp(*argv, "--lpbw") == 0) { // IQ lowpass BW / kHz + double bw = 0.0; + ++argv; + if (*argv) bw = atof(*argv); + else return -1; + if (bw > 4.6 && bw < 24.0) lpIQ_bw = bw*1e3; + option_lp = 1; + } + else if (strcmp(*argv, "--dc") == 0) { option_dc = 1; } + else if (strcmp(*argv, "--min") == 0) { + option_min = 1; + } + else if (strcmp(*argv, "--json") == 0) { + gpx.option.jsn = 1; + gpx.option.ecc = 2; + gpx.option.crc = 1; + } + else if (strcmp(*argv, "--jsn_cfq") == 0) { + int frq = -1; // center frequency / Hz + ++argv; + if (*argv) frq = atoi(*argv); else return -1; + if (frq < 300000000) frq = -1; + cfreq = frq; + } + else if (strcmp(*argv, "--rawhex") == 0) { rawhex = 2; } // raw hex input + else if (strcmp(*argv, "--xorhex") == 0) { rawhex = 2; xorhex = 1; } // raw xor input + else if (strcmp(*argv, "-") == 0) { + int sample_rate = 0, bits_sample = 0, channels = 0; + ++argv; + if (*argv) sample_rate = atoi(*argv); else return -1; + ++argv; + if (*argv) bits_sample = atoi(*argv); else return -1; + channels = 2; + if (sample_rate < 1 || (bits_sample != 8 && bits_sample != 16 && bits_sample != 32)) { + fprintf(stderr, "- \n"); + return -1; + } + pcm.sr = sample_rate; + pcm.bps = bits_sample; + pcm.nch = channels; + option_pcmraw = 1; + } + else { + fp = fopen(*argv, "rb"); + if (fp == NULL) { + fprintf(stderr, "error: open %s\n", *argv); + return -1; + } + wavloaded = 1; + } + ++argv; + } + if (!wavloaded) fp = stdin; + + + if (gpx.option.ecc < 2) gpx.option.ecc = 1; // turn off for ber-measurement + + if (gpx.option.ecc) { + rs_init_RS255(&gpx.RS); // RS, GF + } + + // init gpx + memcpy(gpx.frame, rs41_header_bytes, sizeof(rs41_header_bytes)); // 8 header bytes + + if (cfreq > 0) gpx.jsn_freq = (cfreq+500)/1000; + + + #ifdef EXT_FSK + if (!option_bin && !option_softin) { + option_softin = 1; + fprintf(stderr, "reading float32 soft symbols\n"); + } + #endif + + if (!rawhex) { + + if (!option_bin && !option_softin) { + + if (option_iq == 0 && option_pcmraw) { + fclose(fp); + fprintf(stderr, "error: raw data not IQ\n"); + return -1; + } + if (option_iq) sel_wavch = 0; + + pcm.sel_ch = sel_wavch; + if (option_pcmraw == 0) { + k = read_wav_header(&pcm, fp); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + } + + if (cfreq > 0) { + int fq_kHz = (cfreq - dsp.xlt_fq*pcm.sr + 500)/1e3; + gpx.jsn_freq = fq_kHz; + } + + // rs41: BT=0.5, h=0.8,1.0 ? + symlen = 1; + + // init dsp + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = symlen; + dsp._spb = dsp.sps*symlen; + dsp.hdr = rs41_header; + dsp.hdrlen = strlen(rs41_header); + dsp.BT = 0.5; // bw/time (ISI) // 0.3..0.5 + dsp.h = 0.6; //0.7; // 0.7..0.8? modulation index abzgl. BT + dsp.opt_iq = option_iq; + dsp.opt_iqdc = option_iqdc; + dsp.opt_lp = option_lp; + dsp.lpIQ_bw = lpIQ_bw; // 7.4e3 (6e3..8e3) // IF lowpass bandwidth + dsp.lpFM_bw = 6e3; // FM audio lowpass + dsp.opt_dc = option_dc; + dsp.opt_IFmin = option_min; + + if ( dsp.sps < 8 ) { + fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); + } + + + k = init_buffers(&dsp); // BT=0.5 (IQ-Int: BT > 0.5 ?) + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return -1; + } + + //if (option_iq >= 2) bitofs += 1; // FM: +1 , IQ: +2 + bitofs += shift; + } + else { + if (option_bin && option_softin) option_bin = 0; + // init circular header bit buffer + hdb.hdr = rs41_header; + hdb.len = strlen(rs41_header); + hdb.thb = 1.0 - 3.1/(float)hdb.len; // 1.0-max_bit_errors/hdrlen + hdb.bufpos = -1; + hdb.buf = calloc(hdb.len, sizeof(char)); + if (hdb.buf == NULL) { + fprintf(stderr, "error: malloc\n"); + return -1; + } + hdb.ths = 0.7; // caution/test false positive + hdb.sbuf = calloc(hdb.len, sizeof(float)); + if (hdb.sbuf == NULL) { + fprintf(stderr, "error: malloc\n"); + return -1; + } + } + + + while ( 1 ) + { + if (option_bin) { + header_found = find_binhead(fp, &hdb, &_mv); + } + else if (option_softin) { + header_found = find_softbinhead(fp, &hdb, &_mv); + } + else { // FM-audio: + header_found = find_header(&dsp, thres, 4, bitofs, dsp.opt_dc); // optional 2nd pass: dc=0 + _mv = dsp.mv; + } + if (header_found == EOF) break; + + // mv == correlation score + if (_mv *(0.5-gpx.option.inv) < 0) { + if (gpx.option.aut == 0) header_found = 0; + else gpx.option.inv ^= 0x1; + } + + if (header_found) + { + byte_count = FRAMESTART; + bitpos = 0; // byte_count*8-HEADLEN + b8pos = 0; + difbyte = 0; + + while ( byte_count < FRAME_LEN ) + { + if (option_bin) { + bitQ = fgetc(fp); + if (bitQ != EOF) { + bit = bitQ & 0x1; + hsbit.hb = bit; + hsbit.sb = 2*bit-1; + } + } + else if (option_softin) { + float s = 0.0; + bitQ = f32soft_read(fp, &s); + if (bitQ != EOF) { + bit = (s>=0.0); + hsbit.hb = bit; + hsbit.sb = s; + } + } + else { + float bl = -1; + if (option_iq > 2) bl = 2.0; + //bitQ = read_slbit(&dsp, &bit, 0, bitofs, bitpos, bl, 0); // symlen=1 + bitQ = read_softbit2p(&dsp, &hsbit, 0, bitofs, bitpos, bl, 0, &hsbit1); // symlen=1 + bit = hsbit.hb; + if (gpx.option.ecc >= 3) bit = (hsbit.sb+hsbit1.sb)>=0; + + if (bitpos < FRAME_LEN*BITS && hsbit.sb*hsbit1.sb < 0) { + difbyte |= 1< 0) { + + pbuf = fgets(buffer_rawhex, 2*FRAME_LEN+12, fp); + if (pbuf == NULL) break; + buffer_rawhex[2*FRAME_LEN] = '\0'; + buf_sp = strchr(buffer_rawhex, ' '); + if (buf_sp != NULL && buf_sp-buffer_rawhex < 2*FRAME_LEN) { + buffer_rawhex[buf_sp-buffer_rawhex] = '\0'; + } + len = strlen(buffer_rawhex) / 2; + if (len > pos_SondeID+10) { + for (i = 0; i < len; i++) { //%2x SCNx8=%hhx(inttypes.h) + sscanf(buffer_rawhex+2*i, "%2hhx", &frmbyte); + // wenn ohne %hhx: sscanf(buffer_rawhex+rawhex*i, "%2x", &byte); frame[frameofs+i] = (ui8_t)byte; + if (xorhex) frmbyte ^= mask[(frameofs+i) % MASK_LEN]; + gpx.frame[frameofs+i] = frmbyte; + } + print_frame(&gpx, frameofs+len); + } + } + } + + + fclose(fp); + + return 0; +} + diff --git a/imet/imet1rs_dft.c b/imet/imet1rs_dft.c index cdcf8d4..af1f34e 100644 --- a/imet/imet1rs_dft.c +++ b/imet/imet1rs_dft.c @@ -453,7 +453,7 @@ offset bytes description 1 1 PKT_ID = 0x03 2 2 N = number of data bytes to follow 3+N 2 CRC (16-bit) -N=8, ID=0x01: Ozonesonde (MSB) +N=8, ID=0x01: ECC Ozonesonde (MSB) 3 1 Instrument_type = 0x01 (ID) 4 1 Instrument_number 5 2 Icell, uA (I = n/1000) @@ -469,7 +469,6 @@ ID=0x19: COBALD (Compact Optical Backscatter Aerosol Detector) */ int print_xdata(int pos, ui8_t N) { - int P, U; ui8_t InstrumentNum; short Tpump; unsigned short Icell, Ipump, Vbat; @@ -620,8 +619,13 @@ int print_frame(int len) { fprintf(stdout, ", \"aux\": \"%s\"", gpx.xdata ); } if (gpx.jsn_freq > 0) { - fprintf(stdout, ", \"freq\": %d", gpx.jsn_freq); + fprintf(stdout, ", \"freq\": %d", gpx.jsn_freq ); } + + // Reference time/position + fprintf(stdout, ", \"ref_datetime\": \"%s\"", "GPS" ); // {"GPS", "UTC"} GPS-UTC=leap_sec + fprintf(stdout, ", \"ref_position\": \"%s\"", "MSL" ); // {"GPS", "MSL"} GPS=ellipsoid , MSL=geoid + #ifdef VER_JSN_STR ver_jsn = VER_JSN_STR; #endif diff --git a/mk2a/mk2a1680mod.c b/mk2a/mk2a1680mod.c index 5495dfa..c1440bb 100644 --- a/mk2a/mk2a1680mod.c +++ b/mk2a/mk2a1680mod.c @@ -45,6 +45,11 @@ #define M_PI (3.1415926535897932384626433832795) #endif +#define LP_IQ 1 +#define LP_FM 2 +#define LP_IQFM 4 + + typedef unsigned char ui8_t; typedef unsigned short ui16_t; typedef unsigned int ui32_t; @@ -96,6 +101,8 @@ typedef struct { float mv; ui32_t mv_pos; // + float mv2; + ui32_t mv2_pos; // IQ-data int opt_iq; @@ -126,6 +133,7 @@ typedef struct { // decimate + int opt_nolut; // default: LUT int opt_IFmin; int decM; ui32_t sr_base; @@ -152,7 +160,13 @@ typedef struct { int lpFMtaps; // ui32_t float *ws_lpFM; float *lpFM_buf; - float *fm_buffer; + float *fm_buffer; + + // IQFM: lowpass + int lpIQFM_bw; + int lpIQFMtaps; // ui32_t + float *ws_lpIQFM; + float *lpIQFM_buf; int opt_fmdec; int decFM; @@ -319,9 +333,8 @@ static int getCorrDFT(dsp_t *dsp) { ui32_t mpos = 0; ui32_t pos = dsp->sample_out; - double dc = 0.0; - int mp_ofs = 0; float *sbuf = dsp->bufs; + float *dcbuf = dsp->fm_buffer; dsp->mv = 0.0; dsp->dc = 0.0; @@ -330,44 +343,35 @@ static int getCorrDFT(dsp_t *dsp) { if (dsp->sample_out < dsp->L) return -2; - if (dsp->opt_iq > 1 && dsp->opt_iq < 6 && dsp->opt_dc) { - mp_ofs = (dsp->sps-1)/2; - sbuf = dsp->fm_buffer; - } - else { - sbuf = dsp->bufs; - } - for (i = 0; i < dsp->K + dsp->L; i++) (dsp->DFT).xn[i] = sbuf[(pos+dsp->M -(dsp->K + dsp->L-1) + i) % dsp->M]; - while (i < dsp->DFT.N) (dsp->DFT).xn[i++] = 0.0; + for (i = 0; i < dsp->K + dsp->L; i++) dsp->DFT.xn[i] = sbuf[(pos+dsp->M -(dsp->K + dsp->L-1) + i) % dsp->M]; + while (i < dsp->DFT.N) dsp->DFT.xn[i++] = 0.0; rdft(&dsp->DFT, dsp->DFT.xn, dsp->DFT.X); if (dsp->opt_dc) { + /* //X[0] = 0; // nicht ueber gesamte Laenge ... M10 // // L < K ? // only last 2L samples (avoid M10 carrier offset) - //dc = 0.0; - //for (i = dsp->K - dsp->L; i < dsp->K + dsp->L; i++) dc += (dsp->DFT).xn[i]; + //for (i = dsp->K - dsp->L; i < dsp->K + dsp->L; i++) dc += dsp->DFT.xn[i]; //dc /= 2.0*(float)dsp->L; - dc = 0.0; - for (i = dsp->K /*- dsp->L*/; i < dsp->K + dsp->L; i++) dc += (dsp->DFT).xn[i]; + for (i = dsp->K; i < dsp->K + dsp->L; i++) dc += dsp->DFT.xn[i]; dc /= 1.0*(float)dsp->L; - - dsp->DFT.X[0] -= dsp->DFT.N * dc ;//* 0.95; - Nidft(&dsp->DFT, dsp->DFT.X, (dsp->DFT).cx); - for (i = 0; i < dsp->DFT.N; i++) (dsp->DFT).xn[i] = creal((dsp->DFT).cx[i])/(float)dsp->DFT.N; + dsp->DFT.X[0] -= dsp->DFT.N * dc * 0.95; // dc * dsp->L + */ + dsp->DFT.X[0] = 0; + Nidft(&dsp->DFT, dsp->DFT.X, dsp->DFT.cx); + for (i = 0; i < dsp->DFT.N; i++) (dsp->DFT).xn[i] = creal(dsp->DFT.cx[i])/(float)dsp->DFT.N; } for (i = 0; i < dsp->DFT.N; i++) dsp->DFT.Z[i] = dsp->DFT.X[i]*dsp->DFT.Fm[i]; Nidft(&dsp->DFT, dsp->DFT.Z, dsp->DFT.cx); - if (fabs(dc) < 0.5) dsp->dc = dc; - // relativ Peak - Normierung erst zum Schluss; // dann jedoch nicht zwingend corr-Max wenn FM-Amplitude bzw. norm(x) nicht konstant @@ -388,36 +392,83 @@ static int getCorrDFT(dsp_t *dsp) { mpos = pos - (dsp->K + dsp->L-1) + mp; // t = L-1 - // header: mpos-L .. mpos (CA CA CA 24 52) - // dc(header) ? -> Mk2a: 0xCA preamble, mpos-L .. mpos-2/5*L - if (dsp->opt_dc) - { - dc = 0.0; - //for (i = 0; i < dsp->L; i++) dc += sbuf[(mpos - i + dsp->M) % dsp->M]; - //dc /= (float)dsp->L; dc *= 0.8f; - for (i = 2*dsp->L/5; i < dsp->L; i++) dc += sbuf[(mpos - i + dsp->M) % dsp->M]; - dc /= (float)dsp->L*3/5.0; - dsp->dc = dc; - } - - - //xnorm = sqrt(dsp->qs[(mpos + 2*dsp->M) % dsp->M]); // Nvar = L xnorm = 0.0; - for (i = 0; i < dsp->L; i++) xnorm += (dsp->DFT).xn[mp-i]*(dsp->DFT).xn[mp-i]; + for (i = 0; i < dsp->L; i++) xnorm += dsp->DFT.xn[mp-i]*dsp->DFT.xn[mp-i]; xnorm = sqrt(xnorm); - mx /= xnorm*(dsp->DFT).N; - - if (dsp->opt_iq > 1 && dsp->opt_iq < 6 && dsp->opt_dc) mpos += mp_ofs; + mx /= xnorm*dsp->DFT.N; dsp->mv = mx; dsp->mv_pos = mpos; if (pos == dsp->sample_out) dsp->buffered = dsp->sample_out - mpos; -// FM: s = gain * carg(w)/M_PI = gain * dphi / PI // gain=0.8 -// FM audio gain? dc relative to FM-envelope?! -// + + dsp->mv2 = 0.0f; + dsp->mv2_pos = 0; + if (dsp->opt_dc) { + if (dsp->opt_iq >= 2 && dsp->opt_iq < 6 && !dsp->locked) { + mx = 0.0f; + mpos = 0; + + for (i = 0; i < dsp->K + dsp->L; i++) dsp->DFT.xn[i] = dcbuf[(pos+dsp->M -(dsp->K + dsp->L-1) + i) % dsp->M]; + while (i < dsp->DFT.N) dsp->DFT.xn[i++] = 0.0; + rdft(&dsp->DFT, dsp->DFT.xn, dsp->DFT.X); + + dsp->DFT.X[0] = 0; + Nidft(&dsp->DFT, dsp->DFT.X, dsp->DFT.cx); + for (i = 0; i < dsp->DFT.N; i++) dsp->DFT.xn[i] = creal(dsp->DFT.cx[i])/(float)dsp->DFT.N; + + for (i = 0; i < dsp->DFT.N; i++) dsp->DFT.Z[i] = dsp->DFT.X[i]*dsp->DFT.Fm[i]; + + Nidft(&dsp->DFT, dsp->DFT.Z, dsp->DFT.cx); + + mx2 = 0.0; // t = L-1 + for (i = dsp->L-1; i < dsp->K + dsp->L; i++) { // i=t .. i=t+K < t+1+K + re_cx = creal(dsp->DFT.cx[i]); // imag(cx)=0 + if (re_cx*re_cx > mx2) { + mx = re_cx; + mx2 = mx*mx; + mp = i; + } + } + if (mp == dsp->L-1 || mp == dsp->K + dsp->L-1) return -4; // Randwert + // mp == t mp == K+t + + mpos = pos - (dsp->K + dsp->L-1) + mp; // t = L-1 + + xnorm = 0.0; + for (i = 0; i < dsp->L; i++) xnorm += dsp->DFT.xn[mp-i]*dsp->DFT.xn[mp-i]; + xnorm = sqrt(xnorm); + + mx /= xnorm*(dsp->DFT).N; + + + dsp->mv2 = mx; + dsp->mv2_pos = mpos; + } + } + + + // header: mpos-L .. mpos (CA CA CA 24 52) + // dc(header) ? -> Mk2a: 0xCA preamble, mpos-L .. mpos-2/5*L + if (dsp->opt_dc) + { + double dc = 0.0; + int mp_ofs = 0; + if (dsp->opt_iq >= 2 && dsp->opt_iq < 6 && dsp->mv2_pos == 0) { + mp_ofs = (dsp->lpFMtaps - dsp->lpIQFMtaps - (dsp->sps-1))/(2*dsp->decFM); + } + dc = 0.0; + for (i = 2*dsp->L/5; i < dsp->L; i++) dc += dcbuf[(mp_ofs + mpos - i + dsp->M) % dsp->M]; + dc /= (float)dsp->L*3/5.0; + dsp->dc = dc; + } + + + // FM: s = gain * carg(w)/M_PI = gain * dphi / PI // gain=0.8 + // FM audio gain? dc relative to FM-envelope?! + // dsp->dDf = dsp->sr * dsp->dc / (2.0*FM_GAIN); // remaining freq offset return mp; @@ -677,36 +728,6 @@ static int lowpass_init(float f, int taps, float **pws) { } -static int lowpass_update(float f, int taps, float *ws) { - double *h, *w; - double norm = 0; - int n; - - if (taps % 2 == 0) taps++; // odd/symmetric - - if ( taps < 1 ) taps = 1; - - h = (double*)calloc( taps+1, sizeof(double)); if (h == NULL) return -1; - w = (double*)calloc( taps+1, sizeof(double)); if (w == NULL) return -1; - - for (n = 0; n < taps; n++) { - w[n] = 7938/18608.0 - 9240/18608.0*cos(2*M_PI*n/(taps-1)) + 1430/18608.0*cos(4*M_PI*n/(taps-1)); // Blackmann - h[n] = 2*f*sinc(2*f*(n-(taps-1)/2)); - ws[n] = w[n]*h[n]; - norm += ws[n]; // 1-norm - } - for (n = 0; n < taps; n++) { - ws[n] /= norm; // 1-norm - } - - for (n = 0; n < taps; n++) ws[taps+n] = ws[n]; - - free(h); h = NULL; - free(w); w = NULL; - - return taps; -} - static float complex lowpass(float complex buffer[], ui32_t sample, ui32_t taps, float *ws) { ui32_t n; ui32_t s = sample % taps; @@ -732,6 +753,7 @@ static float re_lowpass(float buffer[], ui32_t sample, ui32_t taps, float *ws) { static int f32buf_sample(dsp_t *dsp, int inv) { float s = 0.0; + float s_fm = s; float complex z, w, z0; double gain = FM_GAIN; @@ -749,14 +771,21 @@ int f32buf_sample(dsp_t *dsp, int inv) { { double t = _sample / (double)dsp->sr; - if (dsp->opt_iq) { - + if (dsp->opt_iq) + { if (dsp->opt_iq >= 5) { ui32_t s_reset = dsp->dectaps*dsp->lut_len; int j; if ( f32read_cblock(dsp) < dsp->decM ) return EOF; for (j = 0; j < dsp->decM; j++) { - z = dsp->decMbuf[j] * dsp->ex[dsp->sample_dec % dsp->lut_len]; + if (dsp->opt_nolut) { + double _s_base = (double)(_sample*dsp->decM+j); // dsp->sample_dec + double f0 = dsp->xlt_fq*_s_base - dsp->Df*_s_base/(double)dsp->sr_base; + z = dsp->decMbuf[j] * cexp(f0*2*M_PI*I); + } + else { + z = dsp->decMbuf[j] * dsp->ex[dsp->sample_dec % dsp->lut_len]; + } dsp->decXbuffer[dsp->sample_dec % dsp->dectaps] = z; dsp->sample_dec += 1; if (dsp->sample_dec == s_reset) dsp->sample_dec = 0; @@ -768,11 +797,13 @@ int f32buf_sample(dsp_t *dsp, int inv) { } else if ( f32read_csample(dsp, &z) == EOF ) return EOF; - z *= cexp(-t*2*M_PI*dsp->Df*I); + if (dsp->opt_dc && !dsp->opt_nolut) { + z *= cexp(-t*2*M_PI*dsp->Df*I); + } // IF-lowpass - if (dsp->opt_lp & 1) { + if (dsp->opt_lp & LP_IQ) { dsp->lpIQ_buf[_sample % dsp->lpIQtaps] = z; z = lowpass(dsp->lpIQ_buf, _sample, dsp->lpIQtaps, dsp->ws_lpIQ); } @@ -780,83 +811,95 @@ int f32buf_sample(dsp_t *dsp, int inv) { z0 = dsp->rot_iqbuf[(_sample-1 + dsp->N_IQBUF) % dsp->N_IQBUF]; w = z * conj(z0); - s = gain * carg(w)/M_PI; + s_fm = gain * carg(w)/M_PI; dsp->rot_iqbuf[_sample % dsp->N_IQBUF] = z; - // FM-lowpass - if (dsp->opt_lp & 2) { - dsp->lpFM_buf[_sample % dsp->lpFMtaps] = s; - if (m+1 == decFM) { - s = re_lowpass(dsp->lpFM_buf, _sample, dsp->lpFMtaps, dsp->ws_lpFM); + if (dsp->opt_iq >= 2 && dsp->opt_iq < 6) + { + if (0) { // not L band + double xbit = 0.0; + //float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps); + double f1 = -dsp->h*dsp->sr/(2*dsp->sps); + double f2 = -f1; + + float complex X0 = 0; + float complex X = 0; + + int n = dsp->sps; + double tn = (_sample-n) / (double)dsp->sr; + //t = _sample / (double)dsp->sr; + //z = dsp->rot_iqbuf[_sample % dsp->N_IQBUF]; + z0 = dsp->rot_iqbuf[(_sample-n + dsp->N_IQBUF) % dsp->N_IQBUF]; + + // f1 + X0 = z0 * cexp(-tn*2*M_PI*f1*I); // alt + X = z * cexp(-t *2*M_PI*f1*I); // neu + dsp->F1sum += X - X0; + + // f2 + X0 = z0 * cexp(-tn*2*M_PI*f2*I); // alt + X = z * cexp(-t *2*M_PI*f2*I); // neu + dsp->F2sum += X - X0; + + xbit = cabs(dsp->F2sum) - cabs(dsp->F1sum); + + s = xbit / dsp->sps; } - } + else { + double xbit = 0.0; + float _sps = dsp->sps * decFM; + //float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps); + double f1 = -dsp->h*dsp->sr/(2*_sps); + double f2 = -f1; - dsp->fm_buffer[(_sample - dsp->lpFMtaps/2 + dsp->M) % dsp->M] = s; + float complex X1 = 0; + float complex X2 = 0; + int n = _sps; + float sk = _sps/2.4f; - if (0 && dsp->opt_iq >= 2 && dsp->opt_iq < 6) - { - double xbit = 0.0; - //float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps); - double f1 = -dsp->h*dsp->sr/(2*dsp->sps); - double f2 = -f1; - - float complex X0 = 0; - float complex X = 0; - - int n = dsp->sps; - double tn = (_sample-n) / (double)dsp->sr; - //t = _sample / (double)dsp->sr; - //z = dsp->rot_iqbuf[_sample % dsp->N_IQBUF]; - z0 = dsp->rot_iqbuf[(_sample-n + dsp->N_IQBUF) % dsp->N_IQBUF]; - - // f1 - X0 = z0 * cexp(-tn*2*M_PI*f1*I); // alt - X = z * cexp(-t *2*M_PI*f1*I); // neu - dsp->F1sum += X - X0; - - // f2 - X0 = z0 * cexp(-tn*2*M_PI*f2*I); // alt - X = z * cexp(-t *2*M_PI*f2*I); // neu - dsp->F2sum += X - X0; - - xbit = cabs(dsp->F2sum) - cabs(dsp->F1sum); - - s = xbit / dsp->sps; - } - else if (dsp->opt_iq >= 2 && dsp->opt_iq < 6) - { - double xbit = 0.0; - //float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps); - double f1 = -dsp->h*dsp->sr/(2*dsp->sps); - double f2 = -f1; - - float complex X1 = 0; - float complex X2 = 0; - - int n = dsp->sps; - float sk = dsp->sps/2.4f; - - while (n > 0) { - n--; - if (n > sk && n < dsp->sps-sk) - { - t = -n / (double)dsp->sr; - z = dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]; // +1 - X1 += z*cexp(-t*2*M_PI*f1*I); - X2 += z*cexp(-t*2*M_PI*f2*I); + while (n > 0) { + n--; + if (n > sk && n < _sps-sk) + { + t = -n / (double)dsp->sr; + z = dsp->rot_iqbuf[(_sample - n + dsp->N_IQBUF) % dsp->N_IQBUF]; + X1 += z*cexp(-t*2*M_PI*f1*I); + X2 += z*cexp(-t*2*M_PI*f2*I); + } } + + xbit = cabs(X2) - cabs(X1); + + s = xbit / _sps; //opt_iq==5 } - - xbit = cabs(X2) - cabs(X1); - - s = xbit / dsp->sps; + } + else { + s = s_fm; //opt_iq=1,6 } } else { if (f32read_sample(dsp, &s) == EOF) return EOF; + s_fm = s; //opt_iq==0 + } + + // FM-lowpass + if (dsp->opt_lp & LP_FM) { + dsp->lpFM_buf[_sample % dsp->lpFMtaps] = s_fm; + if (m+1 == decFM) { + s_fm = re_lowpass(dsp->lpFM_buf, _sample, dsp->lpFMtaps, dsp->ws_lpFM); + if (dsp->opt_iq < 2 || dsp->opt_iq > 5) s = s_fm; //opt_iq==0,1,6 + } + } + + // IQFM-lowpass II / separate IQ-FM lowpass + if (dsp->opt_lp & LP_IQFM) { // opt_iq==5 + dsp->lpIQFM_buf[_sample % dsp->lpIQFMtaps] = s; + if (m+1 == decFM) { + s = re_lowpass(dsp->lpIQFM_buf, _sample, dsp->lpIQFMtaps, dsp->ws_lpIQFM); + } } _sample += 1; @@ -866,6 +909,8 @@ int f32buf_sample(dsp_t *dsp, int inv) { if (inv) s = -s; dsp->bufs[dsp->sample_in % dsp->M] = s; + dsp->fm_buffer[dsp->sample_in % dsp->M] = s_fm; + dsp->sample_out = dsp->sample_in - dsp->delay; dsp->sample_in += 1; @@ -880,20 +925,23 @@ static int read_bufbit(dsp_t *dsp, int symlen, char *bits, ui32_t mvp, int pos) ui32_t rcount = ceil(rbitgrenze);//+0.99; // dfm? double sum = 0.0; + double dc = 0.0; + + if (dsp->opt_dc && (dsp->opt_iq < 2 || dsp->opt_iq > 5)) dc = dsp->dc; // bei symlen=2 (Manchester) kein dc noetig: -dc+dc=0 ; // allerdings M10-header mit symlen=1 rbitgrenze += dsp->sps; do { - sum += dsp->bufs[(rcount + mvp + dsp->M) % dsp->M] - dsp->dc; + sum += dsp->bufs[(rcount + mvp + dsp->M) % dsp->M] - dc; rcount++; } while (rcount < rbitgrenze); // n < dsp->sps if (symlen == 2) { rbitgrenze += dsp->sps; do { - sum -= dsp->bufs[(rcount + mvp + dsp->M) % dsp->M] - dsp->dc; + sum -= dsp->bufs[(rcount + mvp + dsp->M) % dsp->M] - dc; rcount++; } while (rcount < rbitgrenze); // n < dsp->sps } @@ -919,8 +967,6 @@ static int headcmp(dsp_t *dsp, int opt_dc) { int len = dsp->hdrlen/dsp->symhd; int inv = dsp->mv < 0; - if (opt_dc == 0 || dsp->opt_iq > 1) dsp->dc = 0; // reset? e.g. 2nd pass - if (dsp->symhd != 1) step = 2; if (inv) sign=1; @@ -958,8 +1004,8 @@ int read_softbit2p(dsp_t *dsp, hsbit_t *shb, int inv, int ofs, int pos, float l, ui8_t bit = 0, bit1 = 0; - - if (dsp->opt_dc && dsp->opt_iq < 2) dc = dsp->dc; + // whole frame, dsp->dDf correction before (!dsp->opt_iq can miss frame) + if (dsp->opt_dc && (dsp->opt_iq < 2 || dsp->opt_iq > 5)) dc = dsp->dc; if (pos == 0) { bg = 0; @@ -1037,7 +1083,7 @@ int read_softbit2p(dsp_t *dsp, hsbit_t *shb, int inv, int ofs, int pos, float l, #define IF_SAMPLE_RATE_MIN 32000 #define IF_TRANSITION_BW (8e3) // (min) transition width -#define FM_TRANSITION_BW (2e3) // (min) transition width +#define FM_TRANSITION_BW (4e3) // (min) transition width #define SQRT2 1.4142135624 // sqrt(2) // sigma = sqrt(log(2)) / (2*PI*BT): @@ -1077,6 +1123,7 @@ int init_buffers_Lband(dsp_t *dsp) { float *m = NULL; + // decimate if (dsp->opt_iq >= 5) { int IF_sr = IF_SAMPLE_RATE*Lscale; // designated IF sample rate @@ -1117,40 +1164,42 @@ int init_buffers_Lband(dsp_t *dsp) { } if (dsp->opt_iq >= 5) { - // look up table, exp-rotation - int W = 2*8; // 16 Hz window - int d = 1; // 1..W , groesster Teiler d <= W von sr_base - int freq = (int)( dsp->xlt_fq * (double)dsp->sr_base + 0.5); - int freq0 = freq; // init - double f0 = freq0 / (double)dsp->sr_base; // init + if (!dsp->opt_nolut) + { + // look up table, exp-rotation + int W = 2*8; // 16 Hz window + int d = 1; // 1..W , groesster Teiler d <= W von sr_base + int freq = (int)( dsp->xlt_fq * (double)dsp->sr_base + 0.5); + int freq0 = freq; // init + double f0 = freq0 / (double)dsp->sr_base; // init - for (d = W; d > 0; d--) { // groesster Teiler d <= W von sr - if (dsp->sr_base % d == 0) break; - } - if (d == 0) d = 1; // d >= 1 ? - - for (k = 0; k < W/2; k++) { - if ((freq+k) % d == 0) { - freq0 = freq + k; - break; + for (d = W; d > 0; d--) { // groesster Teiler d <= W von sr + if (dsp->sr_base % d == 0) break; } - if ((freq-k) % d == 0) { - freq0 = freq - k; - break; + if (d == 0) d = 1; // d >= 1 ? + + for (k = 0; k < W/2; k++) { + if ((freq+k) % d == 0) { + freq0 = freq + k; + break; + } + if ((freq-k) % d == 0) { + freq0 = freq - k; + break; + } + } + + dsp->lut_len = dsp->sr_base / d; + f0 = freq0 / (double)dsp->sr_base; + + dsp->ex = calloc(dsp->lut_len+1, sizeof(float complex)); + if (dsp->ex == NULL) return -1; + for (n = 0; n < dsp->lut_len; n++) { + t = f0*(double)n; + dsp->ex[n] = cexp(t*2*M_PI*I); } } - dsp->lut_len = dsp->sr_base / d; - f0 = freq0 / (double)dsp->sr_base; - - dsp->ex = calloc(dsp->lut_len+1, sizeof(float complex)); - if (dsp->ex == NULL) return -1; - for (n = 0; n < dsp->lut_len; n++) { - t = f0*(double)n; - dsp->ex[n] = cexp(t*2*M_PI*I); - } - - dsp->decXbuffer = calloc( dsp->dectaps+1, sizeof(float complex)); if (dsp->decXbuffer == NULL) return -1; @@ -1158,13 +1207,12 @@ int init_buffers_Lband(dsp_t *dsp) { if (dsp->decMbuf == NULL) return -1; } - // IQ lowpass - if (dsp->opt_iq && (dsp->opt_lp & 1)) + // IF lowpass + if (dsp->opt_iq && (dsp->opt_lp & LP_IQ)) { float f_lp; // lowpass_bw int taps; // lowpass taps: 4*sr/transition_bw - // IF lowpass f_lp = 160e3/(float)dsp->sr/2.0; // default if (dsp->lpIQ_bw) f_lp = dsp->lpIQ_bw/(float)dsp->sr/2.0; taps = 4*dsp->sr/IF_TRANSITION_BW; @@ -1185,14 +1233,11 @@ int init_buffers_Lband(dsp_t *dsp) { if (dsp->opt_dc) { dsp->locked = 0; dsp->ws_lpIQ = dsp->ws_lpIQ0; - //taps = lowpass_update(1.5*dsp->lpIQ_fbw, dsp->lpIQtaps, dsp->ws_lpIQ); if (taps < 0) return -1; } - // locked: - //taps = lowpass_update(dsp->lpIQ_fbw, dsp->lpIQtaps, dsp->ws_lpIQ); if (taps < 0) return -1; } // FM lowpass - if (dsp->opt_lp & 2) + if (dsp->opt_lp & LP_FM) { float f_lp; // lowpass_bw int taps; // lowpass taps: 4*sr/transition_bw @@ -1200,8 +1245,9 @@ int init_buffers_Lband(dsp_t *dsp) { f_lp = 10e3/(float)dsp->sr; // default if (dsp->lpFM_bw > 0) f_lp = dsp->lpFM_bw/(float)dsp->sr; taps = 4*dsp->sr/FM_TRANSITION_BW; - if (dsp->decFM > 1) { - f_lp *= 2.0; + if (dsp->decFM > 1) + { + f_lp *= 2; //if (dsp->opt_iq >= 2 && dsp->opt_iq < 6) f_lp *= 2; taps = taps/2; } if (dsp->sr > 100e3) taps = taps/2; @@ -1215,6 +1261,32 @@ int init_buffers_Lband(dsp_t *dsp) { if (dsp->lpFM_buf == NULL) return -1; } + // IQFM lowpass + if (dsp->opt_lp & LP_IQFM) // opt_iq==5 + { + float f_lp; // lowpass_bw + int taps; // lowpass taps: 4*sr/transition_bw + + f_lp = 10e3/(float)dsp->sr; // default + //if (dsp->lpFM_bw > 0) f_lp = dsp->lpFM_bw/(float)dsp->sr; + taps = 4*dsp->sr/FM_TRANSITION_BW; + //if (dsp->decFM > 1) + { + f_lp *= 2.0*2; + taps = taps/2; + } + if (dsp->sr > 100e3) taps = taps/2; + if (dsp->sr > 200e3) taps = taps/2; + taps = taps/2; + taps = taps/2; + if (taps%2==0) taps++; + taps = lowpass_init(f_lp, taps, &dsp->ws_lpIQFM); if (taps < 0) return -1; + + dsp->lpIQFMtaps = taps; + dsp->lpIQFM_buf = calloc( dsp->lpIQFMtaps+3, sizeof(float complex)); + if (dsp->lpIQFM_buf == NULL) return -1; + } + memset(&IQdc, 0, sizeof(IQdc)); IQdc.maxlim = dsp->sr; IQdc.maxcnt = IQdc.maxlim/32; // 32,16,8,4,2,1 @@ -1357,23 +1429,32 @@ int free_buffers(dsp_t *dsp) { { if (dsp->decXbuffer) { free(dsp->decXbuffer); dsp->decXbuffer = NULL; } if (dsp->decMbuf) { free(dsp->decMbuf); dsp->decMbuf = NULL; } - if (dsp->ex) { free(dsp->ex); dsp->ex = NULL; } + if (!dsp->opt_nolut) { + if (dsp->ex) { free(dsp->ex); dsp->ex = NULL; } + } if (ws_dec) { free(ws_dec); ws_dec = NULL; } } // IF lowpass - if (dsp->opt_iq && (dsp->opt_lp & 1)) + if (dsp->opt_iq && (dsp->opt_lp & LP_IQ)) { if (dsp->ws_lpIQ0) { free(dsp->ws_lpIQ0); dsp->ws_lpIQ0 = NULL; } if (dsp->ws_lpIQ1) { free(dsp->ws_lpIQ1); dsp->ws_lpIQ1 = NULL; } if (dsp->lpIQ_buf) { free(dsp->lpIQ_buf); dsp->lpIQ_buf = NULL; } } - if (dsp->opt_lp & 2) + // FM lowpass + if (dsp->opt_lp & LP_FM) { if (dsp->ws_lpFM) { free(dsp->ws_lpFM); dsp->ws_lpFM = NULL; } if (dsp->lpFM_buf) { free(dsp->lpFM_buf); dsp->lpFM_buf = NULL; } } + // IQFM lowpass + if (dsp->opt_lp & LP_IQFM) + { + if (dsp->ws_lpIQFM) { free(dsp->ws_lpIQFM); dsp->ws_lpIQFM = NULL; } + if (dsp->lpIQFM_buf) { free(dsp->lpIQFM_buf); dsp->lpIQFM_buf = NULL; } + } if (dsp->fm_buffer) { free(dsp->fm_buffer); dsp->fm_buffer = NULL; } @@ -1404,23 +1485,22 @@ int find_header(dsp_t *dsp, float thres, int hdmax, int bitofs, int opt_dc) { continue; } - if (dsp->mv > thres || dsp->mv < -thres) + if ( dsp->mv > thres || dsp->mv < -thres || + dsp->mv2 > thres || dsp->mv2 < -thres ) { if (dsp->opt_dc) { - dsp->Df += dsp->dDf*0.4; + dsp->Df += dsp->dDf*0.5; if (dsp->opt_iq) { if (fabs(dsp->dDf) > 20*1e3) { // L-band if (dsp->locked) { dsp->locked = 0; dsp->ws_lpIQ = dsp->ws_lpIQ0; - // alt: lowpass_update(1.5*dsp->lpIQ_fbw, dsp->lpIQtaps, dsp->ws_lpIQ); } } else { if (dsp->locked == 0) { dsp->locked = 1; dsp->ws_lpIQ = dsp->ws_lpIQ1; - // alt: lowpass_update(dsp->lpIQ_fbw, dsp->lpIQtaps, dsp->ws_lpIQ); } } } @@ -1857,7 +1937,7 @@ static void print_frame(gpx_t *gpx, int len, dsp_t *dsp) { } printf("\n"); } - + //else // - Print raw *and* JSON data if enabled. { if (gpx->frame_bytes[OFS] == 0x4D && len/BITS > pos_FullID+4) { if ( !crc_err ) { @@ -1932,6 +2012,11 @@ static void print_frame(gpx_t *gpx, int len, dsp_t *dsp) { if (gpx->jsn_freq > 0) { printf(", \"freq\": %d", gpx->jsn_freq); } + + // Reference time/position + printf(", \"ref_datetime\": \"%s\"", "GPS" ); // {"GPS", "UTC"} GPS-UTC=leap_sec + printf(", \"ref_position\": \"%s\"", "GPS" ); // {"GPS", "MSL"} GPS=ellipsoid , MSL=geoid + #ifdef VER_JSN_STR ver_jsn = VER_JSN_STR; #endif @@ -1972,6 +2057,7 @@ int main(int argc, char **argv) { int option_lp = 0; int option_dc = 0; int option_decFM = 0; + int option_noLUT = 0; int k; @@ -2055,21 +2141,27 @@ int main(int argc, char **argv) { if (fq > 0.5) fq = 0.5; dsp.xlt_fq = -fq; // S(t) -> S(t)*exp(-f*2pi*I*t) } - else if (strcmp(*argv, "--lpIQ") == 0) { option_lp |= 1; } // IQ lowpass + else if (strcmp(*argv, "--lpIQ") == 0) { option_lp |= LP_IQ; } // IQ lowpass else if (strcmp(*argv, "--lpbw") == 0) { // IQ lowpass BW / kHz double bw = 0.0; ++argv; if (*argv) bw = atof(*argv); else return -1; if (bw > 100.0 && bw < 240.0) lpIQ_bw = bw*1e3; - option_lp |= 1; + option_lp |= LP_IQ; } - else if (strcmp(*argv, "--lpFM") == 0) { option_lp |= 2; } // FM lowpass + else if (strcmp(*argv, "--lpFM") == 0) { option_lp |= LP_FM; } // FM lowpass else if (strcmp(*argv, "--decFM") == 0) { // FM decimation - option_lp |= 2; + option_decFM = 4; + } + else if (strcmp(*argv, "--decFM2") == 0) { // FM decimation + option_decFM = 2; + } + else if (strcmp(*argv, "--decFM1") == 0) { // FM decimation option_decFM = 1; } else if (strcmp(*argv, "--dc") == 0) { option_dc = 1; } + else if (strcmp(*argv, "--noLUT") == 0) { option_noLUT = 1; } else if (strcmp(*argv, "--min") == 0) { option_min = 1; } @@ -2154,17 +2246,19 @@ int main(int argc, char **argv) { dsp.br = (float)BAUD_RATE; if (option_decFM) { + if (option_iq == 5) option_lp |= LP_IQFM; + else option_lp |= LP_FM; if (dsp.sr > 4*44000) dsp.opt_fmdec = 1; } dsp.sps = (float)dsp.sr/dsp.br; dsp.decFM = 1; if (dsp.opt_fmdec) { - dsp.decFM = FM_DEC; + dsp.decFM = option_decFM; while (dsp.sr % dsp.decFM > 0 && dsp.decFM > 1) dsp.decFM /= 2; dsp.sps /= (float)dsp.decFM; } - if (option_iq == 5 && option_dc) option_lp |= 2; + if (option_iq == 5 && option_dc) option_lp |= LP_FM; dsp.symlen = symlen; dsp.symhd = 1; @@ -2179,6 +2273,7 @@ int main(int argc, char **argv) { dsp.lpIQ_bw = lpIQ_bw; // IF lowpass bandwidth dsp.lpFM_bw = 10e3; // FM audio lowpass iq0: 10e3 , iq 0.0: 7e3-8e3 if (option_iq == 6) dsp.lpFM_bw = 6.8e3; + else if (option_iq == 5) dsp.lpFM_bw = 8e3; dsp.opt_dc = option_dc; dsp.opt_IFmin = option_min; @@ -2192,6 +2287,12 @@ int main(int argc, char **argv) { fprintf(stderr, "sps corr: %.4f\n", dsp.sps); } + // LUT faster, however frequency correction after decimation + // LUT recommonded if decM > 2 + // + if (option_noLUT && option_iq >= 5) dsp.opt_nolut = 1; else dsp.opt_nolut = 0; + + k = init_buffers_Lband(&dsp); if ( k < 0 ) { fprintf(stderr, "error: init buffers\n");