#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <math.h>

#ifdef CYGWIN

#include <fcntl.h> // cygwin: _setmode()

#include <io.h>

#ifdef VERSION_JSN

#include "version_jsn.h"

#include "demod_mod.h"

#ifdef INCLUDESTATIC

#include "bch_ecc_mod.c"

#include "bch_ecc_mod.h"

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 aux; // aux/ozone

i8_t jsn; // JSON output (auto_rx)

i8_t ngp;

i8_t dbg;

} option_t;

typedef struct {

int typ;

int msglen;

int msgpos;

int parpos;

int hdrlen;

int frmlen;

} rscfg_t;

static rscfg_t cfg_rs92 = { 92, 240-6-24, 6, 240-24, 6, 240};

#define BITS (1+8+1) // 10

#define FRAMESTART 6 //((HEADLEN)/BITS)

#define FRAME_LEN 240

static char rs92_rawheader[] = //"10100110011001101001"

//"10100110011001101001"

//"10100110011001101001"

"10100110011001101001"

"1010011001100110100110101010100110101001";

static ui8_t rs92_header_bytes[6] = { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10};

#include "nav_gps_vel.c"

typedef struct {

i8_t opt_vergps;

i8_t opt_iter;

i8_t opt_vel;

float dop_limit; // 9.9

float d_err; // 10000

int almanac;

int ephem;

int exSat; // -1

ui8_t WEEK1024epoch; // SEM almanac, GPS epoch (1: 1999-2019)

ui8_t sat_status[12];

ui8_t prn[12]; // valide PRN 0,..,k-1

ui8_t prn32toggle; // 0x1

ui8_t prn32next;

EPHEM_t alm[33];

EPHEM_t *ephs;

SAT_t sat[33];

SAT_t sat1s[33];

} GPS_t;

typedef struct {

int frnr;

char id[11];

int week; 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 vU;

int sats[4];

double dop;

ui16_t conf_kt; // kill timer (sec)

int freq; // freq/kHz (RS92)

int jsn_freq; // freq/kHz (SDR)

ui32_t crc;

ui8_t frame[FRAME_LEN]; // { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}

//

ui8_t cal_state[2];

ui8_t calfrms;

ui8_t calibytes[32*16];

ui8_t calfrchk[32];

float cal_f32[256];

float T;

float _RH; float RH;

float _P; float P;

//

ui8_t xcal16[16];

ui8_t xptu16[16];

int rs_type;

//

unsigned short aux[4];

double diter;

option_t option;

RS_t RS;

GPS_t gps;

} gpx_t;

#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};

#define BAUD_RATE 4800

static int bits2byte(char bits[]) {

int i, byteval=0, d=1;

//if (bits[0] != 0) return 0x100; // erasure?

//if (bits[9] != 1) return 0x100; // erasure?

for (i = 1; i <= 8; i++) { // little endian

/* for (i = 8; i > 1; i--) { // big endian */

if (bits[i] == 1) byteval += d;

else if (bits[i] == 0) byteval += 0;

d <<= 1;

}

return byteval;

}

#define WEEKSEC 604800

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;

}

#define RS92SGP 0

#define RS92AGP 1

#define RS92NGP 2

#define crc_FRAME (1<<0)

#define pos_FrameNb 0x08 // 2 byte

#define pos_SondeID 0x0C // 8 byte // oder: 0x0A, 10 byte?

#define pos_CalData 0x17 // 1 byte, counter 0x00..0x1f

#define pos_Calfreq 0x1A // 2 byte, calfr 0x00

#define crc_GPS (1<<2)

#define posGPS_TOW 0x48 // 4 byte

#define posGPS_PRN 0x4E // 12*5 bit in 8 byte

#define posGPS_STATUS 0x56 // 12 byte

#define posGPS_DATA 0x62 // 12*8 byte

#define crc_PTU (1<<1)

#define pos_PTU 0x2C // 24 byte

#define crc_AUX (1<<3)

#define pos_AUX 0xC6 // 10 byte

#define pos_AuxData 0xC8 // 8 byte

#define BLOCK_CFG 0x6510 // frame[pos_FrameNb-2], frame[pos_FrameNb-1]

#define BLOCK_PTU 0x690C

#define BLOCK_GPS 0x673D // frame[posGPS_TOW-2], frame[posGPS_TOW-1]

#define BLOCK_AUX 0x6805

