kopia lustrzana https://github.com/rs1729/RS
m10mod: (finally) included M10+(gtop) (although it was probably only an intermediate version, replacing trimble gps)
Zilog80
rodzic
b6fa4a7995
commit
0abb85ae9e
|
|
@ -61,6 +61,7 @@ dduudduudduudduu duduudduuduudduu ddududuudduduudd uduuddududududud uudduduuddu
|
|||
//"0111011010011111"; // M10: 76 9F , w/ aux-data
|
||||
//"0110010001001001"; // M10-dop: 64 49 09
|
||||
//"0110010010101111"; // M10+: 64 AF w/ gtop-GPS
|
||||
//"0100010100100000"; // M20: 45 20 (baud=9600)
|
||||
static char rawheader[] = "10011001100110010100110010011001";
|
||||
|
||||
#define FRAME_LEN (100+1) // 0x64+1
|
||||
|
|
@ -70,6 +71,11 @@ static char rawheader[] = "10011001100110010100110010011001";
|
|||
#define BITAUX_LEN (AUX_LEN*BITS)
|
||||
|
||||
|
||||
#define t_M2K2 0x8F
|
||||
#define t_M10 0x9F
|
||||
#define t_M10plus 0xAF
|
||||
#define t_M20 0x20
|
||||
|
||||
typedef struct {
|
||||
int week; int tow_ms; int gpssec;
|
||||
int jahr; int monat; int tag;
|
||||
|
|
@ -87,6 +93,7 @@ typedef struct {
|
|||
char frame_bits[BITFRAME_LEN+BITAUX_LEN+8];
|
||||
int auxlen; // 0 .. 0x76-0x64
|
||||
option_t option;
|
||||
ui8_t type;
|
||||
} gpx_t;
|
||||
|
||||
|
||||
|
|
@ -202,6 +209,7 @@ M10 w/ Sierra Wireless Airprime X1110
|
|||
|
||||
|
||||
#define stdFLEN 0x64 // pos[0]=0x64
|
||||
// Trimble GPS
|
||||
#define pos_GPSTOW 0x0A // 4 byte
|
||||
#define pos_GPSlat 0x0E // 4 byte
|
||||
#define pos_GPSlon 0x12 // 4 byte
|
||||
|
|
@ -213,9 +221,21 @@ M10 w/ Sierra Wireless Airprime X1110
|
|||
#define pos_GPSvE 0x04 // 2 byte
|
||||
#define pos_GPSvN 0x06 // 2 byte
|
||||
#define pos_GPSvU 0x08 // 2 byte
|
||||
|
||||
#define pos_SN 0x5D // 2+3 byte
|
||||
#define pos_CNT 0x62 // 1 byte
|
||||
#define pos_Check (stdFLEN-1) // 2 byte
|
||||
|
||||
// Gtop GPS
|
||||
#define pos_gtopGPSlat 0x04 // 4 byte
|
||||
#define pos_gtopGPSlon 0x08 // 4 byte
|
||||
#define pos_gtopGPSalt 0x0C // 3 byte
|
||||
#define pos_gtopGPSvE 0x0F // 2 byte
|
||||
#define pos_gtopGPSvN 0x11 // 2 byte
|
||||
#define pos_gtopGPSvU 0x13 // 2 byte
|
||||
#define pos_gtopGPStime 0x15 // 3 byte
|
||||
#define pos_gtopGPSdate 0x18 // 3 byte
|
||||
|
||||
|
||||
#define ANSI_COLOR_RED "\x1b[31m"
|
||||
#define ANSI_COLOR_GREEN "\x1b[32m"
|
||||
|
|
@ -227,6 +247,7 @@ M10 w/ Sierra Wireless Airprime X1110
|
|||
|
||||
#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" // 4 byte
|
||||
#define col_GPSdate "\x1b[38;5;94m" //111
|
||||
|
|
@ -235,11 +256,12 @@ M10 w/ Sierra Wireless Airprime X1110
|
|||
#define col_GPSalt "\x1b[38;5;82m" // 4 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_CSno "\x1b[38;5;1m"
|
||||
#define col_TXT "\x1b[38;5;244m"
|
||||
#define col_FRTXT "\x1b[38;5;244m"
|
||||
|
||||
/*
|
||||
$ for code in {0..255}
|
||||
|
|
@ -444,6 +466,95 @@ static int get_SN(gpx_t *gpx) {
|
|||
return 0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
//
|
||||
// M10+ w/ Gtop
|
||||
|
||||
static int get_gtopGPSpos(gpx_t *gpx) {
|
||||
int i;
|
||||
ui8_t bytes[4];
|
||||
int val;
|
||||
|
||||
for (i = 0; i < 4; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSlat + i];
|
||||
val = 0;
|
||||
for (i = 0; i < 4; i++) val |= bytes[i] << (8*(3-i));
|
||||
gpx->lat = val/1e6;
|
||||
|
||||
for (i = 0; i < 4; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSlon + i];
|
||||
val = 0;
|
||||
for (i = 0; i < 4; i++) val |= bytes[i] << (8*(3-i));
|
||||
gpx->lon = val/1e6;
|
||||
|
||||
for (i = 0; i < 3; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSalt + i];
|
||||
val = 0;
|
||||
for (i = 0; i < 3; i++) val |= bytes[i] << (8*(2-i));
|
||||
if (val & 0x800000) val -= 0x1000000; // alt: signed 24bit?
|
||||
gpx->alt = val/1e2;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_gtopGPSvel(gpx_t *gpx) {
|
||||
int i;
|
||||
ui8_t bytes[2];
|
||||
short vel16;
|
||||
double vx, vy, vz, dir;
|
||||
|
||||
for (i = 0; i < 2; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSvE + i];
|
||||
vel16 = bytes[0] << 8 | bytes[1];
|
||||
vx = vel16 / 1e2; // east
|
||||
|
||||
for (i = 0; i < 2; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSvN + i];
|
||||
vel16 = bytes[0] << 8 | bytes[1];
|
||||
vy= vel16 / 1e2; // north
|
||||
|
||||
for (i = 0; i < 2; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSvU + i];
|
||||
vel16 = bytes[0] << 8 | bytes[1];
|
||||
vz = vel16 / 1e2; // up
|
||||
|
||||
gpx->vx = vx;
|
||||
gpx->vy = vy;
|
||||
gpx->vH = sqrt(vx*vx+vy*vy);
|
||||
dir = atan2(vx, vy) * 180 / M_PI;
|
||||
if (dir < 0) dir += 360;
|
||||
gpx->vD = dir;
|
||||
gpx->vV = vz;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_gtopGPStime(gpx_t *gpx) {
|
||||
int i;
|
||||
ui8_t bytes[4];
|
||||
int time;
|
||||
|
||||
for (i = 0; i < 3; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPStime + i];
|
||||
time = 0;
|
||||
for (i = 0; i < 3; i++) time |= bytes[i] << (8*(2-i));
|
||||
|
||||
gpx->std = time/10000;
|
||||
gpx->min = (time%10000)/100;
|
||||
gpx->sek = (time%100)/1.0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_gtopGPSdate(gpx_t *gpx) {
|
||||
int i;
|
||||
ui8_t bytes[4];
|
||||
int date;
|
||||
|
||||
for (i = 0; i < 3; i++) bytes[i] = gpx->frame_bytes[pos_gtopGPSdate + i];
|
||||
date = 0;
|
||||
for (i = 0; i < 3; i++) date |= bytes[i] << (8*(2-i));
|
||||
|
||||
gpx->jahr = 2000 + date%100;
|
||||
gpx->monat = (date%10000)/100;
|
||||
gpx->tag = date/10000;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/*
|
||||
g : F^n -> F^16 // checksum, linear
|
||||
|
|
@ -730,26 +841,41 @@ static int print_pos(gpx_t *gpx, int csOK) {
|
|||
int err, err2;
|
||||
|
||||
err = 0;
|
||||
err |= get_GPSweek(gpx);
|
||||
err |= get_GPStime(gpx);
|
||||
err |= get_GPSlat(gpx);
|
||||
err |= get_GPSlon(gpx);
|
||||
err |= get_GPSalt(gpx);
|
||||
err2 = get_GPSvel(gpx);
|
||||
|
||||
if (gpx->type == t_M10) {
|
||||
err |= get_GPSweek(gpx);
|
||||
err |= get_GPStime(gpx);
|
||||
err |= get_GPSlat(gpx);
|
||||
err |= get_GPSlon(gpx);
|
||||
err |= get_GPSalt(gpx);
|
||||
err2 = get_GPSvel(gpx);
|
||||
}
|
||||
else if (gpx->type == t_M10plus) {
|
||||
err |= get_gtopGPStime(gpx);
|
||||
err |= get_gtopGPSdate(gpx);
|
||||
err |= get_gtopGPSpos(gpx);
|
||||
err2 = get_gtopGPSvel(gpx);
|
||||
}
|
||||
else err = 0xFF;
|
||||
|
||||
if (!err) {
|
||||
|
||||
Gps2Date(gpx->week, gpx->gpssec, &gpx->jahr, &gpx->monat, &gpx->tag);
|
||||
|
||||
if (gpx->type == t_M10) {
|
||||
Gps2Date(gpx->week, gpx->gpssec, &gpx->jahr, &gpx->monat, &gpx->tag);
|
||||
}
|
||||
gpx->T = get_Temp(gpx);
|
||||
gpx->_RH = get_RH(gpx);
|
||||
gpx->Ti = get_intTemp(gpx);
|
||||
gpx->batV = get_BatV(gpx);
|
||||
|
||||
|
||||
if (gpx->option.col) {
|
||||
fprintf(stdout, col_TXT);
|
||||
if (gpx->option.vbs >= 3) fprintf(stdout, " (W "col_GPSweek"%d"col_TXT") ", gpx->week);
|
||||
fprintf(stdout, col_GPSTOW"%s"col_TXT" ", weekday[gpx->wday]);
|
||||
if (gpx->type == t_M10)
|
||||
{
|
||||
if (gpx->option.vbs >= 3) 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);
|
||||
|
|
@ -784,8 +910,11 @@ static int print_pos(gpx_t *gpx, int csOK) {
|
|||
fprintf(stdout, ANSI_COLOR_RESET"");
|
||||
}
|
||||
else {
|
||||
if (gpx->option.vbs >= 3) fprintf(stdout, " (W %d) ", gpx->week);
|
||||
fprintf(stdout, "%s ", weekday[gpx->wday]);
|
||||
if (gpx->type == t_M10)
|
||||
{
|
||||
if (gpx->option.vbs >= 3) 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);
|
||||
|
|
@ -836,11 +965,21 @@ static int print_pos(gpx_t *gpx, int csOK) {
|
|||
utc_week -= 1;
|
||||
utc_s += 604800; // 604800sec = 1week
|
||||
}
|
||||
Gps2Date(utc_week, utc_s, &utc_jahr, &utc_monat, &utc_tag);
|
||||
utc_s %= (24*3600); // 86400sec = 1day
|
||||
utc_std = utc_s/3600;
|
||||
utc_min = (utc_s%3600)/60;
|
||||
utc_sek = utc_s%60 + (gpx->tow_ms % 1000)/1000.0;
|
||||
if (gpx->type == t_M10) {
|
||||
Gps2Date(utc_week, utc_s, &utc_jahr, &utc_monat, &utc_tag);
|
||||
utc_s %= (24*3600); // 86400sec = 1day
|
||||
utc_std = utc_s/3600;
|
||||
utc_min = (utc_s%3600)/60;
|
||||
utc_sek = utc_s%60 + (gpx->tow_ms % 1000)/1000.0;
|
||||
}
|
||||
else {
|
||||
utc_jahr = gpx->jahr;
|
||||
utc_monat = gpx->monat;
|
||||
utc_tag = gpx->tag;
|
||||
utc_std = gpx->std;
|
||||
utc_min = gpx->min;
|
||||
utc_sek = gpx->sek;
|
||||
}
|
||||
|
||||
strncpy(sn_id+4, gpx->SN, 12);
|
||||
sn_id[15] = '\0';
|
||||
|
|
@ -864,6 +1003,7 @@ static int print_pos(gpx_t *gpx, int csOK) {
|
|||
if (gpx->_RH > -0.5) fprintf(stdout, ", \"humidity\": %.1f", gpx->_RH);
|
||||
}
|
||||
}
|
||||
fprintf(stdout, ", \"subtype\": \"0x%02X\"", gpx->type);
|
||||
fprintf(stdout, " }\n");
|
||||
fprintf(stdout, "\n");
|
||||
}
|
||||
|
|
@ -891,20 +1031,40 @@ static int print_frame(gpx_t *gpx, int pos) {
|
|||
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);
|
||||
|
||||
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) {
|
||||
if (gpx->option.col && gpx->frame_bytes[1] != 0x49 && (gpx->type == t_M10 || gpx->type == t_M10plus)) {
|
||||
fprintf(stdout, col_FRTXT);
|
||||
for (i = 0; i < FRAME_LEN+gpx->auxlen; i++) {
|
||||
byte = gpx->frame_bytes[i];
|
||||
if ((i >= pos_GPSTOW) && (i < pos_GPSTOW+4)) 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+4)) fprintf(stdout, col_GPSalt);
|
||||
if ((i >= pos_GPSweek) && (i < pos_GPSweek+2)) fprintf(stdout, col_GPSweek);
|
||||
if ((i >= pos_GPSvE) && (i < pos_GPSvE+6)) fprintf(stdout, col_GPSvel);
|
||||
if ((i >= pos_SN) && (i < pos_SN+5)) fprintf(stdout, col_SN);
|
||||
if ((i >= pos_Check+gpx->auxlen) && (i < pos_Check+gpx->auxlen+2)) fprintf(stdout, col_Check);
|
||||
if (i == 1) fprintf(stdout, col_Mtype);
|
||||
if (gpx->type == t_M10) {
|
||||
if ((i >= pos_GPSTOW) && (i < pos_GPSTOW+4)) 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+4)) fprintf(stdout, col_GPSalt);
|
||||
if ((i >= pos_GPSweek) && (i < pos_GPSweek+2)) fprintf(stdout, col_GPSweek);
|
||||
if ((i >= pos_GPSvE) && (i < pos_GPSvE+6)) fprintf(stdout, col_GPSvel);
|
||||
}
|
||||
else {
|
||||
if ((i >= pos_gtopGPSlat) && (i < pos_gtopGPSlat+4)) fprintf(stdout, col_GPSlat);
|
||||
if ((i >= pos_gtopGPSlon) && (i < pos_gtopGPSlon+4)) fprintf(stdout, col_GPSlon);
|
||||
if ((i >= pos_gtopGPSalt) && (i < pos_gtopGPSalt+3)) fprintf(stdout, col_GPSalt);
|
||||
if ((i >= pos_gtopGPSvE) && (i < pos_gtopGPSvE+6)) fprintf(stdout, col_GPSvel);
|
||||
if ((i >= pos_gtopGPStime) && (i < pos_gtopGPStime+3)) fprintf(stdout, col_GPSTOW);
|
||||
if ((i >= pos_gtopGPSdate) && (i < pos_gtopGPSdate+3)) fprintf(stdout, col_GPSweek);
|
||||
}
|
||||
if ((i >= pos_SN) && (i < pos_SN+5)) fprintf(stdout, col_SN);
|
||||
if (i == pos_CNT) fprintf(stdout, col_CNT);
|
||||
if ((i >= pos_Check+gpx->auxlen) && (i < pos_Check+gpx->auxlen+2)) fprintf(stdout, col_Check);
|
||||
fprintf(stdout, "%02x", byte);
|
||||
fprintf(stdout, col_FRTXT);
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue