Hi !
Ausgehend von diesem Post (http://fpv-community.de/showthread....f%FCr-Multiwii&p=247898&viewfull=1#post247898) geht es jetzt weiter. Ziel ist es die MTK 3329 GPS Unterstützung zu verbessern. Dabei soll ein binäres Protokoll zu Einsatz kommen (wie z.B bei Ublox) das hier dokumentiert ist: stuff.storediydrones.com/reference/CustomizeFunctionSpecificationv16.doc. Aktuell experimentiere ich mit der FW AXN1.51_2722_3329_384.1151100.5.bin von hier:http://code.google.com/p/i2c-gps-nav/downloads/detail?name=MTK-firmware-tools-for-2.1.zip&can=2&q= .
Die Auslesung des MTK Binärprotokolls in der aktuellen eos bandi Version (auch APM) ERSCHEINT mir aus mehreren Gründen im Zusammenhang mit der FW (s.o) problematisch/fehlerhaft. Da der tatsächliche, serielle Datenstrom einige unschöne Überraschungen bereit hält und von der Dokumentation abweicht.
Hier ist ein Codevorschlag, wie man die seriellen Binärdaten auslesen könnte. Ich habe mich dabei an dem tatsächlich beobachteten Datenstrom der FW (s.o) orientiert.
Ausserdem könnte man dem GPS noch einen "Spikefilter" zur Seite stellen, der möglichst Ausreisserwerte erkennt und eliminiert. Das könnte dann z.B so bewerkstelligt werden http://fpv-community.de/showthread....f%FCr-Multiwii&p=244379&viewfull=1#post244379
Programmiert könnte es so aussehen (Beispiel für GPS Lat/Lon Daten):
Im Anschluss könnte man die Spike - gefilterten GPS Daten noch einem kurzen, gleitenden Mittelwertfilter zuführen. Da die GPS Daten bei Weiterverarbeitung schnell die 32 Bit sprengen, müssen 64Bit für die Mittelwertsumme verwendet werden. Das könnte programmiert dann so aussehen:
Tja, dann bin ich noch auf die Idee gekommen, den hardware nahen mwii-seriellen code in die eos bandi Version ein zu bauen.
In der Summe kann ich hier nur eine hoch experimentelle ALPHA Version anbieten. Die Version ist NUR als Codebeispiel gedacht! Voraussetzung ist die AXN1.51_2722_3329_384.1151100.5.bin.
LG
Rob
Ausgehend von diesem Post (http://fpv-community.de/showthread....f%FCr-Multiwii&p=247898&viewfull=1#post247898) geht es jetzt weiter. Ziel ist es die MTK 3329 GPS Unterstützung zu verbessern. Dabei soll ein binäres Protokoll zu Einsatz kommen (wie z.B bei Ublox) das hier dokumentiert ist: stuff.storediydrones.com/reference/CustomizeFunctionSpecificationv16.doc. Aktuell experimentiere ich mit der FW AXN1.51_2722_3329_384.1151100.5.bin von hier:http://code.google.com/p/i2c-gps-nav/downloads/detail?name=MTK-firmware-tools-for-2.1.zip&can=2&q= .
Die Auslesung des MTK Binärprotokolls in der aktuellen eos bandi Version (auch APM) ERSCHEINT mir aus mehreren Gründen im Zusammenhang mit der FW (s.o) problematisch/fehlerhaft. Da der tatsächliche, serielle Datenstrom einige unschöne Überraschungen bereit hält und von der Dokumentation abweicht.
Hier ist ein Codevorschlag, wie man die seriellen Binärdaten auslesen könnte. Ich habe mich dabei an dem tatsächlich beobachteten Datenstrom der FW (s.o) orientiert.
Code:
bool GPS_MTK_newFrame(uint8_t data)
{
static uint8_t pstep; // Parse Step
static uint8_t lastbyte; // Last Byte for Sync detection
static uint8_t LSLshifter; // Bitshiftvalue
static int32_t lat; // MTK Dataset
static int32_t lon; // MTK Dataset
static int32_t alt; // MTK Dataset
static int32_t grspeed; // MTK Dataset
static int32_t grcourse; // MTK Dataset
static uint8_t satn; // MTK Dataset
int32_t tmp32;
bool parsed;
parsed = false;
if(pstep == 0 || pstep>5){ // Only search for Sync when idle or lat and lon already decoded.
if (data == 0xdd && lastbyte == 0xd0) pstep = 100;// Detect Sync "0xD0,0xDD"
}
lastbyte = data;
switch(pstep) {
case 0: // Special Case: Do Nothing
break;
case 100: // Special Case: Jump into decoding on next run
pstep = 1;
break;
case 1: // Payload Byte is always $20! (This is the first Byte after sync preamble)
if (data == 0x20) pstep++; // Since it is always $20 we take it as extended, undocumented syncword "preamble3"
else pstep =0;
break;
case 2: // Read Dataset Latitude
lat = data;
LSLshifter = 0;
pstep++;
break;
case 3:
LSLshifter = LSLshifter+8;
tmp32 = data;
tmp32 = tmp32<<LSLshifter;
lat = lat | tmp32;
if (LSLshifter == 24){lat = lat * 10; pstep++;}
break;
case 4: // Read Dataset Longitude
lon = data;
LSLshifter = 0;
pstep++;
break;
case 5:
LSLshifter = LSLshifter+8;
tmp32 = data;
tmp32 = tmp32<<LSLshifter;
lon = lon | tmp32;
if (LSLshifter == 24){lon = lon * 10; pstep++;}
break;
case 6: // Read Dataset MSL Altitude
alt = data;
LSLshifter = 0;
pstep++;
break;
case 7:
LSLshifter = LSLshifter+8;
tmp32 = data;
tmp32 = tmp32<<LSLshifter;
alt = alt | tmp32;
if (LSLshifter == 24){alt = alt/100; pstep++;}
break;
case 8: // Read Dataset Ground Speed
grspeed = data;
LSLshifter = 0;
pstep++;
break;
case 9:
LSLshifter = LSLshifter+8;
tmp32 = data;
tmp32 = tmp32<<LSLshifter;
grspeed = grspeed | tmp32;
if (LSLshifter == 24) pstep++;
break;
case 10: // Read Dataset Heading
grcourse = data;
LSLshifter = 0;
pstep++;
break;
case 11:
LSLshifter = LSLshifter+8;
tmp32 = data;
tmp32 = tmp32<<LSLshifter;
grcourse = grcourse | tmp32;
if (LSLshifter == 24) pstep++;
break;
case 12: // Read number of satellites in view
satn = data;
pstep++;
break;
case 13: // Read Fix Type and finish readout, mwii does not need the rest
if (data==1){ // No Fix
i2c_dataset.status.gps2dfix = 0;
i2c_dataset.status.gps3dfix = 0;}
if (data==2){ // GPS 2D Fix
i2c_dataset.status.gps2dfix = 1;
i2c_dataset.status.gps3dfix = 0;}
if (data==3){ // GPS 3D Fix
i2c_dataset.status.gps2dfix = 1;
i2c_dataset.status.gps3dfix = 1;}
GPS_read[LAT] = lat;
GPS_read[LON] = lon;
i2c_dataset.altitude = alt;
i2c_dataset.ground_speed = grspeed;
i2c_dataset.ground_course = grcourse;
i2c_dataset.status.numsats = satn;
parsed = true; // RDY
pstep = 0; // Do nothing with the next bytes
break;
}
return parsed;
}Programmiert könnte es so aussehen (Beispiel für GPS Lat/Lon Daten):
Code:
// AT THIS POINT: We have a valid GGA frame and we have lat and lon in GPS_read_lat and GPS_read_lon
int32_t extmp;
uint8_t rdy,sortidx,maxsortidx;
extmp = GPS_read[LAT];
LatSpikeTab[4] = extmp;
LatSpikeTab[0] = extmp;
extmp = GPS_read[LON];
LonSpikeTab[4] = extmp;
LonSpikeTab[0] = extmp;
rdy = 0; maxsortidx=4;
while(rdy == 0){
rdy = 1;
for (sortidx = 0; sortidx<maxsortidx;sortidx++){
extmp = LatSpikeTab[sortidx];
if (extmp > LatSpikeTab[sortidx+1]) {LatSpikeTab[sortidx] = LatSpikeTab[sortidx+1]; LatSpikeTab[sortidx+1] = extmp; rdy = 0;}
}
maxsortidx --;}
rdy = 0; maxsortidx=4;
while(rdy == 0){
rdy = 1;
for (sortidx = 0; sortidx<maxsortidx;sortidx++){
extmp = LonSpikeTab[sortidx];
if (extmp > LonSpikeTab[sortidx+1]) {LonSpikeTab[sortidx] = LonSpikeTab[sortidx+1]; LonSpikeTab[sortidx+1] = extmp; rdy = 0;}
}
maxsortidx --;}
GPS_read[LAT] = LatSpikeTab[2]; // Fill Filter
GPS_read[LON] = LonSpikeTab[2];
Code:
LatTab[Avgidx] = GPS_read[LAT]; // Fill Filter
LonTab[Avgidx] = GPS_read[LON];
Avgidx++; if (Avgidx==GPS_FILTER_VECTOR_LENGTH) Avgidx=0;
uint8_t IdxCnt=0;
int64_t tmp64Lat=0,tmp64Lon=0;
while(IdxCnt<GPS_FILTER_VECTOR_LENGTH){tmp64Lat = tmp64Lat + LatTab[IdxCnt]; tmp64Lon = tmp64Lon + LonTab[IdxCnt]; IdxCnt++;}
i2c_dataset.gps_loc.lat = tmp64Lat/GPS_FILTER_VECTOR_LENGTH;
i2c_dataset.gps_loc.lon = tmp64Lon/GPS_FILTER_VECTOR_LENGTH;In der Summe kann ich hier nur eine hoch experimentelle ALPHA Version anbieten. Die Version ist NUR als Codebeispiel gedacht! Voraussetzung ist die AXN1.51_2722_3329_384.1151100.5.bin.
LG
Rob
Zuletzt bearbeitet:
