mirror
/
RS-tracker
kopia lustrzana https://github.com/rs1729/RS
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
RS-tracker/rs_module/rs_rs92.c

1012 wiersze
28 KiB
C
Czysty Bezpośredni odnośnik Wina Historia

#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) {
memset(rs92_cal, 0, sizeof(*rs92_cal));
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
// && prns[ind_prn32+1] != 3 ?
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; }
*/
}
prn32next = prns[ind_prn32+1]; // -> ind_prn32<11 && ind_prn32 % 3 != 2
}
}
}
// 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];
if (pos_GPS_DATA+8*(j+1) > rs_data->pos) {
Sat[j].prn = 0;
}
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;
if ((Sat[j].status & 0x0F) != 0xF) Sat[j].prn = 0;
}
}
// 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
if (diter >= d_err)
{
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) {
if (Sat_B[exN].prn == prn32next) {
prn32toggle ^= 0x1; // wenn zuvor mit prn32next valider Fix, dann eventuell nicht aendern;
// 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
gdop = 100.0;
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
) {
if ( diter0 < diter && gdop0 < gdop+2.0 ) {
diter = diter0;
gdop = gdop0;
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;
}
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;
}
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
while (frmlen < parpos) frmlen++;
while (frmlen < rs_data->frame_len) frame[frmlen++] = 0xFF; // besser bei 00-error-frames
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;
ui32_t n;
for (n = 0; n < rs_data->pos; n++) {
frame[n] = rs_data->bits2byte(rs_data, rawframebits+(BITS*n));
}
return 0;
}
int rs92_process(void *data, int raw, int options) {
rs_data_t *rs_data = data;
int err=0, ret=0;
ui32_t n;
if (rs_data->input < 8) {
rs92_framebits2bytes(rs_data);
}
for (n = rs_data->pos; n < rs_data->frame_len; n++) {
rs_data->frame_bytes[n] = 0;
}
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];
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
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;
}
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik