driver_ubx.c 150 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751375237533754375537563757375837593760376137623763376437653766376737683769377037713772377337743775377637773778377937803781378237833784378537863787378837893790379137923793379437953796379737983799380038013802380338043805380638073808380938103811381238133814381538163817381838193820382138223823382438253826382738283829383038313832383338343835383638373838383938403841384238433844384538463847384838493850385138523853385438553856385738583859386038613862386338643865386638673868386938703871387238733874387538763877387838793880388138823883388438853886388738883889389038913892389338943895389638973898389939003901390239033904390539063907390839093910391139123913391439153916391739183919392039213922392339243925392639273928392939303931393239333934393539363937393839393940394139423943394439453946394739483949395039513952395339543955395639573958395939603961396239633964396539663967396839693970397139723973397439753976397739783979398039813982398339843985398639873988398939903991399239933994399539963997399839994000400140024003400440054006400740084009401040114012401340144015401640174018401940204021402240234024402540264027402840294030403140324033403440354036403740384039404040414042404340444045404640474048404940504051405240534054405540564057405840594060406140624063406440654066406740684069407040714072407340744075407640774078407940804081408240834084408540864087408840894090409140924093409440954096409740984099410041014102410341044105410641074108410941104111411241134114411541164117411841194120412141224123412441254126412741284129413041314132413341344135413641374138413941404141414241434144414541464147414841494150415141524153415441554156415741584159416041614162416341644165416641674168416941704171417241734174417541764177417841794180418141824183418441854186418741884189419041914192419341944195419641974198419942004201420242034204420542064207420842094210421142124213421442154216421742184219422042214222422342244225422642274228422942304231423242334234423542364237423842394240424142424243424442454246424742484249425042514252425342544255425642574258425942604261426242634264426542664267426842694270427142724273427442754276427742784279428042814282428342844285428642874288428942904291429242934294429542964297429842994300430143024303430443054306430743084309431043114312431343144315431643174318431943204321432243234324432543264327432843294330433143324333433443354336433743384339434043414342434343444345434643474348434943504351435243534354435543564357435843594360436143624363436443654366436743684369437043714372437343744375437643774378437943804381438243834384438543864387438843894390439143924393439443954396439743984399440044014402440344044405440644074408440944104411441244134414441544164417441844194420442144224423442444254426442744284429443044314432443344344435443644374438443944404441444244434444444544464447
  1. /*
  2. * UBX driver. For u-blox binary, also includes Antaris4 binary
  3. * Reference manuals are at
  4. * http://www.u-blox.com/en/download/documents-a-resources/u-blox-6-gps-modules-resources.html
  5. *
  6. * updated for u-blox 8
  7. * http://www.ublox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
  8. *
  9. * Week counters are not limited to 10 bits. It's unknown what
  10. * the firmware is doing to disambiguate them, if anything; it might just
  11. * be adding a fixed offset based on a hidden epoch value, in which case
  12. * unhappy things will occur on the next rollover.
  13. *
  14. * For the Antaris 4, the default leap-second offset (before getting one from
  15. * the sats, one presumes) is 0sec; for the u-blox 6 it's 15sec.
  16. *
  17. * This file is Copyright 2010 by the GPSD project
  18. * SPDX-License-Identifier: BSD-2-clause
  19. *
  20. */
  21. #include "../include/gpsd_config.h" /* must be before all includes */
  22. #include <assert.h>
  23. #include <math.h>
  24. #include <stdbool.h>
  25. #include <stdio.h>
  26. #include <stdlib.h>
  27. #include <string.h>
  28. #include <unistd.h>
  29. #include "../include/compiler.h" // for FALLTHROUGH
  30. #include "../include/gpsd.h"
  31. #if defined(UBLOX_ENABLE) && defined(BINARY_ENABLE)
  32. #include "../include/driver_ubx.h"
  33. #include "../include/bits.h" // For UINT2INT()
  34. #include "../include/timespec.h"
  35. /*
  36. * A ubx packet looks like this:
  37. * leader: 0xb5 0x62
  38. * message class: 1 byte
  39. * message type: 1 byte
  40. * length of payload: 2 bytes
  41. * payload: variable length
  42. * checksum: 2 bytes
  43. *
  44. * see also the FV25 and UBX documents on reference.html
  45. */
  46. #define UBX_PREFIX_LEN 6
  47. #define UBX_CLASS_OFFSET 2
  48. #define UBX_TYPE_OFFSET 3
  49. /* because we hates magic numbers forever */
  50. #define USART1_ID 1
  51. #define USART2_ID 2
  52. #define USB_ID 3
  53. #define UBX_PROTOCOL_MASK 0x01
  54. #define NMEA_PROTOCOL_MASK 0x02
  55. #define RTCM_PROTOCOL_MASK 0x04
  56. #define RTCM3_PROTOCOL_MASK 0x20 // protVer 20+
  57. #define UBX_CFG_LEN 20
  58. #define outProtoMask 14
  59. static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
  60. size_t len);
  61. static gps_mask_t ubx_msg_log_batch(struct gps_device_t *session,
  62. unsigned char *buf, size_t data_len);
  63. static gps_mask_t ubx_msg_log_info(struct gps_device_t *session,
  64. unsigned char *buf, size_t data_len);
  65. static gps_mask_t ubx_msg_log_retrievepos(struct gps_device_t *session,
  66. unsigned char *buf, size_t data_len);
  67. static gps_mask_t ubx_msg_log_retrieveposextra(struct gps_device_t *session,
  68. unsigned char *buf,
  69. size_t data_len);
  70. static gps_mask_t ubx_msg_log_retrievestring(struct gps_device_t *session,
  71. unsigned char *buf,
  72. size_t data_len);
  73. static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
  74. unsigned char *buf, size_t data_len);
  75. static gps_mask_t ubx_msg_nav_eoe(struct gps_device_t *session,
  76. unsigned char *buf, size_t data_len);
  77. static gps_mask_t ubx_msg_inf(struct gps_device_t *session, unsigned char *buf,
  78. size_t data_len);
  79. static gps_mask_t ubx_msg_nav_posecef(struct gps_device_t *session,
  80. unsigned char *buf, size_t data_len);
  81. static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t *session,
  82. unsigned char *buf, size_t data_len);
  83. static gps_mask_t ubx_msg_mon_ver(struct gps_device_t *session,
  84. unsigned char *buf, size_t data_len);
  85. static gps_mask_t ubx_msg_nav_sat(struct gps_device_t *session,
  86. unsigned char *buf, size_t data_len);
  87. static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
  88. unsigned char *buf, size_t data_len);
  89. static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session,
  90. unsigned char *buf, size_t data_len);
  91. static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session,
  92. unsigned char *buf, size_t data_len);
  93. static gps_mask_t ubx_msg_nav_velecef(struct gps_device_t *session,
  94. unsigned char *buf, size_t data_len);
  95. static gps_mask_t ubx_msg_nav_sbas(struct gps_device_t *session,
  96. unsigned char *buf, size_t data_len);
  97. static gps_mask_t ubx_msg_tim_tp(struct gps_device_t *session,
  98. unsigned char *buf, size_t data_len);
  99. static void ubx_mode(struct gps_device_t *session, int mode);
  100. typedef struct {
  101. const char *fw_string;
  102. const float protver;
  103. } fw_protver_map_entry_t;
  104. /* based on u-blox document no. GPS.G7-SW-12001-B1 (15 June 2018) */
  105. /* capture decimal parts of protVer info even when session->protver currently
  106. * is integer (which _might_ change in the future, so avoid having to revisit
  107. * the info at that time).
  108. * This list is substantially incomplete and over specific. */
  109. static const fw_protver_map_entry_t fw_protver_map[] = {
  110. {"2.10", 8.10}, // antaris 4, version 8 is a guess
  111. {"2.11", 8.11}, // antaris 4, version 8 is a guess
  112. {"3.04", 9.00}, // antaris 4, version 9 is a guess
  113. {"4.00", 10.00}, // antaris 4, and u-blox 5
  114. {"4.01", 10.01}, // antaris 4, and u-blox 5
  115. {"5.00", 11.00}, // u-blox 5 and antaris 4
  116. {"6.00", 12.00}, // u-blox 5 and 6
  117. {"6.02", 12.02}, // u-blox 5 and 6
  118. {"7.01", 13.01}, // u-blox 7
  119. {"7.03", 13.03}, // u-blox 7
  120. {"1.00", 14.00}, // u-blox 6 w/ GLONASS, and 7
  121. // protVer >14 should carry explicit protVer in MON-VER extension
  122. {NULL, 0.0},
  123. };
  124. /*
  125. * Model Fw Protver
  126. * M10 SPG 5.00 34.00
  127. */
  128. /* Convert a ubx PRN to an NMEA 4.0 (extended) PRN and ubx gnssid, svid
  129. *
  130. * return 0 on fail
  131. */
  132. static short ubx_to_prn(int ubx_PRN, unsigned char *gnssId,
  133. unsigned char *svId)
  134. {
  135. *gnssId = 0;
  136. *svId = 0;
  137. // IRNSS??
  138. if (1 > ubx_PRN) {
  139. // skip 0 PRN
  140. return 0;
  141. } else if (32 >= ubx_PRN) {
  142. // GPS 1..32 -> 1..32
  143. *gnssId = 0;
  144. *svId = ubx_PRN;
  145. } else if (64 >= ubx_PRN) {
  146. // BeiDou, 159..163,33..64 -> 1..5,6..37
  147. *gnssId = 3;
  148. *svId = ubx_PRN - 27;
  149. } else if (96 >= ubx_PRN) {
  150. // GLONASS 65..96 -> 1..32
  151. *gnssId = 6;
  152. *svId = ubx_PRN - 64;
  153. } else if (120 > ubx_PRN) {
  154. // Huh?
  155. return 0;
  156. } else if (158 >= ubx_PRN) {
  157. // SBAS 120..158 -> 120..158
  158. *gnssId = 1;
  159. *svId = ubx_PRN;
  160. } else if (163 >= ubx_PRN) {
  161. // BeiDou, 159..163 -> 1..5
  162. *gnssId = 3;
  163. *svId = ubx_PRN - 158;
  164. } else if (173 > ubx_PRN) {
  165. // Huh?
  166. return 0;
  167. } else if (182 >= ubx_PRN) {
  168. // IMES 173..182 -> 1..5, in u-blox 8, bot u-blox 9
  169. *gnssId = 4;
  170. *svId = ubx_PRN - 172;
  171. } else if (193 > ubx_PRN) {
  172. // Huh?
  173. return 0;
  174. } else if (199 >= ubx_PRN) {
  175. // QZSS 193..197 -> 1..5
  176. // ZED-F9T also see 198 and 199
  177. *gnssId = 5;
  178. *svId = ubx_PRN - 192;
  179. } else if (211 > ubx_PRN) {
  180. // Huh?
  181. return 0;
  182. } else if (246 >= ubx_PRN) {
  183. // Galileo 211..246 -> 1..36
  184. *gnssId = 2;
  185. *svId = ubx_PRN - 210;
  186. } else {
  187. // greater than 246, GLONASS (255), unused, or other unknown
  188. return 0;
  189. }
  190. return ubx2_to_prn(*gnssId, *svId);
  191. }
  192. // UBX-CFG-RATE
  193. // Deprecated in u-blox 10
  194. static void ubx_msg_cfg_rate(struct gps_device_t *session, unsigned char *buf,
  195. size_t data_len)
  196. {
  197. uint16_t measRate, navRate, timeRef;
  198. if (6 > data_len) {
  199. GPSD_LOG(LOG_WARN, &session->context->errout,
  200. "UBX-CFG-RATE message, runt payload len %zd", data_len);
  201. return;
  202. }
  203. measRate = getleu16(buf, 0); // Measurement rate (ms)
  204. navRate = getleu16(buf, 2); // Navigation rate (cycles)
  205. timeRef = getleu16(buf, 4); // Time system, e.g. UTC, GPS, ...
  206. GPSD_LOG(LOG_PROG, &session->context->errout,
  207. "UBX-CFG-RATE: measRate %ums, navRate %u cycle(s), "
  208. "timeRef %u\n",
  209. (unsigned)measRate, (unsigned)navRate,
  210. (unsigned)timeRef);
  211. // Update our notion of what the device's measurement rate is
  212. MSTOTS(&session->gpsdata.dev.cycle, measRate);
  213. return;
  214. }
  215. /* UBX-ESF-ALG
  216. *
  217. * UBX-ESF-ALG, and UBX-ESF-INS are synchronous to the GNSS epoch.
  218. * They need to be combined and reported together with the rest of
  219. * the epoch.
  220. */
  221. static gps_mask_t
  222. ubx_msg_esf_alg(struct gps_device_t *session, unsigned char *buf,
  223. size_t data_len)
  224. {
  225. unsigned version, flags, error, reserved1;
  226. unsigned long yaw;
  227. int pitch, roll;
  228. static gps_mask_t mask = 0;
  229. if (16 > data_len) {
  230. GPSD_LOG(LOG_WARN, &session->context->errout,
  231. "UBX-ESF-ALG message, runt payload len %zd", data_len);
  232. return mask;
  233. }
  234. // UBX-ESF-ALG is aligned with the GNSS epoch.
  235. session->driver.ubx.iTOW = getleu32(buf, 0);
  236. version = getub(buf, 4);
  237. flags = getub(buf, 5);
  238. error = getub(buf, 6);
  239. reserved1 = getub(buf, 7);
  240. yaw = getleu32(buf, 8);
  241. pitch = getles16(buf, 12);
  242. roll = getles16(buf, 14);
  243. if (0 == (2 & error)) {
  244. // no yawAlgError
  245. session->gpsdata.attitude.yaw = 0.01 * yaw;
  246. mask |= ATTITUDE_SET;
  247. }
  248. if (0 == (5 & error)) {
  249. // no tiltAlgError or angleError
  250. session->gpsdata.attitude.roll = 0.01 * roll;
  251. session->gpsdata.attitude.pitch = 0.01 * pitch;
  252. mask |= ATTITUDE_SET;
  253. }
  254. if (0 != mask) {
  255. timespec_t ts_tow;
  256. // got good data, set the measurement time
  257. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  258. session->gpsdata.attitude.mtime =
  259. gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
  260. }
  261. GPSD_LOG(LOG_PROG, &session->context->errout,
  262. "UBX-ESF-ALG: iTOW %lld version %u flags x%x error x%x"
  263. " reserved1 x%x yaw %ld pitch %u roll %u\n",
  264. (long long)session->driver.ubx.iTOW, version, flags, error,
  265. reserved1, yaw, pitch, roll);
  266. return mask;
  267. }
  268. /* UBX-ESF-INS
  269. *
  270. * protVer 19 and up. ADR and UDR only
  271. *
  272. * UBX-ESF-ALG, and UBX-ESF-INS are synchronous to the GNSS epoch.
  273. * They need to be combined and reported together with the rest of
  274. * the epoch.
  275. */
  276. static gps_mask_t
  277. ubx_msg_esf_ins(struct gps_device_t *session, unsigned char *buf,
  278. size_t data_len)
  279. {
  280. unsigned long long bitfield0, reserved1;
  281. long xAngRate, yAngRate, zAngRate;
  282. long xAccel, yAccel, zAccel;
  283. static gps_mask_t mask = 0;
  284. if (16 > data_len) {
  285. GPSD_LOG(LOG_WARN, &session->context->errout,
  286. "UBX-ESF-INS message, runt payload len %zd", data_len);
  287. return mask;
  288. }
  289. bitfield0 = getleu32(buf, 0);
  290. reserved1 = getleu32(buf, 4);
  291. // UBX-ESF-INS is aligned with the GNSS epoch.
  292. session->driver.ubx.iTOW = getleu32(buf, 8);
  293. xAngRate = getles32(buf, 12);
  294. yAngRate = getles32(buf, 16);
  295. zAngRate = getles32(buf, 20);
  296. xAccel = getles32(buf, 24);
  297. yAccel = getles32(buf, 28);
  298. zAccel = getles32(buf, 32);
  299. if (0x100 == (0x100 & bitfield0)) {
  300. // xAngRateValid
  301. session->gpsdata.attitude.gyro_x = 0.001 * xAngRate; // deg/s
  302. mask |= ATTITUDE_SET;
  303. }
  304. if (0x200 == (0x200 & bitfield0)) {
  305. // yAngRateValid
  306. session->gpsdata.attitude.gyro_x = 0.001 * yAngRate; // deg/s
  307. mask |= ATTITUDE_SET;
  308. }
  309. if (0x400 == (0x400 & bitfield0)) {
  310. // zAngRateValid
  311. session->gpsdata.attitude.gyro_x = 0.001 * zAngRate; // deg/s
  312. mask |= ATTITUDE_SET;
  313. }
  314. if (0x800 == (0x800 & bitfield0)) {
  315. // xAccelValid
  316. session->gpsdata.attitude.acc_x = 0.01 * xAccel; // m/s^2
  317. mask |= ATTITUDE_SET;
  318. }
  319. if (0x1000 == (0x1000 & bitfield0)) {
  320. // yAccelValid
  321. session->gpsdata.attitude.acc_y = 0.01 * yAccel; // m/s^2
  322. mask |= ATTITUDE_SET;
  323. }
  324. if (0x2000 == (0x2000 & bitfield0)) {
  325. // zAccelValid
  326. session->gpsdata.attitude.acc_z = 0.01 * zAccel; // m/s^2
  327. mask |= ATTITUDE_SET;
  328. }
  329. if (0 != mask) {
  330. timespec_t ts_tow;
  331. // got good data, set the measurement time
  332. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  333. session->gpsdata.attitude.mtime =
  334. gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
  335. }
  336. GPSD_LOG(LOG_PROG, &session->context->errout,
  337. "UBX-ESF-INS: bitfield0 %llu, reserved1 %llu iTOW %lld"
  338. " xAngRate %ld yAngRate %ld zAngRate %ld"
  339. " xAccel %ld yAccel %ld zAccel %ld\n",
  340. bitfield0, reserved1,
  341. (long long)session->driver.ubx.iTOW,
  342. xAngRate, yAngRate, zAngRate, xAccel, yAccel, zAccel);
  343. return mask;
  344. }
  345. /* UBX-ESF-MEAS
  346. *
  347. * protVer 15 and up. ADR only
  348. * protVer 19 and up. ADR and UDR only
  349. *
  350. * asynchronous to the GNSS epoch, and at a higher rate.
  351. * Needs to be reported immediately.
  352. *
  353. */
  354. static gps_mask_t
  355. ubx_msg_esf_meas(struct gps_device_t *session, unsigned char *buf,
  356. size_t data_len)
  357. {
  358. unsigned flags, id, numMeas, expected_len;
  359. gps_mask_t mask = 0;
  360. unsigned i;
  361. // where to store the IMU data.
  362. struct attitude_t *datap = &session->gpsdata.imu[0];
  363. if (8 > data_len) {
  364. GPSD_LOG(LOG_WARN, &session->context->errout,
  365. "UBX-ESF-MEAS message, runt payload len %zd", data_len);
  366. return mask;
  367. }
  368. // do not acumulate IMU data
  369. gps_clear_att(datap);
  370. (void)strlcpy(datap->msg, "UBX-ESF-MEAS", sizeof(datap->msg));
  371. datap->timeTag = getleu32(buf, 0);
  372. flags = getleu16(buf, 4);
  373. numMeas = (flags >> 11) & 0x01f;
  374. id = getleu16(buf, 6);
  375. expected_len = 8 + (4 * numMeas);
  376. if (0x08 & flags) {
  377. expected_len += 4;
  378. }
  379. if (expected_len != data_len) {
  380. GPSD_LOG(LOG_WARN, &session->context->errout,
  381. "UBX-ESF-MEAS: bad length. Got %zd, expected %u",
  382. data_len, expected_len);
  383. return 0;
  384. }
  385. GPSD_LOG(LOG_PROG, &session->context->errout,
  386. "UBX-ESF-MEAS: timeTag %lu flags x%x (numMeas %u) id %u\n",
  387. datap->timeTag, flags, numMeas, id);
  388. for (i = 0; i < numMeas; i++) {
  389. unsigned long data, dataField;
  390. long dataF;
  391. unsigned char dataType;
  392. data = getleu32(buf, 8 + (i * 4));
  393. dataType = (unsigned char)(data >> 24) & 0x3f;
  394. dataField = data & BITMASK(24);
  395. switch (dataType) {
  396. case 5: // gyro z angular rate, deg/s^2
  397. dataF = UINT2INT(dataField, 24);
  398. datap->gyro_z = dataF / 4096.0;
  399. mask |= IMU_SET;
  400. break;
  401. case 12: // gyro temp, deg C
  402. dataF = UINT2INT(dataField, 24);
  403. datap->gyro_temp = dataF / 100.0;
  404. mask |= IMU_SET;
  405. break;
  406. case 13: // gyro y angular rate, deg/s^2
  407. dataF = UINT2INT(dataField, 24);
  408. datap->gyro_y = dataF / 4096.0;
  409. mask |= IMU_SET;
  410. break;
  411. case 14: // gyro x angular rate, deg/s^2
  412. dataF = UINT2INT(dataField, 24);
  413. datap->gyro_x = dataF / 4096.0;
  414. mask |= IMU_SET;
  415. break;
  416. case 16: // accel x, m/s^2
  417. dataF = UINT2INT(dataField, 24);
  418. datap->acc_x = dataF / 1024.0;
  419. mask |= IMU_SET;
  420. break;
  421. case 17: // accel y, m/s^2
  422. dataF = UINT2INT(dataField, 24);
  423. datap->acc_y = dataF / 1024.0;
  424. mask |= IMU_SET;
  425. break;
  426. case 18: // accel z, m/s^2
  427. dataF = UINT2INT(dataField, 24);
  428. datap->acc_z = dataF / 1024.0;
  429. mask |= IMU_SET;
  430. break;
  431. // case 6: // front-left wheel ticks
  432. // case 7: // front-right wheel ticks
  433. // case 8: // rear-left wheel ticks
  434. // case 9: // rear-right wheel ticks
  435. // case 10: // speed tick
  436. // case 11: // speed, m/s
  437. default:
  438. // ignore all else
  439. dataF = dataField;
  440. break;
  441. }
  442. GPSD_LOG(LOG_PROG + 1, &session->context->errout,
  443. "UBX-ESF-MEAS: dataType %2u dataField %9ld\n",
  444. dataType, dataF);
  445. }
  446. return mask;
  447. }
  448. /* UBX-ESF-RAW
  449. *
  450. * protVer 15 and up. ADR only
  451. * protVer 19 and up. ADR and UDR only
  452. *
  453. * asynchronous to the GNSS epoch, and a a higher rate.
  454. * Needs to be reported immediately.
  455. *
  456. */
  457. static gps_mask_t
  458. ubx_msg_esf_raw(struct gps_device_t *session, unsigned char *buf,
  459. size_t data_len)
  460. {
  461. unsigned long reserved1, last_sTtag = 0;
  462. unsigned i;
  463. uint16_t blocks;
  464. gps_mask_t mask = 0;
  465. struct attitude_t *datap = NULL;
  466. int max_imu, cur_imu = -1;
  467. max_imu = sizeof(session->gpsdata.imu) / sizeof(struct attitude_t);
  468. if (4 > data_len) {
  469. GPSD_LOG(LOG_WARN, &session->context->errout,
  470. "UBX-ESF-RAW message, runt payload len %zd", data_len);
  471. return mask;
  472. }
  473. reserved1 = getleu32(buf, 0); // reserved1
  474. if (0 != ((data_len - 4) % 8)) {
  475. GPSD_LOG(LOG_WARN, &session->context->errout,
  476. "UBX-ESF-RAW message, weird payload len %zd", data_len);
  477. return mask;
  478. }
  479. blocks = (data_len - 4) / 8;
  480. GPSD_LOG(LOG_PROG, &session->context->errout,
  481. "UBX-ESF-RAW: reserved1 x%lx, blocks %u\n",
  482. reserved1, blocks);
  483. // loop over all blocks, use the next imu[] when time changes.
  484. for (i = 0; i < blocks; i++) {
  485. unsigned long data, dataField, sTtag;
  486. long dataF;
  487. unsigned char dataType;
  488. sTtag = getleu32(buf, 8 + (i * 8));
  489. if ((-1 == cur_imu) ||
  490. (last_sTtag != sTtag)) {
  491. cur_imu++;
  492. if (max_imu <= cur_imu) {
  493. GPSD_LOG(LOG_WARN, &session->context->errout,
  494. "UBX-ESF-RAW message, too many imu max %d block %u\n",
  495. max_imu, i);
  496. break;
  497. }
  498. last_sTtag = sTtag;
  499. datap = &session->gpsdata.imu[cur_imu];
  500. // do not acumulate IMU data
  501. gps_clear_att(datap);
  502. (void)strlcpy(datap->msg, "UBX-ESF-RAW", sizeof(datap->msg));
  503. }
  504. if (NULL == datap) {
  505. // paranoia
  506. continue;
  507. }
  508. data = getleu32(buf, 4 + (i * 8));
  509. dataType = (unsigned char)(data >> 24) & 0x3f;
  510. dataField = data & BITMASK(24);
  511. datap->timeTag = sTtag;
  512. switch (dataType) {
  513. case 5: // gyro z angular rate, deg/s^2
  514. dataF = UINT2INT(dataField, 24);
  515. datap->gyro_z = dataF / 4096.0;
  516. mask |= IMU_SET;
  517. break;
  518. case 12: // gyro temp, deg C
  519. dataF = UINT2INT(dataField, 24);
  520. datap->gyro_temp = dataF / 100.0;
  521. mask |= IMU_SET;
  522. break;
  523. case 13: // gyro y angular rate, deg/s^2
  524. dataF = UINT2INT(dataField, 24);
  525. datap->gyro_y = dataF / 4096.0;
  526. mask |= IMU_SET;
  527. break;
  528. case 14: // gyro x angular rate, deg/s^2
  529. dataF = UINT2INT(dataField, 24);
  530. datap->gyro_x = dataF / 4096.0;
  531. mask |= IMU_SET;
  532. break;
  533. case 16: // accel x, m/s^2
  534. dataF = UINT2INT(dataField, 24);
  535. datap->acc_x = dataF / 1024.0;
  536. mask |= IMU_SET;
  537. break;
  538. case 17: // accel y, m/s^2
  539. dataF = UINT2INT(dataField, 24);
  540. datap->acc_y = dataF / 1024.0;
  541. mask |= IMU_SET;
  542. break;
  543. case 18: // accel z, m/s^2
  544. dataF = UINT2INT(dataField, 24);
  545. datap->acc_z = dataF / 1024.0;
  546. mask |= IMU_SET;
  547. break;
  548. // case 6: // front-left wheel ticks
  549. // case 7: // front-right wheel ticks
  550. // case 8: // rear-left wheel ticks
  551. // case 9: // rear-right wheel ticks
  552. // case 10: // speed tick
  553. // case 11: // speed, m/s
  554. default:
  555. // ignore all else
  556. dataF = dataField;
  557. break;
  558. }
  559. GPSD_LOG(LOG_PROG + 1, &session->context->errout,
  560. "UBX-ESF-RAW: dataType %2u dataField %9ld sTtag %lu\n",
  561. dataType, dataF, datap->timeTag);
  562. }
  563. return mask;
  564. }
  565. // UBX-ESF-STATUS
  566. static gps_mask_t
  567. ubx_msg_esf_status(struct gps_device_t *session, unsigned char *buf,
  568. size_t data_len)
  569. {
  570. unsigned version, fusionMode, numSens, expected_len;
  571. static gps_mask_t mask = 0;
  572. if (16 > data_len) {
  573. GPSD_LOG(LOG_WARN, &session->context->errout,
  574. "UBX-ESF-STATUS message, runt payload len %zd", data_len);
  575. return mask;
  576. }
  577. session->driver.ubx.iTOW = getleu32(buf, 0);
  578. version = getub(buf, 4);
  579. fusionMode = getub(buf, 12);
  580. numSens = getub(buf, 15);
  581. expected_len = 16 + (4 * numSens);
  582. if (expected_len != data_len) {
  583. GPSD_LOG(LOG_WARN, &session->context->errout,
  584. "UBX-ESF-STATUS: bad length. Expected %u got %zd",
  585. expected_len, data_len);
  586. return mask;
  587. }
  588. GPSD_LOG(LOG_PROG, &session->context->errout,
  589. "UBX-ESF-STATUS: iTOW %lld version %u fusionMode %u numSens %u\n",
  590. (long long)session->driver.ubx.iTOW, version, fusionMode, numSens);
  591. return mask;
  592. }
  593. /**
  594. * HNR Attitude solution
  595. * UBX-HNR-ATT Class x28, ID 1
  596. *
  597. * Not before u-blox 8, protVer 19.2 and up.
  598. * only on ADR, and UDR
  599. */
  600. static gps_mask_t
  601. ubx_msg_hnr_att(struct gps_device_t *session, unsigned char *buf,
  602. size_t data_len)
  603. {
  604. uint8_t version;;
  605. int64_t iTOW;
  606. timespec_t ts_tow;
  607. gps_mask_t mask = 0;
  608. if (32 > data_len) {
  609. GPSD_LOG(LOG_WARN, &session->context->errout,
  610. "UBX-HNR-ATT message, runt payload len %zd", data_len);
  611. return 0;
  612. }
  613. if (19 > session->driver.ubx.protver) {
  614. // this GPS is at least protver 19.2
  615. session->driver.ubx.protver = 19;
  616. }
  617. // don't set session->driver.ubx.iTOW, HNR is off-cycle
  618. iTOW = getleu32(buf, 0);
  619. MSTOTS(&ts_tow, iTOW);
  620. session->gpsdata.attitude.mtime =
  621. gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
  622. version = (unsigned int)getub(buf, 4);
  623. session->gpsdata.attitude.roll = 1e-5 * getles32(buf, 8);
  624. session->gpsdata.attitude.pitch = 1e-5 * getles32(buf, 12);
  625. // seems to be true heading
  626. session->gpsdata.attitude.heading = 1e-5 * getles32(buf, 16);
  627. mask |= ATTITUDE_SET;
  628. GPSD_LOG(LOG_PROG, &session->context->errout,
  629. "HNR-ATT: iTOW %lld version %u roll %.5f pitch %.5f heading %.5f\n",
  630. (long long)iTOW,
  631. version,
  632. session->gpsdata.attitude.roll,
  633. session->gpsdata.attitude.pitch,
  634. session->gpsdata.attitude.heading);
  635. return mask;
  636. }
  637. /**
  638. * HNR Vehicle dynamics information
  639. * UBX-HNR-INS Class x28, ID 2
  640. *
  641. * Not before u-blox 8, protVer 19.1 and up.
  642. * only on ADR, and UDR
  643. */
  644. static gps_mask_t
  645. ubx_msg_hnr_ins(struct gps_device_t *session, unsigned char *buf,
  646. size_t data_len)
  647. {
  648. uint8_t version;;
  649. uint32_t bitfield0;
  650. gps_mask_t mask = 0;
  651. int64_t iTOW;
  652. if (36 > data_len) {
  653. GPSD_LOG(LOG_WARN, &session->context->errout,
  654. "UBX-HNR-INS message, runt payload len %zd", data_len);
  655. return 0;
  656. }
  657. if (19 > session->driver.ubx.protver) {
  658. // this GPS is at least protver 19.1
  659. session->driver.ubx.protver = 19;
  660. }
  661. version = (unsigned int)getub(buf, 0);
  662. bitfield0 = getleu32(buf, 0);
  663. // don't set session->driver.ubx.iTOW, HNR is off-cycle
  664. iTOW = getleu32(buf, 8);
  665. if (0x100 == (0x100 & bitfield0)) {
  666. // xAngRateValid
  667. session->gpsdata.attitude.gyro_x = 0.001 * getles32(buf, 12); // deg/s
  668. mask |= ATTITUDE_SET;
  669. }
  670. if (0x200 == (0x200 & bitfield0)) {
  671. // yAngRateValid
  672. session->gpsdata.attitude.gyro_x = 0.001 * getles32(buf, 16); // deg/s
  673. mask |= ATTITUDE_SET;
  674. }
  675. if (0x400 == (0x400 & bitfield0)) {
  676. // zAngRateValid
  677. session->gpsdata.attitude.gyro_x = 0.001 * getles32(buf, 20); // deg/s
  678. mask |= ATTITUDE_SET;
  679. }
  680. if (0x800 == (0x800 & bitfield0)) {
  681. // xAccelValid
  682. session->gpsdata.attitude.acc_x = 0.01 * getles32(buf, 24); // m/s^2
  683. mask |= ATTITUDE_SET;
  684. }
  685. if (0x1000 == (0x1000 & bitfield0)) {
  686. // yAccelValid
  687. session->gpsdata.attitude.acc_y = 0.01 * getles32(buf, 28); // m/s^2
  688. mask |= ATTITUDE_SET;
  689. }
  690. if (0x2000 == (0x2000 & bitfield0)) {
  691. // zAccelValid
  692. session->gpsdata.attitude.acc_z = 0.01 * getles32(buf, 32); // m/s^2
  693. mask |= ATTITUDE_SET;
  694. }
  695. if (0 != mask) {
  696. timespec_t ts_tow;
  697. // got good data, set the measurement time
  698. MSTOTS(&ts_tow, iTOW);
  699. session->gpsdata.attitude.mtime =
  700. gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
  701. }
  702. GPSD_LOG(LOG_PROG, &session->context->errout,
  703. "HNR-INS: iTOW %lld version %u bitfield0 x%x "
  704. "gyro_x %.3f gyro_y %.3f gyro_z %.3f "
  705. "acc_x %.3f acc_y %.3f acc_z %.3f\n",
  706. (long long)iTOW,
  707. version, bitfield0,
  708. session->gpsdata.attitude.gyro_x,
  709. session->gpsdata.attitude.gyro_y,
  710. session->gpsdata.attitude.gyro_z,
  711. session->gpsdata.attitude.acc_x,
  712. session->gpsdata.attitude.acc_y,
  713. session->gpsdata.attitude.acc_z);
  714. return mask;
  715. }
  716. /**
  717. * High rate output of PVT solution
  718. * UBX-HNR-PVT Class x28, ID 2
  719. *
  720. * Not before u-blox 8, protVer 19 and up.
  721. * only on ADR, and UDR
  722. */
  723. static gps_mask_t
  724. ubx_msg_hnr_pvt(struct gps_device_t *session, unsigned char *buf,
  725. size_t data_len)
  726. {
  727. char ts_buf[TIMESPEC_LEN];
  728. gps_mask_t mask = 0;
  729. int64_t iTOW;
  730. int *mode = &session->newdata.mode;
  731. int *status = &session->newdata.status;
  732. struct tm unpacked_date;
  733. uint8_t flags;
  734. uint8_t gpsFix;
  735. uint8_t valid;
  736. if (72 > data_len) {
  737. GPSD_LOG(LOG_WARN, &session->context->errout,
  738. "UBX-HNR-PVT message, runt payload len %zd", data_len);
  739. return 0;
  740. }
  741. if (19 > session->driver.ubx.protver) {
  742. // this GPS is at least protver 19
  743. session->driver.ubx.protver = 19;
  744. }
  745. // don't set session->driver.ubx.iTOW, HNR is off-cycle
  746. iTOW = getleu32(buf, 0);
  747. // valid same as UBX-NAV-PVT valid
  748. valid = (unsigned int)getub(buf, 11);
  749. // gpsFix same as UBX-NAV-PVT fixType
  750. gpsFix = (unsigned char)getub(buf, 16);
  751. // flags NOT same as UBX-NAV-PVT flags
  752. flags = (unsigned int)getub(buf, 17);
  753. switch (gpsFix) {
  754. case UBX_MODE_TMONLY:
  755. // 5 - Surveyed-in, so a precise 3D.
  756. *mode = MODE_3D;
  757. *status = STATUS_TIME;
  758. mask |= STATUS_SET | MODE_SET;
  759. break;
  760. case UBX_MODE_3D:
  761. // 3
  762. FALLTHROUGH
  763. case UBX_MODE_GPSDR:
  764. // 4
  765. if (*mode != MODE_3D) {
  766. *mode = MODE_3D;
  767. mask |= MODE_SET;
  768. }
  769. if (UBX_NAV_PVT_FLAG_DGPS == (flags & UBX_NAV_PVT_FLAG_DGPS)) {
  770. *status = STATUS_DGPS;
  771. mask |= STATUS_SET;
  772. } else {
  773. *status = STATUS_GPS;
  774. mask |= STATUS_SET;
  775. }
  776. mask |= LATLON_SET;
  777. break;
  778. case UBX_MODE_2D:
  779. // 2
  780. FALLTHROUGH
  781. case UBX_MODE_DR: // consider this too as 2D
  782. // 1
  783. if (MODE_2D != *mode) {
  784. *mode = MODE_2D;
  785. mask |= MODE_SET;
  786. };
  787. if (STATUS_GPS != *status) {
  788. // FIXME: Set DR status if it is DR
  789. *status = STATUS_GPS;
  790. mask |= STATUS_SET;
  791. }
  792. mask |= LATLON_SET | SPEED_SET;
  793. break;
  794. case UBX_MODE_NOFIX:
  795. // 0
  796. FALLTHROUGH
  797. default:
  798. // huh?
  799. if (*mode != MODE_NO_FIX) {
  800. *mode = MODE_NO_FIX;
  801. mask |= MODE_SET;
  802. };
  803. if (*status != STATUS_UNK) {
  804. *status = STATUS_UNK;
  805. mask |= STATUS_SET;
  806. }
  807. break;
  808. }
  809. if (UBX_NAV_PVT_VALID_DATE_TIME == (valid & UBX_NAV_PVT_VALID_DATE_TIME)) {
  810. unpacked_date.tm_year = (uint16_t)getleu16(buf, 4) - 1900;
  811. unpacked_date.tm_mon = (uint8_t)getub(buf, 6) - 1;
  812. unpacked_date.tm_mday = (uint8_t)getub(buf, 7);
  813. unpacked_date.tm_hour = (uint8_t)getub(buf, 8);
  814. unpacked_date.tm_min = (uint8_t)getub(buf, 9);
  815. unpacked_date.tm_sec = (uint8_t)getub(buf, 10);
  816. unpacked_date.tm_isdst = 0;
  817. unpacked_date.tm_wday = 0;
  818. unpacked_date.tm_yday = 0;
  819. session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
  820. // field 9, nano, can be negative! So normalize
  821. session->newdata.time.tv_nsec = getles32(buf, 12);
  822. TS_NORM(&session->newdata.time);
  823. mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
  824. }
  825. session->newdata.longitude = 1e-7 * getles32(buf, 20);
  826. session->newdata.latitude = 1e-7 * getles32(buf, 24);
  827. // altitude WGS84
  828. session->newdata.altHAE = 1e-3 * getles32(buf, 28);
  829. // altitude MSL
  830. session->newdata.altMSL = 1e-3 * getles32(buf, 32);
  831. // Let gpsd_error_model() deal with geoid_sep
  832. // gSpeed (2D)
  833. session->newdata.speed = 1e-3 * (int32_t)getles32(buf, 36);
  834. // offset 40, Speed (3D) do what with it?
  835. // u-blox calls this headMot (Heading of motion 2-D)
  836. session->newdata.track = 1e-5 * (int32_t)getles32(buf, 44);
  837. // offset 48, headVeh (Heading of Vehicle 2-D)
  838. mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET;
  839. /* u-blox does not document the basis for the following "accuracy"
  840. * estimates. Maybe CEP(50), one sigma, two sigma, CEP(99), etc. */
  841. // Horizontal Accuracy estimate, in mm
  842. session->newdata.eph = (double)(getles32(buf, 52) / 1000.0);
  843. // Vertical Accuracy estimate, in mm
  844. session->newdata.epv = (double)(getles32(buf, 56) / 1000.0);
  845. // Speed Accuracy estimate, in mm/s
  846. session->newdata.eps = (double)(getles32(buf, 60) / 1000.0);
  847. // headAcc (Heading Accuracy)
  848. session->newdata.epd = (double)getles32(buf, 64) * 1e-5;
  849. /* let gpsd_error_model() do the rest */
  850. // 4 final bytes reserved
  851. mask |= HERR_SET | SPEEDERR_SET | VERR_SET;
  852. // HNR-PVT interleaves with the normal cycle, so cycle end is a mess
  853. mask |= REPORT_IS;
  854. GPSD_LOG(LOG_PROG, &session->context->errout,
  855. "HNR-PVT: iTOW %lld flags %02x time %s lat %.2f lon %.2f altHAE %.2f "
  856. "track %.2f speed %.2f climb %.2f mode %d status %d used %d\n",
  857. (long long)iTOW, flags,
  858. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  859. session->newdata.latitude,
  860. session->newdata.longitude,
  861. session->newdata.altHAE,
  862. session->newdata.track,
  863. session->newdata.speed,
  864. session->newdata.climb,
  865. session->newdata.mode,
  866. session->newdata.status,
  867. session->gpsdata.satellites_used);
  868. return mask;
  869. }
  870. /**
  871. * Receiver/Software Version
  872. * UBX-MON-VER
  873. *
  874. * sadly more info than fits in session->swtype for now.
  875. * so squish the data hard.
  876. */
  877. static gps_mask_t ubx_msg_mon_ver(struct gps_device_t *session,
  878. unsigned char *buf,
  879. size_t data_len)
  880. {
  881. int n = 0; // extended info counter
  882. int num_ext = (data_len - 40) / 30; // number of extensions
  883. char obuf[128]; // temp version string buffer
  884. char *cptr;
  885. if (40 > data_len) {
  886. GPSD_LOG(LOG_WARN, &session->context->errout,
  887. "UBX-MON-VER message, runt payload len %zd", data_len);
  888. return 0;
  889. }
  890. // save SW and HW Version as subtype
  891. (void)snprintf(obuf, sizeof(obuf),
  892. "SW %.30s,HW %.10s",
  893. (char *)buf,
  894. (char *)(buf + 30));
  895. // save what we can
  896. (void)strlcpy(session->subtype, obuf, sizeof(session->subtype));
  897. obuf[0] = '\0';
  898. // extract Extended info strings.
  899. for (n = 0; n < num_ext; n++) {
  900. int start_of_str = 40 + (30 * n);
  901. if (0 < n) {
  902. // commas between elements
  903. (void)strlcat(obuf, ",", sizeof(obuf));
  904. }
  905. (void)strlcat(obuf, (char *)&buf[start_of_str], sizeof(obuf));
  906. }
  907. // save what we can in subtype1
  908. (void)strlcpy(session->subtype1, obuf, sizeof(session->subtype1));
  909. // find PROTVER literal, followed by single separator character
  910. cptr = strstr(obuf, "PROTVER="); // protVer 18 and above
  911. if (NULL == cptr) {
  912. cptr = strstr(obuf, "PROTVER "); // protVer 17 and below
  913. }
  914. if (NULL != cptr) {
  915. int protver = atoi(cptr + 8);
  916. if (7 < protver) {
  917. /* protver 8, u-blox Antaris, is the oldest we know, but never
  918. * used explicitly. protver 15, u-blox 8, is oldest seen. */
  919. session->driver.ubx.protver = protver;
  920. }
  921. }
  922. /* MON-VER did not contain PROTVER in any extension field (typical for
  923. * protVer < 15), so use mapping table to try to derive protVer from
  924. * firmware revision number carried in swVersion field */
  925. if (0 == session->driver.ubx.protver) {
  926. for (n = 0; NULL != fw_protver_map[n].fw_string; n++) {
  927. // skip "SW " prefix in session->subtype
  928. cptr = strstr(session->subtype + 3, fw_protver_map[n].fw_string);
  929. // use only when swVersion field starts with fw_string
  930. if (cptr == (session->subtype + 3)) {
  931. session->driver.ubx.protver =
  932. (unsigned char)fw_protver_map[n].protver;
  933. break;
  934. }
  935. }
  936. if (0 == session->driver.ubx.protver) {
  937. // Still not found, old chip. Set to one so we know we tried.
  938. session->driver.ubx.protver = 1;
  939. }
  940. }
  941. // output SW and HW Version at LOG_INF
  942. GPSD_LOG(LOG_INF, &session->context->errout,
  943. "UBX-MON-VER: %s %s PROTVER %u\n",
  944. session->subtype, session->subtype1,
  945. session->driver.ubx.protver);
  946. return 0;
  947. }
  948. /* UBX-MON-TXBUF
  949. * Present in u-blox 5+ through at least protVer 23.01
  950. * Supported but deprecated in M9P protVer 27.11
  951. * Supported but deprecated in M9N protVer 32.00 */
  952. static gps_mask_t
  953. ubx_msg_mon_txbuf(struct gps_device_t *session, unsigned char *buf,
  954. size_t data_len)
  955. {
  956. unsigned int tUsage, tPeakusage;
  957. unsigned char errors, limit, reserved1;
  958. int i;
  959. if (28 != data_len) {
  960. GPSD_LOG(LOG_WARN, &session->context->errout,
  961. "UBX-MON-TXBUF message, runt payload len %zd\n", data_len);
  962. return 0;
  963. }
  964. errors = limit = getub(buf, 26);
  965. for (i = 0; i < 6; i++) {
  966. unsigned int pending = getleu16(buf, i * 2);
  967. unsigned int usage = getub(buf, 12 + i);
  968. unsigned int peakUsage = getub(buf, 18 + i);
  969. GPSD_LOG(LOG_INF, &session->context->errout,
  970. "TXBUF: target %d, limit %u, pending %4u bytes, "
  971. "usage %3u%%, peakUsage %3d%%\n",
  972. i, limit & 1, pending, usage, peakUsage);
  973. limit = limit >> 1;
  974. }
  975. tUsage = getub(buf, 24);
  976. tPeakusage = getub(buf, 25);
  977. reserved1 = getub(buf, 27);
  978. GPSD_LOG(LOG_INF, &session->context->errout,
  979. "TXBUF: tUsage %3u%%, tPeakusage %3u%%, errors 0x%02x, "
  980. "reserved1 0x%02x\n",
  981. tUsage, tPeakusage, errors, reserved1);
  982. if ((errors & 0x40) == 0x40 || (errors & 0x80) == 0x80) {
  983. GPSD_LOG(LOG_WARN, &session->context->errout,
  984. "TXBUF: alloc %u, mem %u\n",
  985. errors >> 7, (errors >> 6) & 1);
  986. }
  987. return 0;
  988. }
  989. /* UBX-MON-RXBUF
  990. * Present in u-blox 5+ through at least protVer 23.01
  991. * Supported but deprecated in M9P protVer 27.11
  992. * Supported but deprecated in M9N protVer 32.00 */
  993. static gps_mask_t
  994. ubx_msg_mon_rxbuf(struct gps_device_t *session, unsigned char *buf,
  995. size_t data_len)
  996. {
  997. int i;
  998. if (24 != data_len) {
  999. GPSD_LOG(LOG_WARN, &session->context->errout,
  1000. "UBX-MON-RXBUF message, runt payload len %zd\n", data_len);
  1001. return 0;
  1002. }
  1003. for (i = 0; i < 6; i++) {
  1004. unsigned int pending = getleu16(buf, i * 2);
  1005. unsigned int usage = getub(buf, 12 + i);
  1006. unsigned int peakUsage = getub(buf, 18 + i);
  1007. GPSD_LOG(LOG_INF, &session->context->errout,
  1008. "RXBUF: target %d, pending %4u bytes, "
  1009. "usage %3u%%, peakUsage %3d%%\n",
  1010. i, pending, usage, peakUsage);
  1011. }
  1012. return 0;
  1013. }
  1014. /**
  1015. * UBX-LOG-BATCH entry only part of UBX protocol
  1016. * Used for GPS standalone operation (internal batch retrieval)
  1017. */
  1018. static gps_mask_t
  1019. ubx_msg_log_batch(struct gps_device_t *session, unsigned char *buf UNUSED,
  1020. size_t data_len)
  1021. {
  1022. struct tm unpacked_date = {0};
  1023. unsigned char contentValid, timeValid, flags, psmState;
  1024. bool gnssFixOK, diffSoln;
  1025. char ts_buf[TIMESPEC_LEN];
  1026. gps_mask_t mask = 0;
  1027. gps_clear_log(&session->gpsdata.log);
  1028. /* u-blox 8 100 bytes payload */
  1029. if (100 > data_len) {
  1030. GPSD_LOG(LOG_WARN, &session->context->errout,
  1031. "UBX-LOG-BATCH: runt len %zd", data_len);
  1032. return 0;
  1033. }
  1034. timeValid = getub(buf, 15);
  1035. if (3 != (timeValid & 3)) {
  1036. // No time, pointless...
  1037. return 0;
  1038. }
  1039. unpacked_date.tm_year = getleu16(buf, 8) - 1900;
  1040. unpacked_date.tm_mon = getub(buf, 10) - 1;
  1041. unpacked_date.tm_mday = getub(buf, 11);
  1042. unpacked_date.tm_hour = getub(buf, 12);
  1043. unpacked_date.tm_min = getub(buf, 13);
  1044. unpacked_date.tm_sec = getub(buf, 14);
  1045. contentValid = getub(buf, 1);
  1046. session->gpsdata.log.index_cnt = getleu16(buf, 2);
  1047. session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
  1048. session->gpsdata.log.then.tv_nsec = getles32(buf, 20);
  1049. TS_NORM(&session->gpsdata.log.then);
  1050. session->gpsdata.log.fixType = getub(buf, 24);
  1051. flags = getub( buf, 25);
  1052. gnssFixOK = flags & 1;
  1053. diffSoln = flags & 2;
  1054. psmState = ((flags >> 2) & 7);
  1055. // flags2 undocumented
  1056. // flags2 = getub( buf, 26);
  1057. if ((gnssFixOK &&
  1058. 2 <= session->gpsdata.log.fixType)) {
  1059. // good 2D fix
  1060. session->gpsdata.log.lon = 1.0e-7 * getles32(buf, 28);
  1061. session->gpsdata.log.lat = 1.0e-7 * getles32(buf, 32);
  1062. session->gpsdata.log.gSpeed = 1.0e-3 * getles32(buf, 64);
  1063. // seems to be true heading
  1064. session->gpsdata.log.heading = 1.0e-5 * getles32(buf, 68);
  1065. if (diffSoln) {
  1066. session->gpsdata.log.status = STATUS_DGPS;
  1067. } else {
  1068. session->gpsdata.log.status = STATUS_GPS;
  1069. }
  1070. if (3 <= session->gpsdata.log.fixType) {
  1071. // good 3D fix
  1072. session->gpsdata.log.altHAE = 1.0e-3 * getles32(buf, 36);
  1073. }
  1074. }
  1075. session->gpsdata.log.hAcc = 1.0e-3 * getleu32(buf, 44);
  1076. GPSD_LOG(LOG_INF, &session->context->errout,
  1077. "UBX-LOG-BATCH: time=%s index_cnt=%u fixType=%u lon=%.7f lat=%.7f"
  1078. " gSpeed=%.3f heading=%.5f altHae=%.3f psmState=%u hAcc=%.3f\n",
  1079. timespec_str(&session->gpsdata.log.then, ts_buf, sizeof(ts_buf)),
  1080. session->gpsdata.log.index_cnt, session->gpsdata.log.fixType,
  1081. session->gpsdata.log.lon, session->gpsdata.log.lat,
  1082. session->gpsdata.log.gSpeed, session->gpsdata.log.heading,
  1083. session->gpsdata.log.altHAE, psmState,
  1084. session->gpsdata.log.hAcc);
  1085. if (1 == (contentValid & 1)) {
  1086. // extraPVT valid
  1087. // iTOW = getleu32(buf, 4);
  1088. session->gpsdata.log.tAcc = (double)getleu32(buf, 16);
  1089. session->gpsdata.log.numSV = getub(buf, 27);
  1090. session->gpsdata.log.altMSL = 1.0e-3 * getles32(buf, 40);
  1091. session->gpsdata.log.vAcc = 1.0e-3 * getleu32(buf, 48);
  1092. session->gpsdata.log.velN = 1.0e-3 * getles32(buf, 52);
  1093. session->gpsdata.log.velE = 1.0e-3 * getles32(buf, 56);
  1094. session->gpsdata.log.velD = 1.0e-3 * getles32(buf, 60);
  1095. session->gpsdata.log.sAcc = 1.0e-3 * getleu32(buf, 72);
  1096. session->gpsdata.log.headAcc = 1.0e-5 * getleu32(buf, 76);
  1097. session->gpsdata.log.pDOP = 1.0e-2 * getleu32(buf, 80);
  1098. GPSD_LOG(LOG_INF, &session->context->errout,
  1099. "UBX-LOG-BATCH extraPVT: time=%s index_cnt=%d"
  1100. " tAcc=%.2f numSV=%d altMSL=%.3f hAcc=%.2f vAcc=%.3f"
  1101. " velN=%.3f velE=%.3f velD=%.3f sAcc=%.3f headAcc=%.5f"
  1102. " pDOP=%.5f\n",
  1103. timespec_str(&session->gpsdata.log.then, ts_buf,
  1104. sizeof(ts_buf)),
  1105. session->gpsdata.log.index_cnt,
  1106. session->gpsdata.log.tAcc, session->gpsdata.log.numSV,
  1107. session->gpsdata.log.altMSL, session->gpsdata.log.hAcc,
  1108. session->gpsdata.log.vAcc, session->gpsdata.log.velN,
  1109. session->gpsdata.log.velE, session->gpsdata.log.velD,
  1110. session->gpsdata.log.sAcc, session->gpsdata.log.headAcc,
  1111. session->gpsdata.log.pDOP);
  1112. }
  1113. if (2 == (contentValid & 2)) {
  1114. session->gpsdata.log.distance = getleu32(buf, 84);
  1115. session->gpsdata.log.totalDistance = getleu32(buf, 88);
  1116. session->gpsdata.log.distanceStd = getleu32(buf, 92);
  1117. GPSD_LOG(LOG_INF, &session->context->errout,
  1118. "UBX-LOG-BATCH extraOdo: time=%s index_cnt=%d distance=%.0f"
  1119. " totalDistance=%.0f distanceStd=%.0f\n",
  1120. timespec_str(&session->gpsdata.log.then, ts_buf,
  1121. sizeof(ts_buf)),
  1122. session->gpsdata.log.index_cnt, session->gpsdata.log.distance,
  1123. session->gpsdata.log.totalDistance,
  1124. session->gpsdata.log.distanceStd);
  1125. }
  1126. mask |= LOG_SET;
  1127. return mask;
  1128. }
  1129. /**
  1130. * UBX-LOG-INFO info of log status
  1131. * u-blox 7,8,9. protVer 14 to 29
  1132. * WIP: Initial decode, log only.
  1133. *
  1134. */
  1135. static gps_mask_t
  1136. ubx_msg_log_info(struct gps_device_t *session, unsigned char *buf UNUSED,
  1137. size_t data_len)
  1138. {
  1139. struct tm oldest_date = {0}, newest_date = {0};
  1140. timespec_t oldest = {0, 0};
  1141. timespec_t newest = {0, 0};
  1142. unsigned char version, status;
  1143. unsigned long filestoreCapacity;
  1144. unsigned long currentMaxLogSize;
  1145. unsigned long currentLogSize;
  1146. unsigned long entryCount;
  1147. char ts_buf[TIMESPEC_LEN];
  1148. char ts_buf1[TIMESPEC_LEN];
  1149. gps_mask_t mask = 0;
  1150. gps_clear_log(&session->gpsdata.log);
  1151. // u-blox 7/8/9 48 bytes payload
  1152. if (48 > data_len) {
  1153. GPSD_LOG(LOG_WARN, &session->context->errout,
  1154. "UBX-LOG-INFO: runt len %zd", data_len);
  1155. return 0;
  1156. }
  1157. // u-blox 7/8/9 version 1
  1158. version = getub(buf, 0);
  1159. filestoreCapacity = getleu32(buf, 4);
  1160. currentMaxLogSize = getleu32(buf, 16);
  1161. currentLogSize = getleu32(buf, 20);
  1162. entryCount = getleu32(buf, 24);
  1163. status = getub(buf, 44);
  1164. oldest_date.tm_year = getleu16(buf, 28);
  1165. if (0 != oldest_date.tm_year) {
  1166. oldest_date.tm_year -= 1900;
  1167. oldest_date.tm_mon = getub(buf, 30) - 1;
  1168. oldest_date.tm_mday = getub(buf, 31);
  1169. oldest_date.tm_hour = getub(buf, 32);
  1170. oldest_date.tm_min = getub(buf, 33);
  1171. oldest_date.tm_sec = getub(buf, 34);
  1172. oldest.tv_sec = mkgmtime(&oldest_date);
  1173. oldest.tv_nsec = 0;
  1174. TS_NORM(&oldest);
  1175. }
  1176. newest_date.tm_year = getleu16(buf, 36);
  1177. if (0 != newest_date.tm_year) {
  1178. newest_date.tm_year -= 1900;
  1179. newest_date.tm_mon = getub(buf, 38) - 1;
  1180. newest_date.tm_mday = getub(buf, 39);
  1181. newest_date.tm_hour = getub(buf, 40);
  1182. newest_date.tm_min = getub(buf, 41);
  1183. newest_date.tm_sec = getub(buf, 42);
  1184. newest.tv_sec = mkgmtime(&newest_date);
  1185. newest.tv_nsec = 0;
  1186. TS_NORM(&newest);
  1187. }
  1188. GPSD_LOG(LOG_INF, &session->context->errout,
  1189. "UBX-LOG-INFO: version=%u status=x%x Cap=%lu MaxSize=%lu "
  1190. "Size=%lu cnt=%lu oldest=%s newest=%s\n",
  1191. version, status,
  1192. filestoreCapacity,
  1193. currentMaxLogSize,
  1194. currentLogSize,
  1195. entryCount,
  1196. timespec_str(&oldest, ts_buf, sizeof(ts_buf)),
  1197. timespec_str(&newest, ts_buf1, sizeof(ts_buf1)));
  1198. // mask |= LOG_SET;
  1199. return mask;
  1200. }
  1201. /*
  1202. * UBX-LOG-RETRIEVEPOS (Indexed PVT entry)
  1203. * Used for GPS standalone operation and host saved logs
  1204. * u-blox 7,8,9. protVer 14 to 29
  1205. */
  1206. static gps_mask_t
  1207. ubx_msg_log_retrievepos(struct gps_device_t *session, unsigned char *buf UNUSED,
  1208. size_t data_len)
  1209. {
  1210. struct tm unpacked_date = {0};
  1211. unsigned char fixType;
  1212. gps_mask_t mask = 0;
  1213. gps_clear_log(&session->gpsdata.log);
  1214. /* u-blox 40 bytes payload */
  1215. if (40 > data_len) {
  1216. GPSD_LOG(LOG_WARN, &session->context->errout,
  1217. "UBX-LOG-RETRIEVEPOS: runt len %zd", data_len);
  1218. return 0;
  1219. }
  1220. unpacked_date.tm_year = getleu16(buf, 30);
  1221. if (1900 > unpacked_date.tm_year) {
  1222. // useless, no date
  1223. return 0;
  1224. }
  1225. unpacked_date.tm_year -= 1900;
  1226. unpacked_date.tm_mon = getub(buf, 32) - 1;
  1227. unpacked_date.tm_mday = getub(buf, 33);
  1228. unpacked_date.tm_hour = getub(buf, 34);
  1229. unpacked_date.tm_min = getub(buf, 35);
  1230. unpacked_date.tm_sec = getub(buf, 36);
  1231. session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
  1232. session->gpsdata.log.index_cnt = getleu32(buf, 0);
  1233. session->gpsdata.log.lon = getleu32(buf, 4) * 1.0e-7;
  1234. session->gpsdata.log.lat = getleu32(buf, 8) * 1.0e-7;
  1235. session->gpsdata.log.altMSL = getleu32(buf, 12) * 1.0e-3;
  1236. // hAcc CEP() unspecified...
  1237. session->gpsdata.log.hAcc = getleu32(buf, 16) * 1.0e-3;
  1238. session->gpsdata.log.gSpeed = getleu32(buf, 20) * 1.0e-3;
  1239. // seems to be true heading
  1240. session->gpsdata.log.heading = getleu32(buf, 24) * 1.0e-5;
  1241. fixType = getub(buf, 29);
  1242. session->gpsdata.log.numSV = getub(buf, 38);
  1243. switch (fixType) {
  1244. case 1:
  1245. // doc is unclear: 2D or 3D?
  1246. session->gpsdata.log.fixType = MODE_3D;
  1247. session->gpsdata.log.status = STATUS_DR;
  1248. break;
  1249. case 2:
  1250. session->gpsdata.log.fixType = MODE_2D;
  1251. session->gpsdata.log.status = STATUS_GPS;
  1252. break;
  1253. case 3:
  1254. session->gpsdata.log.fixType = MODE_3D;
  1255. session->gpsdata.log.status = STATUS_GPS;
  1256. break;
  1257. case 4:
  1258. // doc is unclear: 2D or 3D?
  1259. session->gpsdata.log.fixType = MODE_3D;
  1260. session->gpsdata.log.status = STATUS_GNSSDR;
  1261. break;
  1262. case 0:
  1263. FALLTHROUGH
  1264. default:
  1265. // huh?
  1266. session->gpsdata.log.fixType = MODE_NO_FIX;
  1267. session->gpsdata.log.status = STATUS_UNK;
  1268. break;
  1269. }
  1270. // (long long) because of time_t
  1271. GPSD_LOG(LOG_INF, &session->context->errout,
  1272. "UBX-LOG-RETRIEVEPOS: time=%lld entryIndex=%d"
  1273. " lon=%.7f lat=%.7f altMSL=%.3f hAcc=%.3f"
  1274. " gspeed=%.3f heading=%.5f fixType=%d numSV=%d\n",
  1275. (long long)session->gpsdata.log.then.tv_sec,
  1276. session->gpsdata.log.index_cnt, session->gpsdata.log.lon,
  1277. session->gpsdata.log.lat, session->gpsdata.log.altMSL,
  1278. session->gpsdata.log.hAcc, session->gpsdata.log.gSpeed,
  1279. session->gpsdata.log.heading, session->gpsdata.log.fixType,
  1280. session->gpsdata.log.numSV);
  1281. mask |= LOG_SET;
  1282. return mask;
  1283. }
  1284. /*
  1285. * UBX-LOG-RETRIEVEPOSEXTRA (Indexed Odometry entry)
  1286. * Used for GPS standalone operation and host saved logs
  1287. * u-blox 7,8,9. protVer 14 to 29
  1288. */
  1289. static gps_mask_t
  1290. ubx_msg_log_retrieveposextra(struct gps_device_t *session,
  1291. unsigned char *buf UNUSED, size_t data_len)
  1292. {
  1293. struct tm unpacked_date = {0};
  1294. gps_mask_t mask = 0;
  1295. gps_clear_log(&session->gpsdata.log);
  1296. /* u-blox 32 bytes payload */
  1297. if (32 > data_len) {
  1298. GPSD_LOG(LOG_WARN, &session->context->errout,
  1299. "UBX-LOG-RETRIEVEPOSEXTRA: runt len %zd", data_len);
  1300. return 0;
  1301. }
  1302. unpacked_date.tm_year = getleu16(buf, 6);
  1303. if (1900 > unpacked_date.tm_year) {
  1304. // useless, no date
  1305. return 0;
  1306. }
  1307. unpacked_date.tm_year -= 1900;
  1308. unpacked_date.tm_mon = getub(buf, 8) - 1;
  1309. unpacked_date.tm_mday = getub(buf, 9);
  1310. unpacked_date.tm_hour = getub(buf, 10);
  1311. unpacked_date.tm_min = getub(buf, 11);
  1312. unpacked_date.tm_sec = getub(buf, 12);
  1313. session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
  1314. session->gpsdata.log.index_cnt = getleu32(buf, 0);
  1315. // distance units undocumented! Assume meters, as in UBX-LOG-BATCH
  1316. session->gpsdata.log.distance = (double)getleu32(buf, 16);
  1317. // (long long) because of time_t
  1318. GPSD_LOG(LOG_INF, &session->context->errout,
  1319. "UBX-LOG-RETRIEVEPOSEXTRA:"
  1320. " time=%lld entryindex=%u distance=%.0f\n",
  1321. (long long)session->gpsdata.log.then.tv_sec,
  1322. session->gpsdata.log.index_cnt, session->gpsdata.log.distance);
  1323. mask |= LOG_SET;
  1324. return mask;
  1325. }
  1326. /*
  1327. * UBX-LOG-RETRIEVESTRING
  1328. * Used for GPS standalone operation and host saved logs
  1329. * u-blox 7,8,9. protVer 14 to 29
  1330. */
  1331. static gps_mask_t
  1332. ubx_msg_log_retrievestring(struct gps_device_t *session,
  1333. unsigned char *buf UNUSED, size_t data_len)
  1334. {
  1335. struct tm unpacked_date = {0};
  1336. unsigned int byteCount;
  1337. gps_mask_t mask = 0;
  1338. gps_clear_log(&session->gpsdata.log);
  1339. /* u-blox 16+ bytes payload */
  1340. if (16 > data_len) {
  1341. GPSD_LOG(LOG_WARN, &session->context->errout,
  1342. "UBX-LOG-RETRIEVESTRING: runt len %zd", data_len);
  1343. return 0;
  1344. }
  1345. unpacked_date.tm_year = getleu16(buf, 6);
  1346. if (1900 > unpacked_date.tm_year) {
  1347. // useless, no date
  1348. return 0;
  1349. }
  1350. unpacked_date.tm_year -= 1900;
  1351. unpacked_date.tm_mon = getub(buf, 8) - 1;
  1352. unpacked_date.tm_mday = getub(buf, 9);
  1353. unpacked_date.tm_hour = getub(buf, 10);
  1354. unpacked_date.tm_min = getub(buf, 11);
  1355. unpacked_date.tm_sec = getub(buf, 12);
  1356. session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
  1357. session->gpsdata.log.index_cnt = getleu32(buf, 0);
  1358. byteCount = getleu16(buf, 14);
  1359. // string could be 0 to 256 bytes, plus NUL
  1360. (void)strlcpy(session->gpsdata.log.string, (const char*)&buf[16],
  1361. sizeof(session->gpsdata.log.string));
  1362. // (long long) because of time_t
  1363. GPSD_LOG(LOG_INF, &session->context->errout,
  1364. "UBX-LOG-RETRIEVESTRING:"
  1365. " time=%lld entryindex=%u byteCount=%u string=%s\n",
  1366. (long long)session->gpsdata.log.then.tv_sec,
  1367. session->gpsdata.log.index_cnt,
  1368. byteCount, session->gpsdata.log.string);
  1369. mask |= LOG_SET;
  1370. return mask;
  1371. }
  1372. /*
  1373. * UBX-NAV-HPPOSECEF - High Precision Position Solution in ECEF
  1374. *
  1375. * Present in u-blox 8 and above, protVwer 20.00 and up.
  1376. * Only with High Precision firmware.
  1377. */
  1378. static gps_mask_t
  1379. ubx_msg_nav_hpposecef(struct gps_device_t *session, unsigned char *buf,
  1380. size_t data_len)
  1381. {
  1382. gps_mask_t mask = ECEF_SET;
  1383. int version;
  1384. if (28 > data_len) {
  1385. GPSD_LOG(LOG_WARN, &session->context->errout,
  1386. "UBX-NAV-HPPOSECEF message, runt payload len %zd", data_len);
  1387. return 0;
  1388. }
  1389. version = getub(buf, 0);
  1390. session->driver.ubx.iTOW = getleu32(buf, 4);
  1391. session->newdata.ecef.x = ((getles32(buf, 8) +
  1392. (getsb(buf, 20) * 1e-2)) * 1e-2);
  1393. session->newdata.ecef.y = ((getles32(buf, 12) +
  1394. (getsb(buf, 21) * 1e-2)) * 1e-2);
  1395. session->newdata.ecef.z = ((getles32(buf, 16) +
  1396. (getsb(buf, 22) * 1e-2)) * 1e-2);
  1397. session->newdata.ecef.pAcc = getleu32(buf, 24) * 1e-4;
  1398. /* (long long) cast for 32-bit compat */
  1399. GPSD_LOG(LOG_PROG, &session->context->errout,
  1400. "UBX-NAV-HPPOSECEF: version %d iTOW=%lld ECEF x=%.4f y=%.4f z=%.4f "
  1401. "pAcc=%.4f\n",
  1402. version,
  1403. (long long)session->driver.ubx.iTOW,
  1404. session->newdata.ecef.x,
  1405. session->newdata.ecef.y,
  1406. session->newdata.ecef.z,
  1407. session->newdata.ecef.pAcc);
  1408. return mask;
  1409. }
  1410. /**
  1411. * High Precision Geodetic Position Solution
  1412. * UBX-NAV-HPPOSLLH, Class 1, ID x14
  1413. *
  1414. * No mode, so limited usefulness.
  1415. *
  1416. * Present in u-blox 8 and above, protVwer 20.00 and up.
  1417. * Only with High Precision firmware.
  1418. */
  1419. static gps_mask_t
  1420. ubx_msg_nav_hpposllh(struct gps_device_t *session, unsigned char *buf,
  1421. size_t data_len)
  1422. {
  1423. int version;
  1424. gps_mask_t mask = 0;
  1425. if (36 > data_len) {
  1426. GPSD_LOG(LOG_WARN, &session->context->errout,
  1427. "UBX-NAV-HPPOSLLH message, runt payload len %zd", data_len);
  1428. return mask;
  1429. }
  1430. mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET;
  1431. version = getub(buf, 0);
  1432. session->driver.ubx.iTOW = getles32(buf, 4);
  1433. session->newdata.longitude = (1e-7 * (getles32(buf, 8) +
  1434. (getsb(buf, 24) * 1e-2)));
  1435. session->newdata.latitude = (1e-7 * (getles32(buf, 12) + \
  1436. (getsb(buf, 25) * 1e-2)));
  1437. /* altitude WGS84 */
  1438. session->newdata.altHAE = (1e-3 * (getles32(buf, 16) + \
  1439. (getsb(buf, 26) * 1e-2)));
  1440. /* altitude MSL */
  1441. session->newdata.altMSL = (1e-3 * (getles32(buf, 20) + \
  1442. (getsb(buf, 27) * 1e-2)));
  1443. /* Let gpsd_error_model() deal with geoid_sep */
  1444. /* Horizontal accuracy estimate in .1 mm, unknown est type */
  1445. session->newdata.eph = getleu32(buf, 28) * 1e-4;
  1446. /* Vertical accuracy estimate in .1 mm, unknown est type */
  1447. session->newdata.epv = getleu32(buf, 32) * 1e-4;
  1448. GPSD_LOG(LOG_PROG, &session->context->errout,
  1449. "UBX-NAV-HPPOSLLH: version %d iTOW=%lld lat=%.4f lon=%.4f "
  1450. "altHAE=%.4f\n",
  1451. version,
  1452. (long long)session->driver.ubx.iTOW,
  1453. session->newdata.latitude,
  1454. session->newdata.longitude,
  1455. session->newdata.altHAE);
  1456. return mask;
  1457. }
  1458. /*
  1459. * Navigation Position ECEF message
  1460. *
  1461. * This message does not bother to tell us if it is valid.
  1462. */
  1463. static gps_mask_t
  1464. ubx_msg_nav_posecef(struct gps_device_t *session, unsigned char *buf,
  1465. size_t data_len)
  1466. {
  1467. gps_mask_t mask = ECEF_SET;
  1468. if (20 > data_len) {
  1469. GPSD_LOG(LOG_WARN, &session->context->errout,
  1470. "UBX-NAV-POSECEF message, runt payload len %zd", data_len);
  1471. return 0;
  1472. }
  1473. session->driver.ubx.iTOW = getleu32(buf, 0);
  1474. /* all in cm */
  1475. session->newdata.ecef.x = getles32(buf, 4) * 1e-2;
  1476. session->newdata.ecef.y = getles32(buf, 8) * 1e-2;
  1477. session->newdata.ecef.z = getles32(buf, 12) * 1e-2;
  1478. session->newdata.ecef.pAcc = getleu32(buf, 16) * 1e-2;
  1479. /* (long long) cast for 32-bit compat */
  1480. GPSD_LOG(LOG_PROG, &session->context->errout,
  1481. "UBX-NAV-POSECEF: iTOW=%lld ECEF x=%.2f y=%.2f z=%.2f pAcc=%.2f\n",
  1482. (long long)session->driver.ubx.iTOW,
  1483. session->newdata.ecef.x,
  1484. session->newdata.ecef.y,
  1485. session->newdata.ecef.z,
  1486. session->newdata.ecef.pAcc);
  1487. return mask;
  1488. }
  1489. /**
  1490. * Navigation Position Velocity Time solution message
  1491. * UBX-NAV-PVT Class 1, ID 7
  1492. *
  1493. * Not in u-blox 5 or 6, present in u-blox 7
  1494. * u-blox 6 w/ GLONASS, protver 14 have NAV-PVT
  1495. */
  1496. static gps_mask_t
  1497. ubx_msg_nav_pvt(struct gps_device_t *session, unsigned char *buf,
  1498. size_t data_len)
  1499. {
  1500. uint8_t valid;
  1501. uint8_t flags;
  1502. uint8_t fixType;
  1503. struct tm unpacked_date;
  1504. int *status = &session->newdata.status;
  1505. int *mode = &session->newdata.mode;
  1506. gps_mask_t mask = 0;
  1507. char ts_buf[TIMESPEC_LEN];
  1508. /* u-blox 6 and 7 are 84 bytes, u-blox 8 and 9 are 92 bytes */
  1509. if (84 > data_len) {
  1510. GPSD_LOG(LOG_WARN, &session->context->errout,
  1511. "UBX-NAV-PVT message, runt payload len %zd", data_len);
  1512. return 0;
  1513. }
  1514. if (14 > session->driver.ubx.protver) {
  1515. /* this GPS is at least protver 14 */
  1516. session->driver.ubx.protver = 14;
  1517. }
  1518. session->driver.ubx.iTOW = getleu32(buf, 0);
  1519. valid = (unsigned int)getub(buf, 11);
  1520. fixType = (unsigned char)getub(buf, 20);
  1521. flags = (unsigned int)getub(buf, 21);
  1522. switch (fixType) {
  1523. case UBX_MODE_TMONLY:
  1524. // 5 - Surveyed-in, so a precise 3D.
  1525. *mode = MODE_3D;
  1526. *status = STATUS_TIME;
  1527. mask |= STATUS_SET | MODE_SET;
  1528. break;
  1529. case UBX_MODE_3D:
  1530. // 3
  1531. FALLTHROUGH
  1532. case UBX_MODE_GPSDR:
  1533. // 4
  1534. if (*mode != MODE_3D) {
  1535. *mode = MODE_3D;
  1536. mask |= MODE_SET;
  1537. }
  1538. if ((flags & UBX_NAV_PVT_FLAG_DGPS) == UBX_NAV_PVT_FLAG_DGPS) {
  1539. *status = STATUS_DGPS;
  1540. mask |= STATUS_SET;
  1541. } else {
  1542. *status = STATUS_GPS;
  1543. mask |= STATUS_SET;
  1544. }
  1545. mask |= LATLON_SET;
  1546. break;
  1547. case UBX_MODE_2D:
  1548. // 2
  1549. FALLTHROUGH
  1550. case UBX_MODE_DR: /* consider this too as 2D */
  1551. // 1
  1552. if (MODE_2D != *mode) {
  1553. *mode = MODE_2D;
  1554. mask |= MODE_SET;
  1555. };
  1556. if (STATUS_GPS != *status) {
  1557. // FIXME: Set DR if this is DR
  1558. *status = STATUS_GPS;
  1559. mask |= STATUS_SET;
  1560. }
  1561. mask |= LATLON_SET | SPEED_SET;
  1562. break;
  1563. case UBX_MODE_NOFIX:
  1564. // 0
  1565. FALLTHROUGH
  1566. default:
  1567. // huh?
  1568. if (*mode != MODE_NO_FIX) {
  1569. *mode = MODE_NO_FIX;
  1570. mask |= MODE_SET;
  1571. };
  1572. if (*status != STATUS_UNK) {
  1573. *status = STATUS_UNK;
  1574. mask |= STATUS_SET;
  1575. }
  1576. break;
  1577. }
  1578. if ((valid & UBX_NAV_PVT_VALID_DATE_TIME) == UBX_NAV_PVT_VALID_DATE_TIME) {
  1579. unpacked_date.tm_year = (uint16_t)getleu16(buf, 4) - 1900;
  1580. unpacked_date.tm_mon = (uint8_t)getub(buf, 6) - 1;
  1581. unpacked_date.tm_mday = (uint8_t)getub(buf, 7);
  1582. unpacked_date.tm_hour = (uint8_t)getub(buf, 8);
  1583. unpacked_date.tm_min = (uint8_t)getub(buf, 9);
  1584. unpacked_date.tm_sec = (uint8_t)getub(buf, 10);
  1585. unpacked_date.tm_isdst = 0;
  1586. unpacked_date.tm_wday = 0;
  1587. unpacked_date.tm_yday = 0;
  1588. session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
  1589. /* field 16, nano, can be negative! So normalize */
  1590. session->newdata.time.tv_nsec = getles32(buf, 16);
  1591. TS_NORM(&session->newdata.time);
  1592. mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
  1593. }
  1594. session->newdata.longitude = 1e-7 * getles32(buf, 24);
  1595. session->newdata.latitude = 1e-7 * getles32(buf, 28);
  1596. /* altitude WGS84 */
  1597. session->newdata.altHAE = 1e-3 * getles32(buf, 32);
  1598. /* altitude MSL */
  1599. session->newdata.altMSL = 1e-3 * getles32(buf, 36);
  1600. /* Let gpsd_error_model() deal with geoid_sep */
  1601. session->newdata.speed = 1e-3 * (int32_t)getles32(buf, 60);
  1602. /* u-blox calls this Heading of motion (2-D) */
  1603. session->newdata.track = 1e-5 * (int32_t)getles32(buf, 64);
  1604. mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET;
  1605. /* u-blox does not document the basis for the following "accuracy"
  1606. * estimates. Maybe CEP(50), one sigma, two sigma, CEP(99), etc. */
  1607. /* Horizontal Accuracy estimate, in mm */
  1608. session->newdata.eph = (double)(getles32(buf, 40) / 1000.0);
  1609. /* Vertical Accuracy estimate, in mm */
  1610. session->newdata.epv = (double)(getles32(buf, 44) / 1000.0);
  1611. /* Speed Accuracy estimate, in mm/s */
  1612. session->newdata.eps = (double)(getles32(buf, 68) / 1000.0);
  1613. /* let gpsd_error_model() do the rest */
  1614. mask |= HERR_SET | SPEEDERR_SET | VERR_SET;
  1615. // if cycle ender worked, could get rid of this REPORT_IS.
  1616. // mask |= REPORT_IS;
  1617. GPSD_LOG(LOG_PROG, &session->context->errout,
  1618. "NAV-PVT: flags=%02x time=%s lat=%.2f lon=%.2f altHAE=%.2f "
  1619. "track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
  1620. flags,
  1621. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  1622. session->newdata.latitude,
  1623. session->newdata.longitude,
  1624. session->newdata.altHAE,
  1625. session->newdata.track,
  1626. session->newdata.speed,
  1627. session->newdata.climb,
  1628. session->newdata.mode,
  1629. session->newdata.status,
  1630. session->gpsdata.satellites_used);
  1631. if (92 <= data_len) {
  1632. // u-blox 8 and 9 extended
  1633. double magDec = NAN;
  1634. double magAcc = NAN;
  1635. #ifdef __UNUSED
  1636. if (flags & UBX_NAV_PVT_FLAG_HDG_OK) {
  1637. /* u-blox calls this Heading of vehicle (2-D)
  1638. * why is it different than earlier track? */
  1639. session->newdata.track = (double)(getles32(buf, 84) * 1e-5);
  1640. }
  1641. #endif // __UNUSED
  1642. if (valid & UBX_NAV_PVT_VALID_MAG) {
  1643. magDec = (double)(getles16(buf, 88) * 1e-2);
  1644. magAcc = (double)(getleu16(buf, 90) * 1e-2);
  1645. }
  1646. GPSD_LOG(LOG_PROG, &session->context->errout,
  1647. " headVeh %.5f magDec %.2f magAcc %.2f\n",
  1648. session->newdata.track, magDec, magAcc);
  1649. }
  1650. return mask;
  1651. }
  1652. /**
  1653. * High Precision Relative Positioning Information in NED frame
  1654. * UBX-NAV-RELPOSNED, Class 1, ID x3c
  1655. * HP GNSS only, protver 20+
  1656. */
  1657. static gps_mask_t
  1658. ubx_msg_nav_relposned(struct gps_device_t *session, unsigned char *buf,
  1659. size_t data_len)
  1660. {
  1661. int version;
  1662. unsigned flags;
  1663. double accN = NAN, accE = NAN, accD = NAN, accL = NAN, accH = NAN;;
  1664. gps_mask_t mask = 0;
  1665. if (40 > data_len) {
  1666. GPSD_LOG(LOG_WARN, &session->context->errout,
  1667. "UBX-NAV-RELPOSNED:0 message, runt payload len %zd",
  1668. data_len);
  1669. return mask;
  1670. }
  1671. version = getub(buf, 0);
  1672. /* WTF? u-blox did not make this sentence upward compatible
  1673. * 40 bytes in Version 0, protVer 20 to 27
  1674. * 64 bytes in Version 1, protVer 27.11+ */
  1675. session->newdata.dgps_station = getleu16(buf, 2); // 0 to 4095
  1676. session->driver.ubx.iTOW = getleu32(buf, 4);
  1677. if (1 > version) {
  1678. // version 0
  1679. flags = getleu32(buf, 36);
  1680. if (1 != (1 & flags)) {
  1681. // not gnssFixOK
  1682. GPSD_LOG(LOG_PROG, &session->context->errout,
  1683. "UBX-NAV-RELPOSNED:0 no fix");
  1684. return mask;
  1685. }
  1686. if (4 & flags) {
  1687. // rePosValid
  1688. session->newdata.NED.relPosN = (1e-2 * (getles32(buf, 8) +
  1689. (getsb(buf, 20) * 1e-2)));
  1690. session->newdata.NED.relPosE = (1e-2 * (getles32(buf, 12) +
  1691. (getsb(buf, 21) * 1e-2)));
  1692. session->newdata.NED.relPosD = (1e-2 * (getles32(buf, 16) +
  1693. (getsb(buf, 22) * 1e-2)));
  1694. accN = 1e-4 * getles32(buf, 24);
  1695. accE = 1e-4 * getles32(buf, 28);
  1696. accD = 1e-4 * getles32(buf, 32);
  1697. mask |= NED_SET;
  1698. }
  1699. } else {
  1700. // assume version 1
  1701. if (64 > data_len) {
  1702. GPSD_LOG(LOG_WARN, &session->context->errout,
  1703. "UBX-NAV-RELPOSNED:1 message, runt payload len %zd",
  1704. data_len);
  1705. return mask;
  1706. }
  1707. flags = getleu32(buf, 60);
  1708. if (1 != (1 & flags)) {
  1709. // not gnssFixOK
  1710. GPSD_LOG(LOG_PROG, &session->context->errout,
  1711. "UBX-NAV-RELPOSNED:1 no fix");
  1712. return mask;
  1713. }
  1714. if (4 & flags) {
  1715. // rePosValid
  1716. session->newdata.NED.relPosN = (1e-2 * (getles32(buf, 8) +
  1717. (getsb(buf, 32) * 1e-2)));
  1718. session->newdata.NED.relPosE = (1e-2 * (getles32(buf, 12) +
  1719. (getsb(buf, 33) * 1e-2)));
  1720. session->newdata.NED.relPosD = (1e-2 * (getles32(buf, 16) +
  1721. (getsb(buf, 34) * 1e-2)));
  1722. session->newdata.NED.relPosL = (1e-2 * (getles32(buf, 20) +
  1723. (getsb(buf, 35) * 1e-2)));
  1724. accN = 1e-4 * getles32(buf, 36);
  1725. accE = 1e-4 * getles32(buf, 40);
  1726. accD = 1e-4 * getles32(buf, 44);
  1727. accL = 1e-4 * getles32(buf, 48);
  1728. accH = 1e-4 * getles32(buf, 52);
  1729. if (0x100 & flags) {
  1730. // relPosHeadingValid
  1731. session->newdata.NED.relPosH = 1e-5 * getles32(buf, 24);
  1732. }
  1733. mask |= NED_SET;
  1734. }
  1735. }
  1736. GPSD_LOG(LOG_PROG, &session->context->errout,
  1737. "UBX-NAV-RELPOSNED: version %d iTOW=%lld refStationId %u flags x%x\n"
  1738. "UBX-NAV-RELPOSNED: relPos N=%.4f E=%.4f D=%.4f\n"
  1739. "UBX-NAV-RELPOSNED: acc N=%.4f E=%.4f D=%.4f L=%.4f H=%.4f\n",
  1740. version,
  1741. (long long)session->driver.ubx.iTOW,
  1742. session->newdata.dgps_station,
  1743. flags,
  1744. session->newdata.NED.relPosN,
  1745. session->newdata.NED.relPosE,
  1746. session->newdata.NED.relPosD,
  1747. accN, accE, accD, accL, accH);
  1748. if (5 != (flags & 5)) {
  1749. /* gnssFixOK or relPosValid are false, no fix */
  1750. return 0;
  1751. }
  1752. return mask;
  1753. }
  1754. /**
  1755. * Navigation solution message: UBX-NAV-SOL
  1756. *
  1757. * UBX-NAV-SOL, present in Antaris, up to 23,01
  1758. * deprecated in u-blox 6, gone in u-blox 9.
  1759. * Use UBX-NAV-PVT instead
  1760. *
  1761. * UBX-NAV-SOL has ECEF and VECEF, so no need for UBX-NAV-POSECEF and
  1762. * UBX-NAV-VELECEF
  1763. */
  1764. static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
  1765. unsigned char *buf, size_t data_len)
  1766. {
  1767. unsigned flags, pdop;
  1768. unsigned char navmode;
  1769. gps_mask_t mask;
  1770. char ts_buf[TIMESPEC_LEN];
  1771. if (52 > data_len) {
  1772. GPSD_LOG(LOG_WARN, &session->context->errout,
  1773. "UBX-NAV-SOL message, runt payload len %zd", data_len);
  1774. return 0;
  1775. }
  1776. session->driver.ubx.iTOW = getleu32(buf, 0);
  1777. flags = (unsigned int)getub(buf, 11);
  1778. mask = 0;
  1779. #define DATE_VALID (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)
  1780. if ((flags & DATE_VALID) == DATE_VALID) {
  1781. unsigned short week;
  1782. timespec_t ts_tow;
  1783. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  1784. ts_tow.tv_nsec += (long)getles32(buf, 4);
  1785. TS_NORM(&ts_tow);
  1786. week = (unsigned short)getles16(buf, 8);
  1787. session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
  1788. mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
  1789. }
  1790. #undef DATE_VALID
  1791. session->newdata.ecef.x = getles32(buf, 12) / 100.0;
  1792. session->newdata.ecef.y = getles32(buf, 16) / 100.0;
  1793. session->newdata.ecef.z = getles32(buf, 20) / 100.0;
  1794. session->newdata.ecef.pAcc = getleu32(buf, 24) / 100.0;
  1795. session->newdata.ecef.vx = getles32(buf, 28) / 100.0;
  1796. session->newdata.ecef.vy = getles32(buf, 32) / 100.0;
  1797. session->newdata.ecef.vz = getles32(buf, 36) / 100.0;
  1798. session->newdata.ecef.vAcc = getleu32(buf, 40) / 100.0;
  1799. mask |= ECEF_SET | VECEF_SET;
  1800. session->newdata.eps = (double)(getles32(buf, 40) / 100.0);
  1801. mask |= SPEEDERR_SET;
  1802. pdop = getleu16(buf, 44);
  1803. if (9999 > pdop) {
  1804. session->gpsdata.dop.pdop = (double)(pdop / 100.0);
  1805. mask |= DOP_SET;
  1806. }
  1807. session->gpsdata.satellites_used = (int)getub(buf, 47);
  1808. navmode = (unsigned char)getub(buf, 10);
  1809. switch (navmode) {
  1810. case UBX_MODE_TMONLY:
  1811. // Surveyed-in, better not have moved
  1812. session->newdata.mode = MODE_3D;
  1813. session->newdata.status = STATUS_TIME;
  1814. break;
  1815. case UBX_MODE_3D:
  1816. session->newdata.mode = MODE_3D;
  1817. session->newdata.status = STATUS_GPS;
  1818. break;
  1819. case UBX_MODE_2D:
  1820. session->newdata.mode = MODE_2D;
  1821. session->newdata.status = STATUS_GPS;
  1822. break;
  1823. case UBX_MODE_DR: // consider this too as 2D
  1824. session->newdata.mode = MODE_2D;
  1825. session->newdata.status = STATUS_DR;
  1826. break;
  1827. case UBX_MODE_GPSDR: // DR-aided GPS is valid 3D
  1828. session->newdata.mode = MODE_3D;
  1829. session->newdata.status = STATUS_GNSSDR;
  1830. break;
  1831. default:
  1832. session->newdata.mode = MODE_NO_FIX;
  1833. session->newdata.status = STATUS_UNK;
  1834. break;
  1835. }
  1836. if (0 != (flags & UBX_SOL_FLAG_DGPS))
  1837. session->newdata.status = STATUS_DGPS;
  1838. mask |= MODE_SET | STATUS_SET;
  1839. // older u-blox, cycle ender may be iffy
  1840. // so err o nthe side of over-reporting TPV
  1841. mask |= REPORT_IS;
  1842. GPSD_LOG(LOG_PROG, &session->context->errout,
  1843. "UBX-NAV-SOL: time=%s ecef x:%.2f y:%.2f z:%.2f track=%.2f "
  1844. "speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
  1845. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  1846. session->newdata.ecef.x,
  1847. session->newdata.ecef.y,
  1848. session->newdata.ecef.z,
  1849. session->newdata.track,
  1850. session->newdata.speed,
  1851. session->newdata.climb,
  1852. session->newdata.mode,
  1853. session->newdata.status,
  1854. session->gpsdata.satellites_used);
  1855. return mask;
  1856. }
  1857. /**
  1858. * Receiver navigation status
  1859. * UBX-NAV-STATUS Class 1, ID 3
  1860. *
  1861. * Present in Antaris to 9-series
  1862. */
  1863. static gps_mask_t
  1864. ubx_msg_nav_status(struct gps_device_t *session, unsigned char *buf,
  1865. size_t data_len)
  1866. {
  1867. uint8_t gpsFix;
  1868. uint8_t flags;
  1869. uint8_t fixStat;
  1870. uint8_t flags2;
  1871. uint32_t ttff;
  1872. uint32_t msss;
  1873. int *status = &session->newdata.status;
  1874. int *mode = &session->newdata.mode;
  1875. gps_mask_t mask = 0;
  1876. if (16 > data_len) {
  1877. GPSD_LOG(LOG_WARN, &session->context->errout,
  1878. "UBX-NAV-STATUS message, runt payload len %zd", data_len);
  1879. return 0;
  1880. }
  1881. session->driver.ubx.iTOW = getleu32(buf, 0);
  1882. gpsFix = getub(buf, 4);
  1883. flags = getub(buf, 5);
  1884. fixStat = getub(buf, 6);
  1885. flags2 = getub(buf, 7);
  1886. ttff = getleu32(buf, 8);
  1887. msss = getleu32(buf, 12);
  1888. // FIXME: how does this compare with other places ubx sets mode/status?
  1889. if (0 == (1 & flags)) {
  1890. // gpsFix not OK
  1891. *mode = MODE_NO_FIX;
  1892. *status = STATUS_UNK;
  1893. } else {
  1894. switch (gpsFix) {
  1895. case UBX_MODE_TMONLY:
  1896. // 5 - Surveyed-in, so a precise 3D.
  1897. *mode = MODE_3D;
  1898. *status = STATUS_TIME;
  1899. break;
  1900. case UBX_MODE_3D:
  1901. // 3
  1902. FALLTHROUGH
  1903. case UBX_MODE_GPSDR:
  1904. // 4
  1905. *mode = MODE_3D;
  1906. if (2 == (2 & fixStat)) {
  1907. *status = STATUS_DGPS;
  1908. } else {
  1909. // FIXME: Set DR if this is DR
  1910. *status = STATUS_GPS;
  1911. }
  1912. break;
  1913. case UBX_MODE_2D:
  1914. // 2
  1915. FALLTHROUGH
  1916. case UBX_MODE_DR: // consider this too as 2D
  1917. // 1
  1918. *mode = MODE_2D;
  1919. if (2 == (2 & fixStat)) {
  1920. *status = STATUS_DGPS;
  1921. } else {
  1922. // FIXME: Set DR if this is DR
  1923. *status = STATUS_GPS;
  1924. }
  1925. break;
  1926. case UBX_MODE_NOFIX:
  1927. // 0
  1928. FALLTHROUGH
  1929. default:
  1930. // > 5
  1931. *mode = MODE_NO_FIX;
  1932. *status = STATUS_UNK;
  1933. break;
  1934. }
  1935. }
  1936. mask |= STATUS_SET | MODE_SET;
  1937. GPSD_LOG(LOG_PROG, &session->context->errout,
  1938. "NAV-STATUS: iTOW=%lld gpsFix=%u flags=%02x fixStat=%02x flags2=%02x "
  1939. "ttff=%llu msss=%llu mode=%u status=%u\n",
  1940. (long long)session->driver.ubx.iTOW,
  1941. gpsFix,
  1942. flags,
  1943. fixStat,
  1944. flags2,
  1945. (long long unsigned)ttff,
  1946. (long long unsigned)msss,
  1947. session->newdata.mode,
  1948. session->newdata.status);
  1949. return mask;
  1950. }
  1951. /**
  1952. * Navigation time to leap second: UBX-NAV-TIMELS
  1953. *
  1954. * Sets leap_notify if leap second is < 23 hours away.
  1955. * Not in u-blox 5
  1956. */
  1957. static gps_mask_t
  1958. ubx_msg_nav_timels(struct gps_device_t *session, unsigned char *buf,
  1959. size_t data_len)
  1960. {
  1961. int version;
  1962. unsigned int flags;
  1963. int valid_curr_ls;
  1964. int valid_time_to_ls_event;
  1965. #define UBX_TIMELS_VALID_CURR_LS 0x01
  1966. #define UBX_TIMELS_VALID_TIME_LS_EVT 0x01
  1967. if (24 > data_len) {
  1968. GPSD_LOG(LOG_WARN, &session->context->errout,
  1969. "UBX-NAV-TIMELS: unexpected length %zd, expecting 24\n",
  1970. data_len);
  1971. return 0;
  1972. }
  1973. session->driver.ubx.iTOW = getleu32(buf, 0);
  1974. version = getsb(buf, 4);
  1975. // Only version 0 is defined up to ub-blox 9
  1976. flags = (unsigned int)getub(buf, 23);
  1977. GPSD_LOG(LOG_PROG, &session->context->errout,
  1978. "UBX-NAV-TIMELS: flags 0x%x message version %d\n",
  1979. flags, version);
  1980. valid_curr_ls = flags & UBX_TIMELS_VALID_CURR_LS;
  1981. valid_time_to_ls_event = flags & UBX_TIMELS_VALID_TIME_LS_EVT;
  1982. if (valid_curr_ls) {
  1983. unsigned int src_of_curr_ls = getub(buf,8);
  1984. int curr_ls = getsb(buf,9);
  1985. char *src = "Unknown";
  1986. static char *srcOfCurrLs[] = {
  1987. "firmware",
  1988. "GPS GLONASS difference",
  1989. "GPS",
  1990. "SBAS",
  1991. "BeiDou",
  1992. "Galileo",
  1993. "Aided data",
  1994. "Configured"
  1995. };
  1996. if (src_of_curr_ls < (sizeof(srcOfCurrLs) / sizeof(srcOfCurrLs[0])))
  1997. src = srcOfCurrLs[src_of_curr_ls];
  1998. GPSD_LOG(LOG_PROG, &session->context->errout,
  1999. "UBX-NAV-TIMELS: source_of_current_leapsecond=%u:%s "
  2000. "curr_ls=%d\n",
  2001. src_of_curr_ls, src,curr_ls);
  2002. session->context->leap_seconds = curr_ls;
  2003. session->context->valid |= LEAP_SECOND_VALID;
  2004. } /* Valid current leap second */
  2005. if (valid_time_to_ls_event) {
  2006. char *src = "Unknown";
  2007. unsigned int src_of_ls_change;
  2008. unsigned short dateOfLSGpsWn, dateOfLSGpsDn;
  2009. int lsChange = getsb(buf, 11);
  2010. int timeToLsEvent = getles32(buf, 12);
  2011. static char *srcOfLsChange[] = {
  2012. "No Source",
  2013. "Undefined",
  2014. "GPS",
  2015. "SBAS",
  2016. "BeiDou",
  2017. "Galileo",
  2018. "GLONASS",
  2019. };
  2020. src_of_ls_change = getub(buf,10);
  2021. if (src_of_ls_change <
  2022. (sizeof(srcOfLsChange) / sizeof(srcOfLsChange[0]))) {
  2023. src = srcOfLsChange[src_of_ls_change];
  2024. }
  2025. dateOfLSGpsWn = getles16(buf,16);
  2026. dateOfLSGpsDn = getles16(buf,18);
  2027. GPSD_LOG(LOG_PROG, &session->context->errout,
  2028. "UBX-NAV-TIMELS: source_of_leapsecond_change %u:%s "
  2029. "leapSecondChage %d timeToLsEvent %d\n",
  2030. src_of_ls_change,src,lsChange,timeToLsEvent);
  2031. GPSD_LOG(LOG_PROG, &session->context->errout,
  2032. "UBX-NAV-TIMELS: dateOfLSGpsWn=%d dateOfLSGpsDn=%d\n",
  2033. dateOfLSGpsWn,dateOfLSGpsDn);
  2034. if ((0 != lsChange) && (0 < timeToLsEvent) &&
  2035. ((60 * 60 * 23) > timeToLsEvent)) {
  2036. if (1 == lsChange) {
  2037. session->context->leap_notify = LEAP_ADDSECOND;
  2038. GPSD_LOG(LOG_INF, &session->context->errout,
  2039. "UBX-NAV-TIMELS: Positive leap second today\n");
  2040. } else if (-1 == lsChange) {
  2041. session->context->leap_notify = LEAP_DELSECOND;
  2042. GPSD_LOG(LOG_INF, &session->context->errout,
  2043. "UBX-NAV-TIMELS: Negative leap second today\n");
  2044. }
  2045. } else {
  2046. session->context->leap_notify = LEAP_NOWARNING;
  2047. GPSD_LOG(LOG_PROG, &session->context->errout,
  2048. "UBX-NAV-TIMELS: leap_notify %d, none today\n",
  2049. session->context->leap_notify);
  2050. }
  2051. }
  2052. return 0;
  2053. }
  2054. /**
  2055. * Geodetic position solution message
  2056. * UBX-NAV-POSLLH, Class 1, ID 2
  2057. *
  2058. * This message does not bother to tell us if it is valid.
  2059. * No mode, so limited usefulness
  2060. */
  2061. static gps_mask_t
  2062. ubx_msg_nav_posllh(struct gps_device_t *session, unsigned char *buf,
  2063. size_t data_len UNUSED)
  2064. {
  2065. gps_mask_t mask = 0;
  2066. if (28 > data_len) {
  2067. GPSD_LOG(LOG_WARN, &session->context->errout,
  2068. "UBX-NAV-POSLLH message, runt payload len %zd", data_len);
  2069. return 0;
  2070. }
  2071. session->driver.ubx.iTOW = getleu32(buf, 0);
  2072. session->newdata.longitude = 1e-7 * getles32(buf, 4);
  2073. session->newdata.latitude = 1e-7 * getles32(buf, 8);
  2074. /* altitude WGS84 */
  2075. session->newdata.altHAE = 1e-3 * getles32(buf, 12);
  2076. /* altitude MSL */
  2077. session->newdata.altMSL = 1e-3 * getles32(buf, 16);
  2078. /* Let gpsd_error_model() deal with geoid_sep */
  2079. /* Horizontal accuracy estimate in mm, unknown type */
  2080. session->newdata.eph = getleu32(buf, 20) * 1e-3;
  2081. /* Vertical accuracy estimate in mm, unknown type */
  2082. session->newdata.epv = getleu32(buf, 24) * 1e-3;
  2083. GPSD_LOG(LOG_PROG, &session->context->errout,
  2084. "UBX-NAV-POSLLH: iTOW=%lld lat=%.3f lon=%.3f altHAE=%.3f "
  2085. "eph %.3f epv %.3f\n",
  2086. (long long)session->driver.ubx.iTOW,
  2087. session->newdata.latitude,
  2088. session->newdata.longitude,
  2089. session->newdata.altHAE,
  2090. session->newdata.eph,
  2091. session->newdata.epv);
  2092. mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET;
  2093. return mask;
  2094. }
  2095. /**
  2096. * Clock Solution
  2097. *
  2098. * Present in u-blox 7
  2099. */
  2100. static gps_mask_t
  2101. ubx_msg_nav_clock(struct gps_device_t *session, unsigned char *buf,
  2102. size_t data_len)
  2103. {
  2104. long clkB, clkD;
  2105. unsigned long tAcc, fAcc;
  2106. if (20 > data_len) {
  2107. GPSD_LOG(LOG_WARN, &session->context->errout,
  2108. "UBX-NAV-CLOCK message, runt payload len %zd", data_len);
  2109. return 0;
  2110. }
  2111. session->driver.ubx.iTOW = getleu32(buf, 0);
  2112. clkB = getles32(buf, 4);
  2113. clkD = getles32(buf, 8);
  2114. tAcc = getleu32(buf, 12);
  2115. fAcc = getleu32(buf, 16);
  2116. GPSD_LOG(LOG_PROG, &session->context->errout,
  2117. "NAV-CLOCK: iTOW=%lld clkB %ld clkD %ld tAcc %lu fAcc %lu\n",
  2118. (long long)session->driver.ubx.iTOW, clkB, clkD, tAcc, fAcc);
  2119. return 0;
  2120. }
  2121. /**
  2122. * DGPS Data Used for NAV
  2123. *
  2124. * May be good cycle ender
  2125. *
  2126. * Present in u-blox 7
  2127. */
  2128. static gps_mask_t ubx_msg_nav_dgps(struct gps_device_t *session,
  2129. unsigned char *buf, size_t data_len)
  2130. {
  2131. long age;
  2132. if (16 > data_len) {
  2133. GPSD_LOG(LOG_WARN, &session->context->errout,
  2134. "UBX-NAV-DGPS message, runt payload len %zd", data_len);
  2135. return 0;
  2136. }
  2137. session->driver.ubx.iTOW = getleu32(buf, 0);
  2138. age = getleu32(buf, 4);
  2139. GPSD_LOG(LOG_PROG, &session->context->errout,
  2140. "NAV-DGPS: iTOW=%lld age %ld\n",
  2141. (long long)session->driver.ubx.iTOW, age);
  2142. return 0;
  2143. }
  2144. /**
  2145. * Dilution of precision message
  2146. */
  2147. static gps_mask_t
  2148. ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf,
  2149. size_t data_len)
  2150. {
  2151. unsigned u;
  2152. gps_mask_t mask = 0;
  2153. if (18 > data_len) {
  2154. GPSD_LOG(LOG_WARN, &session->context->errout,
  2155. "UBX-NAV-DOP message, runt payload len %zd", data_len);
  2156. return 0;
  2157. }
  2158. session->driver.ubx.iTOW = getleu32(buf, 0);
  2159. /*
  2160. * We make a deliberate choice not to clear DOPs from the
  2161. * last skyview here, but rather to treat this as a supplement
  2162. * to our calculations from the visibility matrix, trusting
  2163. * the firmware algorithms over ours.
  2164. */
  2165. u = getleu16(buf, 4);
  2166. if (9999 > u) {
  2167. session->gpsdata.dop.gdop = (double)(u / 100.0);
  2168. mask |= DOP_SET;
  2169. }
  2170. u = getleu16(buf, 6);
  2171. if (9999 > u) {
  2172. session->gpsdata.dop.pdop = (double)(u / 100.0);
  2173. mask |= DOP_SET;
  2174. }
  2175. u = getleu16(buf, 8);
  2176. if (9999 > u) {
  2177. session->gpsdata.dop.tdop = (double)(u / 100.0);
  2178. mask |= DOP_SET;
  2179. }
  2180. u = getleu16(buf, 10);
  2181. if (9999 > u) {
  2182. session->gpsdata.dop.vdop = (double)(u / 100.0);
  2183. mask |= DOP_SET;
  2184. }
  2185. u = getleu16(buf, 12);
  2186. if (9999 > u) {
  2187. session->gpsdata.dop.hdop = (double)(u / 100.0);
  2188. mask |= DOP_SET;
  2189. }
  2190. GPSD_LOG(LOG_PROG, &session->context->errout,
  2191. "NAV-DOP: gdop=%.2f pdop=%.2f "
  2192. "hdop=%.2f vdop=%.2f tdop=%.2f mask={DOP}\n",
  2193. session->gpsdata.dop.gdop,
  2194. session->gpsdata.dop.hdop,
  2195. session->gpsdata.dop.vdop,
  2196. session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);
  2197. return mask;
  2198. }
  2199. /**
  2200. * Position error ellipse parameters
  2201. * protVer 19.1 and up
  2202. * Not in u-blox 5, 6 or 7
  2203. * Present in some u-blox 8, 9 and 10 (ADR, HPS)
  2204. */
  2205. static gps_mask_t
  2206. ubx_msg_nav_eell(struct gps_device_t *session, unsigned char *buf,
  2207. size_t data_len)
  2208. {
  2209. unsigned version;
  2210. unsigned errEllipseOrient;
  2211. unsigned long errEllipseMajor, errEllipseMinor;
  2212. if (16 > data_len) {
  2213. GPSD_LOG(LOG_WARN, &session->context->errout,
  2214. "UBX-NAV-EELL message, runt payload len %zd", data_len);
  2215. return 0;
  2216. }
  2217. if (18 > session->driver.ubx.protver) {
  2218. /* this GPS is at least protver 18 */
  2219. session->driver.ubx.protver = 18;
  2220. }
  2221. session->driver.ubx.iTOW = getleu32(buf, 0);
  2222. version = getub(buf, 4);
  2223. errEllipseOrient = getleu16(buf, 6);
  2224. errEllipseMajor = getleu32(buf, 8);
  2225. errEllipseMinor = getleu32(buf, 12);
  2226. GPSD_LOG(LOG_PROG, &session->context->errout,
  2227. "UBX-NAV-EELL: iTOW %lld version %u errEllipseOrient %u "
  2228. "errEllipseMajor %lu errEllipseMinor %lu\n",
  2229. (long long)session->driver.ubx.iTOW, version, errEllipseOrient,
  2230. errEllipseMajor, errEllipseMinor);
  2231. return 0;
  2232. }
  2233. /**
  2234. * End of Epoch
  2235. * Not in u-blox 5, 6 or 7
  2236. * Present in u-blox 8 and 9
  2237. */
  2238. static gps_mask_t
  2239. ubx_msg_nav_eoe(struct gps_device_t *session, unsigned char *buf,
  2240. size_t data_len)
  2241. {
  2242. if (4 > data_len) {
  2243. GPSD_LOG(LOG_WARN, &session->context->errout,
  2244. "UBX-NAV-EOE message, runt payload len %zd", data_len);
  2245. return 0;
  2246. }
  2247. if (18 > session->driver.ubx.protver) {
  2248. /* this GPS is at least protver 18 */
  2249. session->driver.ubx.protver = 18;
  2250. }
  2251. session->driver.ubx.iTOW = getleu32(buf, 0);
  2252. GPSD_LOG(LOG_PROG, &session->context->errout, "NAV-EOE: iTOW=%lld\n",
  2253. (long long)session->driver.ubx.iTOW);
  2254. // nothing to report, but the iTOW for cycle ender is good
  2255. return 0;
  2256. }
  2257. /**
  2258. * GPS Leap Seconds - UBX-NAV-TIMEGPS
  2259. */
  2260. static gps_mask_t
  2261. ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf,
  2262. size_t data_len)
  2263. {
  2264. uint8_t valid; /* Validity Flags */
  2265. gps_mask_t mask = 0;
  2266. char ts_buf[TIMESPEC_LEN];
  2267. if (16 > data_len) {
  2268. GPSD_LOG(LOG_WARN, &session->context->errout,
  2269. "UBX-NAV-TIMEGPS message, runt payload len %zd", data_len);
  2270. return 0;
  2271. }
  2272. session->driver.ubx.iTOW = getleu32(buf, 0);
  2273. valid = getub(buf, 11);
  2274. // Valid leap seconds ?
  2275. if ((valid & UBX_TIMEGPS_VALID_LEAP_SECOND) ==
  2276. UBX_TIMEGPS_VALID_LEAP_SECOND) {
  2277. session->context->leap_seconds = (int)getub(buf, 10);
  2278. session->context->valid |= LEAP_SECOND_VALID;
  2279. }
  2280. // Valid GPS time of week and week number
  2281. #define VALID_TIME (UBX_TIMEGPS_VALID_TIME | UBX_TIMEGPS_VALID_WEEK)
  2282. if ((valid & VALID_TIME) == VALID_TIME) {
  2283. #undef VALID_TIME
  2284. uint16_t week;
  2285. double tAcc; /* Time Accuracy Estimate in ns */
  2286. timespec_t ts_tow;
  2287. week = getles16(buf, 8);
  2288. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  2289. ts_tow.tv_nsec += (long)getles32(buf, 4);
  2290. TS_NORM(&ts_tow);
  2291. session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
  2292. tAcc = (double)getleu32(buf, 12); // tAcc in ns
  2293. session->newdata.ept = tAcc * 1e-9;
  2294. mask |= (TIME_SET | NTPTIME_IS);
  2295. }
  2296. GPSD_LOG(LOG_PROG, &session->context->errout,
  2297. "TIMEGPS: time=%s mask={TIME}\n",
  2298. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)));
  2299. return mask;
  2300. }
  2301. /**
  2302. * UBX-NAV-TIMEUTC
  2303. */
  2304. static gps_mask_t
  2305. ubx_msg_nav_timeutc(struct gps_device_t *session, unsigned char *buf,
  2306. size_t data_len)
  2307. {
  2308. uint8_t valid; // Validity Flags
  2309. gps_mask_t mask = 0;
  2310. if (20 > data_len) {
  2311. GPSD_LOG(LOG_WARN, &session->context->errout,
  2312. "UBX-NAV-TIMEUTC message, runt payload len %zd", data_len);
  2313. return 0;
  2314. }
  2315. session->driver.ubx.iTOW = getleu32(buf, 0);
  2316. valid = getub(buf, 19);
  2317. if (4 == (4 & valid)) {
  2318. // UTC is valid
  2319. struct tm date = {0};
  2320. // mask |= (TIME_SET | NTPTIME_IS);
  2321. uint32_t tAcc = getleu32(buf, 4); // tAcc in ns
  2322. // nano can be negative, so this is not normalized UTC.
  2323. int32_t nano = getles32(buf, 8); // fract sec in ns
  2324. date.tm_year = getleu16(buf, 12) - 1900; // year, 1999..2099
  2325. date.tm_mon = getub(buf, 14) - 1; // month 1..12
  2326. date.tm_mday = getub(buf, 15); // day 1..31
  2327. date.tm_hour = getub(buf, 16); // hour 0..23
  2328. date.tm_min = getub(buf, 17); // min 0..59
  2329. date.tm_sec = getub(buf, 18); // sec 0..60
  2330. session->newdata.time.tv_sec = mkgmtime(&date);
  2331. session->newdata.time.tv_nsec = nano;
  2332. // nano, can be negative! So normalize
  2333. TS_NORM(&session->newdata.time);
  2334. // other timestamped messages lack nano, so time will jump around...
  2335. mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
  2336. GPSD_LOG(LOG_PROG, &session->context->errout,
  2337. "UBX-NAV-TIMEUTC: iTOW=%lld valid=%02x %04d-%02d-%02d "
  2338. "%02d:%02d:%02d.%09d tAcc=%llu time %lld.%09lld\n",
  2339. (long long)session->driver.ubx.iTOW,
  2340. valid, date.tm_year + 1900, date.tm_mon + 1, date.tm_mday,
  2341. date.tm_hour, date.tm_min, date.tm_sec, nano,
  2342. (long long unsigned)tAcc,
  2343. (long long)session->newdata.time.tv_sec,
  2344. (long long)session->newdata.time.tv_nsec);
  2345. } else {
  2346. GPSD_LOG(LOG_PROG, &session->context->errout,
  2347. "TIMEUTC: iTOW=%lld valid=%02x\n",
  2348. (long long)session->driver.ubx.iTOW,
  2349. valid);
  2350. }
  2351. return mask;
  2352. }
  2353. /**
  2354. * GPS Satellite Info -- new style UBX-NAV-SAT
  2355. * Not in u-blox 5
  2356. * Present in u-blox 8, protocol version 15+
  2357. */
  2358. static gps_mask_t
  2359. ubx_msg_nav_sat(struct gps_device_t *session, unsigned char *buf,
  2360. size_t data_len)
  2361. {
  2362. unsigned int i, nchan, nsv, st, ver;
  2363. timespec_t ts_tow;
  2364. if (8 > data_len) {
  2365. GPSD_LOG(LOG_PROG, &session->context->errout,
  2366. "UBX-NAV-SAT runt datalen %zd\n", data_len);
  2367. return 0;
  2368. }
  2369. if (15 > session->driver.ubx.protver) {
  2370. /* this GPS is at least protver 15 */
  2371. session->driver.ubx.protver = 15;
  2372. }
  2373. session->driver.ubx.iTOW = getleu32(buf, 0);
  2374. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  2375. session->gpsdata.skyview_time =
  2376. gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
  2377. ver = (unsigned int)getub(buf, 4);
  2378. if (1 != ver) {
  2379. GPSD_LOG(LOG_WARN, &session->context->errout,
  2380. "NAV-SAT message unknown version %d", ver);
  2381. return 0;
  2382. }
  2383. nchan = (unsigned int)getub(buf, 5);
  2384. if (nchan > MAXCHANNELS) {
  2385. GPSD_LOG(LOG_WARN, &session->context->errout,
  2386. "UBX-NAV-SAT message, runt >%d reported visible",
  2387. MAXCHANNELS);
  2388. return 0;
  2389. }
  2390. /* two "unused" bytes at buf[6:7] */
  2391. gpsd_zero_satellites(&session->gpsdata);
  2392. nsv = 0;
  2393. for (i = st = 0; i < nchan; i++) {
  2394. unsigned int off = 8 + 12 * i;
  2395. short nmea_PRN = 0;
  2396. unsigned char gnssId = getub(buf, off + 0);
  2397. short svId = (short)getub(buf, off + 1);
  2398. unsigned char cno = getub(buf, off + 2);
  2399. /* health data in flags. */
  2400. uint32_t flags = getleu32(buf, off + 8);
  2401. bool used = (bool)(flags & 0x08);
  2402. int tmp;
  2403. /* Notice NO sigid! */
  2404. nmea_PRN = ubx2_to_prn(gnssId, svId);
  2405. #ifdef __UNUSED
  2406. // debug
  2407. GPSD_LOG(LOG_ERROR, &session->context->errout,
  2408. "NAV-SAT gnssid %d, svid %d nmea_PRN %d\n",
  2409. gnssId, svId, nmea_PRN);
  2410. #endif // __UNUSED
  2411. session->gpsdata.skyview[st].gnssid = gnssId;
  2412. session->gpsdata.skyview[st].svid = svId;
  2413. session->gpsdata.skyview[st].PRN = nmea_PRN;
  2414. session->gpsdata.skyview[st].ss = (double)cno;
  2415. tmp = getsb(buf, off + 3);
  2416. if (90 >= abs(tmp)) {
  2417. session->gpsdata.skyview[st].elevation = (double)tmp;
  2418. }
  2419. tmp = getles16(buf, off + 4);
  2420. if (359 > tmp && 0 <= tmp) {
  2421. session->gpsdata.skyview[st].azimuth = (double)tmp;
  2422. }
  2423. session->gpsdata.skyview[st].used = used;
  2424. /* by some coincidence, our health flags matches u-blox's */
  2425. session->gpsdata.skyview[st].health = (flags >> 4) & 3;
  2426. /* sbas_in_use is not same as used */
  2427. if (used) {
  2428. nsv++;
  2429. session->gpsdata.skyview[st].used = true;
  2430. }
  2431. st++;
  2432. }
  2433. session->gpsdata.satellites_visible = (int)st;
  2434. session->gpsdata.satellites_used = (int)nsv;
  2435. GPSD_LOG(LOG_PROG, &session->context->errout,
  2436. "SAT: visible=%d used=%d mask={SATELLITE|USED}\n",
  2437. session->gpsdata.satellites_visible,
  2438. session->gpsdata.satellites_used);
  2439. return SATELLITE_SET | USED_IS;
  2440. }
  2441. /**
  2442. * GPS Satellite Info -- deprecated - UBX-NAV-SVINFO
  2443. * Not in u-blox 9 or 10, use UBX-NAV-SAT instead
  2444. */
  2445. static gps_mask_t
  2446. ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf,
  2447. size_t data_len)
  2448. {
  2449. unsigned int i, nchan, nsv, st;
  2450. timespec_t ts_tow;
  2451. if (8 > data_len) {
  2452. GPSD_LOG(LOG_PROG, &session->context->errout,
  2453. "UBX-NAV-SVINFO runt datalen %zd\n", data_len);
  2454. return 0;
  2455. }
  2456. session->driver.ubx.iTOW = getleu32(buf, 0);
  2457. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  2458. session->gpsdata.skyview_time =
  2459. gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
  2460. nchan = (unsigned int)getub(buf, 4);
  2461. if (nchan > MAXCHANNELS) {
  2462. GPSD_LOG(LOG_WARN, &session->context->errout,
  2463. "UBX-NAV SVINFO message, runt >%d reported visible",
  2464. MAXCHANNELS);
  2465. return 0;
  2466. }
  2467. gpsd_zero_satellites(&session->gpsdata);
  2468. nsv = 0;
  2469. for (i = st = 0; i < nchan; i++) {
  2470. unsigned int off = 8 + 12 * i;
  2471. short nmea_PRN;
  2472. short ubx_PRN = (short)getub(buf, off + 1);
  2473. unsigned char snr = getub(buf, off + 4);
  2474. bool used = (bool)(getub(buf, off + 2) & 0x01);
  2475. unsigned char flags = getub(buf, off + 12) & 3;
  2476. int tmp;
  2477. nmea_PRN = ubx_to_prn(ubx_PRN,
  2478. &session->gpsdata.skyview[st].gnssid,
  2479. &session->gpsdata.skyview[st].svid);
  2480. #ifdef __UNUSED
  2481. // debug
  2482. GPSD_LOG(LOG_ERROR, &session->context->errout,
  2483. "NAV-SVINFO ubx_prn %d gnssid %d, svid %d nmea_PRN %d\n",
  2484. ubx_PRN,
  2485. session->gpsdata.skyview[st].gnssid,
  2486. session->gpsdata.skyview[st].svid, nmea_PRN);
  2487. #endif // __UNUSED
  2488. if (1 > nmea_PRN) {
  2489. // skip bad PRN
  2490. continue;
  2491. }
  2492. session->gpsdata.skyview[st].PRN = nmea_PRN;
  2493. session->gpsdata.skyview[st].ss = (double)snr;
  2494. tmp = getsb(buf, off + 5);
  2495. if (90 >= abs(tmp)) {
  2496. session->gpsdata.skyview[st].elevation = (double)tmp;
  2497. }
  2498. tmp = (double)getles16(buf, off + 6);
  2499. if (359 > tmp && 0 <= tmp) {
  2500. session->gpsdata.skyview[st].azimuth = (double)tmp;
  2501. }
  2502. session->gpsdata.skyview[st].used = used;
  2503. if (0x10 & flags) {
  2504. session->gpsdata.skyview[st].health = SAT_HEALTH_BAD;
  2505. } else {
  2506. session->gpsdata.skyview[st].health = SAT_HEALTH_OK;
  2507. }
  2508. /* sbas_in_use is not same as used */
  2509. if (used) {
  2510. /* not really 'used', just integrity data from there */
  2511. nsv++;
  2512. session->gpsdata.skyview[st].used = true;
  2513. }
  2514. st++;
  2515. }
  2516. session->gpsdata.satellites_visible = (int)st;
  2517. session->gpsdata.satellites_used = (int)nsv;
  2518. GPSD_LOG(LOG_PROG, &session->context->errout,
  2519. "SVINFO: visible=%d used=%d mask={SATELLITE|USED}\n",
  2520. session->gpsdata.satellites_visible,
  2521. session->gpsdata.satellites_used);
  2522. return SATELLITE_SET | USED_IS;
  2523. }
  2524. /*
  2525. * Velocity Position ECEF message, UBX-NAV-VELECEF
  2526. */
  2527. static gps_mask_t
  2528. ubx_msg_nav_velecef(struct gps_device_t *session, unsigned char *buf,
  2529. size_t data_len)
  2530. {
  2531. gps_mask_t mask = VECEF_SET;
  2532. if (20 > data_len) {
  2533. GPSD_LOG(LOG_WARN, &session->context->errout,
  2534. "UBX-NAV-VELECEF message, runt payload len %zd", data_len);
  2535. return 0;
  2536. }
  2537. session->driver.ubx.iTOW = getleu32(buf, 0);
  2538. session->newdata.ecef.vx = getles32(buf, 4) / 100.0;
  2539. session->newdata.ecef.vy = getles32(buf, 8) / 100.0;
  2540. session->newdata.ecef.vz = getles32(buf, 12) / 100.0;
  2541. session->newdata.ecef.vAcc = getleu32(buf, 16) / 100.0;
  2542. GPSD_LOG(LOG_PROG, &session->context->errout,
  2543. "UBX-NAV-VELECEF: iTOW=%lld ECEF vx=%.2f vy=%.2f vz=%.2f vAcc=%.2f\n",
  2544. (long long)session->driver.ubx.iTOW,
  2545. session->newdata.ecef.vx,
  2546. session->newdata.ecef.vy,
  2547. session->newdata.ecef.vz,
  2548. session->newdata.ecef.vAcc);
  2549. return mask;
  2550. }
  2551. /*
  2552. * Velocity NED message, UBX-NAV-VELNED
  2553. * protocol versions 15+
  2554. */
  2555. static gps_mask_t
  2556. ubx_msg_nav_velned(struct gps_device_t *session, unsigned char *buf,
  2557. size_t data_len)
  2558. {
  2559. gps_mask_t mask = VNED_SET;
  2560. if (36 > data_len) {
  2561. GPSD_LOG(LOG_WARN, &session->context->errout,
  2562. "UBX-NAV-VELNED message, runt payload len %zd", data_len);
  2563. return 0;
  2564. }
  2565. session->driver.ubx.iTOW = getleu32(buf, 0);
  2566. session->newdata.NED.velN = getles32(buf, 4) / 100.0;
  2567. session->newdata.NED.velE = getles32(buf, 8) / 100.0;
  2568. session->newdata.NED.velD = getles32(buf, 12) / 100.0;
  2569. /* ignore speed for now */
  2570. GPSD_LOG(LOG_PROG, &session->context->errout,
  2571. "UBX-NAV-VELNED: iTOW=%lld NED velN=%.2f velE=%.2f velD=%.2f\n",
  2572. (long long)session->driver.ubx.iTOW,
  2573. session->newdata.NED.velN,
  2574. session->newdata.NED.velE,
  2575. session->newdata.NED.velD);
  2576. return mask;
  2577. }
  2578. /*
  2579. * SBAS Info UBX-NAV-SBAS
  2580. * in u-blox 4_
  2581. * in NEO-M9N
  2582. * Not in some u-blox 9
  2583. * Decode looks good, but data only goes to log.
  2584. */
  2585. static gps_mask_t
  2586. ubx_msg_nav_sbas(struct gps_device_t *session, unsigned char *buf,
  2587. size_t data_len)
  2588. {
  2589. unsigned i, cnt;
  2590. unsigned ubx_PRN;
  2591. short nmea_PRN;
  2592. unsigned char gnssid = 0;
  2593. unsigned char svid = 0;
  2594. if (12 > data_len) {
  2595. GPSD_LOG(LOG_WARN, &session->context->errout,
  2596. "UBX-NAV-SBAS message, runt payload len %zd", data_len);
  2597. return 0;
  2598. }
  2599. session->driver.ubx.iTOW = getleu32(buf, 0);
  2600. ubx_PRN = getub(buf, 4);
  2601. cnt = getub(buf, 8);
  2602. GPSD_LOG(LOG_PROG, &session->context->errout,
  2603. "UBX-NAV-SBAS iTOW %lu geo %u mode %u sys %u service x%x "
  2604. "cnt %u\n",
  2605. (unsigned long)session->driver.ubx.iTOW,
  2606. ubx_PRN, (unsigned)getub(buf, 5),
  2607. (unsigned)getub(buf, 6), (unsigned)getub(buf, 7),
  2608. cnt);
  2609. if (MAXCHANNELS < cnt) {
  2610. // too many sats for us, pacify coverity
  2611. cnt = MAXCHANNELS;
  2612. }
  2613. if (data_len < (12 + (12 * cnt))) {
  2614. // length check, pacify coverity
  2615. GPSD_LOG(LOG_WARN, &session->context->errout,
  2616. "UBX-NAV-SBAS message, bad message length %zd", data_len);
  2617. }
  2618. for (i = 0; i < cnt; i++) {
  2619. int off = 12 + (12 * i);
  2620. unsigned svID = getub(buf, off);
  2621. unsigned flags = getub(buf, off + 1);
  2622. // User Differential Range Error (udre)
  2623. unsigned udre = getub(buf, off + 2);
  2624. int svSys = getsb(buf, off + 3);
  2625. unsigned svService = getub(buf, off + 4);
  2626. int prc = getles16(buf, off + 6);
  2627. int ic = getles16(buf, off + 10);
  2628. GPSD_LOG(LOG_PROG, &session->context->errout,
  2629. "UBX-NAV-SBAS SV%3u flags x%02x udre %u svSys %2d "
  2630. "svService x%x prc %d ic %d\n",
  2631. svID, flags, udre, svSys, svService, prc, ic);
  2632. }
  2633. /* really 'in_use' depends on the sats info, EGNOS is still
  2634. * in test. In WAAS areas one might also check for the type of
  2635. * corrections indicated
  2636. */
  2637. nmea_PRN = ubx_to_prn(ubx_PRN, &gnssid, &svid);
  2638. #ifdef __UNUSED
  2639. // debug
  2640. GPSD_LOG(LOG_ERROR, &session->context->errout,
  2641. "UBX-NAV-SBAS ubx_prn %d gnssid %d, svid %d nmea_PRN %d\n",
  2642. ubx_PRN, gnssid, svid, nmea_PRN);
  2643. #endif // __UNUSED
  2644. session->driver.ubx.sbas_in_use = nmea_PRN;
  2645. return 0;
  2646. }
  2647. /*
  2648. * Multi-GNSS Raw measurement Data -- UBX-RXM-RAWX
  2649. * Not in u-blox 5, 6 or 7
  2650. * u-blox 9, message version 0 (but no version byte!)
  2651. * u-blox 9, message version 1
  2652. */
  2653. static gps_mask_t ubx_msg_rxm_rawx(struct gps_device_t *session,
  2654. const unsigned char *buf,
  2655. size_t data_len)
  2656. {
  2657. double rcvTow;
  2658. uint16_t week;
  2659. int8_t leapS;
  2660. uint8_t numMeas;
  2661. uint8_t recStat;
  2662. uint8_t version;
  2663. int i;
  2664. const char * obs_code;
  2665. timespec_t ts_tow;
  2666. if (16 > data_len) {
  2667. GPSD_LOG(LOG_WARN, &session->context->errout,
  2668. "UBX-RXM-RAWX message, runt payload len %zd", data_len);
  2669. return 0;
  2670. }
  2671. /* Note: this is "approximately" GPS TOW, this is not iTOW */
  2672. rcvTow = getled64((const char *)buf, 0); /* time of week in seconds */
  2673. week = getleu16(buf, 8);
  2674. leapS = getsb(buf, 10);
  2675. numMeas = getub(buf, 11);
  2676. recStat = getub(buf, 12);
  2677. /* byte 13 is version on u-blox 9, reserved on u-blox 8
  2678. * how is that supposed to work?? */
  2679. version = getub(buf, 13);
  2680. GPSD_LOG(LOG_PROG, &session->context->errout,
  2681. "UBX-RXM-RAWX: rcvTow %f week %u leapS %d numMeas %u recStat %d"
  2682. " version %u\n",
  2683. rcvTow, week, leapS, numMeas, recStat, version);
  2684. if (recStat & 1) {
  2685. /* Valid leap seconds */
  2686. session->context->leap_seconds = leapS;
  2687. session->context->valid |= LEAP_SECOND_VALID;
  2688. }
  2689. /* convert GPS weeks and "approximately" GPS TOW to UTC */
  2690. DTOTS(&ts_tow, rcvTow);
  2691. // Do not set newdata.time. set gpsdata.raw.mtime
  2692. session->gpsdata.raw.mtime = gpsd_gpstime_resolv(session, week, ts_tow);
  2693. /* zero the measurement data */
  2694. /* so we can tell which meas never got set */
  2695. memset(session->gpsdata.raw.meas, 0, sizeof(session->gpsdata.raw.meas));
  2696. if (numMeas > MAXCHANNELS) {
  2697. GPSD_LOG(LOG_WARN, &session->context->errout,
  2698. "UBX-RXM-RAWX message, too many measurements (%u)",
  2699. numMeas);
  2700. return 0;
  2701. }
  2702. for (i = 0; i < numMeas; i++) {
  2703. int off = 32 * i;
  2704. /* pseudorange in meters */
  2705. double prMes = getled64((const char *)buf, off + 16);
  2706. /* carrier phase in cycles */
  2707. double cpMes = getled64((const char *)buf, off + 24);
  2708. /* doppler in Hz, positive towards sat */
  2709. double doMes = getlef32((const char *)buf, off + 32);
  2710. uint8_t gnssId = getub(buf, off + 36);
  2711. uint8_t svId = getub(buf, off + 37);
  2712. // reserved in u-blox 8, sigId in u-blox 9 (version 1)
  2713. uint8_t sigId = getub(buf, off + 38);
  2714. /* GLONASS frequency slot */
  2715. uint8_t freqId = getub(buf, off + 39);
  2716. /* carrier phase locktime in ms, max 64500ms */
  2717. uint16_t locktime = getleu16(buf, off + 40);
  2718. /* carrier-to-noise density ratio dB-Hz */
  2719. uint8_t cno = getub(buf, off + 42);
  2720. uint8_t prStdev = getub(buf, off + 43) & 0x0f;
  2721. uint8_t cpStdev = getub(buf, off + 44) & 0x0f;
  2722. uint8_t doStdev = getub(buf, off + 45) & 0x0f;
  2723. /* tracking stat
  2724. * bit 0 - prMes valid
  2725. * bit 1 - cpMes valid
  2726. * bit 2 - halfCycle valid
  2727. * bit 3 - halfCycle subtracted from phase
  2728. */
  2729. uint8_t trkStat = getub(buf, off + 46);
  2730. GPSD_LOG(LOG_PROG, &session->context->errout,
  2731. "%u:%u:%u freqId %u prMes %f cpMes %f doMes %f locktime %u\n"
  2732. "cno %u prStdev %u cpStdev %u doStdev %u rtkStat %u\n",
  2733. gnssId, svId, sigId, freqId, prMes, cpMes, doMes, locktime,
  2734. cno, prStdev, cpStdev, doStdev, trkStat);
  2735. session->gpsdata.raw.meas[i].gnssid = gnssId;
  2736. session->gpsdata.raw.meas[i].sigid = sigId;
  2737. /* some of these are GUESSES as the u-blox codes do not
  2738. * match RINEX codes */
  2739. switch (gnssId) {
  2740. case 0: /* GPS */
  2741. switch (sigId) {
  2742. default:
  2743. /* let PPP figure it out */
  2744. FALLTHROUGH
  2745. case 0: /* L1C/A */
  2746. obs_code = "L1C";
  2747. break;
  2748. case 3: /* L2 CL */
  2749. obs_code = "L2C";
  2750. break;
  2751. case 4: /* L2 CM */
  2752. obs_code = "L2X";
  2753. break;
  2754. }
  2755. break;
  2756. case 1: /* SBAS */
  2757. /* sigId added on protVer 27, and SBAS gone in protVer 27
  2758. * so must be L1C/A */
  2759. svId -= 100; /* adjust for RINEX 3 svid */
  2760. obs_code = "L1C"; /* u-blox calls this L1C/A */
  2761. /* SBAS can do L5I, but the code? */
  2762. switch (sigId) {
  2763. default:
  2764. /* let PPP figure it out */
  2765. break;
  2766. case 0: /* L1C/A */
  2767. obs_code = "L1C";
  2768. break;
  2769. }
  2770. break;
  2771. case 2: /* GALILEO */
  2772. switch (sigId) {
  2773. default:
  2774. /* let PPP figure it out */
  2775. FALLTHROUGH
  2776. case 0: /* */
  2777. obs_code = "L1C"; /* u-blox calls this E1OS or E1C */
  2778. break;
  2779. case 1: /* */
  2780. obs_code = "L1B"; /* u-blox calls this E1B */
  2781. break;
  2782. case 5: /* */
  2783. obs_code = "L7I"; /* u-blox calls this E5bl */
  2784. break;
  2785. case 6: /* */
  2786. obs_code = "L7Q"; /* u-blox calls this E5bQ */
  2787. break;
  2788. }
  2789. break;
  2790. case 3: /* BeiDou */
  2791. switch (sigId) {
  2792. default:
  2793. /* let PPP figure it out */
  2794. FALLTHROUGH
  2795. case 0: /* */
  2796. obs_code = "L2Q"; /* u-blox calls this B1I D1 */
  2797. break;
  2798. case 1: /* */
  2799. obs_code = "L2I"; /* u-blox calls this B1I D2 */
  2800. break;
  2801. case 2: /* */
  2802. obs_code = "L7Q"; /* u-blox calls this B2I D1 */
  2803. break;
  2804. case 3: /* */
  2805. obs_code = "L7I"; /* u-blox calls this B2I D2 */
  2806. break;
  2807. }
  2808. break;
  2809. default: /* huh? */
  2810. case 4: /* IMES. really? */
  2811. obs_code = ""; /* u-blox calls this L1 */
  2812. break;
  2813. case 5: /* QZSS */
  2814. switch (sigId) {
  2815. default:
  2816. /* let PPP figure it out */
  2817. FALLTHROUGH
  2818. case 0: /* */
  2819. obs_code = "L1C"; /* u-blox calls this L1C/A */
  2820. break;
  2821. case 4: /* */
  2822. obs_code = "L2S"; /* u-blox calls this L2CM */
  2823. break;
  2824. case 5: /* */
  2825. obs_code = "L2L"; /* u-blox calls this L2CL*/
  2826. break;
  2827. }
  2828. break;
  2829. case 6: /* GLONASS */
  2830. switch (sigId) {
  2831. default:
  2832. /* let PPP figure it out */
  2833. FALLTHROUGH
  2834. case 0: /* */
  2835. obs_code = "L1C"; /* u-blox calls this L1OF */
  2836. break;
  2837. case 2: /* */
  2838. obs_code = "L2C"; /* u-blox calls this L2OF */
  2839. break;
  2840. }
  2841. break;
  2842. }
  2843. (void)strlcpy(session->gpsdata.raw.meas[i].obs_code, obs_code,
  2844. sizeof(session->gpsdata.raw.meas[i].obs_code));
  2845. session->gpsdata.raw.meas[i].svid = svId;
  2846. session->gpsdata.raw.meas[i].freqid = freqId;
  2847. session->gpsdata.raw.meas[i].snr = cno;
  2848. session->gpsdata.raw.meas[i].satstat = trkStat;
  2849. if (trkStat & 1) {
  2850. /* prMes valid */
  2851. session->gpsdata.raw.meas[i].pseudorange = prMes;
  2852. } else {
  2853. session->gpsdata.raw.meas[i].pseudorange = NAN;
  2854. }
  2855. if ((trkStat & 2) && (5 >= cpStdev)) {
  2856. /* cpMes valid, RTKLIB uses 5 < cpStdev */
  2857. session->gpsdata.raw.meas[i].carrierphase = cpMes;
  2858. } else {
  2859. session->gpsdata.raw.meas[i].carrierphase = NAN;
  2860. }
  2861. session->gpsdata.raw.meas[i].doppler = doMes;
  2862. session->gpsdata.raw.meas[i].codephase = NAN;
  2863. session->gpsdata.raw.meas[i].deltarange = NAN;
  2864. session->gpsdata.raw.meas[i].locktime = locktime;
  2865. if (0 == locktime) {
  2866. /* possible slip */
  2867. session->gpsdata.raw.meas[i].lli = 2;
  2868. }
  2869. }
  2870. return RAW_IS;
  2871. }
  2872. /*
  2873. * Raw Subframes - UBX-RXM-SFRB
  2874. * In u-blox 7, only in raw firmware option
  2875. * Not in u-blox 8 or 9
  2876. */
  2877. static gps_mask_t ubx_msg_rxm_sfrb(struct gps_device_t *session,
  2878. unsigned char *buf, size_t data_len)
  2879. {
  2880. unsigned int i, chan, svid;
  2881. uint32_t words[10];
  2882. if (42 > data_len) {
  2883. GPSD_LOG(LOG_WARN, &session->context->errout,
  2884. "UBX-RXM-SFRB message, runt payload len %zd", data_len);
  2885. return 0;
  2886. }
  2887. chan = (unsigned int)getub(buf, 0);
  2888. svid = (unsigned int)getub(buf, 1);
  2889. GPSD_LOG(LOG_PROG, &session->context->errout,
  2890. "UBX-RXM-SFRB: %u %u\n", chan, svid);
  2891. /* UBX does all the parity checking, but still bad data gets through */
  2892. for (i = 0; i < 10; i++) {
  2893. // bits 24 to 31 undefined, remove them.
  2894. words[i] = (uint32_t)getleu32(buf, 4 * i + 2) & 0x00ffffff;
  2895. }
  2896. // probably GPS, could be SBAS
  2897. return gpsd_interpret_subframe(session, GNSSID_GPS, svid, words);
  2898. }
  2899. /*
  2900. * Raw Subframes - UBX-RXM-SFRBX
  2901. * in u-blox 8, protver 17 and up, time sync firmware only
  2902. * in u-blox F9P abd HPG only
  2903. * not present before u-blox8
  2904. */
  2905. static gps_mask_t ubx_msg_rxm_sfrbx(struct gps_device_t *session,
  2906. unsigned char *buf, size_t data_len)
  2907. {
  2908. unsigned i;
  2909. uint8_t gnssId, svId, freqId, numWords, chn, version;
  2910. uint32_t words[17];
  2911. char *chn_s;
  2912. if (8 > data_len) {
  2913. GPSD_LOG(LOG_WARN, &session->context->errout,
  2914. "UBX-RXM-SFRBX message, runt payload len %zd", data_len);
  2915. return 0;
  2916. }
  2917. numWords = getub(buf, 4);
  2918. if (data_len != (size_t)(8 + (4 * numWords)) ||
  2919. 16 < numWords) {
  2920. // test numwords directly to shut up Coverity
  2921. GPSD_LOG(LOG_WARN, &session->context->errout,
  2922. "UBX-RXM-SFRBX message, wrong payload len %zd, numwords %u "
  2923. "s/b %u",
  2924. data_len, 8 + (4 * numWords), numWords);
  2925. return 0;
  2926. }
  2927. gnssId = getub(buf, 0);
  2928. svId = getub(buf, 1);
  2929. freqId = getub(buf, 2);
  2930. version = getub(buf, 6);
  2931. chn = getub(buf, 5);
  2932. if (1 < version) {
  2933. // receiver channel in version 2 and up.
  2934. // valid range 0 to 13?
  2935. chn_s = "chn";
  2936. } else {
  2937. chn_s = "reserved";
  2938. }
  2939. GPSD_LOG(LOG_PROG, &session->context->errout,
  2940. "UBX-RXM-SFRBX: version %u gnssId %u %s %u svId %u "
  2941. "freqId %u words %u\n",
  2942. version, gnssId, chn_s, chn, svId, freqId, numWords);
  2943. if (0 == version) {
  2944. // unknown ersion
  2945. return 0;
  2946. }
  2947. memset(words, 0, sizeof(words));
  2948. for (i = 0; i < numWords; i++) {
  2949. // grab the words, don't mangle them
  2950. words[i] = (uint32_t)getleu32(buf, 4 * i + 8);
  2951. }
  2952. // do we need freqId or chn?
  2953. return gpsd_interpret_subframe_raw(session, gnssId, svId, words, numWords);
  2954. }
  2955. /**
  2956. * SV Status Info
  2957. *
  2958. * May be good cycle ender
  2959. *
  2960. * Present in u-blox 7
  2961. */
  2962. static gps_mask_t
  2963. ubx_msg_rxm_svsi(struct gps_device_t *session, unsigned char *buf,
  2964. size_t data_len)
  2965. {
  2966. unsigned numVis, numSV;
  2967. if (8 > data_len) {
  2968. GPSD_LOG(LOG_WARN, &session->context->errout,
  2969. "UBX-RXM-SVSI message, runt payload len %zd", data_len);
  2970. return 0;
  2971. }
  2972. session->driver.ubx.iTOW = getleu32(buf, 0);
  2973. session->context->gps_week = getleu16(buf, 4);
  2974. numVis = getub(buf, 6);
  2975. numSV = getub(buf, 7);
  2976. GPSD_LOG(LOG_PROG, &session->context->errout,
  2977. "NAV-CLOCK: iTOW=%lld week %d numVis %u numSV %u\n",
  2978. (long long)session->driver.ubx.iTOW,
  2979. session->context->gps_week, numVis, numSV);
  2980. return 0;
  2981. }
  2982. /* UBX-INF-* */
  2983. static gps_mask_t
  2984. ubx_msg_inf(struct gps_device_t *session, unsigned char *buf, size_t data_len)
  2985. {
  2986. unsigned short msgid;
  2987. static char txtbuf[MAX_PACKET_LENGTH];
  2988. /* No minimum payload length */
  2989. msgid = (unsigned short)((buf[2] << 8) | buf[3]);
  2990. if (data_len > MAX_PACKET_LENGTH - 1)
  2991. data_len = MAX_PACKET_LENGTH - 1;
  2992. (void)strlcpy(txtbuf, (char *)buf + UBX_PREFIX_LEN, sizeof(txtbuf));
  2993. txtbuf[data_len] = '\0';
  2994. switch (msgid) {
  2995. case UBX_INF_DEBUG:
  2996. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-INF-DEBUG: %s\n",
  2997. txtbuf);
  2998. break;
  2999. case UBX_INF_TEST:
  3000. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-INF-TEST: %s\n",
  3001. txtbuf);
  3002. break;
  3003. case UBX_INF_NOTICE:
  3004. GPSD_LOG(LOG_INF, &session->context->errout, "UBX-INF-NOTICE: %s\n",
  3005. txtbuf);
  3006. break;
  3007. case UBX_INF_WARNING:
  3008. GPSD_LOG(LOG_WARN, &session->context->errout, "UBX-INF-WARNING: %s\n",
  3009. txtbuf);
  3010. break;
  3011. case UBX_INF_ERROR:
  3012. GPSD_LOG(LOG_WARN, &session->context->errout, "UBX-INF-ERROR: %s\n",
  3013. txtbuf);
  3014. break;
  3015. default:
  3016. break;
  3017. }
  3018. return 0;
  3019. }
  3020. /**
  3021. * Survey-in data - UBX-TIM-SVIN
  3022. * Time Sync products only
  3023. */
  3024. static gps_mask_t
  3025. ubx_msg_tim_svin(struct gps_device_t *session, unsigned char *buf,
  3026. size_t data_len)
  3027. {
  3028. gps_mask_t mask = ONLINE_SET;
  3029. uint32_t dur;
  3030. int32_t meanX;
  3031. int32_t meanY;
  3032. int32_t meanZ;
  3033. uint32_t meanV;
  3034. uint32_t obs;
  3035. uint8_t valid;
  3036. uint8_t active;
  3037. if (28 > data_len) {
  3038. GPSD_LOG(LOG_WARN, &session->context->errout,
  3039. "UBX-TIM-SVIN message, runt payload len %zd", data_len);
  3040. return 0;
  3041. }
  3042. dur = getleu32(buf, 0);
  3043. meanX = getles32(buf, 4);
  3044. meanY = getles32(buf, 8);
  3045. meanZ = getles32(buf, 12);
  3046. meanV = getleu32(buf, 16);
  3047. obs = getleu32(buf, 20);
  3048. valid = getub(buf, 24);
  3049. active = getub(buf, 25);
  3050. // two reserved bytes
  3051. /* casts for 32 bit compatibility */
  3052. GPSD_LOG(LOG_PROG, &session->context->errout,
  3053. "TIM-SVIN: dur=%lu meanX=%ld meanY=%ld meanZ=%ld meanV=%lu "
  3054. "obs=%lu valid=%u active=%u\n",
  3055. (unsigned long)dur, (long)meanX, (long)meanY, (long)meanZ,
  3056. (long)meanV, (unsigned long)obs, valid, active);
  3057. return mask;
  3058. }
  3059. /**
  3060. * Time Pulse Timedata - UBX-TIM-TP
  3061. */
  3062. static gps_mask_t
  3063. ubx_msg_tim_tp(struct gps_device_t *session, unsigned char *buf,
  3064. size_t data_len)
  3065. {
  3066. gps_mask_t mask = ONLINE_SET;
  3067. uint32_t towMS;
  3068. uint32_t towSubMS;
  3069. int32_t qErr;
  3070. uint16_t week;
  3071. uint8_t flags;
  3072. uint8_t refInfo;
  3073. timespec_t ts_tow;
  3074. if (16 > data_len) {
  3075. GPSD_LOG(LOG_WARN, &session->context->errout,
  3076. "UBX-TIM-TP message, runt payload len %zd", data_len);
  3077. return 0;
  3078. }
  3079. towMS = getleu32(buf, 0);
  3080. // towSubMS always seems zero, which will match the PPS
  3081. towSubMS = getleu32(buf, 4);
  3082. qErr = getles32(buf, 8);
  3083. week = getleu16(buf, 12);
  3084. flags = buf[14];
  3085. refInfo = buf[15];
  3086. /* are we UTC, and towSubMs is zero? */
  3087. if (3 == (flags & 0x03) &&
  3088. 0 == towSubMS) {
  3089. // leap already added!?!?
  3090. int saved_leap = session->context->leap_seconds;
  3091. // remove it!
  3092. session->context->leap_seconds = 0;
  3093. /* good, save qErr and qErr_time */
  3094. session->gpsdata.qErr = qErr;
  3095. MSTOTS(&ts_tow, towMS);
  3096. session->gpsdata.qErr_time = gpsd_gpstime_resolv(session, week, ts_tow);
  3097. // restore leap
  3098. session->context->leap_seconds = saved_leap;
  3099. #ifdef __UNUSED
  3100. {
  3101. struct gps_device_t *ppsonly;
  3102. // FIXME!! should be up a layer so other drivers can use it
  3103. // FIXME!! this qErr can only apply to one PPS!
  3104. /* propagate this in-band-time to all PPS-only devices */
  3105. for (ppsonly = devices; ppsonly < devices + MAX_DEVICES; ppsonly++)
  3106. if (SOURCE_PPS == ppsonly->sourcetype) {
  3107. pps_thread_qErrin(&ppsonly->pps_thread, qErr,
  3108. session->gpsdata.qErr_time);
  3109. }
  3110. }
  3111. #endif // __UNUSED
  3112. }
  3113. /* cast for 32 bit compatibility */
  3114. GPSD_LOG(LOG_PROG, &session->context->errout,
  3115. "TIM-TP: towMS %lu, towSubMS %lu, qErr %ld week %u "
  3116. "flags %#x, refInfo %#x\n",
  3117. (unsigned long)towMS, (unsigned long)towSubMS, (long)qErr,
  3118. week, flags, refInfo);
  3119. return mask;
  3120. }
  3121. gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
  3122. size_t len)
  3123. {
  3124. size_t data_len;
  3125. unsigned short msgid;
  3126. gps_mask_t mask = 0;
  3127. // the packet at least contains a head long enough for an empty message
  3128. if (UBX_PREFIX_LEN > len) {
  3129. return 0;
  3130. }
  3131. session->cycle_end_reliable = true;
  3132. session->driver.ubx.iTOW = -1; // set by decoder
  3133. // extract message id and length
  3134. msgid = (buf[2] << 8) | buf[3];
  3135. data_len = (size_t) getles16(buf, 4);
  3136. switch (msgid) {
  3137. case UBX_ACK_ACK:
  3138. if (2 <= data_len) {
  3139. GPSD_LOG(LOG_PROG, &session->context->errout,
  3140. "UBX-ACK-ACK, class: %02x, id: %02x\n",
  3141. buf[UBX_PREFIX_LEN],
  3142. buf[UBX_PREFIX_LEN + 1]);
  3143. }
  3144. break;
  3145. case UBX_ACK_NAK:
  3146. if (2 <= data_len) {
  3147. GPSD_LOG(LOG_WARN, &session->context->errout,
  3148. "UBX-ACK-NAK, class: %02x, id: %02x\n",
  3149. buf[UBX_PREFIX_LEN],
  3150. buf[UBX_PREFIX_LEN + 1]);
  3151. }
  3152. break;
  3153. case UBX_CFG_NAV5:
  3154. // deprecated in u-blox 10
  3155. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-CFG-NAV5\n");
  3156. break;
  3157. case UBX_CFG_NAVX5:
  3158. // deprecated in u-blox 10
  3159. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-CFG-NAVX5\n");
  3160. break;
  3161. case UBX_CFG_PRT:
  3162. // deprecated in u-blox 10
  3163. if (session->driver.ubx.port_id != buf[UBX_PREFIX_LEN + 0] ) {
  3164. session->driver.ubx.port_id = buf[UBX_PREFIX_LEN + 0];
  3165. GPSD_LOG(LOG_INF, &session->context->errout,
  3166. "UBX-CFG-PRT: port %d\n", session->driver.ubx.port_id);
  3167. }
  3168. break;
  3169. case UBX_CFG_RATE:
  3170. // deprecated in u-blox 10
  3171. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-CFG-RATE\n");
  3172. ubx_msg_cfg_rate(session, &buf[UBX_PREFIX_LEN], data_len);
  3173. break;
  3174. case UBX_ESF_ALG:
  3175. mask = ubx_msg_esf_alg(session, &buf[UBX_PREFIX_LEN], data_len);
  3176. break;
  3177. case UBX_ESF_INS:
  3178. mask = ubx_msg_esf_ins(session, &buf[UBX_PREFIX_LEN], data_len);
  3179. break;
  3180. case UBX_ESF_MEAS:
  3181. mask = ubx_msg_esf_meas(session, &buf[UBX_PREFIX_LEN], data_len);
  3182. break;
  3183. case UBX_ESF_RAW:
  3184. mask = ubx_msg_esf_raw(session, &buf[UBX_PREFIX_LEN], data_len);
  3185. break;
  3186. case UBX_ESF_STATUS:
  3187. mask = ubx_msg_esf_status(session, &buf[UBX_PREFIX_LEN], data_len);
  3188. break;
  3189. case UBX_HNR_ATT:
  3190. mask = ubx_msg_hnr_att(session, &buf[UBX_PREFIX_LEN], data_len);
  3191. break;
  3192. case UBX_HNR_INS:
  3193. mask = ubx_msg_hnr_ins(session, &buf[UBX_PREFIX_LEN], data_len);
  3194. break;
  3195. case UBX_HNR_PVT:
  3196. mask = ubx_msg_hnr_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
  3197. break;
  3198. case UBX_INF_DEBUG:
  3199. FALLTHROUGH
  3200. case UBX_INF_ERROR:
  3201. FALLTHROUGH
  3202. case UBX_INF_NOTICE:
  3203. FALLTHROUGH
  3204. case UBX_INF_TEST:
  3205. FALLTHROUGH
  3206. case UBX_INF_USER:
  3207. FALLTHROUGH
  3208. case UBX_INF_WARNING:
  3209. mask = ubx_msg_inf(session, buf, data_len);
  3210. break;
  3211. case UBX_LOG_BATCH:
  3212. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-LOG-BATCH\n");
  3213. mask = ubx_msg_log_batch(session, &buf[UBX_PREFIX_LEN], data_len);
  3214. break;
  3215. case UBX_LOG_INFO:
  3216. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-LOG-INFO\n");
  3217. mask = ubx_msg_log_info(session, &buf[UBX_PREFIX_LEN], data_len);
  3218. break;
  3219. case UBX_LOG_RETRIEVEPOS:
  3220. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-LOG-RETRIEVEPOS\n");
  3221. mask = ubx_msg_log_retrievepos(session, &buf[UBX_PREFIX_LEN], data_len);
  3222. break;
  3223. case UBX_LOG_RETRIEVEPOSEXTRA:
  3224. GPSD_LOG(LOG_PROG, &session->context->errout,
  3225. "UBX-LOG-RETRIEVEPOSEXTRA\n");
  3226. mask = ubx_msg_log_retrieveposextra(session, &buf[UBX_PREFIX_LEN],
  3227. data_len);
  3228. break;
  3229. case UBX_LOG_RETRIEVESTRING:
  3230. GPSD_LOG(LOG_PROG, &session->context->errout,
  3231. "UBX-LOG-RETRIEVESTRING\n");
  3232. mask = ubx_msg_log_retrievestring(session, &buf[UBX_PREFIX_LEN],
  3233. data_len);
  3234. break;
  3235. case UBX_MON_BATCH:
  3236. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-BATCH\n");
  3237. break;
  3238. case UBX_MON_EXCEPT:
  3239. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-EXCEPT\n");
  3240. break;
  3241. case UBX_MON_GNSS:
  3242. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-GNSS\n");
  3243. break;
  3244. case UBX_MON_HW:
  3245. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-HW\n");
  3246. break;
  3247. case UBX_MON_HW2:
  3248. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-HW2\n");
  3249. break;
  3250. case UBX_MON_HW3:
  3251. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-HW3\n");
  3252. break;
  3253. case UBX_MON_IO:
  3254. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-IO\n");
  3255. break;
  3256. case UBX_MON_IPC:
  3257. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-IPC\n");
  3258. break;
  3259. case UBX_MON_MSGPP:
  3260. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-MSGPP\n");
  3261. break;
  3262. case UBX_MON_PATCH:
  3263. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-PATCH\n");
  3264. break;
  3265. case UBX_MON_RF:
  3266. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-RF\n");
  3267. break;
  3268. case UBX_MON_RXBUF:
  3269. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-RXBUF\n");
  3270. ubx_msg_mon_rxbuf(session, &buf[UBX_PREFIX_LEN], data_len);
  3271. break;
  3272. case UBX_MON_RXR:
  3273. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-RXR\n");
  3274. break;
  3275. case UBX_MON_SCHED:
  3276. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-SCHED\n");
  3277. break;
  3278. case UBX_MON_SMGR:
  3279. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-SMGR\n");
  3280. break;
  3281. case UBX_MON_SPAN:
  3282. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-SPAN\n");
  3283. break;
  3284. case UBX_MON_TXBUF:
  3285. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-TXBUF\n");
  3286. ubx_msg_mon_txbuf(session, &buf[UBX_PREFIX_LEN], data_len);
  3287. break;
  3288. case UBX_MON_USB:
  3289. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-USB\n");
  3290. break;
  3291. case UBX_MON_VER:
  3292. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MON-VER\n");
  3293. mask = ubx_msg_mon_ver(session, &buf[UBX_PREFIX_LEN], data_len);
  3294. break;
  3295. case UBX_NAV_AOPSTATUS:
  3296. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-AOPSTATUS\n");
  3297. break;
  3298. case UBX_NAV_ATT:
  3299. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-ATT\n");
  3300. break;
  3301. case UBX_NAV_CLOCK:
  3302. mask = ubx_msg_nav_clock(session, &buf[UBX_PREFIX_LEN], data_len);
  3303. break;
  3304. case UBX_NAV_DGPS:
  3305. mask = ubx_msg_nav_dgps(session, &buf[UBX_PREFIX_LEN], data_len);
  3306. break;
  3307. case UBX_NAV_DOP:
  3308. // DOP seems to be the last NAV sent in a cycle, unless NAV-EOE
  3309. mask = ubx_msg_nav_dop(session, &buf[UBX_PREFIX_LEN], data_len);
  3310. break;
  3311. case UBX_NAV_EELL:
  3312. mask = ubx_msg_nav_eell(session, &buf[UBX_PREFIX_LEN], data_len);
  3313. break;
  3314. case UBX_NAV_EKFSTATUS:
  3315. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-EKFSTATUS\n");
  3316. break;
  3317. case UBX_NAV_EOE:
  3318. mask = ubx_msg_nav_eoe(session, &buf[UBX_PREFIX_LEN], data_len);
  3319. break;
  3320. case UBX_NAV_GEOFENCE:
  3321. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-GEOFENCE\n");
  3322. break;
  3323. case UBX_NAV_HPPOSECEF:
  3324. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-HPPOSECEF\n");
  3325. mask = ubx_msg_nav_hpposecef(session, &buf[UBX_PREFIX_LEN], data_len);
  3326. break;
  3327. case UBX_NAV_HPPOSLLH:
  3328. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-HPPOSLLH\n");
  3329. mask = ubx_msg_nav_hpposllh(session, &buf[UBX_PREFIX_LEN], data_len);
  3330. break;
  3331. case UBX_NAV_ODO:
  3332. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-ODO\n");
  3333. break;
  3334. case UBX_NAV_ORB:
  3335. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-ORB\n");
  3336. break;
  3337. case UBX_NAV_POSECEF:
  3338. mask = ubx_msg_nav_posecef(session, &buf[UBX_PREFIX_LEN], data_len);
  3339. break;
  3340. case UBX_NAV_POSLLH:
  3341. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-POSLLH\n");
  3342. mask = ubx_msg_nav_posllh(session, &buf[UBX_PREFIX_LEN], data_len);
  3343. break;
  3344. case UBX_NAV_POSUTM:
  3345. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-POSUTM\n");
  3346. break;
  3347. case UBX_NAV_PVT:
  3348. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-PVT\n");
  3349. mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
  3350. break;
  3351. case UBX_NAV_RELPOSNED:
  3352. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-RELPOSNED\n");
  3353. mask = ubx_msg_nav_relposned(session, &buf[UBX_PREFIX_LEN], data_len);
  3354. break;
  3355. case UBX_NAV_RESETODO:
  3356. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-RESETODO\n");
  3357. break;
  3358. case UBX_NAV_SAT:
  3359. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SAT\n");
  3360. mask = ubx_msg_nav_sat(session, &buf[UBX_PREFIX_LEN], data_len);
  3361. break;
  3362. case UBX_NAV_SBAS:
  3363. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SBAS\n");
  3364. mask = ubx_msg_nav_sbas(session, &buf[UBX_PREFIX_LEN], data_len);
  3365. break;
  3366. case UBX_NAV_SIG:
  3367. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SIG\n");
  3368. break;
  3369. case UBX_NAV_SOL:
  3370. /* UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9 and 10.
  3371. * Use UBX-NAV-PVT instead */
  3372. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SOL\n");
  3373. mask = ubx_msg_nav_sol(session, &buf[UBX_PREFIX_LEN], data_len);
  3374. break;
  3375. case UBX_NAV_STATUS:
  3376. mask = ubx_msg_nav_status(session, &buf[UBX_PREFIX_LEN], data_len);
  3377. break;
  3378. case UBX_NAV_SVIN:
  3379. mask = ubx_msg_tim_svin(session, &buf[UBX_PREFIX_LEN], data_len);
  3380. break;
  3381. case UBX_NAV_SVINFO:
  3382. // UBX-NAV-SVINFO deprecated, use UBX-NAV-SAT instead
  3383. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SVINFO\n");
  3384. mask = ubx_msg_nav_svinfo(session, &buf[UBX_PREFIX_LEN], data_len);
  3385. /* this is a hack to move some initialization until after we
  3386. * get some u-blox message so we know the GPS is alive */
  3387. if ('\0' == session->subtype[0]) {
  3388. // one time only
  3389. (void)strlcpy(session->subtype, "Unknown", 8);
  3390. // request SW and HW Versions
  3391. (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
  3392. }
  3393. break;
  3394. case UBX_NAV_TIMEBDS:
  3395. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-TIMEBDS\n");
  3396. break;
  3397. case UBX_NAV_TIMEGAL:
  3398. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-TIMEGAL\n");
  3399. break;
  3400. case UBX_NAV_TIMEGLO:
  3401. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-TIMEGLO\n");
  3402. break;
  3403. case UBX_NAV_TIMEGPS:
  3404. mask = ubx_msg_nav_timegps(session, &buf[UBX_PREFIX_LEN], data_len);
  3405. break;
  3406. case UBX_NAV_TIMELS:
  3407. mask = ubx_msg_nav_timels(session, &buf[UBX_PREFIX_LEN], data_len);
  3408. break;
  3409. case UBX_NAV_TIMEQZSS:
  3410. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-TIMEQZSS\n");
  3411. break;
  3412. case UBX_NAV_TIMEUTC:
  3413. mask = ubx_msg_nav_timeutc(session, &buf[UBX_PREFIX_LEN], data_len);
  3414. break;
  3415. case UBX_NAV_VELECEF:
  3416. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-VELECEF\n");
  3417. mask = ubx_msg_nav_velecef(session, &buf[UBX_PREFIX_LEN], data_len);
  3418. break;
  3419. case UBX_NAV_VELNED:
  3420. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-VELNED\n");
  3421. mask = ubx_msg_nav_velned(session, &buf[UBX_PREFIX_LEN], data_len);
  3422. break;
  3423. case UBX_MGA_ACK:
  3424. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-MGA-ACK\n");
  3425. break;
  3426. case UBX_MGA_DBD:
  3427. gpsd_log(LOG_PROG, &session->context->errout, "UBX-MGA-DBD\n");
  3428. break;
  3429. case UBX_RXM_ALM:
  3430. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-ALM\n");
  3431. break;
  3432. case UBX_RXM_EPH:
  3433. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-EPH\n");
  3434. break;
  3435. case UBX_RXM_IMES:
  3436. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-IMES\n");
  3437. break;
  3438. case UBX_RXM_MEASX:
  3439. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-MEASX\n");
  3440. break;
  3441. case UBX_RXM_PMREQ:
  3442. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-PMREQ\n");
  3443. break;
  3444. case UBX_RXM_POSREQ:
  3445. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-POSREQ\n");
  3446. break;
  3447. case UBX_RXM_RAW:
  3448. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-RAW\n");
  3449. break;
  3450. case UBX_RXM_RAWX:
  3451. mask = ubx_msg_rxm_rawx(session, &buf[UBX_PREFIX_LEN], data_len);
  3452. break;
  3453. case UBX_RXM_RLM:
  3454. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-RLM\n");
  3455. break;
  3456. case UBX_RXM_RTCM:
  3457. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-RTCM\n");
  3458. break;
  3459. case UBX_RXM_SFRB:
  3460. mask = ubx_msg_rxm_sfrb(session, &buf[UBX_PREFIX_LEN], data_len);
  3461. break;
  3462. case UBX_RXM_SFRBX:
  3463. mask = ubx_msg_rxm_sfrbx(session, &buf[UBX_PREFIX_LEN], data_len);
  3464. break;
  3465. case UBX_RXM_SVSI:
  3466. // Gone in u-blox 10, use UBX-NAV-ORB instead
  3467. mask = ubx_msg_rxm_svsi(session, &buf[UBX_PREFIX_LEN], data_len);
  3468. break;
  3469. // undocumented
  3470. // case UBX_SEC_SESSID:
  3471. // GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-SEC-SESSID\n");
  3472. // break;
  3473. case UBX_SEC_SIGN:
  3474. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX_SEC_SIGN\n");
  3475. break;
  3476. case UBX_SEC_UNIQID:
  3477. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX_SEC_UNIQID\n");
  3478. break;
  3479. case UBX_TIM_DOSC:
  3480. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-DOSC\n");
  3481. break;
  3482. case UBX_TIM_FCHG:
  3483. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-FCHG\n");
  3484. break;
  3485. case UBX_TIM_HOC:
  3486. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-HOC\n");
  3487. break;
  3488. case UBX_TIM_SMEAS:
  3489. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-SMEAS\n");
  3490. break;
  3491. case UBX_TIM_SVIN:
  3492. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-SVIN\n");
  3493. break;
  3494. case UBX_TIM_TM:
  3495. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-TM\n");
  3496. break;
  3497. case UBX_TIM_TM2:
  3498. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-TM2\n");
  3499. break;
  3500. case UBX_TIM_TP:
  3501. mask = ubx_msg_tim_tp(session, &buf[UBX_PREFIX_LEN], data_len);
  3502. break;
  3503. case UBX_TIM_TOS:
  3504. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-TOS\n");
  3505. break;
  3506. case UBX_TIM_VCOCAL:
  3507. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-VCOCAL\n");
  3508. break;
  3509. case UBX_TIM_VRFY:
  3510. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-TIM-VRFY\n");
  3511. break;
  3512. default:
  3513. GPSD_LOG(LOG_WARN, &session->context->errout,
  3514. "UBX: unknown packet id x%04hx (length %zd)\n",
  3515. msgid, len);
  3516. }
  3517. #ifdef __UNUSED
  3518. // debug
  3519. GPSD_LOG(LOG_PROG, &session->context->errout,
  3520. "UBX: msgid x%04x end x%04x last x%04x iTOW %lld last %lld\n",
  3521. msgid,
  3522. session->driver.ubx.end_msgid,
  3523. session->driver.ubx.last_msgid,
  3524. (long long)session->driver.ubx.iTOW,
  3525. (long long)session->driver.ubx.last_iTOW);
  3526. #endif
  3527. // iTOW drives the cycle start/end detection
  3528. // iTOW is in ms, can go forward or backward
  3529. if (-1 < session->driver.ubx.iTOW) {
  3530. int64_t iTOW_diff;
  3531. // this sentence has a (maybe good) time
  3532. // end of cycle ?
  3533. if (session->driver.ubx.end_msgid == msgid) {
  3534. // got known cycle ender. Assume end of cycle, report it
  3535. GPSD_LOG(LOG_PROG, &session->context->errout,
  3536. "UBX: cycle end x%04x iTOW %lld\n",
  3537. msgid, (long long)session->driver.ubx.iTOW);
  3538. mask |= REPORT_IS;
  3539. }
  3540. // start of cycle? Start can equal end if only one message per epoch
  3541. // u-blox iTOW can have ms jitter in the same epoch!
  3542. iTOW_diff = session->driver.ubx.last_iTOW - session->driver.ubx.iTOW;
  3543. if (10 < llabs(iTOW_diff)) {
  3544. // time changed more than 10 ms (100 Hz), cycle start
  3545. if (session->driver.ubx.end_msgid !=
  3546. session->driver.ubx.last_msgid) {
  3547. // new cycle ender
  3548. GPSD_LOG(LOG_PROG, &session->context->errout,
  3549. "UBX: new ender x%04x was x%04x iTOW %lld was %lld\n",
  3550. session->driver.ubx.last_msgid,
  3551. session->driver.ubx.end_msgid,
  3552. (long long)session->driver.ubx.iTOW,
  3553. (long long)session->driver.ubx.last_iTOW);
  3554. session->driver.ubx.end_msgid = session->driver.ubx.last_msgid;
  3555. }
  3556. session->driver.ubx.last_iTOW = session->driver.ubx.iTOW;
  3557. mask |= CLEAR_IS;;
  3558. }
  3559. session->driver.ubx.last_msgid = msgid;
  3560. // FIXME: last_time never used...
  3561. session->driver.ubx.last_time = session->newdata.time;
  3562. } else {
  3563. // no time
  3564. /* debug
  3565. GPSD_LOG(LOG_ERROR, &session->context->errout,
  3566. "UBX: No time, msgid %x\n", msgid);
  3567. */
  3568. }
  3569. // Did protver change?
  3570. if (session->driver.ubx.last_protver != session->driver.ubx.protver) {
  3571. /* Assumption: we just did init, but did not have
  3572. * protver then, so init is not complete. Finish now.
  3573. * unless user requested passive mode */
  3574. if (session->mode == O_OPTIMIZE &&
  3575. !session->context->passive) {
  3576. ubx_mode(session, MODE_BINARY);
  3577. }
  3578. GPSD_LOG(LOG_PROG, &session->context->errout,
  3579. "UBX: new PROTVER %u was %u\n",
  3580. session->driver.ubx.protver,
  3581. session->driver.ubx.last_protver);
  3582. session->driver.ubx.last_protver = session->driver.ubx.protver;
  3583. }
  3584. return mask | ONLINE_SET;
  3585. }
  3586. static gps_mask_t parse_input(struct gps_device_t *session)
  3587. {
  3588. if (UBX_PACKET == session->lexer.type) {
  3589. return ubx_parse(session, session->lexer.outbuffer,
  3590. session->lexer.outbuflen);
  3591. }
  3592. return generic_parse_input(session);
  3593. }
  3594. bool ubx_write(struct gps_device_t * session,
  3595. unsigned int msg_class, unsigned int msg_id,
  3596. const unsigned char *msg, size_t data_len)
  3597. {
  3598. unsigned char CK_A, CK_B;
  3599. ssize_t count;
  3600. size_t i;
  3601. bool ok;
  3602. // do not write if -b (readonly) option set
  3603. // "passive" handled earlier
  3604. if (session->context->readonly) {
  3605. return true;
  3606. }
  3607. session->msgbuf[0] = 0xb5;
  3608. session->msgbuf[1] = 0x62;
  3609. CK_A = CK_B = 0;
  3610. session->msgbuf[2] = msg_class;
  3611. session->msgbuf[3] = msg_id;
  3612. session->msgbuf[4] = data_len & 0xff;
  3613. session->msgbuf[5] = (data_len >> 8) & 0xff;
  3614. assert(msg != NULL || data_len == 0);
  3615. if (msg != NULL)
  3616. (void)memcpy(&session->msgbuf[6], msg, data_len);
  3617. // calculate CRC
  3618. for (i = 2; i < 6; i++) {
  3619. CK_A += session->msgbuf[i];
  3620. CK_B += CK_A;
  3621. }
  3622. if (NULL != msg)
  3623. for (i = 0; i < data_len; i++) {
  3624. CK_A += msg[i];
  3625. CK_B += CK_A;
  3626. }
  3627. session->msgbuf[6 + data_len] = CK_A;
  3628. session->msgbuf[7 + data_len] = CK_B;
  3629. session->msgbuflen = data_len + 8;
  3630. GPSD_LOG(LOG_PROG, &session->context->errout,
  3631. "=> GPS: UBX class: %02x, id: %02x, len: %zd, crc: %02x%02x\n",
  3632. msg_class, msg_id, data_len,
  3633. CK_A, CK_B);
  3634. count = gpsd_write(session, session->msgbuf, session->msgbuflen);
  3635. ok = (count == (ssize_t) session->msgbuflen);
  3636. return (ok);
  3637. }
  3638. // not used by gpsd, it's for gpsctl and friends
  3639. static ssize_t ubx_control_send(struct gps_device_t *session, char *msg,
  3640. size_t data_len)
  3641. {
  3642. return ubx_write(session, (unsigned int)msg[0], (unsigned int)msg[1],
  3643. (unsigned char *)msg + 2,
  3644. (size_t)(data_len - 2)) ? ((ssize_t) (data_len + 7)) : -1;
  3645. }
  3646. static void ubx_init_query(struct gps_device_t *session)
  3647. {
  3648. // UBX-MON-VER: query for version information
  3649. (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
  3650. }
  3651. static void ubx_event_hook(struct gps_device_t *session, event_t event)
  3652. {
  3653. if (session->context->readonly ||
  3654. session->context->passive) {
  3655. return;
  3656. }
  3657. if (event == event_identified) {
  3658. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX identified\n");
  3659. // no longer set UBX-CFG-SBAS here, u-blox 9 and 10 do not have it
  3660. /*
  3661. * Turn off NMEA output, turn on UBX on this port.
  3662. */
  3663. if (session->context->passive) {
  3664. // passive mode, do no autoconfig
  3665. } else if (session->mode == O_OPTIMIZE) {
  3666. ubx_mode(session, MODE_BINARY);
  3667. } else {
  3668. ubx_mode(session, MODE_NMEA);
  3669. }
  3670. } else if (event == event_deactivate) {
  3671. /* There used to be a hotstart/reset here.
  3672. * That caused u-blox USB to re-enumerate.
  3673. * Sometimes to a new device name.
  3674. * Bad. Don't do that anymore...
  3675. */
  3676. }
  3677. }
  3678. // generate and send a configuration block
  3679. static gps_mask_t ubx_cfg_prt(struct gps_device_t *session, speed_t speed,
  3680. const char parity,
  3681. const int stopbits, const int mode)
  3682. {
  3683. unsigned long usart_mode = 0;
  3684. unsigned char buf[UBX_CFG_LEN];
  3685. unsigned long i;
  3686. memset(buf, '\0', UBX_CFG_LEN);
  3687. /*
  3688. * When this is called from gpsd, the initial probe for UBX should
  3689. * have picked up the device's port number from the CFG_PRT response.
  3690. */
  3691. // FIXME! Bad test, port_id == 0 is valid too. DDC (I2X) = port 0
  3692. if (session->driver.ubx.port_id != 0) {
  3693. buf[0] = session->driver.ubx.port_id;
  3694. }
  3695. /*
  3696. * This default can be hit if we haven't sent a CFG_PRT query yet,
  3697. * which can happen in gpsmon because it doesn't autoprobe.
  3698. *
  3699. * What we'd like to do here is dispatch to USART1_ID or
  3700. * USB_ID intelligently based on whether this is a USB or RS232
  3701. * source. Unfortunately the GR601-W screws that up by being
  3702. * a USB device with port_id 1. So we bite the bullet and
  3703. * default to port 1.
  3704. *
  3705. * Without further logic, this means gpsmon wouldn't be able to
  3706. * change the speed on the EVK 6H's USB port. But! To pick off
  3707. * the EVK 6H on Linux as a special case, we notice that its
  3708. * USB device name is /dev/ttyACMx - it presents as a USB modem.
  3709. *
  3710. * This logic will fail on any USB u-blox device that presents
  3711. * as an ordinary USB serial device (/dev/ttyUSB*) and actually
  3712. * has port ID 3 the way it "ought" to.
  3713. */
  3714. else if (strstr(session->gpsdata.dev.path, "/ttyACM") != NULL) {
  3715. /* using the built in USB port */
  3716. // FIXME!! USB port has no speed!
  3717. // FIXME!! maybe we know the portid already?
  3718. session->driver.ubx.port_id = buf[0] = USB_ID;
  3719. } else {
  3720. // A guess. Could be UART2, or SPI, or DDC port
  3721. session->driver.ubx.port_id = buf[0] = USART1_ID;
  3722. }
  3723. putle32(buf, 8, speed);
  3724. /*
  3725. * u-blox tech support explains the default contents of the mode
  3726. * field as follows:
  3727. *
  3728. * D0 08 00 00 mode (LSB first)
  3729. *
  3730. * re-ordering bytes: 000008D0
  3731. * dividing into fields: 000000000000000000 00 100 0 11 0 1 0000
  3732. * nStopbits = 00 = 1
  3733. * parity = 100 = none
  3734. * charLen = 11 = 8-bit
  3735. * reserved1 = 1
  3736. *
  3737. * The protocol reference further gives the following subfield values:
  3738. * 01 = 1.5 stop bits (?)
  3739. * 10 = 2 stopbits
  3740. * 000 = even parity
  3741. * 001 = odd parity
  3742. * 10x = no parity
  3743. * 10 = 7 bits
  3744. *
  3745. * Some UBX reference code amplifies this with:
  3746. *
  3747. * prtcfg.mode = (1<<4) | // compatibility with ANTARIS 4
  3748. * (1<<7) | // charLen = 11 = 8 bit
  3749. * (1<<6) | // charLen = 11 = 8 bit
  3750. * (1<<11); // parity = 10x = none
  3751. */
  3752. usart_mode |= (1<<4); // reserved1 Antaris 4 compatibility bit
  3753. usart_mode |= (1<<7); // high bit of charLen
  3754. // u-blox 5+ binary only supports 8N1
  3755. switch (parity) {
  3756. case (int)'E':
  3757. case 2:
  3758. usart_mode |= (1<<7); // 7E
  3759. break;
  3760. case (int)'O':
  3761. case 1:
  3762. usart_mode |= (1<<9) | (1<<7); // 7O
  3763. break;
  3764. case (int)'N':
  3765. case 0:
  3766. default:
  3767. usart_mode |= (1<<11) | (3<<6); // 8N
  3768. break;
  3769. }
  3770. if (2 == stopbits) {
  3771. usart_mode |= (1<<13);
  3772. }
  3773. putle32(buf, 4, usart_mode);
  3774. // enable all input protocols by default
  3775. // RTCM3 is protver 20+
  3776. buf[12] = NMEA_PROTOCOL_MASK | UBX_PROTOCOL_MASK | RTCM_PROTOCOL_MASK |
  3777. RTCM3_PROTOCOL_MASK;
  3778. /* enable all input protocols by default
  3779. * no u-blox has RTCM2 out
  3780. * RTCM3 is protver 20+ */
  3781. buf[outProtoMask] = NMEA_PROTOCOL_MASK | UBX_PROTOCOL_MASK |
  3782. RTCM3_PROTOCOL_MASK;
  3783. // FIXME: use VALGET if protver 24+
  3784. (void)ubx_write(session, UBX_CLASS_CFG, 0x00, buf, sizeof(buf));
  3785. GPSD_LOG(LOG_PROG, &session->context->errout,
  3786. "UBX ubx_cfg_prt mode %d port %d PROTVER %d\n", mode, buf[0],
  3787. session->driver.ubx.protver);
  3788. /* selectively enable output protocols */
  3789. if (mode == MODE_NMEA) {
  3790. /*
  3791. * We have to club the GR601-W over the head to make it stop emitting
  3792. * UBX after we've told it to start. But do not mung the
  3793. * protocol out mask, that breaks things.
  3794. */
  3795. unsigned char msg[3];
  3796. /* nmea to turn on at rate one (multiplier on measurement rate)
  3797. * u-blox 8 default: RMC, VTG, GGA, GSA GSV, GLL
  3798. * who wanted GST? */
  3799. const unsigned char nmea_on[] = {
  3800. 0x00, // msg id = GGA
  3801. // 0x01, /* msg id = GLL, only need RMC */
  3802. 0x02, // msg id = GSA
  3803. 0x03, // msg id = GSV
  3804. 0x04, // msg id = RMC
  3805. 0x05, // msg id = VTG
  3806. 0x07, // msg id = GST, GNSS pseudorange error statistics
  3807. 0x08, // msg id = ZDA, for UTC year
  3808. 0x09, // msg id = GBS, for RAIM errors
  3809. };
  3810. const unsigned char ubx_nav_off[] = {
  3811. 0x01, // msg id = NAV-POSECEF
  3812. 0x04, // msg id = UBX-NAV-DOP
  3813. 0x06, // msg id = NAV-SOL, deprecated in 6, gone in 9
  3814. 0x07, // msg id = NAV-PVT, in u-blox 6 and on
  3815. 0x11, // msg id = NAV-VELECEF
  3816. 0x20, // msg id = UBX-NAV-TIMEGPS
  3817. // 0x26; // msg id = UBX-NAV-TIMELS, allow as low rate
  3818. 0x30, // msg id = NAV-SVINFO, in 4 to 8, not 9
  3819. 0x32, // msg id = NAV-SBAS, in u-blox 4 to 8, not all 9
  3820. 0x35, // msg id = NAV-SAT, in u-blox 8 and 9
  3821. 0x61, // msg id = NAV-EOE
  3822. };
  3823. // enable NMEA first, in case we over-run receiver input buffer.
  3824. // turn on rate one NMEA
  3825. msg[0] = 0xf0; /* class, NMEA */
  3826. msg[2] = 0x01; /* rate, one */
  3827. for (i = 0; i < sizeof(nmea_on); i++) {
  3828. msg[1] = nmea_on[i]; // msg id to turn on
  3829. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3830. }
  3831. // Now turn off UBX-NAV, one at a time.
  3832. msg[0] = 0x01; // class, UBX-NAV
  3833. msg[2] = 0x00; // rate off
  3834. for (i = 0; i < sizeof(ubx_nav_off); i++) {
  3835. msg[1] = ubx_nav_off[i]; // msg id to turn on
  3836. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3837. }
  3838. } else { // MODE_BINARY
  3839. // nmea to turn off
  3840. const unsigned char nmea_off[] = {
  3841. 0x00, // msg id = GGA
  3842. 0x01, // msg id = GLL
  3843. 0x02, // msg id = GSA
  3844. 0x03, // msg id = GSV
  3845. 0x04, // msg id = RMC
  3846. 0x05, // msg id = VTG
  3847. 0x07, // msg id = GST
  3848. 0x08, // msg id = ZDA
  3849. 0x09, // msg id = GBS
  3850. };
  3851. const unsigned char ubx_nav_on[] = {
  3852. 0x04, // msg id = UBX-NAV-DOP
  3853. // UBX-NAV-TIMEGPS is a great cycle ender, NAV-EOE better
  3854. 0x20, // msg id = UBX-NAV-TIMEGPS
  3855. // 0x26, // msg id = UBX-NAV-TIMELS, low rate, skip here
  3856. /* NAV-SBAS errors guranteed by FAA within 6 seconds!
  3857. * in NEO-M8N, but not most other 9-series.
  3858. * Do not set NAV-SBAS as the gpsd decode does not go to JSON,
  3859. * so the data is wasted. */
  3860. // 0x32, // msg id = NAV-SBAS, in u-blox 4 to 8, not 9
  3861. };
  3862. /* UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9.
  3863. * Use UBX-NAV-PVT after u-blox 7 (protver 15+)
  3864. * u-blox 6 w/ GLONASS, protver 14 have NAV-PVT
  3865. * UBX-NAV-SOL has same data from NAV-POSECEF and NAV-VELECEF.
  3866. * Need NAV-SOL for fix type and fix flags.
  3867. * skip NAV-POSLLH as we compute lat/lon/alt/geoid from ECEF.
  3868. *
  3869. * UBX-NAV-SVINFO deprecated in u-blox 8, gone in u-blox 9.
  3870. * Use UBX-NAV-SAT after u-blox 7
  3871. *
  3872. * UBX-NAV-EOE makes a good cycle ender */
  3873. // UBX for protver < 15
  3874. const unsigned char ubx_14_nav_on[] = {
  3875. 0x06, // msg id = NAV-SOL
  3876. 0x30, // msg id = NAV-SVINFO
  3877. };
  3878. // UBX for protver >= 15
  3879. const unsigned char ubx_15_nav_on[] = {
  3880. // Need NAV-POSECEF, NAV-VELECEF and NAV-PVT to replace NAV-SOL
  3881. 0x01, // msg id = NAV-POSECEF
  3882. 0x07, // msg id = NAV-PVT
  3883. 0x11, // msg id = NAV-VELECEF
  3884. 0x35, // msg id = NAV-SAT
  3885. 0x61, // msg id = NAV-EOE, first in protver 18
  3886. };
  3887. /*
  3888. * Just enabling the UBX protocol for output is not enough to
  3889. * actually get UBX output; the sentence mix is initially empty.
  3890. * Fix that...
  3891. */
  3892. /* FIXME: possibly sending too many messages without waiting
  3893. * for u-blox ACK, over running its input buffer.
  3894. *
  3895. * For example, the UBX-MON-VER may fail here, but works in other
  3896. * contexts.
  3897. *
  3898. * Need UBX-MON-VER for protver. Need protver to properly configure
  3899. * the message set.
  3900. */
  3901. unsigned char msg[3] = {0, 0, 0};
  3902. // request SW and HW Versions, prolly already requested at detection
  3903. // ask again
  3904. (void)ubx_write(session, UBX_CLASS_MON, 0x04, msg, 0);
  3905. // turn on common UBX-NAV
  3906. msg[0] = 0x01; // class, UBX-NAV
  3907. msg[2] = 0x01; // rate, one
  3908. for (i = 0; i < sizeof(ubx_nav_on); i++) {
  3909. msg[1] = ubx_nav_on[i]; // msg id to turn on
  3910. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3911. }
  3912. /* if protver unknown, turn on everything. Which may be too
  3913. * much for slower serial port speeds. Hope that we know protver
  3914. * later and can fix things then. */
  3915. if (15 > session->driver.ubx.protver) {
  3916. /* protver 14 or less, or unknown version,
  3917. * turn on pre-15 UBX-NAV */
  3918. msg[0] = 0x01; // class, UBX-NAV
  3919. msg[2] = 0x01; // rate, one
  3920. for (i = 0; i < sizeof(ubx_14_nav_on); i++) {
  3921. msg[1] = ubx_14_nav_on[i]; // msg id to turn on
  3922. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3923. }
  3924. if (0 != session->driver.ubx.protver) {
  3925. // protver 14 or less, known version only.
  3926. // turn off 15 and above UBX-NAV
  3927. msg[0] = 0x01; // class, UBX-NAV
  3928. msg[2] = 0x00; // rate, off
  3929. for (i = 0; i < sizeof(ubx_15_nav_on); i++) {
  3930. msg[1] = ubx_15_nav_on[i]; // msg id to turn off
  3931. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3932. }
  3933. }
  3934. }
  3935. if (15 <= session->driver.ubx.protver ||
  3936. 0 == session->driver.ubx.protver) {
  3937. // protver 15 or more, or unknown version, turn on 15+ UBX-NAV
  3938. msg[0] = 0x01; // class, UBX-NAV
  3939. msg[2] = 0x01; // rate, one
  3940. for (i = 0; i < sizeof(ubx_15_nav_on); i++) {
  3941. msg[1] = ubx_15_nav_on[i]; // msg id to turn on
  3942. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3943. }
  3944. if (15 <= session->driver.ubx.protver) {
  3945. // protver 15 or more, turn off 14 and below UBX-NAV
  3946. msg[0] = 0x01; // class, UBX-NAV
  3947. msg[2] = 0x00; // rate, off
  3948. for (i = 0; i < sizeof(ubx_14_nav_on); i++) {
  3949. msg[1] = ubx_14_nav_on[i]; // msg id to turn off
  3950. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3951. }
  3952. }
  3953. }
  3954. msg[0] = 0x01; // class
  3955. msg[1] = 0x26; // msg id = UBX-NAV-TIMELS
  3956. msg[2] = 0xff; // about every 4 minutes if nav rate is 1Hz
  3957. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3958. // turn off common NMEA
  3959. msg[0] = 0xf0; // class, NMEA
  3960. msg[2] = 0x00; // rate, off
  3961. for (i = 0; i < sizeof(nmea_off); i++) {
  3962. msg[1] = nmea_off[i]; // msg id to turn off
  3963. (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
  3964. }
  3965. }
  3966. return 0;
  3967. }
  3968. static void ubx_mode(struct gps_device_t *session, int mode)
  3969. {
  3970. ubx_cfg_prt(session,
  3971. gpsd_get_speed(session),
  3972. gpsd_get_parity(session),
  3973. gpsd_get_stopbits(session),
  3974. mode);
  3975. }
  3976. static bool ubx_speed(struct gps_device_t *session,
  3977. speed_t speed, char parity, int stopbits)
  3978. {
  3979. ubx_cfg_prt(session,
  3980. speed,
  3981. parity,
  3982. stopbits,
  3983. (session->lexer.type == UBX_PACKET) ? MODE_BINARY : MODE_NMEA);
  3984. return true;
  3985. }
  3986. /* change the sample rate of the GPS */
  3987. static bool ubx_rate(struct gps_device_t *session, double cycletime)
  3988. {
  3989. /* Minimum measurement cycle time currently known from documentation
  3990. * for fastest devices, here in milli seconds. Maintained in
  3991. * struct gps_type_t driver_ubx.
  3992. */
  3993. const int64_t min_cycle = TSTOMS(&session->device_type->min_cycle);
  3994. // cycletime in milli seconds
  3995. int64_t measRate = (int64_t)(cycletime * MS_IN_SEC);
  3996. /* Message to be sent to device. */
  3997. unsigned char msg[6] = {
  3998. 0x00, 0x00, /* U2: Measurement rate (ms), will be set below */
  3999. 0x01, 0x00, /* U2: Navigation rate (cycles), set to 1 */
  4000. 0x00, 0x00, /* U2: Alignment to reference time: 0 = UTC */
  4001. };
  4002. // check max
  4003. if (65535 < measRate) {
  4004. measRate = 65535; // milli seconds
  4005. } else if (min_cycle > measRate) {
  4006. /* Clamp cycle time to lowest bound given in documentation.
  4007. * protVer >= 24 has 25 ms min.
  4008. * protVer < 24 has min of 50ms or more.
  4009. */
  4010. measRate = min_cycle;
  4011. }
  4012. // we now know measRate fits in a U2
  4013. GPSD_LOG(LOG_PROG, &session->context->errout,
  4014. "UBX rate change, measRate %lld millisecs\n",
  4015. (long long) measRate);
  4016. msg[0] = (unsigned char)(measRate & 0xff);
  4017. msg[1] = (unsigned char)(measRate >> 8);
  4018. // UBX-CFG-RATE deprecated in u-blox 10
  4019. return ubx_write(session, UBX_CLASS_CFG, 0x08, msg, 6); // CFG-RATE
  4020. }
  4021. // This is everything we export
  4022. // *INDENT-OFF*
  4023. const struct gps_type_t driver_ubx = {
  4024. .type_name = "u-blox", // Full name of type
  4025. .packet_type = UBX_PACKET, // associated lexer packet type
  4026. .flags = DRIVER_STICKY, // remember this
  4027. .trigger = NULL,
  4028. // Number of satellite channels supported by the device
  4029. // ZED-F0P supports 60
  4030. .channels = 60,
  4031. .probe_detect = NULL, // Startup-time device detector
  4032. // Packet getter (using default routine)
  4033. .get_packet = generic_get,
  4034. .parse_packet = parse_input, // Parse message packets
  4035. // RTCM handler (using default routine)
  4036. .rtcm_writer = gpsd_write,
  4037. .init_query = ubx_init_query, // non-perturbing initial query
  4038. .event_hook = ubx_event_hook, // Fire on various lifetime events
  4039. .speed_switcher = ubx_speed, // Speed (baudrate) switch
  4040. .mode_switcher = ubx_mode, // Mode switcher
  4041. .rate_switcher = ubx_rate, // Message delivery rate switcher
  4042. /* Minimum measurement cycle time currently known from documentation
  4043. * for fastest devices.
  4044. */
  4045. .min_cycle.tv_sec = 0,
  4046. .min_cycle.tv_nsec = 25000000, // Maximum 40Hz sample rate
  4047. .control_send = ubx_control_send, // how to send a control string
  4048. .time_offset = NULL, // no method for NTP fudge factor
  4049. };
  4050. // *INDENT-ON*
  4051. #endif // defined(UBLOX_ENABLE) && defined(BINARY_ENABLE)
  4052. // vim: set expandtab shiftwidth=4