2017-01-25 16:42:29 +00:00
|
|
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <math.h>
|
|
|
|
|
|
|
|
|
|
|
|
#include "rs_data.h"
|
|
|
|
#include "rs_datum.h"
|
|
|
|
#include "rs_bch_ecc.h"
|
|
|
|
|
|
|
|
#include "rs_gps.c"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// --- RS92-SGP: 8N1 manchester ---
|
|
|
|
static // 2A 10
|
|
|
|
char headerbits_rs92sgp[] = "10100110011001101001"
|
|
|
|
"10100110011001101001"
|
|
|
|
"10100110011001101001"
|
|
|
|
"10100110011001101001"
|
|
|
|
"1010011001100110100110101010100110101001";
|
|
|
|
|
|
|
|
static
|
|
|
|
ui8_t headerbytes_rs92sgp[] = { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10};
|
|
|
|
|
|
|
|
|
|
|
|
#define FRAME_LEN 240
|
|
|
|
|
|
|
|
#define BAUD 4800
|
|
|
|
#define BITS (2*(1+8+1)) // 20 (SGP/manchester 8N1)
|
|
|
|
#define BITFRAME_LEN (FRAME_LEN*BITS)
|
|
|
|
#define RAWBITFRAME_LEN (BITFRAME_LEN*2)
|
|
|
|
|
|
|
|
|
|
|
|
static addData_Vaisala_t rs92_addData;
|
|
|
|
|
|
|
|
|
|
|
|
// -------------------------------------------------------------
|
|
|
|
|
|
|
|
static ui32_t u4(ui8_t *bytes) { // 32bit unsigned int
|
|
|
|
ui32_t val = 0;
|
|
|
|
memcpy(&val, bytes, 4);
|
|
|
|
// val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16) | (bytes[3]<<24);
|
|
|
|
return val;
|
|
|
|
}
|
|
|
|
|
|
|
|
static ui32_t u3(ui8_t *bytes) { // 24bit unsigned int
|
|
|
|
int val24 = 0;
|
|
|
|
val24 = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16);
|
|
|
|
// = memcpy(&val, bytes, 3), val &= 0x00FFFFFF;
|
|
|
|
return val24;
|
|
|
|
}
|
|
|
|
|
|
|
|
static ui32_t u2(ui8_t *bytes) { // 16bit unsigned int
|
|
|
|
return bytes[0] | (bytes[1]<<8);
|
|
|
|
}
|
|
|
|
|
|
|
|
static int i3(ui8_t *bytes) { // 24bit signed int
|
|
|
|
int val = 0,
|
|
|
|
val24 = 0;
|
|
|
|
val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16);
|
|
|
|
val24 = val & 0xFFFFFF; if (val24 & 0x800000) val24 -= 0x1000000;
|
|
|
|
return val24;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int crc16(ui8_t bytes[], int len) {
|
|
|
|
int crc16poly = 0x1021;
|
|
|
|
int rem = 0xFFFF, i, j;
|
|
|
|
int byte;
|
|
|
|
|
|
|
|
//if (start+len >= FRAME_LEN) return -1;
|
|
|
|
|
|
|
|
for (i = 0; i < len; i++) {
|
|
|
|
byte = bytes[i];
|
|
|
|
rem = rem ^ (byte << 8);
|
|
|
|
for (j = 0; j < 8; j++) {
|
|
|
|
if (rem & 0x8000) {
|
|
|
|
rem = (rem << 1) ^ crc16poly;
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
rem = (rem << 1);
|
|
|
|
}
|
|
|
|
rem &= 0xFFFF;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return rem;
|
|
|
|
}
|
|
|
|
|
|
|
|
// -------------------------------------------------------------
|
|
|
|
|
|
|
|
static ui32_t rs92_check_CRC(rs_data_t *rs_data, ui32_t pos, ui32_t pck) {
|
|
|
|
ui32_t crclen = 0,
|
|
|
|
crcdat = 0;
|
|
|
|
int ret = 0;
|
|
|
|
|
|
|
|
// ((frame[pos]<<8) | frame[pos+1]) != pck ? // caution: variable block length (rs92?)
|
|
|
|
if ( rs_data->frame_bytes[pos] != ((pck>>8) & 0xFF) ) {
|
|
|
|
ret = 0x10000;
|
|
|
|
}
|
|
|
|
|
|
|
|
crclen = rs_data->frame_bytes[pos+1] * 2;
|
|
|
|
if (pos + crclen + 4 > rs_data->frame_len) ret |= 1;
|
|
|
|
else {
|
|
|
|
crcdat = u2((rs_data->frame_bytes)+pos+2+crclen);
|
|
|
|
if ( crcdat != crc16((rs_data->frame_bytes)+pos+2, crclen) ) {
|
|
|
|
ret |= 1; // CRC NO
|
|
|
|
}
|
|
|
|
//else { }; // CRC OK
|
|
|
|
}
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
// -------------------------------------------------------------
|
|
|
|
|
|
|
|
|
|
|
|
#define pck_CFG 0x6510
|
|
|
|
#define pos_CFG 0x06 // 32 byte
|
|
|
|
#define pos_FrameNb 0x08 // 2 byte
|
|
|
|
#define pos_SondeID 0x0C // 8 byte // oder: 0x0A, 10 byte?
|
|
|
|
#define pos_CalData 0x17 // 1 byte, counter 0x00..0x1f
|
|
|
|
#define pos_Calfreq 0x1A // 2 byte, calfr 0x00
|
|
|
|
|
|
|
|
#define pck_PTU 0x690C
|
|
|
|
#define pos_PTU 0x2A // 24 byte
|
|
|
|
|
|
|
|
#define pck_GPS 0x673D
|
|
|
|
#define pos_GPS 0x46 // 122 byte
|
|
|
|
#define pos_GPS_iTOW 0x48 // 4 byte
|
|
|
|
#define pos_GPS_PRN 0x4E // 12*5 bit in 8 byte
|
|
|
|
#define pos_GPS_STATUS 0x56 // 12 byte
|
|
|
|
#define pos_GPS_DATA 0x62 // 12*8 byte
|
|
|
|
|
|
|
|
#define pck_AUX 0x6805
|
|
|
|
#define pos_AUX 0xC4 // 2+8 byte
|
|
|
|
|
|
|
|
|
|
|
|
#define LEN_CFG (2*(pck_CFG & 0xFF))
|
|
|
|
#define LEN_GPS (2*(pck_GPS & 0xFF))
|
|
|
|
#define LEN_PTU (2*(pck_PTU & 0xFF))
|
|
|
|
|
|
|
|
#define crc_CFG (1<<0)
|
|
|
|
#define crc_PTU (1<<1)
|
|
|
|
#define crc_GPS (1<<2)
|
|
|
|
#define crc_AUX (1<<3)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_FrameNb(rs_data_t *rs_data) {
|
|
|
|
ui8_t *frnr_bytes = NULL;
|
|
|
|
|
|
|
|
frnr_bytes = (rs_data->frame_bytes)+pos_FrameNb;
|
|
|
|
rs_data->frnr = frnr_bytes[0] | (frnr_bytes[1] << 8);
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int rs92_get_SondeID(rs_data_t *rs_data) {
|
|
|
|
int i;
|
|
|
|
ui8_t byte;
|
|
|
|
ui8_t sondeid_bytes[10];
|
|
|
|
|
|
|
|
for (i = 0; i < 8; i++) {
|
|
|
|
byte = rs_data->frame_bytes[pos_SondeID + i];
|
|
|
|
if ((byte < 0x20) || (byte > 0x7E)) return -1;
|
|
|
|
sondeid_bytes[i] = byte;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (i = 0; i < 8; i++) {
|
|
|
|
rs_data->SN[i] = sondeid_bytes[i];
|
|
|
|
}
|
|
|
|
rs_data->SN[8] = '\0';
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_GPStime(rs_data_t *rs_data) {
|
|
|
|
ui8_t *gpstime_bytes;
|
|
|
|
ui32_t gpstime = 0, // 32bit
|
|
|
|
day, ms;
|
|
|
|
|
|
|
|
gpstime_bytes = (rs_data->frame_bytes)+pos_GPS_iTOW;
|
|
|
|
|
|
|
|
memcpy(&gpstime, gpstime_bytes, 4);
|
|
|
|
(rs_data->GPS).msec = gpstime;
|
|
|
|
|
|
|
|
ms = gpstime % 1000;
|
|
|
|
gpstime /= 1000;
|
|
|
|
|
|
|
|
|
|
|
|
day = (gpstime / (24 * 3600)) % 7; // besser CRC-check, da auch
|
|
|
|
//if ((day < 0) || (day > 6)) return -1; // gpssec=604800,604801 beobachtet
|
|
|
|
|
|
|
|
if (day >=0 && day < 7) rs_data->wday = day;
|
|
|
|
gpstime %= (24*3600);
|
|
|
|
|
|
|
|
rs_data->hr = gpstime / 3600;
|
|
|
|
rs_data->min = (gpstime % 3600) / 60;
|
|
|
|
rs_data->sec = gpstime % 60 + ms/1000.0;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#define LEN_CAL 16
|
|
|
|
static int rs92_get_Cal(rs_data_t *rs_data, int verbose) {
|
|
|
|
int i;
|
|
|
|
unsigned byte;
|
|
|
|
ui8_t calfr = 0;
|
|
|
|
//ui8_t burst = 0;
|
|
|
|
int freq = 0;
|
|
|
|
ui8_t freq_bytes[2];
|
|
|
|
int crc = 0;
|
|
|
|
|
|
|
|
ui8_t *frame = rs_data->frame_bytes;
|
|
|
|
ui8_t *calbytes = rs_data->frame_bytes+pos_CalData+1;
|
|
|
|
addData_Vaisala_t *rs92_cal = rs_data->addData;
|
|
|
|
|
|
|
|
calfr = frame[pos_CalData];
|
|
|
|
|
|
|
|
crc = rs92_check_CRC(rs_data, pos_CFG, pck_CFG);
|
|
|
|
// crc == rs_data->crc & crc_CFG ?
|
|
|
|
|
|
|
|
if (crc==0 && strncmp(rs92_cal->SN, rs_data->SN, 8)!=0) {
|
2017-01-26 21:22:53 +00:00
|
|
|
memset(rs92_cal, 0, sizeof(*rs92_cal));
|
2017-01-25 16:42:29 +00:00
|
|
|
strncpy(rs92_cal->SN, rs_data->SN, 9);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (crc == 0) {
|
|
|
|
if (rs92_cal->bytes[calfr][LEN_CAL] == 0) {
|
|
|
|
for (i = 0; i < LEN_CAL; i++) {
|
|
|
|
rs92_cal->bytes[calfr][i] = calbytes[i];
|
|
|
|
}
|
|
|
|
rs92_cal->bytes[calfr][LEN_CAL] = 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (calfr == 0x00) {
|
|
|
|
for (i = 0; i < 2; i++) {
|
|
|
|
freq_bytes[i] = frame[pos_Calfreq + i];
|
|
|
|
}
|
|
|
|
byte = freq_bytes[0] + (freq_bytes[1] << 8);
|
|
|
|
freq = 400000 + 10*byte; // kHz;
|
|
|
|
rs_data->freq = freq;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (verbose) {
|
|
|
|
fprintf(stdout, "[%5d] ", rs_data->frnr);
|
|
|
|
fprintf(stdout, "0x%02x: ", calfr);
|
|
|
|
for (i = 0; i < LEN_CAL; i++) {
|
|
|
|
fprintf(stdout, "%02x ", calbytes[i]);
|
|
|
|
}
|
|
|
|
if (crc == 0) fprintf(stdout, "[OK]");
|
|
|
|
else fprintf(stdout, "[NO]");
|
|
|
|
fprintf(stdout, " ");
|
|
|
|
switch (calfr) {
|
|
|
|
case 0x00: fprintf(stdout, ": fq %d ", freq); break;
|
|
|
|
}
|
|
|
|
fprintf(stdout, "\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_PTUmeas(rs_data_t *rs_data) {
|
|
|
|
//double T, P, H1, H2;
|
|
|
|
|
|
|
|
int temp, pres, hum1, hum2, ref1, ref2, ref3, ref4;
|
|
|
|
|
|
|
|
ui8_t *measdata = rs_data->frame_bytes+pos_PTU+2;
|
|
|
|
|
|
|
|
// crc check
|
|
|
|
if ( (rs_data->crc & crc_PTU) != 0 ) return -1;
|
|
|
|
|
|
|
|
temp = measdata[ 0] | (measdata[ 1]<<8) | (measdata[ 2]<<16); // ch1
|
|
|
|
hum1 = measdata[ 3] | (measdata[ 4]<<8) | (measdata[ 5]<<16); // ch2
|
|
|
|
hum2 = measdata[ 6] | (measdata[ 7]<<8) | (measdata[ 8]<<16); // ch3
|
|
|
|
ref1 = measdata[ 9] | (measdata[10]<<8) | (measdata[11]<<16); // ch4
|
|
|
|
ref2 = measdata[12] | (measdata[13]<<8) | (measdata[14]<<16); // ch5
|
|
|
|
pres = measdata[15] | (measdata[16]<<8) | (measdata[17]<<16); // ch6
|
|
|
|
ref3 = measdata[18] | (measdata[19]<<8) | (measdata[20]<<16); // ch7
|
|
|
|
ref4 = measdata[21] | (measdata[22]<<8) | (measdata[23]<<16); // ch8
|
|
|
|
|
|
|
|
|
|
|
|
// calibration data: float32 poly-coeffs in cal/conf-blocks
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ---------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
//
|
|
|
|
// GPS ---------------------------------------------------------
|
|
|
|
//
|
|
|
|
|
|
|
|
static double d_err = 10000;
|
|
|
|
static int option_iter = 0;
|
|
|
|
static int almanac = 0,
|
|
|
|
ephem = 0;
|
|
|
|
|
|
|
|
static EPHEM_t alm[33];
|
|
|
|
static EPHEM_t *ephs = NULL;
|
|
|
|
|
|
|
|
static SAT_t Sat[12];
|
|
|
|
static ui8_t prns[12]; // PRNs in data
|
|
|
|
static ui8_t sat_status[12];
|
|
|
|
|
|
|
|
static int prn32toggle = 0x1, ind_prn32, prn32next;
|
|
|
|
|
|
|
|
static int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) {
|
|
|
|
int i; /* letztes bit Ueberlauf, wenn 3. PRN = 32 */
|
|
|
|
for (i = 0; i < 15; i++) {
|
|
|
|
bits[15*block+i] = byte16 & 1;
|
|
|
|
byte16 >>= 1;
|
|
|
|
}
|
|
|
|
bits[60+block] = byte16 & 1;
|
|
|
|
return byte16 & 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void rs92_prn12(ui8_t *prn_le, ui8_t prns[12]) {
|
|
|
|
int i, j, d;
|
|
|
|
for (i = 0; i < 12; i++) {
|
|
|
|
prns[i] = 0;
|
|
|
|
d = 1;
|
|
|
|
for (j = 0; j < 5; j++) {
|
|
|
|
if (prn_le[5*i+j]) prns[i] += d;
|
|
|
|
d <<= 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
ind_prn32 = 32;
|
|
|
|
for (i = 0; i < 12; i++) {
|
|
|
|
// PRN-32 overflow
|
|
|
|
if ( (prns[i] == 0) && (sat_status[i] & 0x0F) ) { // 5 bit: 0..31
|
|
|
|
if ( ((i % 3 == 2) && (prn_le[60+i/3] & 1)) // Spalte 2
|
|
|
|
|| ((i % 3 != 2) && (prn_le[5*(i+1)] & 1)) ) { // Spalte 0,1
|
|
|
|
prns[i] = 32; ind_prn32 = i;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else if ((sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ?
|
|
|
|
prns[i] = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
prn32next = 0;
|
|
|
|
if (ind_prn32 < 12) {
|
|
|
|
// PRN-32 overflow
|
|
|
|
if (ind_prn32 % 3 != 2) { // -> ind_prn32<11 // vorausgesetzt im Block folgt auf PRN-32
|
|
|
|
if ((sat_status[ind_prn32+1] & 0x0F) && prns[ind_prn32+1] > 1) { // entweder PRN-1 oder PRN-gerade
|
2017-01-26 21:22:53 +00:00
|
|
|
// && prns[ind_prn32+1] != 3 ?
|
2017-01-25 16:42:29 +00:00
|
|
|
for (j = 0; j < ind_prn32; j++) {
|
|
|
|
if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break;
|
|
|
|
}
|
|
|
|
if (j < ind_prn32) { prn32toggle ^= 0x1; }
|
|
|
|
else {
|
|
|
|
for (j = ind_prn32+2; j < 12; j++) {
|
|
|
|
if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break;
|
|
|
|
}
|
|
|
|
if (j < 12) { prn32toggle ^= 0x1; }
|
|
|
|
}
|
|
|
|
prns[ind_prn32+1] ^= prn32toggle;
|
|
|
|
/*
|
|
|
|
// nochmal testen
|
|
|
|
for (j = 0; j < ind_prn32; j++) { if (prns[j] == prns[ind_prn32+1]) break; }
|
|
|
|
if (j < ind_prn32) prns[ind_prn32+1] = 0;
|
|
|
|
else {
|
|
|
|
for (j = ind_prn32+2; j < 12; j++) { if (prns[j] == prns[ind_prn32+1]) break; }
|
|
|
|
if (j < 12) prns[ind_prn32+1] = 0;
|
|
|
|
}
|
|
|
|
if (prns[ind_prn32+1] == 0) { prn32toggle ^= 0x1; }
|
|
|
|
*/
|
|
|
|
}
|
2017-01-26 21:22:53 +00:00
|
|
|
prn32next = prns[ind_prn32+1]; // -> ind_prn32<11 && ind_prn32 % 3 != 2
|
2017-01-25 16:42:29 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// pseudo.range = -df*pseudo.chips
|
|
|
|
// df = lightspeed/(chips/sec)/2^10
|
|
|
|
const double df = 299792.458/1023.0/1024.0; //0.286183844 // c=299792458m/s, 1023000chips/s
|
|
|
|
// dl = L1/(chips/sec)/4
|
|
|
|
const double dl = 1575.42/1.023/4.0; //385.0 // GPS L1 1575.42MHz=154*10.23MHz, dl=154*10/4
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_pseudorange(rs_data_t *rs_data) {
|
|
|
|
ui32_t gpstime;
|
|
|
|
ui8_t *gpstime_bytes;
|
|
|
|
ui8_t *pseudobytes;
|
|
|
|
int chipbytes, deltabytes; // signed int32
|
|
|
|
int i, j, k;
|
|
|
|
ui8_t bytes[4];
|
|
|
|
ui8_t prn_le[12*5+4];
|
|
|
|
ui16_t byte16;
|
|
|
|
|
|
|
|
ui8_t *frame = rs_data->frame_bytes;
|
|
|
|
|
|
|
|
// GPS-TOW in ms
|
|
|
|
gpstime_bytes = frame+pos_GPS_iTOW;
|
|
|
|
memcpy(&gpstime, gpstime_bytes, 4);
|
|
|
|
|
|
|
|
// Sat Status
|
|
|
|
for (i = 0; i < 12; i++) {
|
|
|
|
sat_status[i] = frame[pos_GPS_STATUS + i];
|
|
|
|
}
|
|
|
|
|
|
|
|
// PRN-Nummern
|
|
|
|
memset(prn_le, 0, sizeof(prn_le)); // size=60+4
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
|
|
for (j = 0; j < 2; j++) {
|
|
|
|
bytes[j] = frame[pos_GPS_PRN+2*i+j];
|
|
|
|
}
|
|
|
|
memcpy(&byte16, bytes, 2);
|
|
|
|
prnbits_le(byte16, prn_le, i);
|
|
|
|
}
|
|
|
|
rs92_prn12(prn_le, prns);
|
|
|
|
|
|
|
|
|
|
|
|
for (j = 0; j < 12; j++) {
|
|
|
|
|
|
|
|
Sat[j].tow = gpstime;
|
|
|
|
Sat[j].prn = prns[j];
|
|
|
|
Sat[j].status = sat_status[j];
|
|
|
|
|
2017-01-27 14:02:03 +00:00
|
|
|
if (pos_GPS_DATA+8*(j+1) > rs_data->pos) {
|
|
|
|
Sat[j].prn = 0;
|
2017-01-25 16:42:29 +00:00
|
|
|
}
|
2017-01-27 14:02:03 +00:00
|
|
|
else {
|
|
|
|
|
|
|
|
// Pseudorange/chips
|
|
|
|
pseudobytes = frame+pos_GPS_DATA+8*j;
|
|
|
|
memcpy(&chipbytes, pseudobytes, 4);
|
|
|
|
|
|
|
|
// delta_pseudochips / 385
|
|
|
|
pseudobytes = frame+pos_GPS_DATA+8*j+4;
|
|
|
|
deltabytes = 0; // bzw. pseudobytes[3]=0 (24 bit);
|
|
|
|
memcpy(&deltabytes, pseudobytes, 3);
|
|
|
|
|
|
|
|
// check deltabytes, status_deltabytes (24..31)
|
|
|
|
if (( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 ) ||
|
|
|
|
( (ui32_t)chipbytes > 0x10000000 && (ui32_t)chipbytes < 0xF0000000 ))
|
|
|
|
{
|
|
|
|
chipbytes = 0;
|
|
|
|
deltabytes = 0;
|
|
|
|
Sat[j].prn = 0;
|
|
|
|
}
|
|
|
|
//0x01400000 //0x013FB0A4
|
|
|
|
Sat[j].pseudorange = - chipbytes * df;
|
|
|
|
Sat[j].pseudorate = - deltabytes * df / dl;
|
2017-01-25 16:42:29 +00:00
|
|
|
|
2017-01-27 14:02:03 +00:00
|
|
|
if ((Sat[j].status & 0x0F) != 0xF) Sat[j].prn = 0;
|
|
|
|
}
|
2017-01-25 16:42:29 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// GPS Sat Pos & Vel
|
|
|
|
if (almanac) gps_satpos_alm(rs_data, alm, gpstime/1000.0, Sat);
|
|
|
|
if (ephem) gps_satpos_rnx(rs_data, ephs, gpstime/1000.0, Sat);
|
|
|
|
|
|
|
|
k = 0;
|
|
|
|
for (j = 0; j < 12; j++) {
|
|
|
|
if (Sat[j].prn > 0) k++;
|
|
|
|
}
|
|
|
|
|
|
|
|
return k;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int rs92_get_GPSvel(double lat, double lon, double vel_ecef[3],
|
|
|
|
double *vH, double *vD, double *vU) {
|
|
|
|
// ECEF-Velocities
|
|
|
|
// ECEF-Vel -> NorthEastUp
|
|
|
|
double phi = lat*M_PI/180.0;
|
|
|
|
double lam = lon*M_PI/180.0;
|
|
|
|
double vN = -vel_ecef[0]*sin(phi)*cos(lam) - vel_ecef[1]*sin(phi)*sin(lam) + vel_ecef[2]*cos(phi);
|
|
|
|
double vE = -vel_ecef[0]*sin(lam) + vel_ecef[1]*cos(lam);
|
|
|
|
*vU = vel_ecef[0]*cos(phi)*cos(lam) + vel_ecef[1]*cos(phi)*sin(lam) + vel_ecef[2]*sin(phi);
|
|
|
|
// NEU -> HorDirVer
|
|
|
|
*vH = sqrt(vN*vN+vE*vE);
|
|
|
|
*vD = atan2(vE, vN) * 180.0 / M_PI;
|
|
|
|
if (*vD < 0) *vD += 360.0;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int rs92_get_GPSkoord(rs_data_t *rs_data, int opt_gg2) {
|
|
|
|
double lat, lon, alt, rx_cl_bias;
|
|
|
|
double vH, vD, vU;
|
|
|
|
double lat0 , lon0 , alt0 , pos0_ecef[3];
|
|
|
|
double vH0, vD0, vU0;
|
|
|
|
double pos_ecef[3], dpos_ecef[3],
|
|
|
|
vel_ecef[3], dvel_ecef[3];
|
|
|
|
double DOP[4] = {0,0,0,0};
|
|
|
|
double gdop, gdop0;
|
|
|
|
//double hdop, vdop, pdop;
|
|
|
|
int j, k, n;
|
|
|
|
SAT_t Sat_B[12]; // N <= 12
|
|
|
|
SAT_t Sat_C[12]; // 11
|
|
|
|
int i0, i1, i2, i3;
|
|
|
|
int j0, j1, j2, j3;
|
|
|
|
double diter, diter0;
|
|
|
|
int exN = -1;
|
|
|
|
int N = 0;
|
|
|
|
|
|
|
|
(rs_data->GPS).lat = (rs_data->GPS).lon = (rs_data->GPS).alt = 0;
|
|
|
|
|
|
|
|
k = 0;
|
|
|
|
for (j = 0; j < 12; j++) {
|
|
|
|
if (Sat[j].prn > 0) Sat_B[k++] = Sat[j];
|
|
|
|
}
|
|
|
|
for (j = k; j < 12; j++) Sat_B[j].prn = 0;
|
|
|
|
N = k;
|
|
|
|
|
|
|
|
// preliminary position
|
|
|
|
//
|
|
|
|
NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias);
|
|
|
|
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
|
|
|
gdop = -1;
|
|
|
|
if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) {
|
|
|
|
gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
|
|
|
}
|
|
|
|
|
|
|
|
NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias);
|
|
|
|
if (option_iter) {
|
|
|
|
for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j];
|
|
|
|
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
|
|
|
}
|
|
|
|
diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]);
|
|
|
|
|
|
|
|
|
|
|
|
// Sat mit schlechten Daten suchen
|
2017-01-26 21:22:53 +00:00
|
|
|
if (diter >= d_err)
|
2017-01-25 16:42:29 +00:00
|
|
|
{
|
|
|
|
if (N > 4) { // N > 5
|
|
|
|
for (n = 0; n < N; n++) {
|
|
|
|
k = 0;
|
|
|
|
for (j = 0; j < N; j++) {
|
|
|
|
if (j != n) {
|
|
|
|
Sat_C[k] = Sat_B[j];
|
|
|
|
k++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
for (j = 0; j < 3; j++) pos0_ecef[j] = 0;
|
|
|
|
NAV_bancroft1(N-1, Sat_C, pos0_ecef, &rx_cl_bias);
|
|
|
|
|
|
|
|
NAV_LinP(N-1, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias);
|
|
|
|
diter0 = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]);
|
|
|
|
ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0);
|
|
|
|
|
|
|
|
if (diter0 < d_err && diter0 < diter) {
|
|
|
|
diter = diter0;
|
|
|
|
exN = n;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (exN >= 0) {
|
|
|
|
|
2017-01-26 21:22:53 +00:00
|
|
|
if (Sat_B[exN].prn == prn32next) {
|
|
|
|
prn32toggle ^= 0x1; // wenn zuvor mit prn32next valider Fix, dann eventuell nicht aendern;
|
2017-01-25 16:42:29 +00:00
|
|
|
// eventuell gleich testen
|
|
|
|
}
|
|
|
|
|
|
|
|
for (k = exN; k < N-1; k++) {
|
|
|
|
Sat_B[k] = Sat_B[k+1];
|
|
|
|
}
|
|
|
|
Sat_B[N-1].prn = 0;
|
|
|
|
N = N-1;
|
|
|
|
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
// 4er-Kombinationen probieren
|
2017-01-27 14:02:03 +00:00
|
|
|
gdop = 100.0;
|
2017-01-25 16:42:29 +00:00
|
|
|
k = N;
|
|
|
|
j0 = j1 = j2 = j3 = 0;
|
|
|
|
for (i0=0;i0<k;i0++) { for (i1=i0+1;i1<k;i1++) { for (i2=i1+1;i2<k;i2++) { for (i3=i2+1;i3<k;i3++) {
|
|
|
|
|
|
|
|
Sat_C[0] = Sat_B[i0];
|
|
|
|
Sat_C[1] = Sat_B[i1];
|
|
|
|
Sat_C[2] = Sat_B[i2];
|
|
|
|
Sat_C[3] = Sat_B[i3];
|
|
|
|
|
|
|
|
for (j = 0; j < 3; j++) pos0_ecef[j] = 0;
|
|
|
|
NAV_bancroft1(4, Sat_C, pos0_ecef, &rx_cl_bias);
|
|
|
|
|
|
|
|
NAV_LinP(4, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias);
|
|
|
|
diter0 = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]);
|
|
|
|
ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0);
|
|
|
|
|
|
|
|
vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0;
|
|
|
|
NAV_LinV(4, Sat_C, pos0_ecef, vel_ecef, 0.0, dvel_ecef, &rx_cl_bias);
|
|
|
|
for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j];
|
|
|
|
rs92_get_GPSvel(lat0, lon0, vel_ecef, &vH0, &vD0, &vU0);
|
|
|
|
|
|
|
|
gdop0 = -1;
|
|
|
|
if (calc_DOPn(4, Sat_C, pos0_ecef, DOP) == 0) {
|
|
|
|
gdop0 = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if ( (diter0 < d_err && gdop0 >= 0)
|
|
|
|
&& (alt0 > -200 && alt0 < 40000) // eventuell mit vorherigen
|
|
|
|
&& (vH0 < 200.0 && vU0*vU0 < 200.0*200.0) // validen Positionen vergleichen
|
|
|
|
) {
|
|
|
|
|
2017-01-27 14:02:03 +00:00
|
|
|
if ( diter0 < diter && gdop0 < gdop+2.0 ) {
|
2017-01-25 16:42:29 +00:00
|
|
|
diter = diter0;
|
2017-01-27 14:02:03 +00:00
|
|
|
gdop = gdop0;
|
2017-01-25 16:42:29 +00:00
|
|
|
j0 = i0; j1 = i1; j2 = i2; j3 = i3;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}}}}
|
|
|
|
|
|
|
|
if (j1 > 0) {
|
|
|
|
Sat_C[0] = Sat_B[j0];
|
|
|
|
Sat_C[1] = Sat_B[j1];
|
|
|
|
Sat_C[2] = Sat_B[j2];
|
|
|
|
Sat_C[3] = Sat_B[j3];
|
|
|
|
N = 4;
|
|
|
|
Sat_B[0] = Sat_C[0];
|
|
|
|
Sat_B[1] = Sat_C[1];
|
|
|
|
Sat_B[2] = Sat_C[2];
|
|
|
|
Sat_B[3] = Sat_C[3];
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// final solution
|
|
|
|
//
|
|
|
|
// position
|
|
|
|
NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias);
|
|
|
|
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
|
|
|
// velocity
|
|
|
|
vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0;
|
|
|
|
NAV_LinV(N, Sat_B, pos_ecef, vel_ecef, 0.0, dvel_ecef, &rx_cl_bias);
|
|
|
|
for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j];
|
|
|
|
rs92_get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU);
|
|
|
|
// DOP
|
|
|
|
gdop = -1;
|
|
|
|
if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) {
|
|
|
|
gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
addData_Vaisala_t *rs92_add = rs_data->addData;
|
|
|
|
int pDOP = -1;
|
|
|
|
if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) {
|
|
|
|
pDOP = sqrt(DOP[0]+DOP[1]+DOP[2]);
|
|
|
|
}
|
|
|
|
(rs92_add->sat).pDOP = pDOP;
|
|
|
|
(rs92_add->sat).Nfix = N;
|
|
|
|
for (j = 0; j < N; j++) {
|
|
|
|
(rs92_add->sat).prn[j] = Sat_B[j].prn;
|
|
|
|
(rs92_add->sat).pseudorange[j] = Sat_B[j].pseudorange;
|
|
|
|
(rs92_add->sat).doppler[j] = Sat_B[j].pseudorate;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2017-01-26 21:22:53 +00:00
|
|
|
if (diter < d_err) {
|
|
|
|
|
|
|
|
(rs_data->GPS).lat = lat;
|
|
|
|
(rs_data->GPS).lon = lon;
|
|
|
|
(rs_data->GPS).alt = alt;
|
|
|
|
|
|
|
|
(rs_data->GPS).vH = vH;
|
|
|
|
(rs_data->GPS).vD = vD;
|
|
|
|
(rs_data->GPS).vU = vU;
|
|
|
|
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
(rs_data->GPS).lat = (rs_data->GPS).lon = (rs_data->GPS).alt = 0;
|
|
|
|
(rs_data->GPS).vH = (rs_data->GPS).vD = (rs_data->GPS).vU = 0;
|
|
|
|
N = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2017-01-25 16:42:29 +00:00
|
|
|
return N;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// ---------------------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_FrameConf(rs_data_t *rs_data, int verbose) {
|
|
|
|
int err;
|
|
|
|
|
|
|
|
err = rs92_check_CRC(rs_data, pos_CFG, pck_CFG);
|
|
|
|
if (err) rs_data->crc |= crc_CFG;
|
|
|
|
|
|
|
|
rs92_get_FrameNb(rs_data);
|
|
|
|
rs92_get_SondeID(rs_data);
|
|
|
|
rs92_get_Cal(rs_data, verbose);
|
|
|
|
|
|
|
|
return err;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int rs92_get_PTU(rs_data_t *rs_data) {
|
|
|
|
int err;
|
|
|
|
|
|
|
|
err = rs92_check_CRC(rs_data, pos_PTU, pck_PTU);
|
|
|
|
if (err) rs_data->crc |= crc_PTU;
|
|
|
|
//else
|
|
|
|
{
|
|
|
|
rs92_get_PTUmeas(rs_data);
|
|
|
|
}
|
|
|
|
return err;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_GPS(rs_data_t *rs_data, int verbose) {
|
|
|
|
int err;
|
|
|
|
int k, n;
|
|
|
|
|
|
|
|
err = rs92_check_CRC(rs_data, pos_GPS, pck_GPS);
|
|
|
|
if (err) rs_data->crc |= crc_GPS;
|
|
|
|
|
|
|
|
rs92_get_GPStime(rs_data);
|
|
|
|
// if alm||eph:
|
|
|
|
k = rs92_get_pseudorange(rs_data);
|
|
|
|
if (k >= 4) {
|
|
|
|
n = rs92_get_GPSkoord(rs_data, verbose);
|
|
|
|
}
|
|
|
|
Gps2Date(rs_data);
|
|
|
|
|
|
|
|
return err;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int rs92_get_Aux(rs_data_t *rs_data, int verbose) {
|
|
|
|
//
|
|
|
|
int err;
|
|
|
|
int i;
|
|
|
|
ui32_t aux;
|
|
|
|
ui8_t *frame = rs_data->frame_bytes;
|
|
|
|
|
|
|
|
err = rs92_check_CRC(rs_data, pos_AUX, pck_AUX);
|
|
|
|
if (err) rs_data->crc |= crc_AUX;
|
|
|
|
|
|
|
|
if (verbose) {
|
|
|
|
fprintf(stdout, "AUX #");
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
|
|
aux = frame[pos_AUX+4+2*i] | (frame[pos_AUX+4+2*i+1]<<8);
|
|
|
|
fprintf(stdout, " %04x", aux);
|
|
|
|
}
|
|
|
|
fprintf(stdout, "\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
return err;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// -------------------------------------------------------------
|
|
|
|
|
|
|
|
//
|
|
|
|
// Reed-Solomon error correction -------------------------------
|
|
|
|
//
|
|
|
|
|
|
|
|
rs_ecccfg_t cfg_rs92ecc = {
|
|
|
|
.typ= 92,
|
|
|
|
.msglen= 240-6-24, // 210 <= rs_K=231
|
|
|
|
.msgpos= 6,
|
|
|
|
.parpos= 240-24,
|
|
|
|
.hdrlen= 6,
|
|
|
|
.frmlen= 240
|
|
|
|
};
|
|
|
|
|
|
|
|
#define rs_N 255
|
|
|
|
#define rs_R 24
|
|
|
|
#define rs_K (rs_N-rs_R)
|
|
|
|
|
|
|
|
static int rs92_ecc(rs_data_t *rs_data) {
|
|
|
|
// richtige framelen wichtig fuer 0-padding
|
|
|
|
|
|
|
|
int i, ret = 0;
|
|
|
|
int errors;
|
|
|
|
ui8_t cw[rs_N];
|
|
|
|
ui8_t err_pos[rs_R],
|
|
|
|
err_val[rs_R];
|
|
|
|
|
|
|
|
ui32_t frmlen = rs_data->pos;
|
|
|
|
ui8_t *frame = rs_data->frame_bytes;
|
|
|
|
|
|
|
|
// frmlen <= cfg_rs92ecc.frmlen; // = 240
|
|
|
|
int msglen = cfg_rs92ecc.msglen; // = 240-6-24=231 // msgpos=6 rs_R=24;
|
|
|
|
int msgpos = cfg_rs92ecc.msgpos; // = 6
|
|
|
|
int parpos = cfg_rs92ecc.parpos; // = 240-24
|
|
|
|
|
2017-01-26 21:22:53 +00:00
|
|
|
while (frmlen < parpos) frmlen++;
|
|
|
|
while (frmlen < rs_data->frame_len) frame[frmlen++] = 0xFF; // besser bei 00-error-frames
|
|
|
|
|
2017-01-25 16:42:29 +00:00
|
|
|
if (frmlen > rs_data->frame_len) frmlen = rs_data->frame_len;
|
|
|
|
|
|
|
|
memset(cw, 0, rs_N);
|
|
|
|
for (i = 0; i < rs_R; i++) cw[i] = frame[parpos+i];
|
|
|
|
for (i = 0; i < msglen; i++) cw[rs_R+i] = frame[msgpos+i];
|
|
|
|
// for (i = msglen; i < rs_K; i++) cw[rs_R+i] = 0;
|
|
|
|
|
|
|
|
errors = rs_decode(cw, err_pos, err_val);
|
|
|
|
|
|
|
|
// Wenn Fehler im 00-padding korrigiert wurden,
|
|
|
|
// war entweder der frame zu kurz, oder
|
|
|
|
// Fehler wurden falsch korrigiert;
|
|
|
|
// allerdings ist bei t=12 die Wahrscheinlichkeit,
|
|
|
|
// dass falsch korrigiert wurde mit 1/t! sehr gering.
|
|
|
|
|
|
|
|
// check CRC32
|
|
|
|
// CRC32 OK:
|
|
|
|
//for (i = 0; i < cfg_rs92ecc.hdrlen; i++) frame[i] = data[i];
|
|
|
|
for (i = 0; i < rs_R; i++) {
|
|
|
|
frame[parpos+i] = cw[i];
|
|
|
|
}
|
|
|
|
for (i = 0; i < msglen; i++) { // msglen <= rs_K
|
|
|
|
frame[msgpos+i] = cw[rs_R+i];
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = errors;
|
|
|
|
if (errors < 0) ret = -1;
|
|
|
|
else rs_data->pos = frmlen;
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
// -------------------------------------------------------------
|
|
|
|
|
|
|
|
//
|
|
|
|
// process bits/bytes
|
|
|
|
//
|
|
|
|
|
|
|
|
static int rs92_framebits2bytes(rs_data_t *rs_data) {
|
|
|
|
|
|
|
|
char *rawframebits = rs_data->frame_rawbits;
|
|
|
|
ui8_t *frame = rs_data->frame_bytes;
|
2017-01-26 21:22:53 +00:00
|
|
|
ui32_t n;
|
2017-01-25 16:42:29 +00:00
|
|
|
|
2017-01-26 21:22:53 +00:00
|
|
|
for (n = 0; n < rs_data->pos; n++) {
|
|
|
|
frame[n] = rs_data->bits2byte(rs_data, rawframebits+(BITS*n));
|
2017-01-25 16:42:29 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int rs92_process(void *data, int raw, int options) {
|
|
|
|
rs_data_t *rs_data = data;
|
|
|
|
int err=0, ret=0;
|
2017-01-26 21:22:53 +00:00
|
|
|
ui32_t n;
|
2017-01-25 16:42:29 +00:00
|
|
|
|
|
|
|
if (rs_data->input < 8) {
|
|
|
|
rs92_framebits2bytes(rs_data);
|
|
|
|
}
|
|
|
|
|
2017-01-26 21:22:53 +00:00
|
|
|
for (n = rs_data->pos; n < rs_data->frame_len; n++) {
|
|
|
|
rs_data->frame_bytes[n] = 0;
|
|
|
|
}
|
|
|
|
|
2017-01-25 16:42:29 +00:00
|
|
|
rs_data->ecc = rs92_ecc(rs_data);
|
|
|
|
rs_data->crc = 0;
|
|
|
|
|
|
|
|
if ( !raw ) {
|
|
|
|
|
|
|
|
err = 0;
|
|
|
|
ret = 0;
|
|
|
|
|
|
|
|
ret = rs92_get_FrameConf(rs_data, options & 0x1);
|
|
|
|
err |= ret<<0;
|
|
|
|
|
|
|
|
ret = rs92_get_PTU(rs_data);
|
|
|
|
err |= ret<<1;
|
|
|
|
|
|
|
|
ret = rs92_get_GPS(rs_data, options & 0);
|
|
|
|
err |= ret<<2;
|
|
|
|
|
|
|
|
ret = rs92_get_Aux(rs_data, options & 0x2);
|
|
|
|
err |= ret<<3;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
return err;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// manchester1 1->10,0->01: 1.bit
|
|
|
|
// manchester2 0->10,1->01: 2.bit
|
|
|
|
// RS92-SGP: 8N1 manchester2
|
|
|
|
char manch(char *mbits) {
|
|
|
|
if (((mbits[0]&1) == 1) && ((mbits[1]&1) == 0)) return 0;
|
|
|
|
else if (((mbits[0]&1) == 0) && ((mbits[1]&1) == 1)) return 1;
|
|
|
|
else return -1;
|
|
|
|
}
|
|
|
|
int rs92_mbits2byte(void *data, char mbits[]) {
|
|
|
|
// 0 eq '0'=0x30
|
|
|
|
// 1 eq '1'=0x31
|
|
|
|
int i, byte=0, d=1;
|
|
|
|
int bit8[8];
|
|
|
|
|
2017-01-26 21:22:53 +00:00
|
|
|
if (manch(mbits+0) != 0) return 0x100; // hier error-00-frames mit pos==frame_len moeglich;
|
|
|
|
for (i = 0; i < 8; i++) { // eventuell hier reync, oder in demod
|
2017-01-25 16:42:29 +00:00
|
|
|
bit8[i] = manch(mbits+2*(i+1));
|
|
|
|
}
|
|
|
|
|
|
|
|
for (i = 0; i < 8; i++) { // little endian
|
|
|
|
/* for (i = 7; i >= 0; i--) { // big endian */
|
|
|
|
if ((bit8[i]&1) == 1) byte += d;
|
|
|
|
else if ((bit8[i]&1) == 0) byte += 0;
|
|
|
|
//else return 0x100;
|
|
|
|
d <<= 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
return byte;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int init_rs92data(rs_data_t *rs_data, int orbdata, char *eph_file) {
|
|
|
|
|
|
|
|
FILE *fp = NULL;
|
|
|
|
|
|
|
|
rs_init_RS255();
|
|
|
|
|
|
|
|
// int in = rs_data->input;
|
|
|
|
// memset(rs_data, 0, sizeof(rs_data_t));
|
|
|
|
// rs_data->input = in
|
|
|
|
|
|
|
|
rs_data->baud = BAUD;
|
|
|
|
rs_data->bits = BITS;
|
|
|
|
|
|
|
|
rs_data->header = calloc(sizeof(headerbits_rs92sgp), 1);
|
|
|
|
if (rs_data->header == NULL) return ERROR_MALLOC;
|
|
|
|
strcpy(rs_data->header, headerbits_rs92sgp);
|
|
|
|
rs_data->header_ofs = 40;
|
|
|
|
rs_data->header_len = 80;
|
|
|
|
|
|
|
|
rs_data->bufpos = -1;
|
|
|
|
rs_data->buf = calloc((rs_data->header_len)+1, 1);
|
|
|
|
if (rs_data->buf == NULL) return ERROR_MALLOC;
|
|
|
|
|
|
|
|
if (rs_data->input < 8) {
|
|
|
|
rs_data->frame_rawbits = calloc(RAWBITFRAME_LEN, 1);
|
|
|
|
if (rs_data->frame_rawbits == NULL) return ERROR_MALLOC;
|
|
|
|
strncpy(rs_data->frame_rawbits, headerbits_rs92sgp, strlen(headerbits_rs92sgp));
|
|
|
|
|
|
|
|
//rs_data->frame_bits = rs_data->frame_rawbits;
|
|
|
|
}
|
|
|
|
|
|
|
|
rs_data->frame_bytes = calloc(FRAME_LEN, 1);
|
|
|
|
if (rs_data->frame_bytes == NULL) return ERROR_MALLOC;
|
|
|
|
memcpy(rs_data->frame_bytes, headerbytes_rs92sgp, sizeof(headerbytes_rs92sgp));
|
|
|
|
|
|
|
|
rs_data->frame_start = (rs_data->header_ofs + rs_data->header_len) / rs_data->bits;
|
|
|
|
rs_data->pos_min = pos_PTU;
|
|
|
|
rs_data->frame_len = FRAME_LEN;
|
|
|
|
|
|
|
|
rs_data->bits2byte = rs92_mbits2byte;
|
|
|
|
rs_data->rs_process = rs92_process;
|
|
|
|
|
|
|
|
rs_data->addData = &rs92_addData;
|
|
|
|
|
|
|
|
|
|
|
|
if (orbdata) {
|
|
|
|
fp = fopen(eph_file, "r"); // txt-mode
|
|
|
|
if (fp == NULL) orbdata = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (orbdata == 1) {
|
|
|
|
if (read_SEMalmanac(fp, alm) == 0) {
|
|
|
|
almanac = 1;
|
|
|
|
}
|
|
|
|
fclose(fp);
|
|
|
|
d_err = 4000;
|
|
|
|
}
|
|
|
|
if (orbdata == 2) {
|
|
|
|
ephs = read_RNXpephs(fp);
|
|
|
|
if (ephs) {
|
|
|
|
ephem = 1;
|
|
|
|
almanac = 0;
|
|
|
|
}
|
|
|
|
fclose(fp);
|
|
|
|
d_err = 1000;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
int free_rs92data(rs_data_t *rs_data) {
|
|
|
|
|
|
|
|
rs_data->header = NULL;
|
|
|
|
|
|
|
|
free(rs_data->buf);
|
|
|
|
rs_data->buf = NULL;
|
|
|
|
|
|
|
|
if (rs_data->input < 8) {
|
|
|
|
// memset(rs_data->frame_rawbits, 0, rs_data->RAWBITFRAME_LEN) ...
|
|
|
|
free(rs_data->frame_rawbits);
|
|
|
|
rs_data->frame_rawbits = NULL;
|
|
|
|
|
|
|
|
//rs_data->frame_bits = NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
// memset(rs_data->frame_bytes, 0, rs_data->FRAME_LEN) ...
|
|
|
|
free(rs_data->frame_bytes);
|
|
|
|
rs_data->frame_bytes = NULL;
|
|
|
|
|
|
|
|
//memset(rs_data, 0, sizeof(rs_data_t));
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|