#define LEN_CFG (2*(BLOCK_CFG & 0xFF))

#define LEN_GPS (2*(BLOCK_GPS & 0xFF))

#define LEN_PTU (2*(BLOCK_PTU & 0xFF))

#define LEN_AUX (2*(BLOCK_AUX & 0xFF))

static int crc16(gpx_t *gpx, int start, int len) {

int crc16poly = 0x1021;

int rem = 0xFFFF, i, j;

int byte;

if (start+len >= 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 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 ui8_t rs92calx170[16] = {

0x36, 0x98, 0x92, 0x25, 0x6b, 0xb3, 0x99, 0xe1, 0x57, 0x05, 0x30, 0x9a, 0xfe, 0x51, 0xf4, 0xab}; // 0x170

static int chk_toggle_type(gpx_t *gpx) {

int toggle = 0;

int n;

// constant rs92-coeffs

//gpx->xcal16[ 0] == 0 && gpx->xcal16[ 1] == 0 &&

//gpx->xcal16[ 5] == 0 && gpx->xcal16[ 6] == 0 &&

//gpx->xcal16[10] == 0 && gpx->xcal16[11] == 0

if ( memcmp(gpx->calibytes+0x170, rs92calx170, 16) == 0 ) {

gpx->rs_type = RS92SGP;

}

else {

gpx->rs_type = RS92NGP;

}

// rs92sgp: conf/calib data after 0x40+0x14A ... zero ?

// check ptu-float32 plausibility

if (gpx->rs_type == RS92SGP && gpx->option.ngp) toggle = 1;

if (gpx->rs_type == RS92NGP && !gpx->option.ngp) toggle = 2;

if (toggle) gpx->option.ngp ^= 1;

return toggle;

}

static int xor_ptu(gpx_t *gpx) {

int j, k;

ui32_t a, c, tmp;

ui8_t *pcal = gpx->calibytes+0x24;

for (j = 0; j < 8; j++) {

tmp = 0x1d89;

for (k = 0; k < 4; k++) {

a = pcal[j+k] & 0xFF;

c = tmp;

//add(A, C, A);

a = a + c;

c = a;

//shl_add(A, 10, C, A);

a = (a << 10) + c;

c = a;

//shr_xor(A, 6, C, A);

a = (a >> 6) ^ c;

tmp = a;

}

a = tmp;

c = a;

//shl_add(A, 3, C, A);

a = (a << 3) + c;

c = a;

//shr_xor(A, 11, C, A);

a = (a >> 11) ^ c;

c = a;

//shl_add(A, 15, C, A);

a = (a << 15) + c;

//y = a & 0xFFFF;

gpx->xptu16[2*j ] = a & 0xFF;

gpx->xptu16[2*j+1] = (a>>8) & 0xFF;

}

return 0;

}

static int get_SondeID(gpx_t *gpx) {

int i, ret=0;

unsigned byte;

ui8_t sondeid_bytes[10];

ui8_t calfr;

int crc_frame, crc;

// BLOCK_CFG == frame[pos_FrameNb-2 .. pos_FrameNb-1] ?

crc_frame = gpx->frame[pos_FrameNb+LEN_CFG] | (gpx->frame[pos_FrameNb+LEN_CFG+1] << 8);

crc = crc16(gpx, pos_FrameNb, LEN_CFG);

if (crc_frame != crc) gpx->crc |= crc_FRAME;

ret = 0;

if (gpx->option.crc && crc != crc_frame) {

ret = -2; // erst wichtig, wenn Cal/Cfg-Data

}

if (crc == crc_frame)

{

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 ) {

memset(gpx->calibytes, 0, 32*16);

memset(gpx->calfrchk, 0, 32);

memset(gpx->cal_f32, 0, 256*4);

gpx->calfrms = 0;

gpx->T = -275.15f;

gpx->_RH = -1.0f;

gpx->_P = -1.0f;

gpx->RH = -1.0f;

gpx->P = -1.0f;

// new ID:

memcpy(gpx->id, sondeid_bytes, 8);

}

memcpy(gpx->cal_state, gpx->frame+(pos_FrameNb + 12), 2);

calfr = gpx->frame[pos_CalData]; // 0..31

if (calfr < 32) {

if (gpx->calfrchk[calfr] == 0) // const?

{

for (i = 0; i < 16; i++) {

gpx->calibytes[calfr*16 + i] = gpx->frame[pos_CalData+1+i];

}

gpx->calfrchk[calfr] = 1;

}

}

if (gpx->calfrms < 32) {

gpx->calfrms = 0;

for (i = 0; i < 32; i++) gpx->calfrms += (gpx->calfrchk[i]>0);

}

if (gpx->calfrms == 32)

{

ui8_t xcal[66*5];

ui8_t *xcal16 = gpx->xcal16;

ui8_t *p = gpx->calibytes+0x170;

ui8_t *q = rs92calx170;

int cal_chk = 0;

gpx->calfrms += 1;

xor_ptu(gpx);

if (gpx->option.dbg) {

printf("XPTU:"); for (int j = 0; j < 16; j++) printf(" %02X", gpx->xptu16[j]); printf("\n");

}

//Xx17: __ 98 __ __ __ __ 99 __ __ __ __ 9a __ __ __ __

// p[0], p[1]=idx, p[2+1], p[3+1], p[4-2], p[5], p[6]=idx, ...

for (int k = 0; k < 3; k++) {

xcal16[5*k] = p[5*k]^q[5*k];

xcal16[5*k+1] = p[5*k+1]^q[5*k+1];

xcal16[5*k+2+1] = p[5*k+2+1]^q[5*k+2];

xcal16[5*k+3+1] = p[5*k+3+1]^q[5*k+3];

xcal16[5*k+4-2] = p[5*k+4-2]^q[5*k+4];

}

xcal16[5*3] = p[5*3]^q[5*3];

if (gpx->option.dbg) {

printf("XCAL:"); for (int j = 0; j < 16; j++) printf(" %02X", xcal16[j]); printf("\n");

}

int tgl = chk_toggle_type(gpx);

for (int j = 0; j < 66*5; j++) {

xcal[j] = gpx->calibytes[0x40+j];

if (gpx->option.ngp) {

xcal[j] ^= gpx->xcal16[j%16];

}

}

for (int j = 0; j < 66; j++) {

ui8_t idx = xcal[5*j];

ui8_t *dat = xcal+(5*j+1);

ui32_t le_dat32 = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24);

ui32_t xx_dat32 = dat[1] | (dat[2]<<8) | (dat[0]<<16) | (dat[3]<<24);

float *pf32 = (float*)&le_dat32;

if (gpx->option.ngp) {

pf32 = (float*)&xx_dat32;

}

gpx->cal_f32[idx] = *pf32;

if (gpx->option.dbg)

{

if (idx/10 == 3 || idx/10 == 4 || idx/10 == 5)

{

printf(" %3d :", idx);

for (int i = 1; i < 5; i++) {

printf(" %02x", xcal[5*j+i]);

}

printf(" : %f", *pf32);

printf("\n");

}

}

}

}

}

return ret;

}

static float poly5(float x, float *a) {

float p = 0.0;

p = ((((a[5]*x+a[4])*x+a[3])*x+a[2])*x+a[1])*x+a[0];

return p;

}

static float nu(float t, float t0, float y0) {

// t=1/f2-1/f , t0=1/f2-1/f1 , 1/freq=meas24

float y = t / t0;

return 1.0f / (y0 - y);

}

static int get_Meas(gpx_t *gpx) {

ui32_t temp, pres, hum1, hum2, ref1, ref2, ref3, ref4;

ui8_t *meas24 = gpx->frame+pos_PTU;

float T, U1, U2, _P, _rh, x;

if ( gpx->option.ngp && (gpx->crc & crc_FRAME) ) return -2; // frame number

for (int j = 0; j < 24; j++) {

ui8_t byte = meas24[j];

if (gpx->option.ngp) {

byte ^= gpx->frame[pos_FrameNb+(j&1)];

byte ^= gpx->xptu16[j%16];

}

meas24[j] = byte;

}

temp = meas24[ 0] | (meas24[ 1]<<8) | (meas24[ 2]<<16); // ch1

hum1 = meas24[ 3] | (meas24[ 4]<<8) | (meas24[ 5]<<16); // ch2

hum2 = meas24[ 6] | (meas24[ 7]<<8) | (meas24[ 8]<<16); // ch3

ref1 = meas24[ 9] | (meas24[10]<<8) | (meas24[11]<<16); // ch4

ref2 = meas24[12] | (meas24[13]<<8) | (meas24[14]<<16); // ch5

pres = meas24[15] | (meas24[16]<<8) | (meas24[17]<<16); // ch6

ref3 = meas24[18] | (meas24[19]<<8) | (meas24[20]<<16); // ch7

ref4 = meas24[21] | (meas24[22]<<8) | (meas24[23]<<16); // ch8

if (gpx->calfrms > 0x20)

{

// Temperature

x = nu( (float)(ref1 - temp), (float)(ref1 - ref4), gpx->cal_f32[37] );

T = poly5(x, gpx->cal_f32+30);

if (T > -120.0f && T < 80.0f) gpx->T = T;

else gpx->T = -273.15f;

// rel. Humidity (ref3 or ref4 ?)

x = nu( (float)(ref1 - hum1), (float)(ref1 - ref3), gpx->cal_f32[47] );

U1 = poly5(x, gpx->cal_f32+40); // c[44]=c[45]=0

x = nu( (float)(ref1 - hum2), (float)(ref1 - ref3), gpx->cal_f32[57] );

U2 = poly5(x, gpx->cal_f32+50); // c[54]=c[55]=0

_rh = U1 > U2 ? U1 : U2; // max(U1,U2), vgl. cal_state[1].bit3

gpx->_RH = _rh;

if (gpx->_RH < 0.0f) gpx->_RH = 0.0f;

if (gpx->_RH > 100.0f) gpx->_RH = 100.0f;

// correction for higher RH-sensor temperature (at low temperatures)?

// (cf. amt-7-4463-2014)

// if (T<-60C || P<100hPa): cal_state[1].bit2=0

// (Hyland and Wexler)

// if (T>-60C && P>100hPa): rh = _rh*vaporSatP(Trh)/vaporSatP(T) ...

// estimate Trh ?

// (uncorrected) Pressure

x = nu( (float)(ref1 - pres), (float)(ref1 - ref4), gpx->cal_f32[17] );

_P = poly5(x, gpx->cal_f32+10);

if (_P < 0.0f && _P > 2000.0f) _P = -1.0f;

gpx->_P = _P;

// correction for x and coefficients?

}

return 0;

}

static int get_PTU(gpx_t *gpx) {

int ret=0;

int crc_frame, crc;

crc_frame = gpx->frame[pos_PTU+LEN_PTU] | (gpx->frame[pos_PTU+LEN_PTU+1] << 8);

crc = crc16(gpx, pos_PTU, LEN_PTU);

if (crc_frame != crc) gpx->crc |= crc_PTU;

ret = 0;

if (gpx->option.crc && crc != crc_frame) {

ret = -2;

}

if (ret == 0) {

if (gpx->calfrms > 0x20) ret = get_Meas(gpx);

}

return ret;

}

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];

ui32_t gpstime = 0; // 32bit

int day;

int ms;

int crc_frame, crc;

// BLOCK_GPS == frame[posGPS_TOW-2 .. posGPS_TOW-1] ?

crc_frame = gpx->frame[posGPS_TOW+LEN_GPS] | (gpx->frame[posGPS_TOW+LEN_GPS+1] << 8);

crc = crc16(gpx, posGPS_TOW, LEN_GPS);

if (crc_frame != crc) gpx->crc |= crc_GPS;

ret = 0;

if (gpx->option.crc && crc != crc_frame) {

ret = -2;

}

for (i = 0; i < 4; i++) {

byte = gpx->frame[posGPS_TOW + i];

gpstime_bytes[i] = byte;

}

memcpy(&gpstime, gpstime_bytes, 4);

ms = gpstime % 1000;

gpstime /= 1000;

gpx->gpssec = gpstime;

day = (gpstime / (24 * 3600)) % 7; // besser CRC-check, da auch

//if ((day < 0) || (day > 6)) return -1; // gpssec=604800,604801 beobachtet

gpstime %= (24*3600);

gpx->wday = day;

gpx->std = gpstime / 3600;

gpx->min = (gpstime % 3600) / 60;

gpx->sek = gpstime % 60 + ms/1000.0;

return ret;

}

static int get_Aux(gpx_t *gpx) {

int i, ret=0;

unsigned short byte;

int crc_frame, crc;

crc_frame = gpx->frame[pos_AUX+LEN_AUX] | (gpx->frame[pos_AUX+LEN_AUX+1] << 8);

crc = crc16(gpx, pos_AUX, LEN_AUX);

if (crc_frame != crc) gpx->crc |= crc_AUX;

ret = 0;

if (gpx->option.crc && crc != crc_frame) {

ret = -2;

}

for (i = 0; i < 4; i++) {

byte = gpx->frame[pos_AuxData+2*i] + (gpx->frame[pos_AuxData+2*i+1]<<8);

gpx->aux[i] = byte;

}

return ret;

}

static int get_Cal(gpx_t *gpx) {

int i;

unsigned byte;

ui8_t calfr = 0;

//ui8_t burst = 0;

ui8_t bytes[2];

int freq = 0;

ui16_t killtime = 0;

byte = gpx->frame[pos_CalData];

calfr = byte;

if (gpx->option.vbs == 4) {

fprintf(stdout, "\n");

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 ((gpx->crc & crc_FRAME)==0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]");

}

if (gpx->option.aux) {

if (gpx->option.vbs == 4) {

fprintf(stdout, " # ");

for (i = 0; i < 8; i++) {

byte = gpx->frame[pos_AuxData+i];

fprintf(stdout, "%02x ", byte);

}

}

}

if (calfr == 0x00) {

for (i = 0; i < 2; i++) {

bytes[i] = gpx->frame[pos_Calfreq + i];

}

byte = bytes[0] + (bytes[1] << 8);

//fprintf(stdout, ":%04x ", byte);

freq = 400000 + 10*byte; // kHz;

if (gpx->option.ngp) freq = 1600000 + 10*byte; // kHz

gpx->freq = freq;

fprintf(stdout, ": fq %d", freq);

for (i = 0; i < 2; i++) {

bytes[i] = gpx->frame[pos_Calfreq + 2 + i];

}

killtime = bytes[0] + (bytes[1] << 8); // signed?

if (killtime < 0xFFFF && gpx->option.vbs == 4) {

fprintf(stdout, "; KT:%ds", killtime);

}

gpx->conf_kt = killtime;

}

return 0;

}

static int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) {

int i; /* letztes bit Ueberlauf, wenn 3. PRN = 32 */

for (i = 0; i < 15; i++) {

bits[15*block+i] = byte16 & 1;

byte16 >>= 1;

}

bits[60+block] = byte16 & 1;

return byte16 & 1;

}

