diff --git a/RX_FSK/RX_FSK.ino b/RX_FSK/RX_FSK.ino index 8391f95..fea214b 100644 --- a/RX_FSK/RX_FSK.ino +++ b/RX_FSK/RX_FSK.ino @@ -14,7 +14,8 @@ #include #include #include "version.h" -#include +#include "geteph.h" +#include "rs92gps.h" // UNCOMMENT one of the constructor lines below U8X8_SSD1306_128X64_NONAME_SW_I2C *u8x8 = NULL; // initialize later after reading config file @@ -407,9 +408,13 @@ struct st_configitems config_list[] = { {"tcp.idformat", "DFM ID Format", -2, &sonde.config.tcpfeed.idformat}, {"tcp.highrate", "Rate limit", 0, &sonde.config.tcpfeed.highrate}, {"---", "---", -1, NULL}, - /* RS41 decoder settings */ + /* decoder settings */ {"rs41.agcbw", "RS41 AGC bandwidth", 0, &sonde.config.rs41.agcbw}, {"rs41.rxbw", "RS41 RX bandwidth", 0, &sonde.config.rs41.rxbw}, + {"rs92.rxbw", "RS92 RX (and AGC) bandwidth", 0, &sonde.config.rs92.rxbw}, + {"rs92.alt2d", "RS92 2D fix default altitude", 0, &sonde.config.rs92.alt2d}, + {"dfm.agcbw", "DFM6/9 AGC bandwidth", 0, &sonde.config.dfm.agcbw}, + {"dfm.rxbw", "DFM6/9 RX bandwidth", 0, &sonde.config.dfm.rxbw}, {"---", "---", -1, NULL}, /* Hardware dependeing settings */ {"oled_sda", "OLED SDA (needs reboot)", 0, &sonde.config.oled_sda}, @@ -857,7 +862,7 @@ void sx1278Task(void *parameter) { resetup = false; sonde.setup(); } - Serial.println("rx task: calling sonde.receive()"); + //Serial.println("rx task: calling sonde.receive()"); sonde.receive(); delay(20); } @@ -1481,7 +1486,7 @@ void wifiConnect(int16_t res) { static int wifi_cto; void loopWifiBackground() { - Serial.printf("WifiBackground: state %d\n", wifi_state); + // Serial.printf("WifiBackground: state %d\n", wifi_state); // handle Wifi station mode in background if (sonde.config.wifi == 0 || sonde.config.wifi == 2) return; // nothing to do if disabled or access point mode @@ -1648,6 +1653,7 @@ void loopWifiScan() { sonde.updateDisplayIP(); wifi_state = WIFI_CONNECTED; geteph(); + get_eph("/brdc"); delay(3000); } enableNetwork(true); diff --git a/RX_FSK/data/config.txt b/RX_FSK/data/config.txt index a937d8d..a1c864c 100644 --- a/RX_FSK/data/config.txt +++ b/RX_FSK/data/config.txt @@ -2,8 +2,8 @@ # Hardware depending settings #-------------------------------# # pin: 255=disabled; x=button x+128=touch button -button_pin=0 -button2_pin=255 +button_pin=130 +button2_pin=142 # LED port led_pout=9 # OLED Setup is depending on hardware of LoRa board @@ -38,9 +38,17 @@ call=N0CALL passcode=12345 #-------------------------------# # Sonde specific settings: bandwidth +# valid values: 3100, 3900, 5200, 6300, 7800, 10400, 12500, +# 15600, 20800, 25000, ... +# other values will be rounded up to the next valid value +# rs92.alt2d: default altitude used by RS92 decoder if only 3 sats available #-------------------------------# -rs41.agcbw=6300 +rs41.agcbw=12500 rs41.rxbw=6300 +rs92.rxbw=12500 +rs92.alt2d=480 +dfm.agcbw=20800 +dfm.rxbw=10400 #-------------------------------# # axudp for sending to aprsmap #-------------------------------# diff --git a/libraries/SX1278FSK/SX1278FSK.cpp b/libraries/SX1278FSK/SX1278FSK.cpp index d6dde01..a9f2bfd 100644 --- a/libraries/SX1278FSK/SX1278FSK.cpp +++ b/libraries/SX1278FSK/SX1278FSK.cpp @@ -542,7 +542,7 @@ int32_t SX1278FSK::getFEI() { int32_t FEI; int16_t regval = (readRegister(REG_FEI_MSB)<<8) | readRegister(REG_FEI_LSB); - Serial.printf("feireg: %04x\n", regval); + //Serial.printf("feireg: %04x\n", regval); FEI = (int32_t)(regval * SX127X_FSTEP); return FEI; } @@ -554,7 +554,7 @@ int32_t SX1278FSK::getAFC() { int32_t AFC; int16_t regval = (readRegister(REG_AFC_MSB)<<8) | readRegister(REG_AFC_LSB); - Serial.printf("afcreg: %04x\n", regval); + //Serial.printf("afcreg: %04x\n", regval); AFC = (int32_t)(regval * SX127X_FSTEP); return AFC; } diff --git a/libraries/SondeLib/DFM.cpp b/libraries/SondeLib/DFM.cpp index 1243b69..34c9b1f 100644 --- a/libraries/SondeLib/DFM.cpp +++ b/libraries/SondeLib/DFM.cpp @@ -36,14 +36,14 @@ int DFM::setup(float frequency, int inv) Serial.println(br); #endif - if(sx1278.setAFCBandwidth(25000)!=0) { - DFM_DBG(Serial.println("Setting AFC bandwidth 25 kHz FAILED")); - return 1; - } - if(sx1278.setRxBandwidth(12000)!=0) { - DFM_DBG(Serial.println("Setting RX bandwidth 12kHz FAILED")); - return 1; - } + if(sx1278.setAFCBandwidth(sonde.config.dfm.agcbw)!=0) { + DFM_DBG(Serial.printf("Setting AFC bandwidth %d Hz FAILED", sonde.config.dfm.agcbw)); + return 1; + } + if(sx1278.setRxBandwidth(sonde.config.dfm.rxbw)!=0) { + DFM_DBG(Serial.printf("Setting RX bandwidth to %d Hz FAILED", sonde.config.dfm.rxbw)); + return 1; + } // Enable auto-AFC, auto-AGC, RX Trigger by preamble if(sx1278.setRxConf(0x1E)!=0) { DFM_DBG(Serial.println("Setting RX Config FAILED")); diff --git a/libraries/SondeLib/Display.cpp b/libraries/SondeLib/Display.cpp index 8da9785..b5488a0 100644 --- a/libraries/SondeLib/Display.cpp +++ b/libraries/SondeLib/Display.cpp @@ -32,11 +32,12 @@ static unsigned char kmh_tiles[] U8X8_PROGMEM = { static unsigned char ms_tiles[] U8X8_PROGMEM = { 0x1F, 0x02, 0x04, 0x02, 0x1F, 0x40, 0x20, 0x10, 0x08, 0x04, 0x12, 0xA4, 0xA4, 0xA4, 0x40, 0x00 }; -static unsigned char stattiles[4][4] = { +static unsigned char stattiles[5][4] = { 0x00, 0x1F, 0x00, 0x00 , // | == ok 0x00, 0x10, 0x10, 0x00 , // . == no header found 0x1F, 0x15, 0x15, 0x00 , // E == decode error - 0x00, 0x00, 0x00, 0x00 }; // ' ' == unknown/unassigned + 0x00, 0x00, 0x00, 0x00 , // ' ' == unknown/unassigned + 0x07, 0x05, 0x07, 0x00 }; // ° = rx ok, but no valid position //static uint8_t halfdb_tile[8]={0x80, 0x27, 0x45, 0x45, 0x45, 0x39, 0x00, 0x00}; diff --git a/libraries/SondeLib/RS41.cpp b/libraries/SondeLib/RS41.cpp index cccb489..033fca7 100644 --- a/libraries/SondeLib/RS41.cpp +++ b/libraries/SondeLib/RS41.cpp @@ -103,11 +103,11 @@ int RS41::setup(float frequency) #endif if(sx1278.setAFCBandwidth(sonde.config.rs41.agcbw)!=0) { - RS41_DBG(Serial.println("Setting AFC bandwidth 25 kHz FAILED")); + RS41_DBG(Serial.printf("Setting AFC bandwidth %d Hz FAILED", sonde.config.rs41.agcbw)); return 1; } if(sx1278.setRxBandwidth(sonde.config.rs41.rxbw)!=0) { - RS41_DBG(Serial.println("Setting RX bandwidth 12kHz FAILED")); + RS41_DBG(Serial.printf("Setting RX bandwidth to %d Hz FAILED", sonde.config.rs41.rxbw)); return 1; } // Enable auto-AFC, auto-AGC, RX Trigger by preamble diff --git a/libraries/SondeLib/RS92.cpp b/libraries/SondeLib/RS92.cpp index d7c931f..3559b94 100644 --- a/libraries/SondeLib/RS92.cpp +++ b/libraries/SondeLib/RS92.cpp @@ -76,6 +76,20 @@ static void Gencrctab(void) } /* end for */ } /* end Gencrctab() */ + +static byte data1[512]={0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; +static byte data2[512]={0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; +static byte *dataptr=data1; + +static uint8_t rxbitc; +static int32_t asynst[10]={0}; +static uint16_t rxbyte; +int rxp=0; + +static int haveNewFrame = 0; +static int lastFrame = 0; +static int headerDetected = 0; + int RS92::setup(float frequency) { #if RS92_DEBUG @@ -84,7 +98,7 @@ int RS92::setup(float frequency) if(!initialized) { Gencrctab(); initrsc(); - get_eph("/brdc.19n"); + // not here for now.... get_eph("/brdc.19n"); initialized = true; } @@ -105,15 +119,15 @@ int RS92::setup(float frequency) Serial.print("Exact bitrate is "); Serial.println(br); #endif + if(sx1278.setAFCBandwidth(sonde.config.rs92.rxbw)!=0) { + RS92_DBG(Serial.printf("Setting AFC bandwidth %d Hz FAILED", sonde.config.rs92.rxbw)); + return 1; + } + if(sx1278.setRxBandwidth(sonde.config.rs92.rxbw)!=0) { + RS92_DBG(Serial.printf("Setting RX bandwidth to %d Hz FAILED", sonde.config.rs92.rxbw)); + return 1; + } - if(sx1278.setAFCBandwidth(sonde.config.rs41.agcbw)!=0) { - RS92_DBG(Serial.println("Setting AFC bandwidth 25 kHz FAILED")); - return 1; - } - if(sx1278.setRxBandwidth(sonde.config.rs41.rxbw)!=0) { - RS92_DBG(Serial.println("Setting RX bandwidth 12kHz FAILED")); - return 1; - } // Enable auto-AFC, auto-AGC, RX Trigger by preamble if(sx1278.setRxConf(0x1E)!=0) { RS92_DBG(Serial.println("Setting RX Config FAILED")); @@ -345,8 +359,10 @@ void RS92::decodeframe92(uint8_t *data) //int calok; //int mesok; //uint32_t calibok; - Serial.printf("rs corr is %d\n", corr); - print_frame(data, 240); + lastFrame = (dataptr==data1)?1:2; + Serial.printf("rs corr is %d --- data:%p data1:%p data2:%p lastframe=%d\n", corr, data, data1, data2, lastFrame); + dataptr = (dataptr==data1)?data2:data1; + //print_frame(data, 240); #if 0 /* from sondemod*/ int p=6; @@ -458,19 +474,10 @@ static uint8_t scramble[64] = {150U,131U,62U,81U,177U,73U,8U,152U,50U,5U,89U, 193U}; #endif -static byte data[5000]={0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; - -static uint8_t rxbitc; -static int32_t asynst[10]={0}; -static uint16_t rxbyte; -int rxp=0; - -static int haveNewFrame = 0; -static int headerDetected = 0; void RS92::stobyte92(uint8_t b) { - data[rxp] = b; + dataptr[rxp] = b; if(rxp>=5 || b=='*') rxp++; else rxp=0; if(rxp==6) { // header detected headerDetected = 1; @@ -478,7 +485,7 @@ void RS92::stobyte92(uint8_t b) if(rxp>=240) { // frame complete... (240 byte) rxp=0; //printRaw(data, 240); - decodeframe92(data); + decodeframe92(dataptr); haveNewFrame = 1; } } /* end stobyte92() */ @@ -506,9 +513,9 @@ void RS92::process8N1data(uint8_t dt) int rssi=sx1278.getRSSI(); int fei=sx1278.getFEI(); int afc=sx1278.getAFC(); - Serial.print("Test: RSSI="); Serial.println(rssi); - Serial.print("Test: FEI="); Serial.println(fei); - Serial.print("Test: AFC="); Serial.println(afc); + Serial.print("Test: RSSI="); Serial.print(rssi); + Serial.print(" FEI="); Serial.print(fei); + Serial.print(" AFC="); Serial.println(afc); sonde.si()->rssi = rssi; sonde.si()->afc = afc; } @@ -516,14 +523,14 @@ void RS92::process8N1data(uint8_t dt) rxbitc = (rxbitc+1)%20; if(rxbitc == 0) { // got startbit, 8 data bit, stop bit //Serial.printf("%03x ",rxbyte); - data[rxp++] = (rxbyte>>1)&0xff; - if(rxp==7 && data[6] != 0x65) { - Serial.printf("wrong start: %02x\n",data[6]); + dataptr[rxp++] = (rxbyte>>1)&0xff; + if(rxp==7 && dataptr[6] != 0x65) { + Serial.printf("wrong start: %02x\n",dataptr[6]); rxsearching = true; } if(rxp>=240) { rxsearching = true; - decodeframe92(data); + decodeframe92(dataptr); haveNewFrame = 1; } } @@ -602,6 +609,19 @@ int RS92::receive() { #define RS92MAXLEN (240) int RS92::waitRXcomplete() { + // called after complete... + Serial.printf("decoding frame %d\n", lastFrame); + print_frame(lastFrame==1?data1:data2, 240); + + SondeInfo *si = sonde.sondeList+sonde.currentSonde; + si->lat = gpx.lat; + si->lon = gpx.lon; + si->alt = gpx.alt; + si->vs = gpx.vU; + si->hs = gpx.vH; + si->dir = gpx.vD; + si->validPos = 0x3f; + #if 0 int res=0; uint32_t t0 = millis(); diff --git a/libraries/SondeLib/Sonde.cpp b/libraries/SondeLib/Sonde.cpp index dc0c08a..b7acb08 100644 --- a/libraries/SondeLib/Sonde.cpp +++ b/libraries/SondeLib/Sonde.cpp @@ -67,8 +67,12 @@ Sonde::Sonde() { config.marker=0; config.showafc=0; config.freqofs=0; - config.rs41.agcbw=25000; - config.rs41.rxbw=12000; + config.rs41.agcbw=12500; + config.rs41.rxbw=6300; + config.rs92.rxbw=12500; + config.rs92.alt2d=480; + config.dfm.agcbw=20800; + config.dfm.rxbw=10400; config.udpfeed.active = 1; config.udpfeed.type = 0; strcpy(config.udpfeed.host, "192.168.42.20"); @@ -145,6 +149,14 @@ void Sonde::setConfig(const char *cfg) { config.rs41.agcbw = atoi(val); } else if(strcmp(cfg,"rs41.rxbw")==0) { config.rs41.rxbw = atoi(val); + } else if(strcmp(cfg,"dfm.agcbw")==0) { + config.dfm.agcbw = atoi(val); + } else if(strcmp(cfg,"dfm.rxbw")==0) { + config.dfm.rxbw = atoi(val); + } else if(strcmp(cfg,"rs92.alt2d")==0) { + config.rs92.alt2d= atoi(val); + } else if(strcmp(cfg,"rs92.rxbw")==0) { + config.rs92.rxbw = atoi(val); } else if(strcmp(cfg,"axudp.active")==0) { config.udpfeed.active = atoi(val)>0; } else if(strcmp(cfg,"axudp.host")==0) { @@ -326,23 +338,20 @@ rxloop: } rxtask.receiveResult = 0xFFFF; Serial.printf("waitRXcomplete returning %04x (%s)\n", res, RXstr[res&0xff]); -#if 0 -//currently not used... -{ - int res; - switch(sondeList[rxtask.currentSonde].type) { + // currently used only by RS92 + // TODO: rxtask.currentSonde might not be the right thing (after sonde channel change) + switch(sondeList[/*rxtask.*/currentSonde].type) { case STYPE_RS41: - res = rs41.waitRXcomplete(); + rs41.waitRXcomplete(); break; case STYPE_RS92: - res = rs92.waitRXcomplete(); + rs92.waitRXcomplete(); break; case STYPE_DFM06: case STYPE_DFM09: - res = dfm.waitRXcomplete(); + dfm.waitRXcomplete(); break; } -#endif memmove(sonde.si()->rxStat+1, sonde.si()->rxStat, 17); sonde.si()->rxStat[0] = res; return res; diff --git a/libraries/SondeLib/Sonde.h b/libraries/SondeLib/Sonde.h index 5f4e3ec..bc7326b 100644 --- a/libraries/SondeLib/Sonde.h +++ b/libraries/SondeLib/Sonde.h @@ -7,7 +7,7 @@ // RX_TIMEOUT: no header detected // RX_ERROR: header detected, but data not decoded (crc error, etc.) // RX_OK: header and data ok -enum RxResult { RX_OK, RX_TIMEOUT, RX_ERROR, RX_UNKNOWN }; +enum RxResult { RX_OK, RX_TIMEOUT, RX_ERROR, RX_UNKNOWN, RX_NOPOS }; #define RX_UPDATERSSI 0xFFFE // Events that change what is displayed (mode, sondenr) @@ -70,6 +70,14 @@ struct st_rs41config { int agcbw; int rxbw; }; +struct st_rs92config { + int rxbw; + int alt2d; +}; +struct st_dfmconfig { + int agcbw; + int rxbw; +}; typedef struct st_rdzconfig { int button_pin; // PIN port number menu button (+128 for touch mode) @@ -96,6 +104,8 @@ typedef struct st_rdzconfig { char call[9]; // APRS callsign char passcode[9]; // APRS passcode struct st_rs41config rs41; // configuration options specific for RS41 receiver + struct st_rs92config rs92; + struct st_dfmconfig dfm; // for now, one feed for each type is enough, but might get extended to more? struct st_feedinfo udpfeed; // target for AXUDP messages struct st_feedinfo tcpfeed; // target for APRS-IS TCP connections @@ -103,7 +113,7 @@ typedef struct st_rdzconfig { typedef struct st_sondeinfo { // receiver configuration - bool active; + bool active; SondeType type; float freq; // decoded ID @@ -116,7 +126,7 @@ typedef struct st_sondeinfo { float alt; // altitude float vs; // vertical speed float hs; // horizontal speed - float dir; // 0..360 + float dir; // 0..360 uint8_t validPos; // bit pattern for validity of above 6 fields // RSSI from receiver int rssi; // signal strength @@ -128,7 +138,7 @@ typedef struct st_sondeinfo { uint32_t viewStart; // millis() timestamp of viewinf this sonde with current display int8_t lastState; // -1: disabled; 0: norx; 1: rx } SondeInfo; -// rxStat: 0=undef[empty] 1=timeout[.] 2=errro[E] 3=ok[1] +// rxStat: 3=undef[empty] 1=timeout[.] 2=errro[E] 3=ok[1] 5=no valid position[°] #define MAXSONDE 99 diff --git a/libraries/SondeLib/nav_gps_vel.cpp b/libraries/SondeLib/nav_gps_vel.cpp index 97e4c1f..89084f8 100644 --- a/libraries/SondeLib/nav_gps_vel.cpp +++ b/libraries/SondeLib/nav_gps_vel.cpp @@ -384,6 +384,20 @@ EPHEM_t *read_RNXpephs(const char *file) { ephem.week = 1; // ephem.gpsweek Serial.printf("Reading ephem for prn %d\n", ui); if(ui<33) { +#if 0 + // no need to do it the difficult way, most recent data is at end of file :-) + double tdiff; + if(te[ui].prn!=ui) { + tdiff = WEEKSEC; + } else { + tdiff = now - te[ui].toe; + if(tdiff>WEEKSEC/2) tdiff -= WEEKSEC; + if(tdiff<-WEEKSEC/2) tdiff += WEEKSEC; + } + double td = now - ephem.toe; + if(td>WEEKSEC/2) td -= WEEKSEC; + if(td<-WEEKSEC/2) td += WEEKSEC; +#endif te[ui] = ephem; } else { Serial.printf("bad prn: %d\n", ui); diff --git a/libraries/SondeLib/rs92gps.cpp b/libraries/SondeLib/rs92gps.cpp index b788d88..7365329 100644 --- a/libraries/SondeLib/rs92gps.cpp +++ b/libraries/SondeLib/rs92gps.cpp @@ -50,28 +50,10 @@ #include #include "nav_gps_vel.h" - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif +#include "rs92gps.h" +#include "Sonde.h" -typedef struct { - int frnr; - char id[11]; - int week; int gpssec; - int jahr; int monat; int tag; - int wday; - int std; int min; float sek; - double lat; double lon; double alt; - double vH; double vD; double vU; - int sats[4]; - double dop; - int freq; - unsigned short aux[4]; - double diter; -} gpx_t; gpx_t gpx; @@ -91,6 +73,7 @@ int option_verbose = 0, // ausfuehrliche Anzeige rawin = 0; double dop_limit = 9.9; double d_err = 10000; +//double fixalt2d = 480; // bei mir zu Hause :-) TODO: make it configurable int rollover = 0, err_gps = 0; @@ -550,6 +533,7 @@ int get_GPStime() { gpstime /= 1000; gpx.gpssec = gpstime; + Serial.printf("GPS time is %04x (%d)\n", gpstime, gpstime); day = (gpstime / (24 * 3600)) % 7; // besser CRC-check, da auch //if ((day < 0) || (day > 6)) return -1; // gpssec=604800,604801 beobachtet @@ -901,6 +885,8 @@ const double df = 299792.458/1023.0/1024.0; //0.286183844 // c=299792458m/s, 102 // 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 +double pr_ofs; +double GPSsatAlt = 20200e3; int get_pseudorange() { uint32_t gpstime; @@ -919,10 +905,12 @@ int get_pseudorange() { memcpy(&gpstime, gpstime_bytes, 4); // Sat Status + Serial.print("Sat status: "); for (i = 0; i < 12; i++) { sat_status[i] = framebyte(posGPS_STATUS + i); - Serial.printf("sat status %d: %d\n", i, sat_status[i]); + Serial.printf("%d:%d ", i, sat_status[i]); } + Serial.print("\n"); // PRN-Nummern for (i = 0; i < 4; i++) { @@ -953,7 +941,7 @@ int get_pseudorange() { pseudobytes[i] = frame[posGPS_DATA+8*j+i]; } memcpy(&chipbytes, pseudobytes, 4); - Serial.printf("Chipbytes(%d): %04x\n",j, chipbytes); + //Serial.printf("Chipbytes(%d): %04x\n",j, chipbytes); // delta_pseudochips / 385 for (i = 0; i < 3; i++) { @@ -985,9 +973,9 @@ int get_pseudorange() { continue; } */ - Serial.printf("j=%d: prns=%d status=%d dist=%f\n", j, prns[j], sat_status[j], dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0)); - int o=prns[j]; - Serial.printf("x=%f y=%f z=%f\n", sat[o].X, sat[o].Y, sat[o].Z); + Serial.printf("j=%d: prns=%d status=%d dist=%f\n ", j, prns[j], sat_status[j], dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0)); + //int o=prns[j]; + //Serial.printf("x=%f y=%f z=%f\n", sat[o].X, sat[o].Y, sat[o].Z); if ( (prns[j] > 0) && ((sat_status[j] & 0x0F) == 0xF) && (dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0) > 6700000) ) { @@ -1020,12 +1008,13 @@ int get_pseudorange() { prj = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr; if (prj < pr0) pr0 = prj; } - for (j = 0; j < k; j++) sat[prn[j]].PR = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6; + for (j = 0; j < k; j++) sat[prn[j]].PR = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + GPSsatAlt; // es kann PRNs geben, die zeitweise stark abweichende PR liefern; // eventuell Standardabweichung ermitteln und fehlerhafte Sats weglassen for (j = 0; j < k; j++) { // sat/sat1s... PR-check - sat1s[prn[j]].PR = sat1s[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6; + sat1s[prn[j]].PR = sat1s[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + GPSsatAlt; } + pr_ofs = pr0; return k; } @@ -1049,6 +1038,80 @@ int get_GPSvel(double lat, double lon, double vel_ecef[3], double DOP[4]; +int naiv_2Dfix(int N, SAT_t sats[], double alt) { +// simple 2D fix: 3 Sats & Alt above ellipsoid +// +// - fuer 3 unbekannte lat, lon, t braucht man mind. 3 Satelliten +// - fuer Iteration braucht man jedoch einen Startwert +// - es gibt direkte Methoden +// - hier werden die vorhandenen Funktionen benutzt: +// - der 4. Satellit im Erdmittelpunkt +// - seine pseudorange(+clock) wird grob geschaetzt +// (pseudochips liefern erst Rueckschluesse, wenn man Position kennt) +// - dann approximieren, bis Hoehe stimmt +// - bei 3-4 Satelliten ist die DOP-Konstellation oft schlecht +// - moeglicherweise ist in einigen Faellen die 2. Loesung besser + + double radius = 6371e3 + alt; // wird dann approximiert + double lat2d, lon2d, alt2d, + pos_ecef[3], rx_cl_bias, + dpos_ecef[3]; + //double d; + double rofs = 200000.0, rdiff = 0.0; + int k, k_limit; + double gdop = -1; + + + sats[3].X = sats[3].Y = sats[3].Z = 0; + + k = 0; + k_limit = 100; + + if (N >= 3) { + + do + { + // PR = pseudorange + clock_corr - pr_ofs + GPSsatAlt + sats[3].X = sats[3].Y = sats[3].Z = 0; + sats[3].PR = radius - rofs; + sats[3].pseudorange = sats[3].PR + pr_ofs - GPSsatAlt; + + + NAV_bancroft1(4, sats, pos_ecef, &rx_cl_bias); + //NAV_bancroft3(4, sats, pos_ecef1, &rx_cl_bias1, pos_ecef2, &rx_cl_bias2); + + ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat2d, &lon2d, &alt2d); + rdiff = alt-alt2d; + + rofs -= rdiff*1.2; + k += 1; + + } while (k < k_limit && fabs(rdiff) > 1.0); + + NAV_LinP(4, sats, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + // for (j=0;j<3;j++) pos_ecef[j] += dpos_ecef[j]; + // NAV_LinP(4, sats, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + // d = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); + + } + + if (calc_DOPn(4, sats, pos_ecef, DOP) == 0) { + gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); + } + //if (gdop > 2*dop_limit) gdop = -1; + //if (gdop < 0) gdop = -1; + + gpx.lat = lat2d; + gpx.lon = lon2d; + gpx.alt = alt2d; + gpx.dop = gdop; + + if ( fabs(alt2d-alt) > 1000.0 ) return -1; + if ( k == k_limit ) return 0; + return 1; +} + + int get_GPSkoord(int N) { double lat, lon, alt, rx_cl_bias; double vH, vD, vU; @@ -1281,8 +1344,8 @@ int get_GPSkoord(int N) { int print_position() { // GPS-Hoehe ueber Ellipsoid - int j, k, n = 0; - int err1, err2; + int j, k = 0, n = 0; + int err1, err2, fix2d = 0; err1 = 0; if (!option_verbose) err1 = err_gps; @@ -1293,13 +1356,22 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid //err2 |= get_GPSweek(); err2 |= get_GPStime(); +#if 0 Serial.printf("ephem=%d\n",ephem); + Serial.printf("print_position: ephs is %p\n", ephs); +#endif + if (!err2 && (almanac || ephem)) { k = get_pseudorange(); Serial.printf("k=%d\n", k); if (k >= 4) { n = get_GPSkoord(k); } + if (k == 3) { + SAT_t Sat_A[4]; + for (j = 0; j < 3; j++) { Sat_A[j] = sat[prn[j]]; } + fix2d = naiv_2Dfix( 3, Sat_A, sonde.config.rs92.alt2d); + } } if (!err1) { @@ -1316,7 +1388,11 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid fprintf(stdout, "%s ", weekday[gpx.wday]); // %04.1f: wenn sek >= 59.950, wird auf 60.0 gerundet fprintf(stdout, "%02d:%02d:%06.3f", gpx.std, gpx.min, gpx.sek); - if (n > 0) { + if (k == 3 && fix2d > 0 && gpx.dop > 0 && gpx.dop < 2*dop_limit) { + fprintf(stdout, " 2D lat: %.5f lon: %.5f alt: %.1f ", gpx.lat, gpx.lon, gpx.alt); + fprintf(stdout, " DOP[%02d,%02d,%02d,0] %.1f ", prn[0], prn[1], prn[2], gpx.dop); + } + else if (n > 0) { fprintf(stdout, " "); if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.alt); @@ -1386,7 +1462,8 @@ void print_frame(uint8_t *data, int len) { } fprintf(stdout, "\n"); } - else print_position(); + //Serial.printf("print_frame: ephs is %p\n", ephs); + print_position(); } void get_eph(const char *file) { @@ -1395,7 +1472,7 @@ void get_eph(const char *file) { ephem = 1; almanac = 0; } - Serial.printf("reading RNX done, result is %d\n", ephem); + Serial.printf("reading RNX done, result is %d, ephs=%p\n", ephem, ephs); if (!option_der) d_err = 1000; } diff --git a/libraries/SondeLib/rs92gps.h b/libraries/SondeLib/rs92gps.h index 229c2ea..a780b78 100644 --- a/libraries/SondeLib/rs92gps.h +++ b/libraries/SondeLib/rs92gps.h @@ -1,3 +1,21 @@ +typedef struct { + int frnr; + char id[11]; + int week; int gpssec; + int jahr; int monat; int tag; + int wday; + int std; int min; float sek; + double lat; double lon; double alt; + double vH; double vD; double vU; + int sats[4]; + double dop; + int freq; + unsigned short aux[4]; + double diter; +} gpx_t; + +extern gpx_t gpx; + void print_frame(uint8_t *data, int len); void get_eph(const char *file);