kopia lustrzana https://github.com/rs1729/RS
rs41: ECEF Velocities
rodzic
821555467d
commit
584eab9343
110
rs41/rs41stdin.c
110
rs41/rs41stdin.c
|
@ -38,6 +38,8 @@ typedef struct {
|
||||||
int wday;
|
int wday;
|
||||||
int std; int min; int sek;
|
int std; int min; int sek;
|
||||||
double lat; double lon; double h;
|
double lat; double lon; double h;
|
||||||
|
double vN; double vE; double vU;
|
||||||
|
double vH; double vD; double vD2;
|
||||||
} gpx_t;
|
} gpx_t;
|
||||||
|
|
||||||
gpx_t gpx;
|
gpx_t gpx;
|
||||||
|
@ -297,6 +299,7 @@ SubHeader, 2byte
|
||||||
#define pos_GPSecefX 0x114 // 4 byte
|
#define pos_GPSecefX 0x114 // 4 byte
|
||||||
#define pos_GPSecefY 0x118 // 4 byte
|
#define pos_GPSecefY 0x118 // 4 byte
|
||||||
#define pos_GPSecefZ 0x11C // 4 byte
|
#define pos_GPSecefZ 0x11C // 4 byte
|
||||||
|
#define pos_GPSecefV 0x120 // 3*2 byte
|
||||||
|
|
||||||
|
|
||||||
#define pos_Hframe 0x039
|
#define pos_Hframe 0x039
|
||||||
|
@ -497,8 +500,12 @@ int get_GPSkoord() {
|
||||||
ui8_t XYZ_bytes[4];
|
ui8_t XYZ_bytes[4];
|
||||||
int XYZ; // 32bit
|
int XYZ; // 32bit
|
||||||
double X[3], lat, lon, h;
|
double X[3], lat, lon, h;
|
||||||
|
ui8_t gpsVel_bytes[2];
|
||||||
|
short vel16; // 16bit
|
||||||
|
double V[3], phi, lam, alpha, dir;
|
||||||
int shift = 0;
|
int shift = 0;
|
||||||
|
|
||||||
|
|
||||||
byte = (frame[pos_Hkoord]<<8) + frame[pos_Hkoord+1];
|
byte = (frame[pos_Hkoord]<<8) + frame[pos_Hkoord+1];
|
||||||
/* fprintf(stdout, "0x%04X ", byte ); // ^ 0xC2EA == 0x7B15 ? */
|
/* fprintf(stdout, "0x%04X ", byte ); // ^ 0xC2EA == 0x7B15 ? */
|
||||||
if (byte != HEAD_koord) {
|
if (byte != HEAD_koord) {
|
||||||
|
@ -521,18 +528,50 @@ int get_GPSkoord() {
|
||||||
byte = byte ^ mask[(pos_GPSecefX + 4*k + i) % MASK_LEN];
|
byte = byte ^ mask[(pos_GPSecefX + 4*k + i) % MASK_LEN];
|
||||||
XYZ_bytes[i] = byte;
|
XYZ_bytes[i] = byte;
|
||||||
}
|
}
|
||||||
|
|
||||||
memcpy(&XYZ, XYZ_bytes, 4);
|
memcpy(&XYZ, XYZ_bytes, 4);
|
||||||
X[k] = XYZ / 100.0;
|
X[k] = XYZ / 100.0;
|
||||||
|
|
||||||
|
for (i = 0; i < 2; i++) {
|
||||||
|
if (shift > 0) byte = shiftLeft(pos_GPSecefV + 2*k + i);
|
||||||
|
else if (shift < 0) byte = shiftRight(pos_GPSecefV + 2*k + i);
|
||||||
|
else byte = frame[pos_GPSecefV + 2*k + i];
|
||||||
|
byte = byte ^ mask[(pos_GPSecefV + 2*k + i) % MASK_LEN];
|
||||||
|
gpsVel_bytes[i] = byte;
|
||||||
|
}
|
||||||
|
vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8;
|
||||||
|
V[k] = vel16 / 100.0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// ECEF-Position
|
||||||
ecef2elli(X, &lat, &lon, &h);
|
ecef2elli(X, &lat, &lon, &h);
|
||||||
gpx.lat = lat;
|
gpx.lat = lat;
|
||||||
gpx.lon = lon;
|
gpx.lon = lon;
|
||||||
gpx.h = h;
|
gpx.h = h;
|
||||||
if ((h < -1000) || (h > 80000)) return -1;
|
if ((h < -1000) || (h > 80000)) return -1;
|
||||||
|
|
||||||
|
|
||||||
|
// ECEF-Velocities
|
||||||
|
// ECEF-Vel -> NorthEastUp
|
||||||
|
phi = lat*M_PI/180.0;
|
||||||
|
lam = lon*M_PI/180.0;
|
||||||
|
gpx.vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi);
|
||||||
|
gpx.vE = -V[0]*sin(lam) + V[1]*cos(lam);
|
||||||
|
gpx.vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi);
|
||||||
|
|
||||||
|
// NEU -> HorDirVer
|
||||||
|
gpx.vH = sqrt(gpx.vN*gpx.vN+gpx.vE*gpx.vE);
|
||||||
|
///*
|
||||||
|
alpha = atan2(gpx.vN, gpx.vE)*180/M_PI; // ComplexPlane (von x-Achse nach links) - GeoMeteo (von y-Achse nach rechts)
|
||||||
|
dir = 90-alpha; // z=x+iy= -> i*conj(z)=y+ix=re(i(pi/2-t)), Achsen und Drehsinn vertauscht
|
||||||
|
if (dir < 0) dir += 360; // atan2(y,x)=atan(y/x)=pi/2-atan(x/y) , atan(1/t) = pi/2 - atan(t)
|
||||||
|
gpx.vD2 = dir;
|
||||||
|
//*/
|
||||||
|
dir = atan2(gpx.vE, gpx.vN) * 180 / M_PI;
|
||||||
|
if (dir < 0) dir += 360;
|
||||||
|
gpx.vD = dir;
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -547,35 +586,39 @@ int get_Cal() {
|
||||||
byte = xorbyte(pos_CalData);
|
byte = xorbyte(pos_CalData);
|
||||||
calfr = byte;
|
calfr = byte;
|
||||||
|
|
||||||
fprintf(stderr, " 0x%02x: ", calfr);
|
if (option_verbose == 2) {
|
||||||
for (i = 0; i < 16; i++) {
|
fprintf(stdout, "\n"); // fflush(stdout);
|
||||||
byte = xorbyte(pos_CalData+1+i);
|
fprintf(stdout, "[%5d] ", gpx.frnr);
|
||||||
fprintf(stderr, "%02x ", byte);
|
fprintf(stdout, " 0x%02x: ", calfr);
|
||||||
|
for (i = 0; i < 16; i++) {
|
||||||
|
byte = xorbyte(pos_CalData+1+i);
|
||||||
|
fprintf(stdout, "%02x ", byte);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (option_verbose == 2) {
|
if (calfr == 0x02 && option_verbose == 2) {
|
||||||
if (calfr == 0x02) {
|
byte = xorbyte(pos_Calburst);
|
||||||
byte = xorbyte(pos_Calburst);
|
burst = byte;
|
||||||
burst = byte;
|
fprintf(stdout, ": BK %02X ", burst);
|
||||||
fprintf(stderr, ": BK %02X ", burst);
|
}
|
||||||
}
|
|
||||||
if (calfr == 0x00) {
|
if (calfr == 0x00 && option_verbose) {
|
||||||
byte = xorbyte(pos_Calfreq) & 0xC0; // erstmal nur oberste beiden bits
|
byte = xorbyte(pos_Calfreq) & 0xC0; // erstmal nur oberste beiden bits
|
||||||
f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40
|
f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40
|
||||||
byte = xorbyte(pos_Calfreq+1);
|
byte = xorbyte(pos_Calfreq+1);
|
||||||
f1 = 40 * byte;
|
f1 = 40 * byte;
|
||||||
freq = 400000 + f1+f0; // kHz;
|
freq = 400000 + f1+f0; // kHz;
|
||||||
fprintf(stderr, ": fq %d ", freq);
|
fprintf(stdout, ": fq %d ", freq);
|
||||||
}
|
}
|
||||||
if (calfr == 0x21) { // eventuell noch zwei bytes in 0x22
|
|
||||||
for (i = 0; i < 9; i++) sondetyp[i] = 0;
|
if (calfr == 0x21 && option_verbose == 2) { // eventuell noch zwei bytes in 0x22
|
||||||
for (i = 0; i < 8; i++) {
|
for (i = 0; i < 9; i++) sondetyp[i] = 0;
|
||||||
byte = xorbyte(pos_CalRSTyp + i);
|
for (i = 0; i < 8; i++) {
|
||||||
if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte;
|
byte = xorbyte(pos_CalRSTyp + i);
|
||||||
else if (byte == 0x00) sondetyp[i] = '\0';
|
if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte;
|
||||||
}
|
else if (byte == 0x00) sondetyp[i] = '\0';
|
||||||
fprintf(stderr, ": %s ", sondetyp);
|
|
||||||
}
|
}
|
||||||
|
fprintf(stdout, ": %s ", sondetyp);
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -600,18 +643,17 @@ int print_position() {
|
||||||
fprintf(stdout, "%s ", weekday[gpx.wday]);
|
fprintf(stdout, "%s ", weekday[gpx.wday]);
|
||||||
fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%02d",
|
fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%02d",
|
||||||
gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek);
|
gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek);
|
||||||
if (option_verbose) fprintf(stdout, " (W %d)", gpx.week);
|
if (option_verbose == 2) fprintf(stdout, " (W %d)", gpx.week);
|
||||||
fprintf(stdout, " ");
|
fprintf(stdout, " ");
|
||||||
fprintf(stdout, " lat: %.5f ", gpx.lat);
|
fprintf(stdout, " lat: %.5f ", gpx.lat);
|
||||||
fprintf(stdout, " lon: %.5f ", gpx.lon);
|
fprintf(stdout, " lon: %.5f ", gpx.lon);
|
||||||
fprintf(stdout, " h: %.2f ", gpx.h);
|
fprintf(stdout, " h: %.2f ", gpx.h);
|
||||||
fprintf(stdout, "\n"); // fflush(stdout);
|
if (option_verbose) {
|
||||||
|
//fprintf(stdout, " (%.1f %.1f %.1f) ", gpx.vN, gpx.vE, gpx.vU);
|
||||||
if (option_verbose) {
|
fprintf(stdout," vH: %.1f D: %.1f° vV: %.1f ", gpx.vH, gpx.vD, gpx.vU);
|
||||||
fprintf(stderr, "[%5d] ", gpx.frnr);
|
|
||||||
get_Cal();
|
|
||||||
fprintf(stderr, "\n");
|
|
||||||
}
|
}
|
||||||
|
get_Cal();
|
||||||
|
fprintf(stdout, "\n"); // fflush(stdout);
|
||||||
}
|
}
|
||||||
|
|
||||||
return err;
|
return err;
|
||||||
|
|
Ładowanie…
Reference in New Issue