static void prn12(GPS_t *gps, ui8_t *prn_le, ui8_t prns[12]) {

int i, j, d;

ui8_t ind_prn32 = 32;

for (i = 0; i < 12; i++) {

prns[i] = 0;

d = 1;

for (j = 0; j < 5; j++) {

if (prn_le[5*i+j]) prns[i] += d;

d <<= 1;

}

}

for (i = 0; i < 12; i++) {

// PRN-32 overflow

if ( (prns[i] == 0) && (gps->sat_status[i] & 0x0F) ) { // 5 bit: 0..31

if ( ((i % 3 == 2) && (prn_le[60+i/3] & 1)) // Spalte 2

|| ((i % 3 != 2) && (prn_le[5*(i+1)] & 1)) ) { // Spalte 0,1

prns[i] = 32; ind_prn32 = i;

}

}

else if ((gps->sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ?

prns[i] = 0;

}

}

gps->prn32next = 0;

if (ind_prn32 < 12) {

// PRN-32 overflow

if (ind_prn32 % 3 != 2) { // -> ind_prn32<11 // vorausgesetzt im Block folgt auf PRN-32

if ((gps->sat_status[ind_prn32+1] & 0x0F) && prns[ind_prn32+1] > 1) { // entweder PRN-1 oder PRN-gerade

// && prns[ind_prn32+1] != 3 ?

for (j = 0; j < ind_prn32; j++) {

if (prns[j] == (prns[ind_prn32+1]^gps->prn32toggle) && (gps->sat_status[j] & 0x0F)) break;

}

if (j < ind_prn32) { gps->prn32toggle ^= 0x1; }

else {

for (j = ind_prn32+2; j < 12; j++) {

if (prns[j] == (prns[ind_prn32+1]^gps->prn32toggle) && (gps->sat_status[j] & 0x0F)) break;

}

if (j < 12) { gps->prn32toggle ^= 0x1; }

}

prns[ind_prn32+1] ^= gps->prn32toggle;

/*

}

gps->prn32next = prns[ind_prn32+1]; // -> ind_prn32<11 && ind_prn32 % 3 != 2

}

}

}

static int calc_satpos_alm(gpx_t *gpx, double t, SAT_t *satp) {

double X, Y, Z, vX, vY, vZ;

int j;

int week;

double cl_corr, cl_drift;

int rollover = 0;

EPHEM_t *alm = gpx->gps.alm;

for (j = 1; j < 33; j++) {

if (alm[j].prn > 0 && alm[j].health == 0) { // prn==j

// Woche hat 604800 sec

if (t-alm[j].toa > WEEKSEC/2) rollover = +1;

else if (t-alm[j].toa < -WEEKSEC/2) rollover = -1;

else rollover = 0;

week = alm[j].week - rollover;

/*if (j == 1)*/ gpx->week = week + gpx->gps.WEEK1024epoch*1024;

if (gpx->gps.opt_vel >= 2) {

GPS_SatellitePositionVelocity_Ephem(

week, t, alm[j],

&cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ

);

satp[alm[j].prn].clock_drift = cl_drift;

satp[alm[j].prn].vX = vX;

satp[alm[j].prn].vY = vY;

satp[alm[j].prn].vZ = vZ;

}

else {

GPS_SatellitePosition_Ephem(

week, t, alm[j],

&cl_corr, &X, &Y, &Z

);

}

satp[alm[j].prn].X = X;

satp[alm[j].prn].Y = Y;

satp[alm[j].prn].Z = Z;

satp[alm[j].prn].clock_corr = cl_corr;

}

}

return 0;

}

static int calc_satpos_rnx2(gpx_t *gpx, double t, SAT_t *satp) {

double X, Y, Z, vX, vY, vZ;

int j;

int week;

double cl_corr, cl_drift;

double tdiff, td;

int count, count0, satfound;

int rollover = 0;

EPHEM_t *eph = gpx->gps.ephs;

for (j = 1; j < 33; j++) {

count = count0 = 0;

satfound = 0;

// Woche hat 604800 sec

tdiff = WEEKSEC;

while (eph[count].prn > 0) {

if (eph[count].prn == j && eph[count].health == 0) {

satfound += 1;

if (t - eph[count].toe > WEEKSEC/2) rollover = +1;

else if (t - eph[count].toe < -WEEKSEC/2) rollover = -1;

else rollover = 0;

td = fabs( t - eph[count].toe - rollover*WEEKSEC);

if ( td < tdiff ) {

tdiff = td;

week = eph[count].week - rollover;

gpx->week = eph[count].gpsweek - rollover;

count0 = count;

}

}

count += 1;

}

if ( satfound )

{

if (gpx->gps.opt_vel >= 2) {

GPS_SatellitePositionVelocity_Ephem(

week, t, eph[count0],

&cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ

);

satp[j].clock_drift = cl_drift;

satp[j].vX = vX;

satp[j].vY = vY;

satp[j].vZ = vZ;

}

else {

GPS_SatellitePosition_Ephem(

week, t, eph[count0],

&cl_corr, &X, &Y, &Z

);

}

satp[j].X = X;

satp[j].Y = Y;

satp[j].Z = Z;

satp[j].clock_corr = cl_corr;

satp[j].ephtime = eph[count0].toe;

}

}

return 0;

}

typedef struct {

ui32_t tow;

ui8_t status;

int chips;

int deltachips;

} RANGE_t;

const double df = 299792.458/1023.0/1024.0; //0.286183844 // c=299792458m/s, 1023000chips/s

const double dl = 1575.42/1.023/4.0; //385.0 // GPS L1 1575.42MHz=154*10.23MHz, dl=154*10/4

static int get_pseudorange(gpx_t *gpx) {

ui32_t gpstime;

ui8_t gpstime_bytes[4];

ui8_t pseudobytes[4];

unsigned chipbytes, deltabytes;

int i, j, k;

ui8_t bytes[4];

ui16_t byte16;

double pr0, prj;

ui8_t prn_le[12*5+4]; // le - little endian

ui8_t prns[12]; // PRNs in data

RANGE_t range[33];

memset(prn_le, 0, sizeof(prn_le));

memset(prns, 0, sizeof(prns));

memset(range, 0, sizeof(range));

// GPS-TOW in ms

for (i = 0; i < 4; i++) {

gpstime_bytes[i] = gpx->frame[posGPS_TOW + i];

}

memcpy(&gpstime, gpstime_bytes, 4);

// Sat Status

for (i = 0; i < 12; i++) {

gpx->gps.sat_status[i] = gpx->frame[posGPS_STATUS + i];