mirror
/
rdz_ttgo_sonde
kopia lustrzana https://github.com/dl9rdz/rdz_ttgo_sonde
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
rdz_ttgo_sonde/libraries/SondeLib/RS92.cpp

676 wiersze
17 KiB
C++
Czysty Wina Historia

/* RS92 decoder functions */
#include "RS92.h"
#include "SX1278FSK.h"
#include "rsc.h"
#include "Sonde.h"
#include <SPIFFS.h>
// well...
#include "rs92gps.inc"
#define RS92_DEBUG 1
#if RS92_DEBUG
#define RS92_DBG(x) x
#else
#define RS92_DBG(x)
#endif
//static uint16_t CRCTAB[256];
uint16_t *CRCTAB = NULL;
#define X2C_DIVR(a, b) ((b) != 0.0f ? (a)/(b) : (a))
#define X2C_DIVL(a, b) ((a)/(b))
static uint32_t X2C_LSH(uint32_t a, int32_t length, int32_t n)
{
uint32_t m;
m = 0;
m = (length == 32) ? 0xFFFFFFFFl : (1 << length) - 1;
if (n > 0) {
if (n >= (int32_t)length)
return 0;
return (a << n) & m;
}
if (n <= (int32_t)-length)
return 0;
return (a >> -n) & m;
}
static double atang2(double x, double y)
{
double w;
if (fabs(x)>fabs(y)) {
w = (double)atan((float)(X2C_DIVL(y,x)));
if (x<0.0) {
if (y>0.0) w = 3.1415926535898+w;
else w = w-3.1415926535898;
}
}
else if (y!=0.0) {
w = (double)(1.5707963267949f-atan((float)(X2C_DIVL(x,
y))));
if (y<0.0) w = w-3.1415926535898;
}
else w = 0.0;
return w;
} /* end atang2() */
static void Gencrctab(void)
{
uint16_t j;
uint16_t i;
uint16_t crc;
if(!CRCTAB) { CRCTAB=(uint16_t *)malloc(256*sizeof(uint16_t)); }
for (i = 0U; i<=255U; i++) {
crc = (uint16_t)(i*256U);
for (j = 0U; j<=7U; j++) {
if ((0x8000U & crc)) crc = X2C_LSH(crc,16,1)^0x1021U;
else crc = X2C_LSH(crc,16,1);
} /* end for */
CRCTAB[i] = X2C_LSH(crc,16,-8)|X2C_LSH(crc,16,8);
} /* end for */
} /* end Gencrctab() */
int RS92::setup(float frequency)
{
#if RS92_DEBUG
Serial.println("Setup sx1278 for RS92 sonde");
#endif
if(!initialized) {
Gencrctab();
initrsc();
get_eph("/brdc.19n");
initialized = true;
}
if(sx1278.ON()!=0) {
RS92_DBG(Serial.println("Setting SX1278 power on FAILED"));
return 1;
}
if(sx1278.setFSK()!=0) {
RS92_DBG(Serial.println("Setting FSJ mode FAILED"));
return 1;
}
if(sx1278.setBitrate(4800)!=0) {
RS92_DBG(Serial.println("Setting bitrate 4800bit/s FAILED"));
return 1;
}
#if RS92_DEBUG
float br = sx1278.getBitrate();
Serial.print("Exact bitrate is ");
Serial.println(br);
#endif
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"));
return 1;
}
// Set autostart_RX to 01, preamble 0, SYNC detect==on, syncsize=3 (==4 byte
//char header[] = "0110.0101 0110.0110 1010.0101 1010.1010";
//const char *SYNC="\x10\xB6\xCA\x11\x22\x96\x12\xF8";
//const char *SYNC="\x08\x6D\x53\x88\x44\x69\x48\x1F";
// was 0x57
//const char *SYNC="\x99\x9A";
const char *SYNC="\x66\x65";
if(sx1278.setSyncConf(0x71, 2, (const uint8_t *)SYNC)!=0) {
RS92_DBG(Serial.println("Setting SYNC Config FAILED"));
return 1;
}
if(sx1278.setPreambleDetect(0xA8)!=0) {
RS92_DBG(Serial.println("Setting PreambleDetect FAILED"));
return 1;
}
// Packet config 1: fixed len, no mancecer, no crc, no address filter
// Packet config 2: packet mode, no home ctrl, no beackn, msb(packetlen)=0)
if(sx1278.setPacketConfig(0x08, 0x40)!=0) {
RS92_DBG(Serial.println("Setting Packet config FAILED"));
return 1;
}
Serial.print("RS92: setting RX frequency to ");
Serial.println(frequency);
int res = sx1278.setFrequency(frequency);
// enable RX
sx1278.setPayloadLength(0); // infinite for now...
sx1278.writeRegister(REG_OP_MODE, FSK_RX_MODE);
#if RS92_DEBUG
RS92_DBG(Serial.println("Setting SX1278 config for RS92 finished\n"); Serial.println());
#endif
return res;
}
#if 0
int RS92::setFrequency(float frequency) {
Serial.print("RS92: setting RX frequency to ");
Serial.println(frequency);
int res = sx1278.setFrequency(frequency);
// enable RX
sx1278.setPayloadLength(0); // infinite for now...
sx1278.writeRegister(REG_OP_MODE, FSK_RX_MODE);
return res;
}
#endif
uint32_t RS92::bits2val(const uint8_t *bits, int len) {
uint32_t val = 0;
for (int j = 0; j < len; j++) {
val |= (bits[j] << (len-1-j));
}
return val;
}
RS92::RS92() {
}
/* RS92 reed solomon decoder, from dxlAPRS
*/
static char crcrs(const byte frame[], uint32_t frame_len,
int32_t from, int32_t to)
{
uint16_t crc;
int32_t i;
int32_t tmp;
crc = 0xFFFFU;
tmp = to-3L;
i = from;
if (i<=tmp) for (;; i++) {
crc = X2C_LSH(crc,16,-8)^CRCTAB[(uint32_t)((crc^(uint16_t)(uint8_t)frame[i])&0xFFU)];
if (i==tmp) break;
} /* end for */
return frame[to-1L]==(char)crc && frame[to-2L]==(char)X2C_LSH(crc,
16,-8);
} /* end crcrs() */
static int32_t getint32(const byte frame[], uint32_t frame_len,
uint32_t p)
{
uint32_t n;
uint32_t i;
n = 0UL;
for (i = 3UL;; i--) {
n = n*256UL+(uint32_t)(uint8_t)frame[p+i];
if (i==0UL) break;
} /* end for */
return (int32_t)n;
} /* end getint32() */
static uint32_t getcard16(const byte frame[], uint32_t frame_len,
uint32_t p)
{
return (uint32_t)(uint8_t)frame[p]+256UL*(uint32_t)(uint8_t)
frame[p+1UL];
} /* end getcard16() */
static int32_t getint16(const byte frame[], uint32_t frame_len,
uint32_t p)
{
uint32_t n;
n = (uint32_t)(uint8_t)frame[p]+256UL*(uint32_t)(uint8_t)
frame[p+1UL];
if (n>=32768UL) return (int32_t)(n-65536UL);
return (int32_t)n;
} /* end getint16() */
static void wgs84r(double x, double y, double z,
double * lat, double * long0,
double * heig)
{
double sl;
double ct;
double st;
double t;
double rh;
double xh;
double h;
h = x*x+y*y;
if (h>0.0) {
rh = (double)sqrt((float)h);
xh = x+rh;
*long0 = atang2(xh, y)*2.0;
if (*long0>3.1415926535898) *long0 = *long0-6.2831853071796;
t = (double)atan((float)(X2C_DIVL(z*1.003364089821,
rh)));
st = (double)sin((float)t);
ct = (double)cos((float)t);
*lat = (double)atan((float)
(X2C_DIVL(z+4.2841311513312E+4*st*st*st,
rh-4.269767270718E+4*ct*ct*ct)));
sl = (double)sin((float)*lat);
*heig = X2C_DIVL(rh,(double)cos((float)*lat))-(double)(X2C_DIVR(6.378137E+6f,
sqrt((float)(1.0-6.6943799901413E-3*sl*sl))));
}
else {
*lat = 0.0;
*long0 = 0.0;
*heig = 0.0;
}
/* lat:=atan(z/(rh*(1.0 - E2))); */
/* heig:=sqrt(h + z*z) - EARTHA; */
} /* end wgs84r() */
static void posrs41(const byte b[], uint32_t b_len, uint32_t p)
{
double dir;
double vu;
double ve;
double vn;
double vz;
double vy;
double vx;
double heig;
double long0;
double lat;
double z;
double y;
double x;
x = (double)getint32(b, b_len, p)*0.01;
y = (double)getint32(b, b_len, p+4UL)*0.01;
z = (double)getint32(b, b_len, p+8UL)*0.01;
wgs84r(x, y, z, &lat, &long0, &heig);
Serial.print(" ");
sonde.si()->lat = (float)(X2C_DIVL(lat,1.7453292519943E-2));
Serial.print(sonde.si()->lat);
Serial.print(" ");
sonde.si()->lon = (float)(X2C_DIVL(long0,1.7453292519943E-2));
Serial.print(sonde.si()->lon);
if (heig<1.E+5 && heig>(-1.E+5)) {
Serial.print(" ");
Serial.print((uint32_t)heig);
Serial.print("m");
}
/*speed */
vx = (double)getint16(b, b_len, p+12UL)*0.01;
vy = (double)getint16(b, b_len, p+14UL)*0.01;
vz = (double)getint16(b, b_len, p+16UL)*0.01;
vn = (-(vx*(double)sin((float)lat)*(double)
cos((float)long0))-vy*(double)
sin((float)lat)*(double)sin((float)
long0))+vz*(double)cos((float)lat);
ve = -(vx*(double)sin((float)long0))+vy*(double)
cos((float)long0);
vu = vx*(double)cos((float)lat)*(double)
cos((float)long0)+vy*(double)
cos((float)lat)*(double)sin((float)
long0)+vz*(double)sin((float)lat);
dir = X2C_DIVL(atang2(vn, ve),1.7453292519943E-2);
if (dir<0.0) dir = 360.0+dir;
sonde.si()->dir = dir;
Serial.print(" ");
sonde.si()->hs = sqrt((float)(vn*vn+ve*ve))*3.6f;
Serial.print(sonde.si()->hs);
Serial.print("km/h ");
Serial.print(dir);
Serial.print("deg ");
Serial.print((float)vu);
sonde.si()->vs = vu;
Serial.print("m/s ");
Serial.print(getcard16(b, b_len, p+18UL)&255UL);
Serial.print("Sats");
sonde.si()->alt = heig;
sonde.si()->validPos = true;
} /* end posrs41() */
static int32_t reedsolomon92(uint8_t *buf, uint32_t buf_len)
{
uint32_t i;
int32_t res;
uint8_t b[256];
uint32_t eraspos[24];
for (i = 0UL; i<=255UL; i++) {
b[i] = 0;
} /* end for */
for (i = 0UL; i<=209UL; i++) {
b[230UL-i] = buf[i+6UL];
} /* end for */
for (i = 0UL; i<=23UL; i++) {
b[254UL-i] = buf[i+216UL];
} /* end for */
res = decodersc((char *)b, eraspos, 0L);
if (res>0L && res<=12L) {
for (i = 0UL; i<=209UL; i++) {
buf[i+6UL] = b[230UL-i];
} /* end for */
for (i = 0UL; i<=23UL; i++) {
buf[i+216UL] = b[254UL-i];
} /* end for */
}
return res;
} /* end reedsolomon92() */
void printRaw(uint8_t *data, int len)
{
char buf[3];
int i;
for(i=0; i<len; i++) {
snprintf(buf, 3, "%02X", data[i]);
Serial.print(buf);
}
Serial.println();
}
static struct CONTEXTR9 contextr9;
void RS92::decodeframe92(uint8_t *data)
{
uint32_t gpstime;
uint32_t flen;
uint32_t j;
int32_t corr;
corr = reedsolomon92(data, 301ul);
int calok;
int mesok;
uint32_t calibok;
Serial.printf("rs corr is %d\n", corr);
print_frame(data, 240);
#if 0
/* from sondemod*/
int p=6;
while(1) {
uint8_t typ = data[p];
if(typ==0xff) break;
++p;
int len = ((uint32_t)data[p])*2 + 2;
Serial.printf("type %c: len=%d\n", typ, len);
//printRaw(data+p, len+2);
if(len>240) {
Serial.print("RS92 frame too long: ");
Serial.println(len);
break;
}
++p;
j=0;
uint16_t crc = 0xFFFF;
while(j<len) {
if(j < len-2) {
for(int ic = 0; ic<=7; ic++) {
if (((0x8000&crc)!=0) != ( ((1<<(7-ic))&data[p])!=0 )) {
crc <<= 1;
crc ^= 0x1021;
} else {
crc <<= 1;
}
}
}
++p;
++j;
if(p>240) {
Serial.println("eof");
return;
}
}
if ( (((uint8_t)(crc&0xff)) != data[p-2]) || (((uint8_t)(crc>>8)) != data[p-1])) {
Serial.printf("************ crc error: expected %04x\n",crc);
continue;
}
switch(typ) {
case 'e':
Serial.println("cal ");
//docalib(sf, 256, objname, 9, &contextr9, &mhz, &frameno);
// ...
break;
case 'i':
if(calok && calibok==0xffffffff) {
//domes(sf, 256, &hp, &hyg, &temp)
mesok = 1;
}
break;
case 'g':
Serial.println("gps ");
if(1||calok) {
//dogps(data+p-len, 256, &contextr9, &contextr9.timems, &gpstime);
}
break;
case 'h':
Serial.println("data "); break;
if(data[p+2]!=3) Serial.println("aux ");
// ..
break;
}
}
#endif
} /* end decodeframe92() */
void RS92::printRaw(uint8_t *data, int len)
{
char buf[3];
int i;
for(i=0; i<len; i++) {
snprintf(buf, 3, "%02X", data[i]);
Serial.print(buf);
}
Serial.println();
}
int RS92::bitsToBytes(uint8_t *bits, uint8_t *bytes, int len)
{
int i;
for(i=0; i<len*4; i++) {
bytes[i/8] = (bytes[i/8]<<1) | (bits[i]?1:0);
}
bytes[(i-1)/8] &= 0x0F;
}
static unsigned char lookup[16] = {
0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf, };
static uint8_t reverse(uint8_t n) {
return (lookup[n&0x0f] << 4) | lookup[n>>4];
}
static uint8_t scramble[64] = {150U,131U,62U,81U,177U,73U,8U,152U,50U,5U,89U,
14U,249U,68U,198U,38U,33U,96U,194U,234U,121U,93U,109U,161U,
84U,105U,71U,12U,220U,232U,92U,241U,247U,118U,130U,127U,7U,
153U,162U,44U,147U,124U,48U,99U,245U,16U,46U,97U,208U,188U,
180U,182U,6U,170U,244U,35U,120U,110U,59U,174U,191U,123U,76U,
193U};
static byte data[5000];
static uint8_t rxbitc;
static int32_t asynst[10]={0};
static uint16_t rxbyte;
int rxp=0;
static int haveNewFrame = 0;
void RS92::stobyte92(uint8_t b)
{
data[rxp] = b;
if(rxp>=5 || b=='*') rxp++; else rxp=0;
if(rxp>=240) { // frame complete... (240 byte)
rxp=0;
//printRaw(data, 240);
decodeframe92(data);
haveNewFrame = 1;
}
} /* end stobyte92() */
void RS92::process8N1data(uint8_t data)
{
// data contains 8 bits, big endian
for(int i=0; i<4; i++) {
uint8_t d = (data&0x80)?1:0;
data = data << 2;
rxbyte = (rxbyte>>1) + (d<<8);
int maxk = 0;
int max0 = 0;
for(int k = 0; k< 10; k++) {
int n = asynst[k] - asynst[(k+1)%10];
if(abs(n)>abs(max0)) {
max0 = n;
maxk = k;
}
}
//Serial.printf("<%d,%d,%d>",max0,maxk,rxbitc);
if(rxbitc == maxk) {
if(max0<0) { rxbyte = rxbyte ^ 0xFF; }
stobyte92( rxbyte&0xff );
}
//Serial.printf("%d:",asynst[rxbitc]);
if(d) {
asynst[rxbitc] += (32767-asynst[rxbitc])/16;
} else {
asynst[rxbitc] -= (32767+asynst[rxbitc])/16;
}
//Serial.printf("%d ",asynst[rxbitc]);
rxbitc = (rxbitc+1) % 10;
}
}
int RS92::receive() {
unsigned long t0 = millis();
Serial.printf("RS92::receive() start at %ld\n",t0);
while( millis() - t0 < 2000 ) {
uint8_t value = sx1278.readRegister(REG_IRQ_FLAGS2);
if ( bitRead(value, 7) ) {
Serial.println("FIFO full");
}
if ( bitRead(value, 4) ) {
Serial.println("FIFO overflow");
}
if ( bitRead(value, 2) == 1 ) {
Serial.println("FIFO: ready()");
sx1278.clearIRQFlags();
}
if(bitRead(value, 6) == 0) { // while FIFO not empty
byte data = sx1278.readRegister(REG_FIFO);
process8N1data(data);
value = sx1278.readRegister(REG_IRQ_FLAGS2);
} else {
if(haveNewFrame) {
Serial.printf("RS92::receive(): new frame complete after %ldms\n", millis()-t0);
haveNewFrame = 0;
return RX_OK;
}
delay(2);
}
}
Serial.printf("RS92::receive() timed out\n");
return RX_TIMEOUT; // TODO RX_OK;
}
#define RS92MAXLEN (240)
int RS92::waitRXcomplete() {
int res=0;
uint32_t t0 = millis();
while( rxtask.receiveResult == 0xFFFF && millis()-t0 < 2000) { delay(20); }
if( rxtask.receiveResult<0 || rxtask.receiveResult==RX_TIMEOUT) {
res = RX_TIMEOUT;
} else if ( rxtask.receiveResult==0) {
res = RX_OK;
} else {
res = RX_ERROR;
}
rxtask.receiveResult = 0xFFFF;
Serial.printf("RS92::waitRXcomplete returning %d (%s)\n", res, RXstr[res]);
return res;
}
#if 0
int oldwaitRXcomplete() {
Serial.println("RS92: receive frame...\n");
sx1278receiveData = true;
delay(6000); // done in other task....
//sx1278receiveData = false;
#if 0
//sx1278.setPayloadLength(518-8); // Expect 320-8 bytes or 518-8 bytes (8 byte header)
//sx1278.setPayloadLength(0); // infinite for now...
////// test code for continuous reception
// sx1278.receive(); /// active FSK RX mode -- already done above...
uint8_t value = sx1278.readRegister(REG_IRQ_FLAGS2);
unsigned long previous = millis();
byte ready=0;
uint32_t wait = 8000;
// while not yet done or FIFO not yet empty
// bit 6: FIFO Empty
// bit 2 payload ready
int by=0;
while( (!ready || bitRead(value,6)==0) && (millis() - previous < wait) )
{
if( bitRead(value, 7) ) { Serial.println("FIFO full"); }
if( bitRead(value, 4) ) { Serial.println("FIFO overflow"); }
if( bitRead(value,2)==1 ) ready=1;
if( bitRead(value, 6) == 0 ) { // FIFO not empty
byte data = sx1278.readRegister(REG_FIFO);
process8N1data(data);
by++;
#if 0
if(di==1) {
int rssi=getRSSI();
int fei=getFEI();
int afc=getAFC();
Serial.print("Test: RSSI="); Serial.println(rssi);
Serial.print("Test: FEI="); Serial.println(fei);
Serial.print("Test: AFC="); Serial.println(afc);
sonde.si()->rssi = rssi;
sonde.si()->afc = afc;
}
if(di>520) {
// TODO
Serial.println("TOO MUCH DATA");
break;
}
previous = millis(); // reset timeout after receiving data
#endif
}
value = sx1278.readRegister(REG_IRQ_FLAGS2);
}
Serial.printf("processed %d bytes before end/timeout\n", by);
#endif
/////
#if 0
int e = sx1278.receivePacketTimeout(1000, data+8);
if(e) { Serial.println("TIMEOUT"); return RX_TIMEOUT; } //if timeout... return 1
printRaw(data, RS92MAXLEN);
//for(int i=0; i<RS92MAXLEN; i++) { data[i] = reverse(data[i]); }
//printRaw(data, MAXLEN);
//for(int i=0; i<RS92MAXLEN; i++) { data[i] = data[i] ^ scramble[i&0x3F]; }
//printRaw(data, MAXLEN);
//int res = decode41(data, RS92MAXLEN);
#endif
int res=0;
return res==0 ? RX_OK : RX_ERROR;
}
#endif
RS92 rs92 = RS92();
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik