mirror
/
RS-tracker
kopia lustrzana https://github.com/rs1729/RS
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

multi: RS41 fw 0x50dd GNSS, M20 SN revisited

master
Zilog80 2025-02-16 09:36:38 +01:00
rodzic f907b0cd78
commit b9b6680648
2 zmienionych plików z 390 dodań i 138 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

267
demod/multi/m20base.c
Wyświetl plik

@ -65,7 +65,7 @@ static char rawheader[] = "10011001100110010100110010011001";
#define FRAME_LEN (100+1) // 0x64+1
#define BITFRAME_LEN (FRAME_LEN*BITS)
#define AUX_LEN 20
#define AUX_LEN 64
#define BITAUX_LEN (AUX_LEN*BITS)
@ -86,8 +86,10 @@ typedef struct {
double vH; double vD; double vV;
double vx; double vy; double vD2;
float T; float RH; float TH; float P;
float batV;
ui8_t numSV;
ui8_t utc_ofs;
//ui8_t utc_ofs;
ui8_t fwVer;
char SN[12+4];
ui8_t SNraw[3];
ui8_t frame_bytes[FRAME_LEN+AUX_LEN+4];
@ -175,12 +177,12 @@ 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[0x16..0x17]: block check (fwVer < 0x06) ; frame[0x16]: SPI1 P[0] (fwVer >= 0x07), frame[0x17]=0x00
frame[0x18..0x19]: GPS ver.Vel. (velU)
frame[0x1A..0x1B]: GPS week
frame[0x1C..0x1F]: GPS latitude
frame[0x20..0x23]: GPS longitude
frame[0x24..0x25]: SPI1 P[1..2] (if pressure sensor)
frame[0x44..0x45]: frame check
*/
@ -200,7 +202,8 @@ frame[0x44..0x45]: frame check
#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 pos_stdFW 0x43 // 1 byte
#define pos_stdCheck (stdFLEN-1) // 2 byte
#define len_BlkChk 0x16 // frame[0x02..0x17] , incl. chk16
@ -232,6 +235,10 @@ frame[0x44..0x45]: frame check
#define col_CSoo "\x1b[38;5;220m"
#define col_CSno "\x1b[38;5;1m"
#define col_CNST "\x1b[38;5;58m" // 3 byte
#define col_ptuP "\x1b[38;5;180m"
#define col_ptuT "\x1b[38;5;110m"
#define col_ptuU "\x1b[38;5;120m"
#define col_ptuTH "\x1b[38;5;115m"
/*
$ for code in {0..255}
@ -239,6 +246,9 @@ $ for code in {0..255}
> done
*/
#define COLOPT(tcol) ((gpx->option.col)?(tcol):(""))
static int get_GPSweek(gpx_t *gpx) {
int i;
unsigned byte;
@ -428,12 +438,10 @@ static int get_GPSvel(gpx_t *gpx) {
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
ui32_t sn24 = (gpx->frame_bytes[pos_SN+2]<<16) | (gpx->frame_bytes[pos_SN+1]<<8) | gpx->frame_bytes[pos_SN];
ui8_t ym = sn24 & 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';
@ -441,15 +449,14 @@ static int get_SN(gpx_t *gpx) {
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);
sprintf(gpx->SN, "%u%02u", y, m);
sprintf(gpx->SN+3, "-%u-", ((sn24>> 7)&0x7)+1);
sprintf(gpx->SN+6, "%u", (sn24>>23)&0x1);
sprintf(gpx->SN+7, "%04u", (sn24>>10)&0x1FFF);
if (sn_val == 0)
if (sn24 == 0)
{ // get_GPStime(gpx);
// replace SN: 001-2-00000 -> 000-0-00000-[_diffcnt]
sprintf(gpx->SN, "%s", "000-0-00000");
@ -683,18 +690,36 @@ static float get_RH(gpx_t *gpx) {
}
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];
ui32_t val = (gpx->frame_bytes[0x25] << 8) | gpx->frame_bytes[0x24]; // cf. DF9DQ
ui8_t p0 = 0x00;
if (gpx->fwVer >= 0x07) { // SPI1_P[0]
p0 = gpx->frame_bytes[0x16];
}
val = (val << 8) | p0;
if (val > 0) {
hPa = val/16.0f;
hPa = val/(float)(16*256); // 4096=0x1000
}
if (hPa > 2560.0f) { // val > 0xA00000
hPa = -1.0f;
}
return hPa;
}
static float get_BatV(gpx_t *gpx) {
float batV = 0.0f;
ui8_t val = gpx->frame_bytes[0x26]; // cf. DF9DQ
batV = val * (3.3f/255); // upper 8 bits ADC
return batV;
}
/* -------------------------------------------------------------------------- */
static int print_pos(gpx_t *gpx, int bcOK, int csOK) {
@ -723,88 +748,59 @@ static int print_pos(gpx_t *gpx, int bcOK, int csOK) {
gpx->P = get_P(gpx); // (optional) pressure
}
gpx->batV = get_BatV(gpx); // battery V
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"%4.1f"col_TXT" D: "col_GPSvel"%5.1f"col_TXT" vV: "col_GPSvel"%3.1f"col_TXT" ", gpx->vH, gpx->vD, gpx->vV);
}
if (gpx->option.vbs >= 1 && (bcOK || csOK)) { // SN
fprintf(stdout, " SN: "col_SN"%s"col_TXT, gpx->SN);
}
if (gpx->option.vbs >= 1) {
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->option.vbs >= 2) {
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"");
fprintf(stdout, "%s", COLOPT(col_TXT));
if (gpx->option.vbs >= 3) {
fprintf(stdout, "[%3d]", gpx->frame_bytes[pos_CNT]);
fprintf(stdout, " (W %s%d%s) ", COLOPT(col_GPSweek), gpx->week, COLOPT(col_TXT));
}
else {
if (gpx->option.vbs >= 3) {
fprintf(stdout, "[%3d]", gpx->frame_bytes[pos_CNT]);
fprintf(stdout, " (W %d) ", gpx->week);
fprintf(stdout, "%s%s%s ", COLOPT(col_GPSTOW), weekday[gpx->wday], COLOPT(col_TXT));
fprintf(stdout, "%s%04d-%02d-%02d%s %s%02d:%02d:%06.3f%s ",
COLOPT(col_GPSdate), gpx->jahr, gpx->monat, gpx->tag, COLOPT(col_TXT),
COLOPT(col_GPSTOW), gpx->std, gpx->min, gpx->sek, COLOPT(col_TXT));
fprintf(stdout, " lat: %s%.5f%s ", COLOPT(col_GPSlat), gpx->lat, COLOPT(col_TXT));
fprintf(stdout, " lon: %s%.5f%s ", COLOPT(col_GPSlon), gpx->lon, COLOPT(col_TXT));
fprintf(stdout, " alt: %s%.2f%s ", COLOPT(col_GPSalt), gpx->alt, COLOPT(col_TXT));
if (!err2) {
fprintf(stdout, " vH: %s%4.1f%s D: %s%5.1f%s vV: %s%3.1f%s ",
COLOPT(col_GPSvel), gpx->vH, COLOPT(col_TXT),
COLOPT(col_GPSvel), gpx->vD, COLOPT(col_TXT),
COLOPT(col_GPSvel), gpx->vV, COLOPT(col_TXT));
}
if (gpx->option.vbs >= 1 && (bcOK || csOK)) { // SN
fprintf(stdout, " SN: %s%s%s", COLOPT(col_SN), gpx->SN, COLOPT(col_TXT));
}
if (gpx->option.vbs >= 1) {
fprintf(stdout, " # ");
if (gpx->fwVer < 0x07) {
if (bcOK > 0) fprintf(stdout, " %s(ok)%s", COLOPT(col_CSok), COLOPT(col_TXT));
else if (bcOK < 0) fprintf(stdout, " %s(oo)%s", COLOPT(col_CSoo), COLOPT(col_TXT));
else fprintf(stdout, " %s(no)%s", COLOPT(col_CSno), COLOPT(col_TXT));
}
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: %4.1f D: %5.1f vV: %3.1f ", gpx->vH, gpx->vD, gpx->vV);
if (csOK) fprintf(stdout, " %s[OK]%s", COLOPT(col_CSok), COLOPT(col_TXT));
else fprintf(stdout, " %s[NO]%s", COLOPT(col_CSno), COLOPT(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->option.vbs >= 2) {
if (gpx->TH > -273.0f) fprintf(stdout, " TH:%.1fC", gpx->TH);
}
if (gpx->option.vbs >= 1 && (bcOK || csOK)) { // SN
fprintf(stdout, " SN: %s", gpx->SN);
}
if (gpx->option.vbs >= 1) {
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->option.vbs >= 2) {
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);
}
if (gpx->P > 0.0f) {
if (gpx->P < 10.0f) fprintf(stdout, " P=%.3fhPa ", gpx->P);
else if (gpx->P < 100.0f) fprintf(stdout, " P=%.2fhPa ", gpx->P);
else fprintf(stdout, " P=%.1fhPa ", gpx->P);
}
}
if (gpx->option.vbs >= 3 && csOK) {
fprintf(stdout, " (bat:%.2fV)", gpx->batV);
}
fprintf(stdout, "%s", COLOPT(ANSI_COLOR_RESET));
fprintf(stdout, "\n");
}
@ -827,6 +823,7 @@ static int print_pos(gpx_t *gpx, int bcOK, int csOK) {
if (gpx->RH > -0.5f) fprintf(stdout, ", \"humidity\": %.1f", gpx->RH );
if (gpx->P > 0.0f) fprintf(stdout, ", \"pressure\": %.2f", gpx->P );
}
fprintf(stdout, ", \"batt\": %.2f", gpx->batV);
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) {
@ -852,7 +849,9 @@ static int print_frame(gpx_t *gpx, int pos, dsp_t *dsp) {
ui8_t byte;
int cs1, cs2;
int bc1, bc2, bc;
int flen = stdFLEN; // stdFLEN=0x64, auxFLEN=0x76; M20:0x45 ?
int flen = stdFLEN; // M10:stdFLEN=0x64,auxFLEN=0x76; M20:stdFLEN=0x45,auxFLEN=0x6F ?
int pos_fw = pos_stdFW;
int pos_check = pos_stdCheck;
bits2bytes(gpx->frame_bits, gpx->frame_bytes);
flen = gpx->frame_bytes[0];
@ -860,10 +859,21 @@ static int print_frame(gpx_t *gpx, int pos, dsp_t *dsp) {
else {
gpx->auxlen = flen - stdFLEN;
//if (gpx->auxlen < 0 || gpx->auxlen > AUX_LEN) gpx->auxlen = 0; // 0x43,0x45
if (gpx->auxlen < 0) {
gpx->auxlen = 0;
pos_fw = flen-2; // only if flen < stdFLEN
}
else if (gpx->auxlen > AUX_LEN) {
gpx->auxlen = AUX_LEN;
flen = stdFLEN+AUX_LEN;
}
}
pos_check = flen-1;
gpx->fwVer = gpx->frame_bytes[pos_fw];
if (gpx->fwVer > 0x20) gpx->fwVer = 0;
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);
cs1 = (gpx->frame_bytes[pos_check] << 8) | gpx->frame_bytes[pos_check+1];
cs2 = checkM10(gpx->frame_bytes, pos_check);
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
@ -881,35 +891,46 @@ static int print_frame(gpx_t *gpx, int pos, dsp_t *dsp) {
if (gpx->option.raw) {
if (gpx->option.col /* && gpx->frame_bytes[1] != 0x49 */) {
fprintf(stdout, col_FRTXT);
if (1 /*&& gpx->frame_bytes[1] != 0x49 */) {
fprintf(stdout, "%s", COLOPT(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);
if (i == 1) fprintf(stdout, "%s", COLOPT(col_Mtype));
if ((i >= pos_GPSTOW) && (i < pos_GPSTOW+3)) fprintf(stdout, "%s", COLOPT(col_GPSTOW));
if ((i >= pos_GPSlat) && (i < pos_GPSlat+4)) fprintf(stdout, "%s", COLOPT(col_GPSlat));
if ((i >= pos_GPSlon) && (i < pos_GPSlon+4)) fprintf(stdout, "%s", COLOPT(col_GPSlon));
if ((i >= pos_GPSalt) && (i < pos_GPSalt+3)) fprintf(stdout, "%s", COLOPT(col_GPSalt));
if ((i >= pos_GPSweek) && (i < pos_GPSweek+2)) fprintf(stdout, "%s", COLOPT(col_GPSweek));
if ((i >= pos_GPSvE) && (i < pos_GPSvE+2)) fprintf(stdout, "%s", COLOPT(col_GPSvel));
if ((i >= pos_GPSvN) && (i < pos_GPSvN+2)) fprintf(stdout, "%s", COLOPT(col_GPSvel));
if ((i >= pos_GPSvU) && (i < pos_GPSvU+2)) fprintf(stdout, "%s", COLOPT(col_GPSvel));
if ((i >= pos_SN) && (i < pos_SN+3)) fprintf(stdout, "%s", COLOPT(col_SN));
if (i == pos_CNT) fprintf(stdout, "%s", COLOPT(col_CNT));
if (gpx->fwVer < 0x07) {
if ((i >= pos_BlkChk) && (i < pos_BlkChk+2)) fprintf(stdout, "%s", COLOPT(col_Check));
} else {
if ((i >= pos_BlkChk+1) && (i < pos_BlkChk+2)) fprintf(stdout, "%s", COLOPT(col_Check));
}
if (i >= 0x02 && i <= 0x03) fprintf(stdout, "%s", COLOPT(col_ptuU));
if (i >= 0x04 && i <= 0x05) fprintf(stdout, "%s", COLOPT(col_ptuT));
if (i >= 0x06 && i <= 0x07) fprintf(stdout, "%s", COLOPT(col_ptuTH));
if (i == 0x16 && gpx->fwVer >= 0x07 || i >= 0x24 && i <= 0x25) fprintf(stdout, "%s", COLOPT(col_ptuP));
if ((i >= pos_check) && (i < pos_check+2)) fprintf(stdout, "%s", COLOPT(col_Check));
fprintf(stdout, "%02x", byte);
fprintf(stdout, col_FRTXT);
fprintf(stdout, "%s", COLOPT(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, " # %s%04x%s", COLOPT(col_Check), cs2, COLOPT(col_FRTXT));
if (gpx->fwVer < 0x07) {
if (bc > 0) fprintf(stdout, " %s(ok)%s", COLOPT(col_CSok), COLOPT(col_TXT));
else if (bc < 0) fprintf(stdout, " %s(oo)%s", COLOPT(col_CSoo), COLOPT(col_TXT));
else fprintf(stdout, " %s(no)%s", COLOPT(col_CSno), COLOPT(col_TXT));
}
if (cs1 == cs2) fprintf(stdout, " %s[OK]%s", COLOPT(col_CSok), COLOPT(col_TXT));
else fprintf(stdout, " %s[NO]%s", COLOPT(col_CSno), COLOPT(col_TXT));
}
fprintf(stdout, ANSI_COLOR_RESET"\n");
fprintf(stdout, "%s\n", COLOPT(ANSI_COLOR_RESET));
}
else {
for (i = 0; i < flen+1; i++) {
@ -918,9 +939,11 @@ static int print_frame(gpx_t *gpx, int pos, dsp_t *dsp) {
}
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 (gpx->fwVer < 0x07) {
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");

261
demod/multi/rs41base.c
Wyświetl plik

@ -67,11 +67,21 @@ static rscfg_t cfg_rs41 = { 41, (320-56)/2, 56, 8, 8, 320}; // const: msgpos, pa
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 {
ui8_t id168;
ui8_t status;
} gnss_t;
typedef struct {
int out;
int frnr;
char id[9];
ui8_t numSV;
ui8_t gnss_numSVb168;
ui8_t gnss_nSVstatus;
gnss_t gnss_sv[32];
ui8_t isUTC;
int week; int tow_ms; int gpssec;
int jahr; int monat; int tag;
int wday;
@ -106,6 +116,7 @@ typedef struct {
ui16_t conf_cd; // kill countdown (sec) (kt or bt)
ui8_t conf_bk; // burst kill
char rstyp[9]; // RS41-SG, RS41-SGP
char rsm[10]; // RSM421
int aux;
char xdata[XDATA_LEN+16]; // xdata: aux_str1#aux_str2 ...
option_t option;
@ -232,15 +243,15 @@ float r4(ui8_t *bytes) {
}
*/
static int crc16(gpx_t *gpx, int start, int len) {
static int crc16(ui8_t data[], int len) {
int crc16poly = 0x1021;
int rem = 0xFFFF, i, j;
int byte;
if (start+len+2 > FRAME_LEN) return -1;
//if (start+len+2 > FRAME_LEN) return -1;
for (i = 0; i < len; i++) {
byte = gpx->frame[start+i];
byte = data[i];
rem = rem ^ (byte << 8);
for (j = 0; j < 8; j++) {
if (rem & 0x8000) {
@ -263,7 +274,7 @@ static int check_CRC(gpx_t *gpx, ui32_t pos, ui32_t pck) {
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) ) {
if ( crcdat != crc16(gpx->frame+pos+2, crclen) ) {
return 1; // CRC NO
}
else return 0; // CRC OK
@ -296,7 +307,8 @@ GPS chip: ublox UBX-G6010-ST
#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
// weitere chars in calfr 0x22/0x23; weitere ID (RSM)
#define pos_CalRSM 0x055 // 6 byte, calfr 0x22
#define crc_PTU (1<<1)
#define xor_PTU 0xE388 // ^0x99A2=0x0x7A2A
@ -343,6 +355,12 @@ GPS chip: ublox UBX-G6010-ST
#define pck_SGM_xTU 0x7F1B // temperature/humidity
#define pck_SGM_CRYPT 0x80A7 // Packet type for an Encrypted payload
// fw 0x50dd
#define pck_960A 0x960A //
#define pck_8226_POSDATETIME 0x8226 // ECEF-POS/VEL , DATE/TIME
#define pck_8329_SATS 0x8329 // GNSS sats
/*
frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320)
frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518)
@ -408,6 +426,7 @@ static int get_SondeID(gpx_t *gpx, int crc, int ofs) {
memset(gpx->calfrchk, 0, 51); // 0x00..0x32
// reset conf data
memset(gpx->rstyp, 0, 9);
memset(gpx->rsm, 0, 10);
gpx->freq = 0;
gpx->conf_fw = 0;
gpx->conf_bt = 0;
@ -421,6 +440,10 @@ static int get_SondeID(gpx_t *gpx, int crc, int ofs) {
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;
gpx->gnss_numSVb168 = 0;
gpx->gnss_nSVstatus = 0;
memset(gpx->gnss_sv, 0, 32*sizeof(gnss_t)); // gpx->gnss_sv[i].id168 = 0; gpx->gnss_sv[i].status = 0;
gpx->isUTC = 0;
gpx->T = -273.15f;
gpx->RH = -1.0f;
gpx->P = -1.0f;
@ -875,6 +898,7 @@ static int get_GPStime(gpx_t *gpx, int ofs) {
gpx->std = gpstime / 3600;
gpx->min = (gpstime % 3600) / 60;
gpx->sek = gpstime % 60 + ms/1000.0;
gpx->isUTC = 0;
return 0;
}
@ -889,6 +913,7 @@ static int get_GPS1(gpx_t *gpx, int ofs) {
// reset GPS1-data (json)
gpx->jahr = 0; gpx->monat = 0; gpx->tag = 0;
gpx->std = 0; gpx->min = 0; gpx->sek = 0.0;
gpx->isUTC = 0;
return -1;
}
@ -938,7 +963,7 @@ static void ecef2elli(double X[], double *lat, double *lon, double *alt) {
*lon = lam*180/M_PI;
}
static int get_GPSkoord(gpx_t *gpx, int ofs) {
static int get_ECEFkoord(gpx_t *gpx, int pos_ecef) {
int i, k;
unsigned byte;
ui8_t XYZ_bytes[4];
@ -954,14 +979,14 @@ static int get_GPSkoord(gpx_t *gpx, int ofs) {
for (k = 0; k < 3; k++) {
for (i = 0; i < 4; i++) {
byte = gpx->frame[pos_GPSecefX+ofs + 4*k + i];
byte = gpx->frame[pos_ecef + 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];
byte = gpx->frame[pos_ecef+12 + 2*k + i];
gpsVel_bytes[i] = byte;
}
vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8;
@ -1001,8 +1026,6 @@ static int get_GPSkoord(gpx_t *gpx, int ofs) {
gpx->vV = vU;
gpx->numSV = gpx->frame[pos_numSats+ofs];
return 0;
}
@ -1019,12 +1042,157 @@ static int get_GPS3(gpx_t *gpx, int ofs) {
gpx->numSV = 0;
return -1;
}
// pos_GPS3+2 = pos_GPSecefX
err |= get_ECEFkoord(gpx, pos_GPS3+ofs+2); // plausibility-check: altitude, if ecef=(0,0,0)
err |= get_GPSkoord(gpx, ofs); // plausibility-check: altitude, if ecef=(0,0,0)
gpx->numSV = gpx->frame[pos_numSats+ofs];
return err;
}
// GNSS1=8226
static int get_posdatetime(gpx_t *gpx, int pos_posdatetime) {
int err=0;
err = check_CRC(gpx, pos_posdatetime, pck_8226_POSDATETIME);
if (err) {
///TODO: fw 0x50dd , ec < 0
gpx->crc |= crc_GPS1 | crc_GPS3;
// reset GPS1-data (json)
gpx->jahr = 0; gpx->monat = 0; gpx->tag = 0;
gpx->std = 0; gpx->min = 0; gpx->sek = 0.0;
gpx->isUTC = 0;
// 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;
}
// ublox M10 UBX-NAV-POSECEF (0x01 0x01) ?
err |= get_ECEFkoord(gpx, pos_posdatetime+2); // plausibility-check: altitude, if ecef=(0,0,0)
// ublox M10 UBX-NAV-PVT (0x01 0x07) ? (UTC)
// date
gpx->jahr = gpx->frame[pos_posdatetime+20] | gpx->frame[pos_posdatetime+21]<<8;
gpx->monat = gpx->frame[pos_posdatetime+22];
gpx->tag = gpx->frame[pos_posdatetime+23];
// time
gpx->std = gpx->frame[pos_posdatetime+24];
gpx->min = gpx->frame[pos_posdatetime+25];
gpx->sek = gpx->frame[pos_posdatetime+26];
if (gpx->frame[pos_posdatetime+27] < 100) gpx->sek += gpx->frame[pos_posdatetime+27]/100.0;
//
gpx->isUTC = 1;
///TODO: numSV/fixOK
//gpx->numSV = gpx->frame[pos_numSats+ofs];
return err;
}
// GNSS2=8329
static int get_gnssSVs(gpx_t *gpx, int pos_gnss2) {
int err=0;
memset(gpx->gnss_sv, 0, 32*sizeof(gnss_t));
gpx->gnss_numSVb168 = 0;
gpx->gnss_nSVstatus = 0;
err = check_CRC(gpx, pos_gnss2, pck_8329_SATS);
if (!err) {
// ublox M10 UBX-NAV-SAT (0x01 0x35) ?
// ublox M10 UBX-NAV-SIG (0x01 0x43) ?
// int pos_gnss1 = 161;
// int pos_gnss2 = 203; == pos_posgnss
// int pos_zero = 248;
int cntSV168 = 0; // 21*8 bits
// 00..31: GPS, PRN+1
// 32..67: GALILEO, GAL_E + 31
for (int j = 0; j < 21; j++) {
int b = gpx->frame[pos_gnss2+2+4+j];
for (int n = 0; n < 8; n++) { //DBG fprintf(stdout, "%d", (b>>n)&1);
int s = (b>>n)&1;
if (s) {
ui8_t svid = j*8+n + 1;
if (cntSV168 < 32) {
gpx->gnss_sv[cntSV168].id168 = svid;
//DBG fprintf(stdout, " %3d", svid);
}
cntSV168 += 1;
}
}
}
gpx->gnss_numSVb168 = cntSV168;
int cntSVstatus = 0; // max 16*2
for (int j = 0; j < 16; j++) {
ui8_t b = gpx->frame[pos_gnss2+2+4+21+j];
ui8_t b0 = b & 0xF; // b & 0x0F
ui8_t b1 = (b>>4) & 0xF; // b & 0xF0
gpx->gnss_sv[2*j ].status = b0; if (b0) cntSVstatus++;
gpx->gnss_sv[2*j+1].status = b1; if (b1) cntSVstatus++;
}
gpx->gnss_nSVstatus = cntSVstatus;
//check: cntSV168 == cntSVstatus ?
///TODO: numSV/fixOK
// used in solution / tracked / searched / visible ?
gpx->numSV = gpx->gnss_nSVstatus; // == gpx->gnss_numSVb168 ?
}
else {
///TODO: fw 0x50dd , ec < 0
gpx->crc |= crc_GPS2;
}
return err;
}
static int prn_gnss_sat2(gpx_t *gpx) {
int n;
fprintf(stdout, "\n");
fprintf(stdout, " numSV168 : %2d", gpx->gnss_numSVb168);
fprintf(stdout, " nSVstatus: %2d", gpx->gnss_nSVstatus);
// DBG fprintf(stdout, " # %d #", gpx->nss_numSV168 - gpx->gnss_nSVstatus);
fprintf(stdout, "\n");
fprintf(stdout, " SVids: ");
for (n = 0; n < 32; n++) {
if (n < gpx->gnss_numSVb168) fprintf(stdout, " %3d", gpx->gnss_sv[n].id168);
if (n < gpx->gnss_nSVstatus) fprintf(stdout, ":%X", gpx->gnss_sv[n].status);
}
fprintf(stdout, "\n");
for (n = 0; n < 32; n++) {
if (n < gpx->gnss_numSVb168 || n < gpx->gnss_nSVstatus) {
if (gpx->gnss_sv[n].id168 < 33) { // 01..32 (GPS ?)
ui8_t prnGPS = gpx->gnss_sv[n].id168;
if (n == 0 && gpx->gnss_sv[n].id168 < 33) fprintf(stdout, " GPS: ");
//fprintf(stdout, " GPS PRN%02d: %X\n", prnGPS, gpx->gnss_sv[n].status);
//fprintf(stdout, " GPS PRN%02d", prnGPS);
fprintf(stdout, " PRN%02d", prnGPS);
}
else if (gpx->gnss_sv[n].id168 < 33+36) { // 33..68 -> 01..36 (GALILEO ??)
ui8_t prnGAL = gpx->gnss_sv[n].id168 - 32;
if (n == 0 || n > 0 && gpx->gnss_sv[n-1].id168 < 33) {
if (n > 0) fprintf(stdout, "\n");
fprintf(stdout, " GAL: ");
}
//fprintf(stdout, " GAL E%02d: %X\n", prnGAL, gpx->gnss_sv[n].status);
//fprintf(stdout, " GAL E%02d", prnGAL);
fprintf(stdout, " E%02d", prnGAL);
}
}
}
fprintf(stdout, "\n");
return 0;
}
static int get_Aux(gpx_t *gpx, int out, int pos) {
//
// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide M211486EN
@ -1046,7 +1214,7 @@ static int get_Aux(gpx_t *gpx, int out, int pos) {
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 ( pos + auxlen + 4 <= FRAME_LEN && auxcrc == crc16(gpx->frame+pos+2, auxlen) ) {
if (count7E == 0) {
if (out) fprintf(stdout, "\n # xdata = ");
}
@ -1088,6 +1256,7 @@ static int get_Calconf(gpx_t *gpx, int out, int ofs) {
ui16_t fw = 0;
int freq = 0, f0 = 0, f1 = 0;
char sondetyp[9];
char rsmtyp[10];
int err = 0;
byte = gpx->frame[pos_CalData+ofs];
@ -1173,6 +1342,17 @@ static int get_Calconf(gpx_t *gpx, int out, int ofs) {
}
}
}
if (calfr == 0x22) {
for (i = 0; i < 10; i++) rsmtyp[i] = 0;
for (i = 0; i < 8; i++) {
byte = gpx->frame[pos_CalRSM+ofs + i];
if ((byte >= 0x20) && (byte < 0x7F)) rsmtyp[i] = byte;
else /*if (byte == 0x00)*/ rsmtyp[i] = '\0';
}
if (out && gpx->option.vbs) fprintf(stdout, ": %s ", rsmtyp);
strcpy(gpx->rsm, rsmtyp);
}
}
return 0;
@ -1330,6 +1510,25 @@ static int prn_gpspos(gpx_t *gpx) {
return 0;
}
static int prn_posdatetime(gpx_t *gpx) {
//Gps2Date(gpx);
//fprintf(stdout, "%s ", weekday[gpx->wday]);
fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%05.2f",
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, " ");
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); ///TODO: used/tracked/searched/visible ?
return 0;
}
static int prn_sat1(gpx_t *gpx, int ofs) {
fprintf(stdout, "\n");
@ -1406,7 +1605,8 @@ static int prn_sat3(gpx_t *gpx, int ofs) {
static int print_position(gpx_t *gpx, int ec) {
int i;
int err, err0, err1, err2, err3;
int err = 1;
int err0 = 1, err1 = 1, err2 = 1, err3 = 1, err13 = 1;
//int output, out_mask;
int encrypted = 0;
int unexp = 0;
@ -1414,6 +1614,7 @@ static int print_position(gpx_t *gpx, int ec) {
int sat = 0;
int pos_aux = 0, cnt_aux = 0;
int ofs_ptu = 0, pck_ptu = 0;
int isGNSS2 = 0;
int ret = 0;
//gpx->out = 0;
@ -1516,6 +1717,23 @@ static int print_position(gpx_t *gpx, int ec) {
if (out) fprintf(stdout, " [%04X] (RS41-SGM) ", pck_SGM_CRYPT);
break;
case pck_960A: // 0x960A
// ? 64 bit data integrity and authenticity ?
break;
case pck_8226_POSDATETIME: // 0x8226
err13 = get_posdatetime(gpx, pos);
if ( !err13 ) {
if (out) prn_posdatetime(gpx);
}
break;
case pck_8329_SATS: // 0x8329
err2 = get_gnssSVs(gpx, pos);
isGNSS2 = 1;
////if ( !err2 ) { if (sat) prn_gnss_sat2(gpx); }
break;
default:
if (blk == 0x7E) {
if (pos_aux == 0) pos_aux = pos; // pos == pos_AUX ?
@ -1555,13 +1773,16 @@ static int print_position(gpx_t *gpx, int ec) {
gpx->crc = 0;
frm_end = FRAME_LEN-2;
if ( isGNSS2 ) {
if (sat && !err2) prn_gnss_sat2(gpx);
}
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
if ( !err && ((!err1 && !err3) || !err13 || 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",
@ -1595,14 +1816,21 @@ static int print_position(gpx_t *gpx, int ec) {
if (gpx->freq > 0) fq_kHz = gpx->freq;
fprintf(stdout, ", \"freq\": %d", fq_kHz);
}
if (*gpx->rsm) { // RSM type
fprintf(stdout, ", \"rs41_mainboard\": \"%s\"", gpx->rsm);
}
if (gpx->conf_fw) { // firmware
fprintf(stdout, ", \"rs41_mainboard_fw\": %d", gpx->conf_fw);
}
// Include frequency derived from subframe information if available.
if (gpx->freq > 0) {
fprintf(stdout, ", \"tx_frequency\": %d", gpx->freq );
}
// Reference time/position
fprintf(stdout, ", \"ref_datetime\": \"%s\"", "GPS" ); // {"GPS", "UTC"} GPS-UTC=leap_sec
// Reference time/position (fw 0x50dd: datetime UTC)
fprintf(stdout, ", \"ref_datetime\": \"%s\"", gpx->isUTC ? "UTC" : "GPS" ); // {"GPS", "UTC"} GPS-UTC=leap_sec
fprintf(stdout, ", \"ref_position\": \"%s\"", "GPS" ); // {"GPS", "MSL"} GPS=ellipsoid , MSL=geoid
fprintf(stdout, " }\n");
@ -1633,6 +1861,7 @@ static int print_position(gpx_t *gpx, int ec) {
pck = (gpx->frame[pos_PTU]<<8) | gpx->frame[pos_PTU+1];
ofs = 0;
///TODO: fw 0x50dd
if (pck < 0x8000) {
//err0 = get_PTU(gpx, 0, pck, 0);