12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708270927102711271227132714271527162717271827192720272127222723272427252726272727282729273027312732273327342735273627372738273927402741274227432744274527462747274827492750275127522753275427552756275727582759276027612762276327642765276627672768276927702771277227732774277527762777277827792780278127822783278427852786278727882789279027912792279327942795279627972798279928002801280228032804280528062807280828092810281128122813281428152816281728182819282028212822282328242825282628272828282928302831283228332834283528362837283828392840284128422843284428452846284728482849285028512852285328542855285628572858285928602861286228632864286528662867286828692870287128722873287428752876287728782879288028812882288328842885288628872888288928902891289228932894289528962897289828992900290129022903290429052906290729082909291029112912291329142915291629172918291929202921292229232924292529262927292829292930293129322933293429352936293729382939294029412942294329442945294629472948294929502951295229532954295529562957295829592960296129622963296429652966296729682969297029712972297329742975297629772978297929802981298229832984298529862987298829892990299129922993299429952996299729982999300030013002300330043005300630073008300930103011301230133014301530163017301830193020302130223023302430253026302730283029303030313032303330343035303630373038303930403041304230433044304530463047304830493050305130523053305430553056305730583059306030613062306330643065306630673068306930703071307230733074307530763077307830793080308130823083308430853086308730883089309030913092309330943095309630973098309931003101310231033104310531063107310831093110311131123113311431153116311731183119312031213122312331243125312631273128312931303131313231333134313531363137313831393140314131423143314431453146314731483149315031513152315331543155315631573158315931603161316231633164316531663167316831693170317131723173317431753176317731783179318031813182318331843185318631873188318931903191319231933194319531963197319831993200320132023203320432053206320732083209321032113212321332143215321632173218321932203221322232233224322532263227322832293230323132323233323432353236323732383239324032413242324332443245324632473248324932503251325232533254325532563257325832593260326132623263326432653266326732683269327032713272327332743275327632773278327932803281328232833284328532863287328832893290329132923293329432953296329732983299330033013302330333043305330633073308330933103311331233133314331533163317331833193320332133223323332433253326332733283329333033313332333333343335333633373338333933403341334233433344334533463347334833493350335133523353335433553356335733583359336033613362336333643365336633673368336933703371337233733374337533763377337833793380338133823383338433853386338733883389339033913392339333943395339633973398339934003401340234033404340534063407340834093410341134123413341434153416341734183419342034213422342334243425342634273428342934303431343234333434343534363437343834393440344134423443344434453446344734483449345034513452345334543455345634573458345934603461346234633464346534663467346834693470347134723473347434753476347734783479348034813482348334843485348634873488348934903491349234933494349534963497349834993500350135023503350435053506350735083509351035113512351335143515351635173518351935203521352235233524352535263527352835293530353135323533353435353536353735383539354035413542354335443545354635473548354935503551355235533554 |
- #include "gpsd_config.h"
- #include <ctype.h>
- #include <stdio.h>
- #include <stdlib.h>
- #include <stdbool.h>
- #include <math.h>
- #include <string.h>
- #include <stdarg.h>
- #include <time.h>
- #include "gpsd.h"
- #include "strfuncs.h"
- #ifdef NMEA0183_ENABLE
- #include "timespec.h"
- static int do_lat_lon(char *field[], struct gps_fix_t *out)
- {
- double d, m;
- double lon;
- double lat;
- if ('\0' == field[0][0] ||
- '\0' == field[1][0] ||
- '\0' == field[2][0] ||
- '\0' == field[3][0]) {
- return 1;
- }
- lat = safe_atof(field[0]);
- m = 100.0 * modf(lat / 100.0, &d);
- lat = d + m / 60.0;
- if ('S' == field[1][0])
- lat = -lat;
- lon = safe_atof(field[2]);
- m = 100.0 * modf(lon / 100.0, &d);
- lon = d + m / 60.0;
- if ('W' == field[3][0])
- lon = -lon;
- if (0 == isfinite(lat) ||
- 0 == isfinite(lon)) {
- return 2;
- }
- out->latitude = lat;
- out->longitude = lon;
- return 0;
- }
- static int faa_mode(char mode)
- {
- int newstatus = STATUS_FIX;
- switch (mode) {
- case '\0':
- newstatus = STATUS_NO_FIX;
- break;
- case 'A':
- default:
- newstatus = STATUS_FIX;
- break;
- case 'D':
- newstatus = STATUS_DGPS_FIX;
- break;
- case 'E':
- newstatus = STATUS_DR;
- break;
- case 'F':
- newstatus = STATUS_RTK_FLT;
- break;
- case 'N':
-
- newstatus = STATUS_NO_FIX;
- break;
- case 'P':
- newstatus = STATUS_DGPS_FIX;
- break;
- case 'R':
- newstatus = STATUS_RTK_FIX;
- break;
- case 'S':
- newstatus = STATUS_NO_FIX;
- break;
- }
- return newstatus;
- }
- #define DD(s) ((int)((s)[0]-'0')*10+(int)((s)[1]-'0'))
- static int merge_ddmmyy(char *ddmmyy, struct gps_device_t *session)
- {
- int yy;
- int mon;
- int mday;
- int year;
- unsigned i;
- if (NULL == ddmmyy) {
- return 1;
- }
- for (i = 0; i < 6; i++) {
-
- if (0 == isdigit((int)ddmmyy[i])) {
-
-
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "merge_ddmmyy(%s), malformed date\n", ddmmyy);
- return 2;
- }
- }
-
- if ('\0' != ddmmyy[6]) {
-
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "merge_ddmmyy(%s), malformed date\n", ddmmyy);
- return 3;
- }
-
- yy = DD(ddmmyy + 4);
- mon = DD(ddmmyy + 2);
- mday = DD(ddmmyy);
-
- if (session->nmea.date.tm_year % 100 == 99 && yy == 0)
- gpsd_century_update(session, session->context->century + 100);
- year = (session->context->century + yy);
-
- if (2080 <= year) {
- year -= 100;
- }
- if ( (1 > mon ) || (12 < mon ) ) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "merge_ddmmyy(%s), malformed month\n", ddmmyy);
- return 4;
- } else if ( (1 > mday ) || (31 < mday ) ) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "merge_ddmmyy(%s), malformed day\n", ddmmyy);
- return 5;
- } else {
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "merge_ddmmyy(%s) sets year %d\n",
- ddmmyy, year);
- session->nmea.date.tm_year = year - 1900;
- session->nmea.date.tm_mon = mon - 1;
- session->nmea.date.tm_mday = mday;
- }
- return 0;
- }
- static int decode_hhmmss(struct tm *date, long *nsec, char *hhmmss,
- struct gps_device_t *session)
- {
- int old_hour = date->tm_hour;
- int i, sublen;
- if (NULL == hhmmss) {
- return 1;
- }
- for (i = 0; i < 6; i++) {
-
- if (0 == isdigit((int)hhmmss[i])) {
-
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "decode_hhmmss(%s), malformed time\n", hhmmss);
- return 2;
- }
- }
-
- date->tm_hour = DD(hhmmss);
- if (date->tm_hour < old_hour)
- date->tm_mday++;
- date->tm_min = DD(hhmmss + 2);
- date->tm_sec = DD(hhmmss + 4);
- if ('.' == hhmmss[6] &&
-
- 0 != isdigit((int)hhmmss[7])) {
- i = atoi(hhmmss + 7);
- sublen = strlen(hhmmss + 7);
- *nsec = (long)i * (long)pow(10.0, 9 - sublen);
- } else {
- *nsec = 0;
- }
- return 0;
- }
- static int merge_hhmmss(char *hhmmss, struct gps_device_t *session)
- {
- int old_hour = session->nmea.date.tm_hour;
- int i, sublen;
- if (NULL == hhmmss) {
- return 1;
- }
- for (i = 0; i < 6; i++) {
-
- if (0 == isdigit((int)hhmmss[i])) {
-
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "merge_hhmmss(%s), malformed time\n", hhmmss);
- return 2;
- }
- }
-
- session->nmea.date.tm_hour = DD(hhmmss);
- if (session->nmea.date.tm_hour < old_hour)
- session->nmea.date.tm_mday++;
- session->nmea.date.tm_min = DD(hhmmss + 2);
- session->nmea.date.tm_sec = DD(hhmmss + 4);
- session->nmea.subseconds.tv_sec = 0;
- if ('.' == hhmmss[6] &&
-
- 0 != isdigit((int)hhmmss[7])) {
- i = atoi(hhmmss + 7);
- sublen = strlen(hhmmss + 7);
- session->nmea.subseconds.tv_nsec = (long)i *
- (long)pow(10.0, 9 - sublen);
- } else {
- session->nmea.subseconds.tv_nsec = 0;
- }
- return 0;
- }
- static void register_fractional_time(const char *tag, const char *fld,
- struct gps_device_t *session)
- {
- char ts_buf[TIMESPEC_LEN];
- if (fld[0] != '\0') {
- session->nmea.last_frac_time = session->nmea.this_frac_time;
- DTOTS(&session->nmea.this_frac_time, safe_atof(fld));
- session->nmea.latch_frac_time = true;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "%s: registers fractional time %s\n",
- tag,
- timespec_str(&session->nmea.this_frac_time, ts_buf,
- sizeof(ts_buf)));
- }
- }
- static gps_mask_t processVTG(int count,
- char *field[],
- struct gps_device_t *session)
- {
- gps_mask_t mask = ONLINE_SET;
- if( (field[1][0] == '\0') || (field[5][0] == '\0')){
- return mask;
- }
-
- if ((count > 9) && ('\0' != field[9][0])) {
- switch (field[9][0]) {
- case 'A':
-
-
- case 'D':
-
-
-
- break;
- case 'E':
-
-
- case 'N':
-
-
-
-
- return mask;
- default:
-
- break;
- }
- }
-
- session->newdata.track = safe_atof(field[1]);
- mask |= TRACK_SET;
-
- if (field[3][0] != '\0'){
- session->newdata.magnetic_track = safe_atof(field[3]);
- mask |= MAGNETIC_TRACK_SET;
- }
- session->newdata.speed = safe_atof(field[5]) * KNOTS_TO_MPS;
- mask |= SPEED_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "VTG: course(T)=%.2f, course(M)=%.2f, speed=%.2f",
- session->newdata.track, session->newdata.magnetic_track,
- session->newdata.speed);
- return mask;
- }
- static gps_mask_t processRMC(int count, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- char status = field[2][0];
- int newstatus;
- switch (status) {
- default:
-
-
- case 'V':
-
- session->gpsdata.status = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- mask |= STATUS_SET | MODE_SET;
- break;
- case 'D':
-
-
- case 'A':
-
-
- if ('\0' != field[1][0] &&
- 9 < count &&
- '\0' != field[9][0]) {
- if (0 == merge_hhmmss(field[1], session) &&
- 0 == merge_ddmmyy(field[9], session)) {
-
- mask |= TIME_SET;
- register_fractional_time(field[0], field[1], session);
- }
- }
-
- if (0 == do_lat_lon(&field[3], &session->newdata)) {
- newstatus = STATUS_FIX;
- mask |= LATLON_SET;
- if (MODE_2D >= session->gpsdata.fix.mode) {
-
-
- session->newdata.mode = MODE_2D;
- mask |= MODE_SET;
- }
- } else {
- newstatus = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- mask |= MODE_SET;
- }
- if ('\0' != field[7][0]) {
- session->newdata.speed = safe_atof(field[7]) * KNOTS_TO_MPS;
- mask |= SPEED_SET;
- }
- if ('\0' != field[8][0]) {
- session->newdata.track = safe_atof(field[8]);
- mask |= TRACK_SET;
- }
-
- if ('\0' != field[10][0] &&
- '\0' != field[11][0]) {
- session->newdata.magnetic_var = safe_atof(field[10]);
- switch (field[11][0]) {
- case 'E':
-
- break;
- case 'W':
- session->newdata.magnetic_var = -session->newdata.magnetic_var;
- break;
- default:
-
- session->newdata.magnetic_var = NAN;
- break;
- }
- if (0 == isfinite(session->newdata.magnetic_var) ||
- 0.09 >= fabs(session->newdata.magnetic_var)) {
-
- session->newdata.magnetic_var = NAN;
- } else {
- mask |= MAGNETIC_TRACK_SET;
- }
- }
- if (count >= 12) {
- newstatus = faa_mode(field[12][0]);
- }
-
- if (3 < session->gpsdata.satellites_used) {
-
- session->newdata.mode = MODE_3D;
- mask |= MODE_SET;
- } else if (0 != isfinite(session->gpsdata.fix.altHAE) ||
- 0 != isfinite(session->gpsdata.fix.altMSL)) {
-
-
- session->newdata.mode = MODE_3D;
- mask |= MODE_SET;
- }
- session->gpsdata.status = newstatus;
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "RMC: ddmmyy=%s hhmmss=%s lat=%.2f lon=%.2f "
- "speed=%.2f track=%.2f mode=%d var=%.1f status=%d\n",
- field[9], field[1],
- session->newdata.latitude,
- session->newdata.longitude,
- session->newdata.speed,
- session->newdata.track,
- session->newdata.mode,
- session->newdata.magnetic_var,
- session->gpsdata.status);
- return mask;
- }
- static gps_mask_t processGLL(int count, char *field[],
- struct gps_device_t *session)
- {
-
- char *status = field[7];
- gps_mask_t mask = ONLINE_SET;
- if (field[5][0] != '\0') {
- if (0 == merge_hhmmss(field[5], session)) {
- register_fractional_time(field[0], field[5], session);
- if (session->nmea.date.tm_year == 0)
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "can't use GLL time until after ZDA or RMC"
- " has supplied a year.\n");
- else {
- mask = TIME_SET;
- }
- }
- }
- if ('\0' == field[6][0] ||
- 'V' == field[6][0]) {
-
- session->gpsdata.status = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- mask |= STATUS_SET | MODE_SET;
- } else if ('A' == field[6][0] &&
- (count < 8 || *status != 'N') &&
- 0 == do_lat_lon(&field[1], &session->newdata)) {
- int newstatus;
- mask |= LATLON_SET;
- newstatus = STATUS_FIX;
- if (count >= 8) {
- newstatus = faa_mode(*status);
- }
-
- if (0 != isfinite(session->gpsdata.fix.altHAE) ||
- 0 != isfinite(session->gpsdata.fix.altMSL)) {
- session->newdata.mode = MODE_3D;
- mask |= MODE_SET;
- } else if (3 < session->gpsdata.satellites_used) {
-
- session->newdata.mode = MODE_3D;
- mask |= MODE_SET;
- } else if (MODE_2D > session->gpsdata.fix.mode ||
- (0 == isfinite(session->oldfix.altHAE) &&
- 0 == isfinite(session->oldfix.altMSL))) {
- session->newdata.mode = MODE_2D;
- mask |= MODE_SET;
- }
- session->gpsdata.status = newstatus;
- } else {
- session->gpsdata.status = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- mask |= STATUS_SET | MODE_SET;
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "GLL: hhmmss=%s lat=%.2f lon=%.2f mode=%d status=%d\n",
- field[5],
- session->newdata.latitude,
- session->newdata.longitude,
- session->newdata.mode,
- session->gpsdata.status);
- return mask;
- }
- static gps_mask_t processGNS(int count UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- int newstatus;
- int satellites_used;
- gps_mask_t mask = ONLINE_SET;
- if (field[1][0] != '\0') {
- if (0 == merge_hhmmss(field[1], session)) {
- register_fractional_time(field[0], field[1], session);
- if (session->nmea.date.tm_year == 0) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "can't use GNS time until after ZDA or RMC"
- " has supplied a year.\n");
- } else {
- mask = TIME_SET;
- }
- }
- }
-
- if ('\0' == field[6][0] ||
- 'N' == field[6][0]) {
- session->newdata.mode = MODE_NO_FIX;
- mask |= MODE_SET;
- return mask;
- }
-
-
- if ('U' == field[13][0] ||
- 'V' == field[13][0]) {
- return mask;
- }
- satellites_used = atoi(field[7]);
- if (0 == do_lat_lon(&field[2], &session->newdata)) {
- mask |= LATLON_SET;
- session->newdata.mode = MODE_2D;
- if ('\0' != field[9][0]) {
-
- session->newdata.altMSL = safe_atof(field[9]);
- if (0 != isfinite(session->newdata.altMSL)) {
- mask |= ALTITUDE_SET;
- if (3 < satellites_used) {
-
- session->newdata.mode = MODE_3D;
- }
- }
-
- if ('\0' != field[10][0]) {
- session->newdata.geoid_sep = safe_atof(field[10]);
- }
-
- }
- } else {
- session->newdata.mode = MODE_NO_FIX;
- mask |= MODE_SET;
- }
- if (field[8][0] != '\0') {
- session->gpsdata.dop.hdop = safe_atof(field[8]);
- }
- newstatus = faa_mode(field[6][0]);
- session->gpsdata.status = newstatus;
- mask |= MODE_SET;
-
- if ('\0' != field[11][0] &&
- '\0' != field[12][0]) {
-
- session->newdata.dgps_age = safe_atof(field[11]);
- session->newdata.dgps_station = atoi(field[12]);
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "GNS: hhmmss=%s lat=%.2f lon=%.2f mode=%d status=%d\n",
- field[1],
- session->newdata.latitude,
- session->newdata.longitude,
- session->newdata.mode,
- session->gpsdata.status);
- return mask;
- }
- static gps_mask_t processGGA(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- int newstatus;
- char last_last_gga_talker = session->nmea.last_gga_talker;
- int fix;
- int satellites_visible;
- session->nmea.last_gga_talker = field[0][1];
- if (0 == strlen(field[6])) {
-
- fix = -1;
- } else {
- fix = atoi(field[6]);
- }
- switch (fix) {
- case 0:
- newstatus = STATUS_NO_FIX;
- break;
- case 1:
-
- newstatus = STATUS_FIX;
- break;
- case 2:
- newstatus = STATUS_DGPS_FIX;
- break;
- case 3:
-
- newstatus = STATUS_PPS_FIX;
- break;
- case 4:
- newstatus = STATUS_RTK_FIX;
- break;
- case 5:
- newstatus = STATUS_RTK_FLT;
- break;
- case 6:
-
- newstatus = STATUS_DR;
- break;
- case 7:
-
- newstatus = STATUS_TIME;
- break;
- case 8:
-
-
- newstatus = STATUS_SIM;
- break;
- default:
- newstatus = -1;
- break;
- }
- if (0 <= newstatus) {
- session->gpsdata.status = newstatus;
- mask = STATUS_SET;
- }
-
- if ('\0' != last_last_gga_talker &&
- last_last_gga_talker != session->nmea.last_gga_talker) {
-
- session->nmea.latch_mode = 0;
- } else {
- session->nmea.latch_mode = strncmp(field[1],
- session->nmea.last_gga_timestamp,
- sizeof(session->nmea.last_gga_timestamp))==0;
- }
- if (session->nmea.latch_mode) {
- session->gpsdata.status = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "xxGGA: latch mode\n");
- } else
- (void)strlcpy(session->nmea.last_gga_timestamp, field[1],
- sizeof(session->nmea.last_gga_timestamp));
-
- satellites_visible = atoi(field[7]);
- if (0 == merge_hhmmss(field[1], session)) {
- register_fractional_time(field[0], field[1], session);
- if (session->nmea.date.tm_year == 0)
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "can't use GGA time until after ZDA or RMC"
- " has supplied a year.\n");
- else {
- mask |= TIME_SET;
- }
- }
- if (0 == do_lat_lon(&field[2], &session->newdata)) {
- session->newdata.mode = MODE_2D;
- mask |= LATLON_SET;
- if ('\0' != field[11][0]) {
- session->newdata.geoid_sep = safe_atof(field[11]);
- } else {
- session->newdata.geoid_sep = wgs84_separation(
- session->newdata.latitude, session->newdata.longitude);
- }
-
- if ('\0' != field[9][0]) {
-
- session->newdata.altMSL = safe_atof(field[9]);
-
- mask |= ALTITUDE_SET;
-
- if (4 <= satellites_visible) {
- session->newdata.mode = MODE_3D;
- }
- }
- if (3 > satellites_visible) {
- session->newdata.mode = MODE_NO_FIX;
- }
- } else {
- session->newdata.mode = MODE_NO_FIX;
- }
- mask |= MODE_SET;
- if ('\0' != field[8][0]) {
-
- session->gpsdata.dop.hdop = safe_atof(field[8]);
- }
-
- if ('\0' != field[13][0] &&
- '\0' != field[14][0]) {
-
- double age;
- int station;
- age = safe_atof(field[13]);
- station = atoi(field[14]);
- if (0.09 < age ||
- 0 < station) {
-
- session->newdata.dgps_age = age;
- session->newdata.dgps_station = station;
- }
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "GGA: hhmmss=%s lat=%.2f lon=%.2f altMSL=%.2f mode=%d status=%d\n",
- field[1],
- session->newdata.latitude,
- session->newdata.longitude,
- session->newdata.altMSL,
- session->newdata.mode,
- session->gpsdata.status);
- return mask;
- }
- static gps_mask_t processGST(int count, char *field[],
- struct gps_device_t *session)
- {
-
- struct tm date;
- timespec_t ts;
- int ret;
- char ts_buf[TIMESPEC_LEN];
- gps_mask_t mask = ONLINE_SET;
- if (0 > count) {
- return mask;
- }
-
-
- if (0 < session->nmea.date.tm_year) {
-
- memset(&date, 0, sizeof(date));
- date.tm_year = session->nmea.date.tm_year;
- date.tm_mon = session->nmea.date.tm_mon;
- date.tm_mday = session->nmea.date.tm_mday;
-
-
- ret = decode_hhmmss(&date, &ts.tv_nsec, field[1], session);
- } else {
- ret = 1;
- }
- if (0 == ret) {
-
- session->gpsdata.gst.utctime.tv_sec = mkgmtime(&date);
- session->gpsdata.gst.utctime.tv_nsec = ts.tv_nsec;
- } else {
-
- session->gpsdata.gst.utctime.tv_sec = 0;
- session->gpsdata.gst.utctime.tv_nsec = 0;
- }
- session->gpsdata.gst.rms_deviation = safe_atof(field[2]);
- session->gpsdata.gst.smajor_deviation = safe_atof(field[3]);
- session->gpsdata.gst.sminor_deviation = safe_atof(field[4]);
- session->gpsdata.gst.smajor_orientation = safe_atof(field[5]);
- session->gpsdata.gst.lat_err_deviation = safe_atof(field[6]);
- session->gpsdata.gst.lon_err_deviation = safe_atof(field[7]);
- session->gpsdata.gst.alt_err_deviation = safe_atof(field[8]);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "GST: utc = %s, rms = %.2f, maj = %.2f, min = %.2f,"
- " ori = %.2f, lat = %.2f, lon = %.2f, alt = %.2f\n",
- timespec_str(&session->gpsdata.gst.utctime, ts_buf,
- sizeof(ts_buf)),
- session->gpsdata.gst.rms_deviation,
- session->gpsdata.gst.smajor_deviation,
- session->gpsdata.gst.sminor_deviation,
- session->gpsdata.gst.smajor_orientation,
- session->gpsdata.gst.lat_err_deviation,
- session->gpsdata.gst.lon_err_deviation,
- session->gpsdata.gst.alt_err_deviation);
- mask = GST_SET | ONLINE_SET;
- return mask;
- }
- static unsigned char nmea_sigid_to_ubx(unsigned char nmea_sigid)
- {
- unsigned char ubx_sigid = 0;
- switch (nmea_sigid) {
- default:
-
- case 0:
-
- ubx_sigid = 0;
- break;
- case 1:
-
- ubx_sigid = 0;
- break;
- case 2:
-
- ubx_sigid = 5;
- break;
- case 3:
-
- ubx_sigid = 2;
- break;
- case 5:
-
- ubx_sigid = 4;
- break;
- case 6:
-
- ubx_sigid = 3;
- break;
- case 7:
-
- ubx_sigid = 0;
- break;
- }
- return ubx_sigid;
- }
- static int nmeaid_to_prn(char *talker, int nmea_satnum,
- int nmea_gnssid,
- unsigned char *ubx_gnssid,
- unsigned char *ubx_svid)
- {
-
- int nmea2_prn = nmea_satnum;
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- if (1 > nmea_satnum) {
-
- nmea2_prn = 0;
- } else if (0 < nmea_gnssid) {
-
- switch (nmea_gnssid) {
- default:
-
-
- case 0:
-
- nmea2_prn = 0;
- break;
- case 1:
- if (33 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = nmea_satnum;
- } else if (65 > nmea_satnum) {
-
- *ubx_gnssid = 1;
- *ubx_svid = 87 + nmea_satnum;
- } else if (152 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (158 > nmea_satnum) {
-
- *ubx_gnssid = 1;
- *ubx_svid = nmea_satnum;
- } else if (193 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (200 > nmea_satnum) {
-
-
- *ubx_gnssid = 3;
- *ubx_svid = nmea_satnum - 192;
- } else {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- }
- break;
- case 2:
-
- *ubx_gnssid = 6;
- *ubx_svid = nmea_satnum;
- break;
- case 3:
-
- *ubx_gnssid = 2;
- *ubx_svid = nmea_satnum;
- nmea2_prn = 300 + nmea_satnum;
- break;
- case 4:
-
- *ubx_gnssid = 3;
- *ubx_svid = nmea_satnum;
- nmea2_prn = 300 + nmea_satnum;
- break;
- }
-
- } else if (32 >= nmea_satnum) {
- *ubx_svid = nmea_satnum;
- switch (talker[0]) {
- case 'G':
- if (talker[1] == 'A') {
-
- nmea2_prn = 300 + nmea_satnum;
- *ubx_gnssid = 2;
- } else if (talker[1] == 'B') {
-
- *ubx_gnssid = 3;
- nmea2_prn = 400 + nmea_satnum;
- } else if (talker[1] == 'L') {
-
- nmea2_prn = 64 + nmea_satnum;
- *ubx_gnssid = 6;
- } else if (talker[1] == 'N') {
-
- } else if (talker[1] == 'P') {
-
- }
- break;
- case 'B':
- if (talker[1] == 'D') {
-
- nmea2_prn = 400 + nmea_satnum;
- *ubx_gnssid = 3;
- }
- break;
- case 'P':
-
- if (talker[1] == 'Q') {
-
- nmea2_prn = 400 + nmea_satnum;
- *ubx_gnssid = 3;
- }
- break;
- case 'Q':
- if (talker[1] == 'Z') {
-
- nmea2_prn = 192 + nmea_satnum;
- *ubx_gnssid = 5;
- }
- break;
- default:
-
- break;
- }
- } else if (64 >= nmea_satnum) {
-
-
- *ubx_gnssid = 1;
- *ubx_svid = 87 + nmea_satnum;
- } else if (96 >= nmea_satnum) {
-
- *ubx_gnssid = 6;
- *ubx_svid = nmea_satnum - 64;
- } else if (120 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (158 >= nmea_satnum) {
-
- *ubx_gnssid = 1;
- *ubx_svid = nmea_satnum;
- } else if (173 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (182 >= nmea_satnum) {
-
- *ubx_gnssid = 4;
- *ubx_svid = nmea_satnum - 172;
- } else if (193 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (197 >= nmea_satnum) {
-
-
- *ubx_gnssid = 5;
- *ubx_svid = nmea_satnum - 192;
- } else if (201 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (237 >= nmea_satnum) {
-
-
- *ubx_gnssid = 3;
- *ubx_svid = nmea_satnum - 200;
- nmea2_prn += 200;
- } else if (301 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (356 >= nmea_satnum) {
-
- *ubx_gnssid = 2;
- *ubx_svid = nmea_satnum - 300;
- } else if (401 > nmea_satnum) {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- } else if (437 >= nmea_satnum) {
-
- *ubx_gnssid = 3;
- *ubx_svid = nmea_satnum - 400;
- } else {
-
- *ubx_gnssid = 0;
- *ubx_svid = 0;
- nmea2_prn = 0;
- }
- return nmea2_prn;
- }
- static gps_mask_t processGSA(int count, char *field[],
- struct gps_device_t *session)
- {
- #define GSA_TALKER field[0][1]
-
- gps_mask_t mask = ONLINE_SET;
- char last_last_gsa_talker = session->nmea.last_gsa_talker;
- int nmea_gnssid = 0;
- int nmea_sigid = 0;
- int ubx_sigid = 0;
-
- if (18 > count) {
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "xxGSA: malformed, setting ONLINE_SET only.\n");
- mask = ONLINE_SET;
- } else if (session->nmea.latch_mode) {
-
- mask = ONLINE_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "xxGSA: non-advancing timestamp\n");
- } else {
- int i;
- session->newdata.mode = atoi(field[2]);
-
- if ('E' == field[2][0])
- mask = ONLINE_SET;
- else
- mask = MODE_SET;
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "xxGSA sets mode %d\n", session->newdata.mode);
- if (19 < count ) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "xxGSA: count %d too long!\n", count);
- } else {
-
- if (field[15][0] != '\0')
- session->gpsdata.dop.pdop = safe_atof(field[15]);
- if (field[16][0] != '\0')
- session->gpsdata.dop.hdop = safe_atof(field[16]);
- if (field[17][0] != '\0')
- session->gpsdata.dop.vdop = safe_atof(field[17]);
- if (19 == count && '\0' != field[18][0]) {
-
- nmea_sigid = atoi(field[18]);
-
- ubx_sigid = nmea_sigid_to_ubx(nmea_sigid);
- }
- }
-
- if ( '\0' == session->nmea.last_gsa_talker
- || (GSA_TALKER == session->nmea.last_gsa_talker
- && 'N' != GSA_TALKER) ) {
- session->gpsdata.satellites_used = 0;
- memset(session->nmea.sats_used, 0, sizeof(session->nmea.sats_used));
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "xxGSA: clear sats_used\n");
- }
- session->nmea.last_gsa_talker = GSA_TALKER;
- if ((session->nmea.last_gsa_talker == 'B') ||
- (session->nmea.last_gsa_talker == 'D') ||
- (session->nmea.last_gsa_talker == 'Q'))
-
- session->nmea.seen_bdgsa = true;
- else if (session->nmea.last_gsa_talker == 'L')
- session->nmea.seen_glgsa = true;
- else if (session->nmea.last_gsa_talker == 'N')
- session->nmea.seen_gngsa = true;
- else if (session->nmea.last_gsa_talker == 'A')
- session->nmea.seen_gagsa = true;
-
- for (i = 0; i < count - 6; i++) {
- int prn;
- int nmea_satnum;
- unsigned char ubx_gnssid;
- unsigned char ubx_svid;
-
- nmea_satnum = atoi(field[i + 3]);
- if (1 > nmea_satnum) {
- continue;
- }
- prn = nmeaid_to_prn(field[0], nmea_satnum, nmea_gnssid,
- &ubx_gnssid, &ubx_svid);
- #ifdef __UNUSED__
-
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "%s nmeaid_to_prn: nmea_gnssid %d nmea_satnum %d ubx_gnssid %d "
- "ubx_svid %d nmea2_prn %d\n", field[0],
- nmea_gnssid, nmea_satnum, ubx_gnssid, ubx_svid, prn);
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "%s count %d\n", field[0], count);
- #endif
- if (prn > 0) {
-
- if (MAXCHANNELS <= session->gpsdata.satellites_used) {
-
- break;
- }
- session->nmea.sats_used[session->gpsdata.satellites_used++] =
- (unsigned short)prn;
- }
- }
- mask |= DOP_SET | USED_IS;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "xxGSA: mode=%d used=%d pdop=%.2f hdop=%.2f vdop=%.2f "
- "ubx_sigid %d\n",
- session->newdata.mode,
- session->gpsdata.satellites_used,
- session->gpsdata.dop.pdop,
- session->gpsdata.dop.hdop,
- session->gpsdata.dop.vdop, ubx_sigid);
- }
-
- if ((session->nmea.seen_glgsa || session->nmea.seen_bdgsa ||
- session->nmea.seen_gagsa) && GSA_TALKER == 'P') {
- mask = ONLINE_SET;
-
-
- } else if ( 'N' != last_last_gsa_talker && 'N' == GSA_TALKER) {
- mask = ONLINE_SET | MODE_SET;
- }
-
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "xxGSA: mask %#llx\n", (long long unsigned)mask);
- return mask;
- #undef GSA_TALKER
- }
- static gps_mask_t processGSV(int count, char *field[],
- struct gps_device_t *session)
- {
- #define GSV_TALKER field[0][1]
-
- int n, fldnum;
- unsigned char nmea_sigid = 0;
- int nmea_gnssid = 0;
- unsigned char ubx_sigid = 0;
- if (count <= 3) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "malformed xxGSV - fieldcount %d <= 3\n",
- count);
- gpsd_zero_satellites(&session->gpsdata);
- return ONLINE_SET;
- }
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "x%cGSV: part %s of %s, last_gsv_talker '%#x' "
- " last_gsv_sigid %u\n",
- GSV_TALKER, field[2], field[1],
- session->nmea.last_gsv_talker,
- session->nmea.last_gsv_sigid);
-
- switch (count % 4) {
- case 0:
-
- break;
- case 1:
-
- nmea_sigid = atoi(field[count - 1]);
- ubx_sigid = nmea_sigid_to_ubx(nmea_sigid);
- break;
- default:
-
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "malformed GPGSV - fieldcount %d %% 4 != 0\n", count);
- gpsd_zero_satellites(&session->gpsdata);
- return ONLINE_SET;
- }
- session->nmea.await = atoi(field[1]);
- if ((session->nmea.part = atoi(field[2])) < 1) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "malformed GPGSV - bad part\n");
- gpsd_zero_satellites(&session->gpsdata);
- return ONLINE_SET;
- }
- if (session->nmea.part == 1) {
-
- if (session->nmea.last_gsv_talker == '\0' ||
- ('P' == GSV_TALKER &&
- 0 == ubx_sigid)) {
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "x%cGSV: new part %d, last_gsv_talker '%#x', zeroing\n",
- GSV_TALKER,
- session->nmea.part,
- session->nmea.last_gsv_talker);
- gpsd_zero_satellites(&session->gpsdata);
- }
- session->nmea.last_gsv_talker = GSV_TALKER;
- session->nmea.last_gsv_sigid = ubx_sigid;
- switch (GSV_TALKER) {
- case 'A':
- session->nmea.seen_gagsv = true;
- break;
- case 'B':
-
- case 'D':
-
- case 'Q':
-
- session->nmea.seen_bdgsv = true;
- break;
- case 'L':
- session->nmea.seen_glgsv = true;
- break;
- case 'P':
- session->nmea.seen_gpgsv = true;
- break;
- case 'Z':
- session->nmea.seen_qzss = true;
- break;
- default:
-
- break;
- }
- }
- for (fldnum = 4; fldnum < count / 4 * 4;) {
- struct satellite_t *sp;
- int nmea_svid;
- if (session->gpsdata.satellites_visible >= MAXCHANNELS) {
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "xxGSV: internal error - too many satellites [%d]!\n",
- session->gpsdata.satellites_visible);
- gpsd_zero_satellites(&session->gpsdata);
- break;
- }
- sp = &session->gpsdata.skyview[session->gpsdata.satellites_visible];
- nmea_svid = atoi(field[fldnum++]);
- if (0 == nmea_svid) {
-
- continue;
- }
-
- sp->PRN = (short)nmeaid_to_prn(field[0], nmea_svid, nmea_gnssid,
- &sp->gnssid, &sp->svid);
- #ifdef __UNUSED__
- {
-
- char ts_buf[TIMESPEC_LEN];
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "%s nmeaid_to_prn: nmea_gnssid %d nmea_satnum %d "
- "ubx_gnssid %d ubx_svid %d nmea2_prn %d\n", field[0],
- nmea_gnssid, nmea_svid, sp->gnssid, sp->svid, sp->PRN);
- }
- #endif
- sp->elevation = (double)atoi(field[fldnum++]);
- sp->azimuth = (double)atoi(field[fldnum++]);
- sp->ss = (double)atoi(field[fldnum++]);
- sp->used = false;
- sp->sigid = ubx_sigid;
-
- if (0 < sp->PRN && 0 < sp->ss) {
- for (n = 0; n < MAXCHANNELS; n++)
- if (session->nmea.sats_used[n] == (unsigned short)sp->PRN) {
- sp->used = true;
- break;
- }
- }
-
- session->gpsdata.satellites_visible++;
- }
- #if __UNUSED
-
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "x%cGSV: vis %d gagsv %d bdgsv %d glgsv %d qzss %d\n",
- GSV_TALKER,
- session->gpsdata.satellites_visible,
- session->nmea.seen_gagsv,
- session->nmea.seen_bdgsv,
- session->nmea.seen_glgsv,
- session->nmea.seen_qzss);
- #endif
-
- if (!(session->nmea.seen_glgsv || session->nmea.seen_bdgsv
- || session->nmea.seen_qzss || session->nmea.seen_gagsv))
- if (session->nmea.part == session->nmea.await
- && atoi(field[3]) != session->gpsdata.satellites_visible)
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "GPGSV field 3 value of %d != actual count %d\n",
- atoi(field[3]), session->gpsdata.satellites_visible);
-
- if (session->nmea.part < session->nmea.await) {
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "xxGSV: Partial satellite data (%d of %d).\n",
- session->nmea.part, session->nmea.await);
- return ONLINE_SET;
- }
-
- for (n = 0; n < session->gpsdata.satellites_visible; n++)
- if (session->gpsdata.skyview[n].azimuth != 0)
- goto sane;
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "xxGSV: Satellite data no good (%d of %d).\n",
- session->nmea.part, session->nmea.await);
- gpsd_zero_satellites(&session->gpsdata);
- return ONLINE_SET;
- sane:
- session->gpsdata.skyview_time.tv_sec = 0;
- session->gpsdata.skyview_time.tv_nsec = 0;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "xxGSV: Satellite data OK (%d of %d).\n",
- session->nmea.part, session->nmea.await);
-
- if ((session->nmea.seen_glgsv || session->nmea.seen_bdgsv
- || session->nmea.seen_qzss || session->nmea.seen_gagsv)
- && GSV_TALKER == 'P')
- return ONLINE_SET;
- #if __UNUSED
-
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "x%cGSV: set skyview_time %s frac_time %.2f\n", GSV_TALKER,
- timespec_str(&session->gpsdata.skyview_time, ts_buf, sizeof(ts_buf)),
- session->nmea.this_frac_time);
- #endif
-
-
- session->gpsdata.dop.xdop = NAN;
- session->gpsdata.dop.ydop = NAN;
- session->gpsdata.dop.tdop = NAN;
- session->gpsdata.dop.gdop = NAN;
- return SATELLITE_SET;
- #undef GSV_TALKER
- }
- static gps_mask_t processPGRME(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if ('M' != field[2][0] ||
- 'M' != field[4][0] ||
- 'M' != field[6][0]) {
- mask = ONLINE_SET;
- } else {
- session->newdata.epx = session->newdata.epy =
- safe_atof(field[1]) * (1 / sqrt(2))
- * (GPSD_CONFIDENCE / CEP50_SIGMA);
- session->newdata.epv =
- safe_atof(field[3]) * (GPSD_CONFIDENCE / CEP50_SIGMA);
- session->newdata.sep =
- safe_atof(field[5]) * (GPSD_CONFIDENCE / CEP50_SIGMA);
- mask = HERR_SET | VERR_SET | PERR_IS;
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PGRME: epx=%.2f epy=%.2f sep=%.2f\n",
- session->newdata.epx,
- session->newdata.epy,
- session->newdata.sep);
- return mask;
- }
- static gps_mask_t processPGRMF(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if (0 == merge_hhmmss(field[4], session) &&
- 0 == merge_ddmmyy(field[3], session)) {
-
- mask |= TIME_SET;
- }
- if ('A' != field[10][0]) {
-
- return mask;
- }
- if ('\0' != field[5][0]) {
- session->context->leap_seconds = atoi(field[5]);
- session->context->valid = LEAP_SECOND_VALID;
- }
- if (0 == do_lat_lon(&field[6], &session->newdata)) {
- mask |= LATLON_SET;
- }
- switch (field[11][0]) {
- default:
-
- break;
- case '0':
- session->newdata.mode = MODE_NO_FIX;
- mask |= MODE_SET;
- break;
- case '1':
- session->newdata.mode = MODE_2D;
- mask |= MODE_SET;
- break;
- case '2':
- session->newdata.mode = MODE_3D;
- mask |= MODE_SET;
- break;
- }
- session->newdata.speed = safe_atof(field[12]) / MPS_TO_KPH;
- session->newdata.track = safe_atof(field[13]);
- mask |= SPEED_SET | TRACK_SET;
- session->gpsdata.dop.pdop = safe_atof(field[14]);
- session->gpsdata.dop.tdop = safe_atof(field[15]);
- mask |= DOP_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PGRMF: pdop %.1f tdop %.1f \n",
- session->gpsdata.dop.pdop,
- session->gpsdata.dop.tdop);
- return mask;
- }
- static gps_mask_t processPGRMM(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if ('\0' != field[1][0]) {
- strlcpy(session->newdata.datum, field[1],
- sizeof(session->newdata.datum));
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PGRMM: datum=%.40s\n",
- session->newdata.datum);
- return mask;
- }
- static gps_mask_t processPGRMZ(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if ('f' == field[2][0] &&
- 0 < strlen(field[1])) {
-
-
- session->newdata.altMSL = atoi(field[1]) / METERS_TO_FEET;
- mask |= (ALTITUDE_SET);
- }
- switch (field[3][0]) {
- default:
-
- break;
- case '1':
- session->newdata.mode = MODE_NO_FIX;
- mask |= MODE_SET;
- break;
- case '2':
- session->newdata.mode = MODE_2D;
- mask |= MODE_SET;
- break;
- case '3':
- session->newdata.mode = MODE_3D;
- mask |= MODE_SET;
- break;
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PGRMZ: altMSL %.2f mode %d\n",
- session->newdata.altMSL,
- session->newdata.mode);
- return mask;
- }
- static gps_mask_t processPMGNST(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- int newmode = atoi(field[3]);
- if ('T' == field[4][0]) {
- switch(newmode) {
- default:
- session->newdata.mode = MODE_NO_FIX;
- break;
- case 2:
- session->newdata.mode = MODE_2D;
- break;
- case 3:
- session->newdata.mode = MODE_3D;
- break;
- }
- } else {
-
- session->newdata.mode = MODE_NO_FIX;
- }
- mask |= MODE_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PMGNST: mode: %d\n",
- session->newdata.mode);
- return mask;
- }
- static gps_mask_t processPSRFEPE(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = STATUS_SET;
-
- if ('\0' != field[1][0]) {
- if (0 == merge_hhmmss(field[1], session)) {
- register_fractional_time(field[0], field[1], session);
- if (session->nmea.date.tm_year == 0) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "can't use PSRFEPE time until after ZDA or RMC"
- " has supplied a year.\n");
- } else {
- mask |= TIME_SET;
- }
- }
- }
- if ('A' != field[2][0]) {
-
- return mask;
- }
- if ('\0' != field[3][0]) {
-
- session->gpsdata.dop.hdop = safe_atof(field[3]);
- mask |= DOP_SET;
- }
- if ('\0' != field[4][0]) {
-
- session->newdata.eph = safe_atof(field[4]);
- mask |= HERR_SET;
- }
- if ('\0' != field[5][0]) {
-
- session->newdata.epv = safe_atof(field[5]);
- mask |= VERR_SET;
- }
- if ('\0' != field[6][0]) {
-
- session->newdata.eps = safe_atof(field[6]);
- }
- if ('\0' != field[7][0]) {
-
- session->newdata.epd = safe_atof(field[7]);
- }
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "PSRFEPE: hdop=%.1f eph=%.1f epv=%.1f eps=%.1f epd=%.1f\n",
- session->gpsdata.dop.hdop,
- session->newdata.eph,
- session->newdata.epv,
- session->newdata.eps,
- session->newdata.epd);
- return mask;
- }
- static gps_mask_t processDTM(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- int i;
- static struct
- {
- char *code;
- char *name;
- } codes[] = {
- {"W84", "WGS84"},
- {"W72", "WGS72"},
- {"S85", "SGS85"},
- {"P90", "PE90"},
- {"999", "User Defined"},
- {"", ""},
- };
- gps_mask_t mask = ONLINE_SET;
- if ('\0' == field[1][0]) {
- return mask;
- }
- for (i = 0; ; i++) {
- if ('\0' == codes[i].code[0]) {
-
- strlcpy(session->newdata.datum, field[1],
- sizeof(session->newdata.datum));
- break;
- }
- if (0 ==strcmp(codes[i].code, field[1])) {
- strlcpy(session->newdata.datum, codes[i].name,
- sizeof(session->newdata.datum));
- break;
- }
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "xxDTM: datum=%.40s\n",
- session->newdata.datum);
- return mask;
- }
- static gps_mask_t processGBS(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
-
- register_fractional_time(field[0], field[1], session);
-
- if (session->nmea.date.tm_hour == DD(field[1])
- && session->nmea.date.tm_min == DD(field[1] + 2)
- && session->nmea.date.tm_sec == DD(field[1] + 4)) {
- session->newdata.epy = safe_atof(field[2]);
- session->newdata.epx = safe_atof(field[3]);
- session->newdata.epv = safe_atof(field[4]);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "GBS: epx=%.2f epy=%.2f epv=%.2f\n",
- session->newdata.epx,
- session->newdata.epy,
- session->newdata.epv);
- mask = HERR_SET | VERR_SET;
- } else {
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "second in $GPGBS error estimates doesn't match.\n");
- }
- return mask;
- }
- static gps_mask_t processZDA(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- int year, mon, mday, century;
- if (field[1][0] == '\0' || field[2][0] == '\0' || field[3][0] == '\0'
- || field[4][0] == '\0') {
- GPSD_LOG(LOG_WARN, &session->context->errout, "ZDA fields are empty\n");
- return mask;
- }
- if (0 != merge_hhmmss(field[1], session)) {
-
- return mask;
- }
-
- year = atoi(field[4]);
- mon = atoi(field[3]);
- mday = atoi(field[2]);
- century = year - year % 100;
- if ( (1900 > year ) || (2200 < year ) ) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "malformed ZDA year: %s\n", field[4]);
- } else if ( (1 > mon ) || (12 < mon ) ) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "malformed ZDA month: %s\n", field[3]);
- } else if ( (1 > mday ) || (31 < mday ) ) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "malformed ZDA day: %s\n", field[2]);
- } else {
- gpsd_century_update(session, century);
- session->nmea.date.tm_year = year - 1900;
- session->nmea.date.tm_mon = mon - 1;
- session->nmea.date.tm_mday = mday;
- mask = TIME_SET;
- }
- return mask;
- }
- static gps_mask_t processHDG(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- double sensor_heading;
- double magnetic_deviation;
- if ( 0 == strlen(field[1])) {
-
- return mask;
- }
- sensor_heading = safe_atof(field[1]);
- if ((0.0 > sensor_heading) || (360.0 < sensor_heading)) {
-
- return mask;
- }
- magnetic_deviation = safe_atof(field[2]);
- if ((0.0 > magnetic_deviation) || (360.0 < magnetic_deviation)) {
-
- return mask;
- }
- switch (field[2][0]) {
- case 'E':
- sensor_heading += magnetic_deviation;
- break;
- case 'W':
- sensor_heading += magnetic_deviation;
- break;
- default:
-
- break;
- }
-
- session->newdata.magnetic_track = sensor_heading;
- mask |= MAGNETIC_TRACK_SET;
-
- if ('\0' != field[3][0] &&
- '\0' != field[4][0]) {
- session->newdata.magnetic_var = safe_atof(field[3]);
- switch (field[4][0]) {
- case 'E':
-
- mask |= MAGNETIC_TRACK_SET;
- break;
- case 'W':
- session->newdata.magnetic_var = -session->newdata.magnetic_var;
- mask |= MAGNETIC_TRACK_SET;
- break;
- default:
-
- session->newdata.magnetic_var = NAN;
- break;
- }
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "$SDHDG heading %lf var %.1f\n",
- session->newdata.magnetic_track,
- session->newdata.magnetic_var);
- return mask;
- }
- static gps_mask_t processHDT(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- double heading;
- if ( 0 == strlen(field[1])) {
-
- return mask;
- }
- heading = safe_atof(field[1]);
- if ((0.0 > heading) || (360.0 < heading)) {
-
- return mask;
- }
-
- session->gpsdata.attitude.heading = heading;
- mask |= (ATTITUDE_SET);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "$HEHDT heading %lf.\n",
- session->gpsdata.attitude.heading);
- return mask;
- }
- static gps_mask_t processDBT(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if (field[3][0] != '\0') {
- session->newdata.depth = safe_atof(field[3]);
- mask |= (ALTITUDE_SET);
- } else if (field[1][0] != '\0') {
- session->newdata.depth = safe_atof(field[1]) / METERS_TO_FEET;
- mask |= (ALTITUDE_SET);
- } else if (field[5][0] != '\0') {
- session->newdata.depth = safe_atof(field[5]) / METERS_TO_FATHOMS;
- mask |= (ALTITUDE_SET);
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "mode %d, depth %lf.\n",
- session->newdata.mode,
- session->newdata.depth);
- return mask;
- }
- static gps_mask_t processTXT(int count, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- int msgType = 0;
- char *msgType_txt = "Unknown";
- if ( 5 != count) {
- return mask;
- }
- msgType = atoi(field[3]);
- switch ( msgType ) {
- case 0:
- msgType_txt = "Error";
- break;
- case 1:
- msgType_txt = "Warning";
- break;
- case 2:
- msgType_txt = "Notice";
- break;
- case 7:
- msgType_txt = "User";
- break;
- }
-
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "TXT: %.10s: %.80s\n",
- msgType_txt, field[4]);
- return mask;
- }
- #ifdef TNT_ENABLE
- static gps_mask_t processTNTHTM(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- session->gpsdata.attitude.heading = safe_atof(field[1]);
- session->gpsdata.attitude.mag_st = *field[2];
- session->gpsdata.attitude.pitch = safe_atof(field[3]);
- session->gpsdata.attitude.pitch_st = *field[4];
- session->gpsdata.attitude.roll = safe_atof(field[5]);
- session->gpsdata.attitude.roll_st = *field[6];
- session->gpsdata.attitude.dip = safe_atof(field[7]);
- session->gpsdata.attitude.mag_x = safe_atof(field[8]);
- mask |= (ATTITUDE_SET);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "$PTNTHTM heading %lf (%c).\n",
- session->gpsdata.attitude.heading,
- session->gpsdata.attitude.mag_st);
- return mask;
- }
- static gps_mask_t processTNTA(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if (strcmp(field[3], "T4") == 0) {
- struct oscillator_t *osc = &session->gpsdata.osc;
- unsigned int quality = atoi(field[2]);
- unsigned int delta = atoi(field[4]);
- unsigned int fine = atoi(field[5]);
- unsigned int status = atoi(field[6]);
- char deltachar = field[4][0];
- osc->running = (quality > 0);
- osc->reference = (deltachar && (deltachar != '?'));
- if (osc->reference) {
- if (delta < 500) {
- osc->delta = fine;
- } else {
- osc->delta = ((delta < 500000000) ? delta : 1000000000 - delta);
- }
- } else {
- osc->delta = 0;
- }
- osc->disciplined = ((quality == 2) && (status == 3));
- mask |= OSCILLATOR_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PTNTA,T4: quality=%s, delta=%s, fine=%s, status=%s\n",
- field[2], field[4], field[5], field[6]);
- }
- return mask;
- }
- #endif
- #ifdef OCEANSERVER_ENABLE
- static gps_mask_t processOHPR(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- session->gpsdata.attitude.heading = safe_atof(field[1]);
- session->gpsdata.attitude.pitch = safe_atof(field[2]);
- session->gpsdata.attitude.roll = safe_atof(field[3]);
- session->gpsdata.attitude.temp = safe_atof(field[4]);
- session->gpsdata.attitude.depth = safe_atof(field[5]) / METERS_TO_FEET;
- session->gpsdata.attitude.mag_len = safe_atof(field[6]);
- session->gpsdata.attitude.mag_x = safe_atof(field[7]);
- session->gpsdata.attitude.mag_y = safe_atof(field[8]);
- session->gpsdata.attitude.mag_z = safe_atof(field[9]);
- session->gpsdata.attitude.acc_len = safe_atof(field[10]);
- session->gpsdata.attitude.acc_x = safe_atof(field[11]);
- session->gpsdata.attitude.acc_y = safe_atof(field[12]);
- session->gpsdata.attitude.acc_z = safe_atof(field[13]);
- session->gpsdata.attitude.gyro_x = safe_atof(field[15]);
- session->gpsdata.attitude.gyro_y = safe_atof(field[16]);
- mask |= (ATTITUDE_SET);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "Heading %lf.\n", session->gpsdata.attitude.heading);
- return mask;
- }
- #endif
- #ifdef ASHTECH_ENABLE
- static gps_mask_t processPASHR(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
- gps_mask_t mask = ONLINE_SET;
- char ts_buf[TIMESPEC_LEN];
- if (0 == strcmp("ACK", field[1])) {
-
- GPSD_LOG(LOG_DATA, &session->context->errout, "PASHR,ACK\n");
- return ONLINE_SET;
- } else if (0 == strcmp("MCA", field[1])) {
-
- GPSD_LOG(LOG_DATA, &session->context->errout, "PASHR,MCA\n");
- return ONLINE_SET;
- } else if (0 == strcmp("NAK", field[1])) {
-
- GPSD_LOG(LOG_DATA, &session->context->errout, "PASHR,NAK\n");
- return ONLINE_SET;
- } else if (0 == strcmp("PBN", field[1])) {
-
-
- GPSD_LOG(LOG_DATA, &session->context->errout, "PASHR,PBN\n");
- return ONLINE_SET;
- } else if (0 == strcmp("POS", field[1])) {
-
- mask |= MODE_SET | STATUS_SET | CLEAR_IS;
- if (0 == strlen(field[2])) {
-
- session->gpsdata.status = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- } else {
- int satellites_used;
-
- session->newdata.mode = MODE_3D;
- if (1 <= atoi(field[2]))
- session->gpsdata.status = STATUS_DGPS_FIX;
- else
- session->gpsdata.status = STATUS_FIX;
-
- satellites_used = atoi(field[3]);
- if (0 == merge_hhmmss(field[4], session)) {
- register_fractional_time(field[0], field[4], session);
- mask |= TIME_SET;
- }
- if (0 == do_lat_lon(&field[5], &session->newdata)) {
- mask |= LATLON_SET;
- if ('\0' != field[9][0]) {
-
- session->newdata.altHAE = safe_atof(field[9]);
- mask |= ALTITUDE_SET;
- }
- }
- session->newdata.track = safe_atof(field[11]);
- session->newdata.speed = safe_atof(field[12]) / MPS_TO_KPH;
- session->newdata.climb = safe_atof(field[13]);
- session->gpsdata.dop.pdop = safe_atof(field[14]);
- session->gpsdata.dop.hdop = safe_atof(field[15]);
- session->gpsdata.dop.vdop = safe_atof(field[16]);
- session->gpsdata.dop.tdop = safe_atof(field[17]);
- mask |= (SPEED_SET | TRACK_SET | CLIMB_SET);
- mask |= DOP_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PASHR,POS: hhmmss=%s lat=%.2f lon=%.2f altHAE=%.f"
- " speed=%.2f track=%.2f climb=%.2f mode=%d status=%d"
- " pdop=%.2f hdop=%.2f vdop=%.2f tdop=%.2f used=%d\n",
- field[4], session->newdata.latitude,
- session->newdata.longitude, session->newdata.altHAE,
- session->newdata.speed, session->newdata.track,
- session->newdata.climb, session->newdata.mode,
- session->gpsdata.status, session->gpsdata.dop.pdop,
- session->gpsdata.dop.hdop, session->gpsdata.dop.vdop,
- session->gpsdata.dop.tdop, satellites_used);
- }
- } else if (0 == strcmp("RID", field[1])) {
- (void)snprintf(session->subtype, sizeof(session->subtype) - 1,
- "%s ver %s", field[2], field[3]);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PASHR,RID: subtype=%s mask={}\n",
- session->subtype);
- return mask;
- } else if (0 == strcmp("SAT", field[1])) {
- struct satellite_t *sp;
- int i, n = session->gpsdata.satellites_visible = atoi(field[2]);
- session->gpsdata.satellites_used = 0;
- for (i = 0, sp = session->gpsdata.skyview;
- sp < session->gpsdata.skyview + n; sp++, i++) {
- sp->PRN = (short)atoi(field[3 + i * 5 + 0]);
- sp->azimuth = (double)atoi(field[3 + i * 5 + 1]);
- sp->elevation = (double)atoi(field[3 + i * 5 + 2]);
- sp->ss = safe_atof(field[3 + i * 5 + 3]);
- sp->used = false;
- if (field[3 + i * 5 + 4][0] == 'U') {
- sp->used = true;
- session->gpsdata.satellites_used++;
- }
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PASHR,SAT: used=%d\n",
- session->gpsdata.satellites_used);
- session->gpsdata.skyview_time.tv_sec = 0;
- session->gpsdata.skyview_time.tv_nsec = 0;
- mask |= SATELLITE_SET | USED_IS;
- } else if (0 == strcmp("T", field[3])) {
-
- if (0 == merge_hhmmss(field[1], session)) {
- register_fractional_time(field[0], field[1], session);
-
- }
- session->gpsdata.attitude.heading = safe_atof(field[2]);
- session->gpsdata.attitude.roll = safe_atof(field[4]);
- session->gpsdata.attitude.pitch = safe_atof(field[5]);
-
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PASHR (OxTS) time %s, heading %lf.\n",
- timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
- session->gpsdata.attitude.heading);
- }
- return mask;
- }
- #endif
- #ifdef MTK3301_ENABLE
- static gps_mask_t processMTK3301(int c UNUSED, char *field[],
- struct gps_device_t *session)
- {
- int msg, reason;
- msg = atoi(&(session->nmea.field[0])[4]);
- switch (msg) {
- case 001:
- reason = atoi(field[2]);
- if (atoi(field[1]) == -1)
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "MTK NACK: unknown sentence\n");
- else if (reason < 3) {
- const char *mtk_reasons[] = {
- "Invalid",
- "Unsupported",
- "Valid but Failed",
- "Valid success"
- };
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "MTK NACK: %s, reason: %s\n",
- field[1], mtk_reasons[reason]);
- }
- else
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "MTK ACK: %s\n", field[1]);
- return ONLINE_SET;
- case 424:
-
-
- if (atoi(field[5]) < 127875)
- (void)nmea_send(session, "$PMTK324,0,0,1,0,127875");
- return ONLINE_SET;
- case 705:
- (void)strlcat(session->subtype, field[1], sizeof(session->subtype));
- (void)strlcat(session->subtype, "-", sizeof(session->subtype));
- (void)strlcat(session->subtype, field[2], sizeof(session->subtype));
- return ONLINE_SET;
- default:
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "MTK: unknown msg: %d\n", msg);
- return ONLINE_SET;
- }
- }
- #endif
- #ifdef SKYTRAQ_ENABLE
- static gps_mask_t processPSTI030(int count, char *field[],
- struct gps_device_t *session)
- {
-
- gps_mask_t mask = ONLINE_SET;
- if ( 16 != count )
- return mask;
- if ('V' == field[3][0] ||
- 'N' == field[13][0]) {
-
- session->gpsdata.status = STATUS_NO_FIX;
- session->newdata.mode = MODE_NO_FIX;
- mask |= MODE_SET;
- } else if ('A' == field[3][0]) {
- double east, north, climb;
-
- if (field[2][0] != '\0' && field[12][0] != '\0') {
-
- if (0 == merge_hhmmss(field[2], session) &&
- 0 == merge_ddmmyy(field[12], session)) {
- mask |= TIME_SET;
- register_fractional_time( "PSTI030", field[2], session);
- }
- }
- if (0 == do_lat_lon(&field[4], &session->newdata)) {
- session->newdata.mode = MODE_2D;
- mask |= LATLON_SET;
- if ('\0' != field[8][0]) {
-
- session->newdata.altMSL = safe_atof(field[8]);
- mask |= ALTITUDE_SET;
- session->newdata.mode = MODE_3D;
-
- }
- mask |= MODE_SET;
- }
-
-
- east = safe_atof(field[9]);
- north = safe_atof(field[10]);
-
- climb = safe_atof(field[11]);
- session->newdata.NED.velN = north;
- session->newdata.NED.velE = east;
- session->newdata.NED.velD = -climb;
- mask |= VNED_SET;
- session->gpsdata.status = faa_mode(field[13][0]);
-
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PSTI,030: ddmmyy=%s hhmmss=%s lat=%.2f lon=%.2f "
- "status=%d, RTK(Age=%s Ratio=%s)\n",
- field[12], field[2],
- session->newdata.latitude,
- session->newdata.longitude,
- session->gpsdata.status,
- field[14], field[15]);
- return mask;
- }
- static gps_mask_t processPSTI(int count, char *field[],
- struct gps_device_t *session)
- {
- gps_mask_t mask = ONLINE_SET;
- if ( 0 != strncmp(session->subtype, "Skytraq", 7) ) {
-
- (void)gpsd_write(session, "\xA0\xA1\x00\x02\x02\x01\x03\x0d\x0a",9);
- }
- if (0 == strcmp("00", field[1]) ) {
- if ( 4 != count )
- return mask;
-
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PSTI,00: Mode: %s, Length: %s, Quant: %s\n",
- field[2], field[3], field[4]);
- return mask;
- }
- if (0 == strcmp("001", field[1])) {
-
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PSTI,001: Count: %d\n", count);
- return mask;
- }
- if (0 == strcmp("005", field[1])) {
-
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PSTI,005: Count: %d\n", count);
- return mask;
- }
- if (0 == strcmp("030", field[1])) {
-
- return processPSTI030(count, field, session);
- }
- if (0 == strcmp("032", field[1])) {
- if ( 16 != count )
- return mask;
-
- if ( 0 == strcmp(field[4], "A")) {
-
- if (field[2][0] != '\0' && field[3][0] != '\0') {
-
- if (0 == merge_hhmmss(field[2], session) &&
- 0 == merge_ddmmyy(field[3], session)) {
- mask |= TIME_SET;
- register_fractional_time( "PSTI032", field[2], session);
- }
- }
- }
- GPSD_LOG( LOG_DATA,&session->context->errout,
- "PSTI,032: stat:%s mode: %s E: %s N: %s U:%s L:%s C:%s\n",
- field[4], field[5],
- field[6], field[7], field[8],
- field[9], field[10]);
- return mask;
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "PSTI,%s: Unknown type, Count: %d\n", field[1], count);
- return mask;
- }
- static gps_mask_t processSTI(int count, char *field[],
- struct gps_device_t *session)
- {
- gps_mask_t mask = ONLINE_SET;
- if ( 0 != strncmp(session->subtype, "Skytraq", 7) ) {
-
- (void)gpsd_write(session, "\xA0\xA1\x00\x02\x02\x01\x03\x0d\x0a",9);
- }
- if ( 0 == strcmp( field[1], "IC") ) {
-
- GPSD_LOG(LOG_ERROR, &session->context->errout,
- "Skytraq: $STI,%s,%s\n", field[1], field[2]);
- return mask;
- }
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "STI,%s: Unknown type, Count: %d\n", field[1], count);
- return mask;
- }
- #endif
- gps_mask_t nmea_parse(char *sentence, struct gps_device_t * session)
- {
- typedef gps_mask_t(*nmea_decoder) (int count, char *f[],
- struct gps_device_t * session);
- static struct
- {
- char *name;
- int nf;
- bool cycle_continue;
- nmea_decoder decoder;
- } nmea_phrase[] = {
- {"PGRMB", 0, false, NULL},
- {"PGRMC", 0, false, NULL},
- {"PGRME", 7, false, processPGRME},
- {"PGRMF", 15, false, processPGRMF},
- {"PGRMH", 0, false, NULL},
- {"PGRMI", 0, false, NULL},
- {"PGRMM", 2, false, processPGRMM},
- {"PGRMO", 0, false, NULL},
- {"PGRMT", 0, false, NULL},
- {"PGRMV", 0, false, NULL},
- {"PGRMZ", 4, false, processPGRMZ},
-
- {"AAM", 0, false, NULL},
- {"APB", 0, false, NULL},
- {"BOD", 0, false, NULL},
-
- {"BWC", 0, false, NULL},
- {"DBT", 7, true, processDBT},
- {"DPT", 0, false, NULL},
- {"DTM", 2, false, processDTM},
- {"GBS", 7, false, processGBS},
- {"GGA", 13, false, processGGA},
- {"GLC", 0, false, NULL},
- {"GLL", 7, false, processGLL},
- {"GNS", 13, false, processGNS},
- {"GRS", 0, false, NULL},
- {"GSA", 18, false, processGSA},
- {"GST", 8, false, processGST},
- {"GSV", 0, false, processGSV},
-
- {"HDG", 0, false, processHDG},
- {"HDT", 1, false, processHDT},
- {"MSS", 0, false, NULL},
- {"MTW", 0, false, NULL},
- {"MWD", 0, false, NULL},
- {"MWV", 0, false, NULL},
- #ifdef OCEANSERVER_ENABLE
- {"OHPR", 18, false, processOHPR},
- #endif
- #ifdef ASHTECH_ENABLE
-
- {"PASHR", 3, false, processPASHR},
- #endif
- {"PMGNST", 8, false, processPMGNST},
- #ifdef MTK3301_ENABLE
- {"PMTK", 3, false, processMTK3301},
-
- {"PMTK001", 3, false, processMTK3301},
- {"PMTK424", 3, false, processMTK3301},
- {"PMTK705", 3, false, processMTK3301},
- {"PMTKCHN", 0, false, NULL},
- #endif
- {"PRWIZCH", 0, false, NULL},
- {"PSRFEPE", 7, false, processPSRFEPE},
- {"PTFTTXT", 0, false, NULL},
- {"PUBX", 0, false, NULL},
- #ifdef TNT_ENABLE
- {"PTNTHTM", 9, false, processTNTHTM},
- {"PTNTA", 8, false, processTNTA},
- #endif
- #ifdef SKYTRAQ_ENABLE
- {"PSTI", 2, false, processPSTI},
- {"STI", 2, false, processSTI},
- #endif
-
- {"PSTM", 0, false, NULL},
-
- {"RMB", 0, false, NULL},
- {"RMC", 8, false, processRMC},
- {"RTE", 0, false, NULL},
- {"TXT", 5, false, processTXT},
- {"VHW", 0, false, NULL},
- {"VLW", 0, false, NULL},
- {"VTG", 5, false, processVTG},
- {"XDR", 0, false, NULL},
- {"XTE", 0, false, NULL},
- {"ZDA", 4, false, processZDA},
- };
- int count;
- gps_mask_t mask = 0;
- unsigned int i, thistag, lasttag;
- char *p, *e;
- volatile char *t;
- uint64_t lasttag_mask = 0;
- uint64_t thistag_mask = 0;
- char ts_buf1[TIMESPEC_LEN];
- char ts_buf2[TIMESPEC_LEN];
- #ifdef SKYTRAQ_ENABLE
- bool skytraq_sti = false;
- #endif
-
- if (strlen(sentence) > NMEA_MAX) {
- GPSD_LOG(LOG_WARN, &session->context->errout,
- "Overlong packet of %zd chars rejected.\n",
- strlen(sentence));
- return ONLINE_SET;
- }
-
- (void)strlcpy((char *)session->nmea.fieldcopy, sentence,
- sizeof(session->nmea.fieldcopy) - 1);
-
- for (p = (char *)session->nmea.fieldcopy;
- (*p != '*') && (*p >= ' ');)
- ++p;
- if (*p == '*')
- *p++ = ',';
- #ifdef SKYTRAQ_ENABLE_UNUSED
-
- if ( 0 != strncmp( "STI,", sentence, 4) ) {
- skytraq_sti = true;
- *p++ = ',';
- }
- #endif
- *p = '\0';
- e = p;
-
- count = 0;
- t = p;
- p = (char *)session->nmea.fieldcopy + 1;
-
- while ((p != NULL) && (p <= t)) {
- session->nmea.field[count] = p;
- if ((p = strchr(p, ',')) != NULL) {
- *p = '\0';
- count++;
- p++;
- }
- }
-
- for (i = (unsigned int)count;
- i <
- (unsigned)(sizeof(session->nmea.field) /
- sizeof(session->nmea.field[0])); i++)
- session->nmea.field[i] = e;
-
- session->nmea.latch_frac_time = false;
- #ifdef __UNUSED
-
- GPSD_LOG(&session->context->errout, 0,
- "NMEA0183: got %s\n", session->nmea.field[0]);
- #endif
-
- for (thistag = i = 0;
- i < (unsigned)(sizeof(nmea_phrase) / sizeof(nmea_phrase[0])); ++i) {
- char *s = session->nmea.field[0];
- if (strlen(nmea_phrase[i].name) == 3
- #ifdef SKYTRAQ_ENABLE
-
- && !skytraq_sti
- #endif
- ) {
- s += 2;
- }
- if (strcmp(nmea_phrase[i].name, s) == 0) {
- if (NULL == nmea_phrase[i].decoder) {
-
- mask = ONLINE_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "No decoder for sentence %s\n",
- session->nmea.field[0]);
- break;
- }
- if (count < nmea_phrase[i].nf) {
-
- mask = ONLINE_SET;
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "Sentence %s too short\n", session->nmea.field[0]);
- break;
- }
- mask = (nmea_phrase[i].decoder)(count, session->nmea.field,
- session);
- if (nmea_phrase[i].cycle_continue)
- session->nmea.cycle_continue = true;
-
- thistag = i + 1;
- break;
- }
- }
-
- if (!str_starts_with(session->nmea.field[0] + 2, "GSV"))
- session->nmea.last_gsv_talker = '\0';
- if (!str_starts_with(session->nmea.field[0] + 2, "GSA"))
- session->nmea.last_gsa_talker = '\0';
-
- if ((mask & TIME_SET) != 0) {
- session->newdata.time = gpsd_utc_resolve(session);
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "%s time is %s = %d-%02d-%02dT%02d:%02d:%02d.%03ldZ\n",
- session->nmea.field[0],
- timespec_str(&session->newdata.time, ts_buf1, sizeof(ts_buf1)),
- 1900 + session->nmea.date.tm_year,
- session->nmea.date.tm_mon + 1,
- session->nmea.date.tm_mday,
- session->nmea.date.tm_hour,
- session->nmea.date.tm_min,
- session->nmea.date.tm_sec,
- session->nmea.subseconds.tv_nsec / 1000000L);
-
- mask |= NTPTIME_IS;
- }
-
-
- GPSD_LOG(LOG_DATA, &session->context->errout,
- "%s time %s last %s latch %d cont %d enders %#llx\n",
- session->nmea.field[0],
- timespec_str(&session->nmea.this_frac_time, ts_buf1,
- sizeof(ts_buf1)),
- timespec_str(&session->nmea.last_frac_time, ts_buf2,
- sizeof(ts_buf2)),
- session->nmea.latch_frac_time,
- session->nmea.cycle_continue,
- (unsigned long long)session->nmea.cycle_enders);
- lasttag = session->nmea.lasttag;
- if (0 < session->nmea.lasttag) {
- lasttag_mask = (uint64_t)1 << lasttag;
- }
- if (0 < thistag) {
- thistag_mask = (uint64_t)1 << thistag;
- }
- if (session->nmea.latch_frac_time) {
- timespec_t ts_delta;
- TS_SUB(&ts_delta, &session->nmea.this_frac_time,
- &session->nmea.last_frac_time);
- if (0.01 < fabs(TSTONS(&ts_delta))) {
-
- mask |= CLEAR_IS;
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "%s starts a reporting cycle. lasttag %d\n",
- session->nmea.field[0], lasttag);
-
- if (0 < lasttag &&
- 0 == (session->nmea.cycle_enders & lasttag_mask) &&
- !session->nmea.cycle_continue) {
- session->nmea.cycle_enders |= lasttag_mask;
-
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "tagged %s as a cycle ender. %#llx\n",
- nmea_phrase[lasttag - 1].name,
- (unsigned long long)lasttag_mask);
- }
- }
- } else {
-
- if (0 != (session->nmea.cycle_enders & lasttag_mask)) {
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "%s is just after a cycle ender.\n",
- session->nmea.field[0]);
- }
- if (session->nmea.cycle_continue) {
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "%s extends the reporting cycle.\n",
- session->nmea.field[0]);
-
- session->nmea.cycle_enders &= ~lasttag_mask;
- session->nmea.cycle_enders |= thistag_mask;
- }
- }
-
- if ((session->nmea.latch_frac_time || session->nmea.cycle_continue)
- && (session->nmea.cycle_enders & thistag_mask)!=0) {
- GPSD_LOG(LOG_PROG, &session->context->errout,
- "%s ends a reporting cycle.\n",
- session->nmea.field[0]);
- mask |= REPORT_IS;
- }
- if (session->nmea.latch_frac_time)
- session->nmea.lasttag = thistag;
-
- if (session->nmea.cycle_enders != 0)
- session->cycle_end_reliable = true;
-
-
- if (MODE_SET == (mask & MODE_SET) &&
- MODE_3D == session->gpsdata.fix.mode &&
- MODE_NO_FIX != session->newdata.mode &&
- (0 != isfinite(session->lastfix.altHAE) ||
- 0 != isfinite(session->oldfix.altHAE) ||
- 0 != isfinite(session->lastfix.altMSL) ||
- 0 != isfinite(session->oldfix.altMSL))) {
- session->newdata.mode = session->gpsdata.fix.mode;
- }
- return mask;
- }
- #endif
- void nmea_add_checksum(char *sentence)
- {
- unsigned char sum = '\0';
- char c, *p = sentence;
- if (*p == '$' || *p == '!') {
- p++;
- }
- while (((c = *p) != '*') && (c != '\0')) {
- sum ^= c;
- p++;
- }
- *p++ = '*';
- (void)snprintf(p, 5, "%02X\r\n", (unsigned)sum);
- }
- ssize_t nmea_write(struct gps_device_t *session, char *buf, size_t len UNUSED)
- {
- (void)strlcpy(session->msgbuf, buf, sizeof(session->msgbuf));
- if (session->msgbuf[0] == '$') {
- (void)strlcat(session->msgbuf, "*", sizeof(session->msgbuf));
- nmea_add_checksum(session->msgbuf);
- } else
- (void)strlcat(session->msgbuf, "\r\n", sizeof(session->msgbuf));
- session->msgbuflen = strlen(session->msgbuf);
- return gpsd_write(session, session->msgbuf, session->msgbuflen);
- }
- ssize_t nmea_send(struct gps_device_t * session, const char *fmt, ...)
- {
- char buf[BUFSIZ];
- va_list ap;
- va_start(ap, fmt);
- (void)vsnprintf(buf, sizeof(buf) - 5, fmt, ap);
- va_end(ap);
- return nmea_write(session, buf, strlen(buf));
- }
|