kopia lustrzana https://github.com/rs1729/RS
imet1rs: PTU/GPS/XDATA/ePTU/eGPS
Zilog80
rodzic
1cee069334
commit
30c5a9f3ae
|
|
@ -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);
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue