From 30c5a9f3ae900857d95326b66c35f8d00bcc3030 Mon Sep 17 00:00:00 2001 From: Zilog80 Date: Mon, 24 Jan 2022 00:45:26 +0100 Subject: [PATCH] imet1rs: PTU/GPS/XDATA/ePTU/eGPS --- imet/imet1rs_dft.c | 246 ++++++++++++++++++++++++++++----------------- 1 file changed, 152 insertions(+), 94 deletions(-) diff --git a/imet/imet1rs_dft.c b/imet/imet1rs_dft.c index 1450982..6d3e6ac 100644 --- a/imet/imet1rs_dft.c +++ b/imet/imet1rs_dft.c @@ -147,6 +147,7 @@ typedef struct { float lon; int alt; int sats; + float vH; float vD; float vV; // eGPS // PTU int frame; float temp; @@ -257,109 +258,57 @@ int crc16(ui8_t bytes[], int len) { /* -------------------------------------------------------------------------- */ #define LEN_GPSePTU (18+20) - - /* -GPS Data Packet (LSB) -offset bytes description - 0 1 SOH = 0x01 - 1 1 PKT_ID = 0x02 - 2 4 Latitude, +/- deg (float) - 6 4 Longitude, +/- deg (float) -10 2 Altitude, meters (Alt = n-5000) -12 1 nSat (0 - 12) -13 3 Time (hr,min,sec) -16 2 CRC (16-bit) -packet size = 18 bytes + standard frame: + 01 02 (GPS) .. .. 01 04 (ePTU) .. .. */ -#define pos_GPSlat 0x02 // 4 byte float -#define pos_GPSlon 0x06 // 4 byte float -#define pos_GPSalt 0x0A // 2 byte int -#define pos_GPSsats 0x0C // 1 byte -#define pos_GPStim 0x0D // 3 byte -#define pos_GPScrc 0x10 // 2 byte -int print_GPS(int pos) { - float lat, lon; - int alt, sats; - int std, min, sek; - int crc_val, crc; - - crc_val = ((byteframe+pos)[pos_GPScrc] << 8) | (byteframe+pos)[pos_GPScrc+1]; - crc = crc16(byteframe+pos, pos_GPScrc); // len=pos - - //lat = *(float*)(byteframe+pos+pos_GPSlat); - //lon = *(float*)(byteframe+pos+pos_GPSlon); - // //raspi: copy into (aligned) float - memcpy(&lat, byteframe+pos+pos_GPSlat, 4); - memcpy(&lon, byteframe+pos+pos_GPSlon, 4); - - alt = ((byteframe+pos)[pos_GPSalt+1]<<8)+(byteframe+pos)[pos_GPSalt] - 5000; - sats = (byteframe+pos)[pos_GPSsats]; - std = (byteframe+pos)[pos_GPStim+0]; - min = (byteframe+pos)[pos_GPStim+1]; - sek = (byteframe+pos)[pos_GPStim+2]; - - fprintf(stdout, "(%02d:%02d:%02d) ", std, min, sek); - fprintf(stdout, " lat: %.6f° ", lat); - fprintf(stdout, " lon: %.6f° ", lon); - fprintf(stdout, " alt: %dm ", alt); - fprintf(stdout, " sats: %d ", sats); - - fprintf(stdout, " # "); - fprintf(stdout, " CRC: %04X ", crc_val); - fprintf(stdout, "- %04X ", crc); - if (crc_val == crc) { - fprintf(stdout, "[OK]"); - gpx.gps_valid = 1; - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - gpx.sats = sats; - gpx.hour = std; - gpx.min = min; - gpx.sec = sek; - } - else { - fprintf(stdout, "[NO]"); - gpx.gps_valid = 0; - } - - return (crc_val != crc); -} +#define SOH_01 0x01 +#define PKT_PTU 0x01 +#define PKT_GPS 0x02 +#define PKT_XDATA 0x03 +#define PKT_ePTU 0x04 +#define PKT_eGPS 0x05 /* PTU (enhanced) Data Packet (LSB) offset bytes description 0 1 SOH = 0x01 - 1 1 PKT_ID = 0x04 + 1 1 PKT_ID = 0x01/0x04 2 2 PKT = packet number 4 3 P, mbs (P = n/100) 7 2 T, °C (T = n/100) 9 2 U, % (U = n/100) 11 1 Vbat, V (V = n/10) -12 2 Tint, °C (Tint = n/100) -14 2 Tpr, °C (Tpr = n/100) -16 2 Tu, °C (Tu = n/100) -18 2 CRC (16-bit) -packet size = 20 bytes + 12 2 Tint, °C (Tint = n/100) + 14 2 Tpr, °C (Tpr = n/100) + 16 2 Tu, °C (Tu = n/100) +12/18 2 CRC (16-bit) +packet size = 14/20 bytes */ #define pos_PCKnum 0x02 // 2 byte #define pos_PTUprs 0x04 // 3 byte #define pos_PTUtem 0x07 // 2 byte int #define pos_PTUhum 0x09 // 2 byte #define pos_PTUbat 0x0B // 1 byte -#define pos_PTUcrc 0x12 // 2 byte +#define pos_PTUcrc 0x0C // 2 byte +#define pos_ePTUtint 0x0C // 2 byte +#define pos_ePTUtpr 0x0E // 2 byte +#define pos_ePTUtu 0x10 // 2 byte +#define pos_ePTUcrc 0x12 // 2 byte -int print_ePTU(int pos) { +int print_ePTU(int pos, ui8_t PKT_ID) { int P, U; short T; int bat, pcknum; - int crc_val, crc; + int crc_val, crc; // 0x04: ePTU 0x01: PTU + int posPTUCRC = (PKT_ID == PKT_ePTU) ? pos_ePTUcrc : pos_PTUcrc; - crc_val = ((byteframe+pos)[pos_PTUcrc] << 8) | (byteframe+pos)[pos_PTUcrc+1]; - crc = crc16(byteframe+pos, pos_PTUcrc); // len=pos + if (PKT_ID != PKT_ePTU && PKT_ID != PKT_PTU) return -1; + + crc_val = ((byteframe+pos)[posPTUCRC] << 8) | (byteframe+pos)[posPTUCRC+1]; + crc = crc16(byteframe+pos, posPTUCRC); // len=pos P = (byteframe+pos)[pos_PTUprs] | ((byteframe+pos)[pos_PTUprs+1]<<8) | ((byteframe+pos)[pos_PTUprs+2]<<16); T = (byteframe+pos)[pos_PTUtem] | ((byteframe+pos)[pos_PTUtem+1]<<8); @@ -379,7 +328,7 @@ int print_ePTU(int pos) { fprintf(stdout, "- %04X ", crc); if (crc_val == crc) { fprintf(stdout, "[OK]"); - gpx.ptu_valid = 1; + gpx.ptu_valid = PKT_ID; gpx.frame = pcknum; gpx.pressure = P/100.0; gpx.temp = T/100.0; @@ -395,6 +344,106 @@ int print_ePTU(int pos) { } +/* +GPS (enhanced) Data Packet (LSB) +offset bytes description + 0 1 SOH = 0x01 + 1 1 PKT_ID = 0x02/0x05 + 2 4 Latitude, +/- deg (float) + 6 4 Longitude, +/- deg (float) +10 2 Altitude, meters (Alt = n-5000) +12 1 nSat (0 - 12) + 13 4 velE m/s (float) + 17 4 velN m/s (float) + 21 4 velU m/s (float) +13/25 3 Time (hr,min,sec) +16/28 2 CRC (16-bit) +packet size = 18/30 bytes +*/ +#define pos_GPSlat 0x02 // 4 byte float +#define pos_GPSlon 0x06 // 4 byte float +#define pos_GPSalt 0x0A // 2 byte int +#define pos_GPSsats 0x0C // 1 byte +#define pos_GPStim 0x0D // 3 byte +#define pos_GPScrc 0x10 // 2 byte +#define pos_eGPSvE 0x0D // 4 byte float +#define pos_eGPSvN 0x11 // 4 byte float +#define pos_eGPSvU 0x15 // 4 byte float +#define pos_eGPStim 0x19 // 3 byte +#define pos_eGPScrc 0x1C // 2 byte + +int print_eGPS(int pos, ui8_t PKT_ID) { + float lat, lon; + float vE, vN, vU, vH, vD; // E,N,U, speed, dir/heading + int alt, sats; + int std, min, sek; + int crc_val, crc; // 0x02: GPS 0x05: eGPS + int posGPStim = (PKT_ID == PKT_GPS) ? pos_GPStim : pos_eGPStim; + int posGPSCRC = (PKT_ID == PKT_GPS) ? pos_GPScrc : pos_eGPScrc; + + if (PKT_ID != PKT_GPS && PKT_ID != PKT_eGPS) return -1; + + crc_val = ((byteframe+pos)[pos_GPScrc] << 8) | (byteframe+pos)[pos_GPScrc+1]; + crc = crc16(byteframe+pos, pos_GPScrc); // len=pos + + //lat = *(float*)(byteframe+pos+pos_GPSlat); + //lon = *(float*)(byteframe+pos+pos_GPSlon); + // //raspi: copy into (aligned) float + memcpy(&lat, byteframe+pos+pos_GPSlat, 4); + memcpy(&lon, byteframe+pos+pos_GPSlon, 4); + + alt = ((byteframe+pos)[pos_GPSalt+1]<<8)+(byteframe+pos)[pos_GPSalt] - 5000; + sats = (byteframe+pos)[pos_GPSsats]; + std = (byteframe+pos)[posGPStim+0]; + min = (byteframe+pos)[posGPStim+1]; + sek = (byteframe+pos)[posGPStim+2]; + + fprintf(stdout, "(%02d:%02d:%02d) ", std, min, sek); + fprintf(stdout, " lat: %.6f° ", lat); + fprintf(stdout, " lon: %.6f° ", lon); + fprintf(stdout, " alt: %dm ", alt); + fprintf(stdout, " sats: %d ", sats); + + gpx.vH = gpx.vD = gpx.vV = 0; + if (PKT_ID == PKT_eGPS) { + memcpy(&vE, byteframe+pos+pos_eGPSvE, 4); + memcpy(&vN, byteframe+pos+pos_eGPSvN, 4); + memcpy(&vU, byteframe+pos+pos_eGPSvU, 4); + vH = sqrt(vE*vE+vN*vN); + vD = atan2(vE, vN) * 180.0 / M_PI; + if (vD < 0) vD += 360.0; + // TODO: TEST eGPS/vel + fprintf(stdout, " vH: %.1fm/s D: %.1f° vV: %.1fm/s ", vH, vD, vU); + } + + fprintf(stdout, " # "); + fprintf(stdout, " CRC: %04X ", crc_val); + fprintf(stdout, "- %04X ", crc); + if (crc_val == crc) { + fprintf(stdout, "[OK]"); + gpx.gps_valid = PKT_ID; + gpx.lat = lat; + gpx.lon = lon; + gpx.alt = alt; + gpx.sats = sats; + gpx.hour = std; + gpx.min = min; + gpx.sec = sek; + if (PKT_ID == PKT_eGPS) { + gpx.vH = vH; + gpx.vD = vD; + gpx.vV = vU; + } + } + else { + fprintf(stdout, "[NO]"); + gpx.gps_valid = 0; + } + + return (crc_val != crc); +} + + /* Extra Data Packet - XDATA offset bytes description @@ -408,7 +457,7 @@ N=8: Ozonesonde (MSB) 5 2 Icell, uA (I = n/1000) 7 2 Tpump, °C (T = n/100) 9 1 Ipump, mA -10 2 Vbat, (V = n/10) +10 1 Vbat, (V = n/10) 11 2 CRC (16-bit) packet size = 12 bytes */ @@ -426,7 +475,8 @@ int print_xdata(int pos, ui8_t N) { fprintf(stdout, " XDATA "); // (byteframe+pos)[2] = N - if (N == 8 && (byteframe+pos)[3] == 0x01) { // Ozonesonde 01030801 (MSB) + if (N == 8 && (byteframe+pos)[3] == 0x01) + { // Ozonesonde 01 03 08 01 .. .. (MSB) InstrumentNum = (byteframe+pos)[4]; Icell = (byteframe+pos)[5+1] | ((byteframe+pos)[5]<<8); // MSB Tpump = (byteframe+pos)[7+1] | ((byteframe+pos)[7]<<8); // MSB @@ -472,8 +522,10 @@ int print_xdata(int pos, ui8_t N) { return (crc_val != crc); } + /* -------------------------------------------------------------------------- */ + int print_frame(int len) { int i; int framelen; @@ -489,14 +541,14 @@ int print_frame(int len) { gpx.gps_valid = 0; gpx.ptu_valid = 0; + framelen = bits2bytes(bitframe, byteframe, len); + if (option_rawbits) { - print_rawbits((LEN_GPSePTU+2)*BITS); + print_rawbits(framelen*BITS); } else { - framelen = bits2bytes(bitframe, byteframe, len); - if (option_raw) { for (i = 0; i < framelen; i++) { // LEN_GPSePTU fprintf(stdout, "%02X ", byteframe[i]); @@ -509,24 +561,26 @@ int print_frame(int len) { ofs = 0; gpx.xdata[0] = '\0'; gpx.paux = gpx.xdata; - while (ofs < framelen && byteframe[ofs] == 0x01) + while (ofs < framelen && byteframe[ofs] == SOH_01) // SOH = 0x01 { ui8_t PKT_ID = byteframe[ofs+1]; - if (PKT_ID == 0x02) // GPS Data Packet + if (PKT_ID == PKT_GPS || PKT_ID == PKT_eGPS) // GPS/eGPS Data Packet { - crc_err1 = print_GPS(ofs); // packet offset in byteframe + int posGPSCRC = (PKT_ID == PKT_GPS) ? pos_GPScrc : pos_eGPScrc; + crc_err1 = print_eGPS(ofs, PKT_ID); // packet offset in byteframe fprintf(stdout, "\n"); - ofs += pos_GPScrc+2; + ofs += posGPSCRC+2; out |= 1; } - else if (PKT_ID == 0x04) // PTU Data Packet + else if (PKT_ID == PKT_ePTU || PKT_ID == PKT_PTU) // ePTU/PTU Data Packet { - crc_err2 = print_ePTU(ofs); // packet offset in byteframe + int posPTUCRC = (PKT_ID == PKT_ePTU) ? pos_ePTUcrc : pos_PTUcrc; + crc_err2 = print_ePTU(ofs, PKT_ID); // packet offset in byteframe fprintf(stdout, "\n"); - ofs += pos_PTUcrc+2; + ofs += posPTUCRC+2; out |= 2; } - else if (PKT_ID == 0x03) // Extra Data Packet + else if (PKT_ID == PKT_XDATA) // Extra Data Packet { ui8_t N = byteframe[ofs+2]; if (N > 0 && ofs+2+N+2 < framelen) @@ -545,7 +599,7 @@ int print_frame(int len) { } } -// // if (crc_err1==0 && crc_err2==0) { } + // if (crc_err1==0 && crc_err2==0) { } if (option_json) { @@ -555,8 +609,12 @@ int print_frame(int len) { fprintf(stdout, "{ \"type\": \"%s\"", "IMET"); fprintf(stdout, ", \"frame\": %d, \"id\": \"iMet\", \"datetime\": \"%02d:%02d:%02dZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %d, \"sats\": %d, \"temp\": %.2f, \"humidity\": %.2f, \"pressure\": %.2f, \"batt\": %.1f", gpx.frame, gpx.hour, gpx.min, gpx.sec, gpx.lat, gpx.lon, gpx.alt, gpx.sats, gpx.temp, gpx.humidity, gpx.pressure, gpx.batt); + // TODO: TEST eGPS/vel + if (0 && gpx.gps_valid == PKT_eGPS) { + fprintf(stdout, ", \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f", gpx.vH, gpx.vD, gpx.vV ); + } if (gpx.xdata[0]) { - fprintf(stdout, ", \"aux\": \"%s\"", gpx.xdata ); + fprintf(stdout, ", \"aux\": \"%s\"", gpx.xdata ); } if (gpx.jsn_freq > 0) { fprintf(stdout, ", \"freq\": %d", gpx.jsn_freq);