Devuan fork of gpsd
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

libgpsd_core.c 62 KiB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931
  1. /* libgpsd_core.c -- manage access to sensors
  2. *
  3. * Access to the driver layer goes through the entry points in this file.
  4. * The idea is to present a session as an abstraction from which you get
  5. * fixes (and possibly other data updates) by calling gpsd_multipoll(). The
  6. * rest is setup and teardown. (For backward compatibility the older gpsd_poll()
  7. * entry point has been retained.)
  8. *
  9. * This file is Copyright (c) 2010-2018 by the GPSD project
  10. * SPDX-License-Identifier: BSD-2-clause
  11. */
  12. #include "gpsd_config.h" /* must be before all includes */
  13. #include <assert.h>
  14. #include <ctype.h>
  15. #include <errno.h>
  16. #include <fcntl.h>
  17. #include <libgen.h>
  18. #include <math.h>
  19. #include <stdarg.h>
  20. #include <stdbool.h>
  21. #include <stdio.h>
  22. #include <stdlib.h>
  23. #include <string.h>
  24. #include <syslog.h>
  25. #include <sys/select.h>
  26. #include <sys/socket.h>
  27. #include <sys/stat.h>
  28. #include <sys/types.h>
  29. #include <sys/wait.h>
  30. #include <time.h>
  31. #include <unistd.h>
  32. #include "gpsd.h"
  33. #include "matrix.h"
  34. #include "strfuncs.h"
  35. #include "timespec.h"
  36. #if defined(NMEA2000_ENABLE)
  37. #include "driver_nmea2000.h"
  38. #endif /* defined(NMEA2000_ENABLE) */
  39. ssize_t gpsd_write(struct gps_device_t *session,
  40. const char *buf,
  41. const size_t len)
  42. /* pass low-level data to devices straight through */
  43. {
  44. return session->context->serial_write(session, buf, len);
  45. }
  46. static void basic_report(const char *buf)
  47. {
  48. (void)fputs(buf, stderr);
  49. }
  50. void errout_reset(struct gpsd_errout_t *errout)
  51. {
  52. errout->debug = LOG_SHOUT;
  53. errout->report = basic_report;
  54. }
  55. static pthread_mutex_t report_mutex;
  56. void gpsd_acquire_reporting_lock(void)
  57. {
  58. int err;
  59. err = pthread_mutex_lock(&report_mutex);
  60. if ( 0 != err ) {
  61. /* POSIX says pthread_mutex_lock() should only fail if the
  62. thread holding the lock has died. Best for gppsd to just die
  63. because things are FUBAR. */
  64. (void) fprintf(stderr,"pthread_mutex_lock() failed: %s\n",
  65. strerror(errno));
  66. exit(EXIT_FAILURE);
  67. }
  68. }
  69. void gpsd_release_reporting_lock(void)
  70. {
  71. int err;
  72. err = pthread_mutex_unlock(&report_mutex);
  73. if ( 0 != err ) {
  74. /* POSIX says pthread_mutex_unlock() should only fail when
  75. trying to unlock a lock that does not exist, or is not owned by
  76. this thread. This should never happen, so best for gpsd to die
  77. because things are FUBAR. */
  78. (void) fprintf(stderr,"pthread_mutex_unlock() failed: %s\n",
  79. strerror(errno));
  80. exit(EXIT_FAILURE);
  81. }
  82. }
  83. #ifndef SQUELCH_ENABLE
  84. static void visibilize(char *outbuf, size_t outlen,
  85. const char *inbuf, size_t inlen)
  86. {
  87. const char *sp;
  88. outbuf[0] = '\0';
  89. for (sp = inbuf; sp < inbuf + inlen && strlen(outbuf)+6 < outlen; sp++)
  90. if (isprint((unsigned char) *sp) || (sp[0] == '\n' && sp[1] == '\0')
  91. || (sp[0] == '\r' && sp[2] == '\0'))
  92. (void)snprintf(outbuf + strlen(outbuf), 2, "%c", *sp);
  93. else
  94. (void)snprintf(outbuf + strlen(outbuf), 6, "\\x%02x",
  95. 0x00ff & (unsigned)*sp);
  96. }
  97. #endif /* !SQUELCH_ENABLE */
  98. static void gpsd_vlog(const int errlevel,
  99. const struct gpsd_errout_t *errout,
  100. char *outbuf, size_t outlen,
  101. const char *fmt, va_list ap)
  102. /* assemble msg in vprintf(3) style, use errout hook or syslog for delivery */
  103. {
  104. #ifdef SQUELCH_ENABLE
  105. (void)errout;
  106. (void)errlevel;
  107. (void)fmt;
  108. #else
  109. char buf[BUFSIZ];
  110. char *err_str;
  111. // errout should never be NULL, but some code analyzers complain anyway
  112. if (NULL == errout ||
  113. errout->debug < errlevel) {
  114. return;
  115. }
  116. gpsd_acquire_reporting_lock();
  117. switch ( errlevel ) {
  118. case LOG_ERROR:
  119. err_str = "ERROR: ";
  120. break;
  121. case LOG_SHOUT:
  122. err_str = "SHOUT: ";
  123. break;
  124. case LOG_WARN:
  125. err_str = "WARN: ";
  126. break;
  127. case LOG_CLIENT:
  128. err_str = "CLIENT: ";
  129. break;
  130. case LOG_INF:
  131. err_str = "INFO: ";
  132. break;
  133. case LOG_DATA:
  134. err_str = "DATA: ";
  135. break;
  136. case LOG_PROG:
  137. err_str = "PROG: ";
  138. break;
  139. case LOG_IO:
  140. err_str = "IO: ";
  141. break;
  142. case LOG_SPIN:
  143. err_str = "SPIN: ";
  144. break;
  145. case LOG_RAW:
  146. err_str = "RAW: ";
  147. break;
  148. default:
  149. err_str = "UNK: ";
  150. }
  151. assert(errout->label != NULL);
  152. (void)strlcpy(buf, errout->label, sizeof(buf));
  153. (void)strlcat(buf, ":", sizeof(buf));
  154. (void)strlcat(buf, err_str, sizeof(buf));
  155. str_vappendf(buf, sizeof(buf), fmt, ap);
  156. visibilize(outbuf, outlen, buf, strlen(buf));
  157. if (getpid() == getsid(getpid()))
  158. syslog((errlevel <= LOG_SHOUT) ? LOG_ERR : LOG_NOTICE, "%s", outbuf);
  159. else if (errout->report != NULL)
  160. errout->report(outbuf);
  161. else
  162. (void)fputs(outbuf, stderr);
  163. gpsd_release_reporting_lock();
  164. #endif /* !SQUELCH_ENABLE */
  165. }
  166. /* assemble msg in printf(3) style, use errout hook or syslog for delivery */
  167. void gpsd_log(const int errlevel, const struct gpsd_errout_t *errout,
  168. const char *fmt, ...)
  169. {
  170. char buf[BUFSIZ];
  171. va_list ap;
  172. buf[0] = '\0';
  173. va_start(ap, fmt);
  174. gpsd_vlog(errlevel, errout, buf, sizeof(buf), fmt, ap);
  175. va_end(ap);
  176. }
  177. const char *gpsd_prettydump(struct gps_device_t *session)
  178. /* dump the current packet in a form optimised for eyeballs */
  179. {
  180. return gpsd_packetdump(session->msgbuf, sizeof(session->msgbuf),
  181. (char *)session->lexer.outbuffer,
  182. session->lexer.outbuflen);
  183. }
  184. /* Define the possible hook strings here so we can get the length */
  185. #define HOOK_ACTIVATE "ACTIVATE"
  186. #define HOOK_DEACTIVATE "DEACTIVATE"
  187. #define HOOK_CMD_MAX (sizeof(DEVICEHOOKPATH) + GPS_PATH_MAX \
  188. + sizeof(HOOK_DEACTIVATE))
  189. static void gpsd_run_device_hook(struct gpsd_errout_t *errout,
  190. char *device_name, char *hook)
  191. {
  192. struct stat statbuf;
  193. if (stat(DEVICEHOOKPATH, &statbuf) == -1)
  194. GPSD_LOG(LOG_PROG, errout,
  195. "no %s present, skipped running %s hook\n",
  196. DEVICEHOOKPATH, hook);
  197. else {
  198. int status;
  199. char buf[HOOK_CMD_MAX];
  200. (void)snprintf(buf, sizeof(buf), "%s %s %s",
  201. DEVICEHOOKPATH, device_name, hook);
  202. GPSD_LOG(LOG_INF, errout, "running %s\n", buf);
  203. status = system(buf);
  204. if (status == -1)
  205. GPSD_LOG(LOG_ERROR, errout, "error running %s\n", buf);
  206. else
  207. GPSD_LOG(LOG_INF, errout,
  208. "%s returned %d\n", DEVICEHOOKPATH,
  209. WEXITSTATUS(status));
  210. }
  211. }
  212. int gpsd_switch_driver(struct gps_device_t *session, char *type_name)
  213. {
  214. const struct gps_type_t **dp;
  215. bool first_sync = (session->device_type != NULL);
  216. unsigned int i;
  217. if (first_sync && strcmp(session->device_type->type_name, type_name) == 0)
  218. return 0;
  219. GPSD_LOG(LOG_PROG, &session->context->errout,
  220. "switch_driver(%s) called...\n", type_name);
  221. for (dp = gpsd_drivers, i = 0; *dp; dp++, i++)
  222. if (strcmp((*dp)->type_name, type_name) == 0) {
  223. GPSD_LOG(LOG_PROG, &session->context->errout,
  224. "selecting %s driver...\n",
  225. (*dp)->type_name);
  226. gpsd_assert_sync(session);
  227. session->device_type = *dp;
  228. session->driver_index = i;
  229. #ifdef RECONFIGURE_ENABLE
  230. session->gpsdata.dev.mincycle = session->device_type->min_cycle;
  231. #endif /* RECONFIGURE_ENABLE */
  232. /* reconfiguration might be required */
  233. if (first_sync && session->device_type->event_hook != NULL)
  234. session->device_type->event_hook(session,
  235. event_driver_switch);
  236. #ifdef RECONFIGURE_ENABLE
  237. if (STICKY(*dp))
  238. session->last_controller = *dp;
  239. #endif /* RECONFIGURE_ENABLE */
  240. return 1;
  241. }
  242. GPSD_LOG(LOG_ERROR, &session->context->errout,
  243. "invalid GPS type \"%s\".\n", type_name);
  244. return 0;
  245. }
  246. void gps_context_init(struct gps_context_t *context,
  247. const char *label)
  248. {
  249. (void)memset(context, '\0', sizeof(struct gps_context_t));
  250. //context.readonly = false;
  251. context->leap_notify = LEAP_NOWARNING;
  252. context->serial_write = gpsd_serial_write;
  253. errout_reset(&context->errout);
  254. context->errout.label = (char *)label;
  255. (void)pthread_mutex_init(&report_mutex, NULL);
  256. }
  257. void gpsd_init(struct gps_device_t *session, struct gps_context_t *context,
  258. const char *device)
  259. /* initialize GPS polling */
  260. {
  261. if (device != NULL)
  262. (void)strlcpy(session->gpsdata.dev.path, device,
  263. sizeof(session->gpsdata.dev.path));
  264. session->device_type = NULL; /* start by hunting packets */
  265. #ifdef RECONFIGURE_ENABLE
  266. session->last_controller = NULL;
  267. #endif /* RECONFIGURE_ENABLE */
  268. session->observed = 0;
  269. session->sourcetype = source_unknown; /* gpsd_open() sets this */
  270. session->servicetype = service_unknown; /* gpsd_open() sets this */
  271. session->context = context;
  272. memset(session->subtype, 0, sizeof(session->subtype));
  273. memset(session->subtype1, 0, sizeof(session->subtype1));
  274. #ifdef NMEA0183_ENABLE
  275. memset(&(session->nmea), 0, sizeof(session->nmea));
  276. #endif /* NMEA0183_ENABLE */
  277. gps_clear_fix(&session->gpsdata.fix);
  278. gps_clear_fix(&session->newdata);
  279. gps_clear_fix(&session->lastfix);
  280. gps_clear_fix(&session->oldfix);
  281. session->gpsdata.set = 0;
  282. gps_clear_att(&session->gpsdata.attitude);
  283. gps_clear_dop(&session->gpsdata.dop);
  284. session->gpsdata.dev.mincycle.tv_sec = 1;
  285. session->gpsdata.dev.mincycle.tv_nsec = 0;
  286. session->gpsdata.dev.cycle.tv_sec = 1;
  287. session->gpsdata.dev.cycle.tv_nsec = 0;
  288. session->sor.tv_sec = 0;
  289. session->sor.tv_nsec = 0;
  290. session->chars = 0;
  291. /* tty-level initialization */
  292. gpsd_tty_init(session);
  293. /* necessary in case we start reading in the middle of a GPGSV sequence */
  294. gpsd_zero_satellites(&session->gpsdata);
  295. /* initialize things for the packet parser */
  296. packet_reset(&session->lexer);
  297. }
  298. /* temporarily release the GPS device */
  299. void gpsd_deactivate(struct gps_device_t *session)
  300. {
  301. #ifdef RECONFIGURE_ENABLE
  302. if (!session->context->readonly
  303. && session->device_type != NULL
  304. && session->device_type->event_hook != NULL) {
  305. session->device_type->event_hook(session, event_deactivate);
  306. }
  307. #endif /* RECONFIGURE_ENABLE */
  308. GPSD_LOG(LOG_INF, &session->context->errout,
  309. "closing GPS=%s (%d)\n",
  310. session->gpsdata.dev.path, session->gpsdata.gps_fd);
  311. #if defined(NMEA2000_ENABLE)
  312. if (session->sourcetype == source_can)
  313. (void)nmea2000_close(session);
  314. else
  315. #endif /* of defined(NMEA2000_ENABLE) */
  316. (void)gpsd_close(session);
  317. if (session->mode == O_OPTIMIZE)
  318. gpsd_run_device_hook(&session->context->errout,
  319. session->gpsdata.dev.path,
  320. HOOK_DEACTIVATE);
  321. /* tell any PPS-watcher thread to die */
  322. session->pps_thread.report_hook = NULL;
  323. /* mark it inactivated */
  324. session->gpsdata.online.tv_sec = 0;
  325. session->gpsdata.online.tv_nsec = 0;
  326. }
  327. static void ppsthread_log(volatile struct pps_thread_t *pps_thread,
  328. int loglevel, const char *fmt, ...)
  329. /* shim function to decouple PPS monitor code from the session structure */
  330. {
  331. struct gps_device_t *device = (struct gps_device_t *)pps_thread->context;
  332. char buf[BUFSIZ];
  333. va_list ap;
  334. switch (loglevel) {
  335. case THREAD_ERROR:
  336. loglevel = LOG_ERROR;
  337. break;
  338. case THREAD_WARN:
  339. loglevel = LOG_WARN;
  340. break;
  341. case THREAD_INF:
  342. loglevel = LOG_INF;
  343. break;
  344. case THREAD_PROG:
  345. loglevel = LOG_PROG;
  346. break;
  347. case THREAD_RAW:
  348. loglevel = LOG_RAW;
  349. break;
  350. }
  351. buf[0] = '\0';
  352. va_start(ap, fmt);
  353. gpsd_vlog(loglevel, &device->context->errout, buf, sizeof(buf), fmt, ap);
  354. va_end(ap);
  355. }
  356. void gpsd_clear(struct gps_device_t *session)
  357. /* device has been opened - clear its storage for use */
  358. {
  359. (void)clock_gettime(CLOCK_REALTIME, &session->gpsdata.online);
  360. lexer_init(&session->lexer);
  361. session->lexer.errout = session->context->errout;
  362. // session->gpsdata.online = 0;
  363. gps_clear_att(&session->gpsdata.attitude);
  364. gps_clear_dop(&session->gpsdata.dop);
  365. gps_clear_fix(&session->gpsdata.fix);
  366. session->gpsdata.status = STATUS_NO_FIX;
  367. session->releasetime = (time_t)0;
  368. session->badcount = 0;
  369. /* clear the private data union */
  370. memset( (void *)&session->driver, '\0', sizeof(session->driver));
  371. /* set up the context structure for the PPS thread monitor */
  372. memset((void *)&session->pps_thread, 0, sizeof(session->pps_thread));
  373. session->pps_thread.devicefd = session->gpsdata.gps_fd;
  374. session->pps_thread.devicename = session->gpsdata.dev.path;
  375. session->pps_thread.log_hook = ppsthread_log;
  376. session->pps_thread.context = (void *)session;
  377. session->opentime = time(NULL);
  378. }
  379. static int parse_uri_dest(struct gps_device_t *session, char *s,
  380. char **host, char **service)
  381. /* split s into host and service parts
  382. * if service is not specified, *service is assigned to NULL
  383. * return: -1 on error, 0 otherwise
  384. */
  385. {
  386. if (s[0] != '[') {
  387. *host = s;
  388. s = strchr(s, ':');
  389. } else { /* IPv6 literal */
  390. char *cb = strchr(s, ']');
  391. if (!cb || (cb[1] && cb[1] != ':')) {
  392. GPSD_LOG(LOG_ERROR, &session->context->errout,
  393. "Malformed URI specified.\n");
  394. return -1;
  395. }
  396. *cb = '\0';
  397. *host = s + 1;
  398. s = cb + 1;
  399. }
  400. if (s && s[0] && s[1]) {
  401. *s = '\0';
  402. *service = s + 1;
  403. } else
  404. *service = NULL;
  405. return 0;
  406. }
  407. int gpsd_open(struct gps_device_t *session)
  408. /* open a device for access to its data *
  409. * return: the opened file descriptor
  410. * PLACEHOLDING_FD - for /dev/ppsX
  411. * UNALLOCATED_FD - for open failure
  412. * -1 - for open failure
  413. */
  414. {
  415. #ifdef NETFEED_ENABLE
  416. /* special case: source may be a URI to a remote GNSS or DGPS service */
  417. if (netgnss_uri_check(session->gpsdata.dev.path)) {
  418. session->gpsdata.gps_fd = netgnss_uri_open(session,
  419. session->gpsdata.dev.path);
  420. session->sourcetype = source_tcp;
  421. GPSD_LOG(LOG_SPIN, &session->context->errout,
  422. "netgnss_uri_open(%s) returns socket on fd %d\n",
  423. session->gpsdata.dev.path, session->gpsdata.gps_fd);
  424. return session->gpsdata.gps_fd;
  425. /* otherwise, could be an TCP data feed */
  426. } else if (str_starts_with(session->gpsdata.dev.path, "tcp://")) {
  427. char server[GPS_PATH_MAX], *host, *port;
  428. socket_t dsock;
  429. (void)strlcpy(server, session->gpsdata.dev.path + 6, sizeof(server));
  430. INVALIDATE_SOCKET(session->gpsdata.gps_fd);
  431. if (parse_uri_dest(session, server, &host, &port) == -1 || !port) {
  432. GPSD_LOG(LOG_ERROR, &session->context->errout,
  433. "Missing service in TCP feed spec.\n");
  434. return -1;
  435. }
  436. GPSD_LOG(LOG_INF, &session->context->errout,
  437. "opening TCP feed at %s, port %s.\n", host,
  438. port);
  439. if ((dsock = netlib_connectsock(AF_UNSPEC, host, port, "tcp")) < 0) {
  440. GPSD_LOG(LOG_ERROR, &session->context->errout,
  441. "TCP device open error %s.\n",
  442. netlib_errstr(dsock));
  443. return -1;
  444. } else
  445. GPSD_LOG(LOG_SPIN, &session->context->errout,
  446. "TCP device opened on fd %d\n", dsock);
  447. session->gpsdata.gps_fd = dsock;
  448. session->sourcetype = source_tcp;
  449. return session->gpsdata.gps_fd;
  450. /* or could be UDP */
  451. } else if (str_starts_with(session->gpsdata.dev.path, "udp://")) {
  452. char server[GPS_PATH_MAX], *host, *port;
  453. socket_t dsock;
  454. (void)strlcpy(server, session->gpsdata.dev.path + 6, sizeof(server));
  455. INVALIDATE_SOCKET(session->gpsdata.gps_fd);
  456. if (parse_uri_dest(session, server, &host, &port) == -1 || !port) {
  457. GPSD_LOG(LOG_ERROR, &session->context->errout,
  458. "Missing service in UDP feed spec.\n");
  459. return -1;
  460. }
  461. GPSD_LOG(LOG_INF, &session->context->errout,
  462. "opening UDP feed at %s, port %s.\n", host,
  463. port);
  464. if ((dsock = netlib_connectsock(AF_UNSPEC, host, port, "udp")) < 0) {
  465. GPSD_LOG(LOG_ERROR, &session->context->errout,
  466. "UDP device open error %s.\n",
  467. netlib_errstr(dsock));
  468. return -1;
  469. } else
  470. GPSD_LOG(LOG_SPIN, &session->context->errout,
  471. "UDP device opened on fd %d\n", dsock);
  472. session->gpsdata.gps_fd = dsock;
  473. session->sourcetype = source_udp;
  474. return session->gpsdata.gps_fd;
  475. }
  476. #endif /* NETFEED_ENABLE */
  477. #ifdef PASSTHROUGH_ENABLE
  478. if (str_starts_with(session->gpsdata.dev.path, "gpsd://")) {
  479. char server[GPS_PATH_MAX], *host, *port;
  480. socket_t dsock;
  481. (void)strlcpy(server, session->gpsdata.dev.path + 7, sizeof(server));
  482. INVALIDATE_SOCKET(session->gpsdata.gps_fd);
  483. if (parse_uri_dest(session, server, &host, &port) == -1)
  484. return -1;
  485. if (!port)
  486. port = DEFAULT_GPSD_PORT;
  487. GPSD_LOG(LOG_INF, &session->context->errout,
  488. "opening remote gpsd feed at %s, port %s.\n",
  489. host, port);
  490. if ((dsock = netlib_connectsock(AF_UNSPEC, host, port, "tcp")) < 0) {
  491. GPSD_LOG(LOG_ERROR, &session->context->errout,
  492. "remote gpsd device open error %s.\n",
  493. netlib_errstr(dsock));
  494. return -1;
  495. } else
  496. GPSD_LOG(LOG_SPIN, &session->context->errout,
  497. "remote gpsd feed opened on fd %d\n", dsock);
  498. /* watch to remote is issued when WATCH is */
  499. session->gpsdata.gps_fd = dsock;
  500. session->sourcetype = source_gpsd;
  501. return session->gpsdata.gps_fd;
  502. }
  503. #endif /* PASSTHROUGH_ENABLE */
  504. #if defined(NMEA2000_ENABLE)
  505. if (str_starts_with(session->gpsdata.dev.path, "nmea2000://")) {
  506. return nmea2000_open(session);
  507. }
  508. #endif /* defined(NMEA2000_ENABLE) */
  509. /* fall through to plain serial open */
  510. /* could be a naked /dev/ppsX */
  511. return gpsd_serial_open(session);
  512. }
  513. int gpsd_activate(struct gps_device_t *session, const int mode)
  514. /* acquire a connection to the GPS device */
  515. {
  516. if (mode == O_OPTIMIZE)
  517. gpsd_run_device_hook(&session->context->errout,
  518. session->gpsdata.dev.path, HOOK_ACTIVATE);
  519. session->gpsdata.gps_fd = gpsd_open(session);
  520. if (mode != O_CONTINUE)
  521. session->mode = mode;
  522. // cppcheck-suppress pointerLessThanZero
  523. if (session->gpsdata.gps_fd < 0) {
  524. /* return could be -1, PLACEHOLDING_FD, of UNALLOCATED_FD */
  525. if ( PLACEHOLDING_FD == session->gpsdata.gps_fd ) {
  526. /* it is /dev/ppsX, need to set devicename, etc. */
  527. gpsd_clear(session);
  528. }
  529. return session->gpsdata.gps_fd;
  530. }
  531. #ifdef NON_NMEA0183_ENABLE
  532. /* if it's a sensor, it must be probed */
  533. if ((session->servicetype == service_sensor) &&
  534. (session->sourcetype != source_can)) {
  535. const struct gps_type_t **dp;
  536. for (dp = gpsd_drivers; *dp; dp++) {
  537. if ((*dp)->probe_detect != NULL) {
  538. GPSD_LOG(LOG_PROG, &session->context->errout,
  539. "Probing \"%s\" driver...\n",
  540. (*dp)->type_name);
  541. /* toss stale data */
  542. (void)tcflush(session->gpsdata.gps_fd, TCIOFLUSH);
  543. if ((*dp)->probe_detect(session) != 0) {
  544. GPSD_LOG(LOG_PROG, &session->context->errout,
  545. "Probe found \"%s\" driver...\n",
  546. (*dp)->type_name);
  547. session->device_type = *dp;
  548. gpsd_assert_sync(session);
  549. goto foundit;
  550. } else
  551. GPSD_LOG(LOG_PROG, &session->context->errout,
  552. "Probe not found \"%s\" driver...\n",
  553. (*dp)->type_name);
  554. }
  555. }
  556. GPSD_LOG(LOG_PROG, &session->context->errout,
  557. "no probe matched...\n");
  558. }
  559. foundit:
  560. #endif /* NON_NMEA0183_ENABLE */
  561. gpsd_clear(session);
  562. GPSD_LOG(LOG_INF, &session->context->errout,
  563. "gpsd_activate(%d): activated GPS (fd %d)\n",
  564. session->mode, session->gpsdata.gps_fd);
  565. /*
  566. * We might know the device's type, but we shouldn't assume it has
  567. * retained its settings. A revert hook might well have undone
  568. * them on the previous close. Fire a reactivate event so drivers
  569. * can do something about this if they choose.
  570. */
  571. if (session->device_type != NULL
  572. && session->device_type->event_hook != NULL)
  573. session->device_type->event_hook(session, event_reactivate);
  574. return session->gpsdata.gps_fd;
  575. }
  576. /*****************************************************************************
  577. Carl Carter of SiRF supplied this algorithm for computing DOPs from
  578. a list of visible satellites (some typos corrected)...
  579. For satellite n, let az(n) = azimuth angle from North and el(n) be elevation.
  580. Let:
  581. a(k, 1) = sin az(k) * cos el(k)
  582. a(k, 2) = cos az(k) * cos el(k)
  583. a(k, 3) = sin el(k)
  584. Then form the line-of-sight matrix A for satellites used in the solution:
  585. | a(1,1) a(1,2) a(1,3) 1 |
  586. | a(2,1) a(2,2) a(2,3) 1 |
  587. | : : : : |
  588. | a(n,1) a(n,2) a(n,3) 1 |
  589. And its transpose A~:
  590. |a(1, 1) a(2, 1) . . . a(n, 1) |
  591. |a(1, 2) a(2, 2) . . . a(n, 2) |
  592. |a(1, 3) a(2, 3) . . . a(n, 3) |
  593. | 1 1 . . . 1 |
  594. Compute the covariance matrix (A~*A)^-1, which is guaranteed symmetric:
  595. | s(x)^2 s(x)*s(y) s(x)*s(z) s(x)*s(t) |
  596. | s(y)*s(x) s(y)^2 s(y)*s(z) s(y)*s(t) |
  597. | s(z)*s(x) s(z)*s(y) s(z)^2 s(z)*s(t) |
  598. | s(t)*s(x) s(t)*s(y) s(t)*s(z) s(t)^2 |
  599. Then:
  600. GDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2 + s(t)^2)
  601. TDOP = sqrt(s(t)^2)
  602. PDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2)
  603. HDOP = sqrt(s(x)^2 + s(y)^2)
  604. VDOP = sqrt(s(z)^2)
  605. Here's how we implement it...
  606. First, each compute element P(i,j) of the 4x4 product A~*A.
  607. If S(k=1,k=n): f(...) is the sum of f(...) as k varies from 1 to n, then
  608. applying the definition of matrix product tells us:
  609. P(i,j) = S(k=1,k=n): B(i, k) * A(k, j)
  610. But because B is the transpose of A, this reduces to
  611. P(i,j) = S(k=1,k=n): A(k, i) * A(k, j)
  612. This is not, however, the entire algorithm that SiRF uses. Carl writes:
  613. > As you note, with rounding accounted for, most values agree exactly, and
  614. > those that don't agree our number is higher. That is because we
  615. > deweight some satellites and account for that in the DOP calculation.
  616. > If a satellite is not used in a solution at the same weight as others,
  617. > it should not contribute to DOP calculation at the same weight. So our
  618. > internal algorithm does a compensation for that which you would have no
  619. > way to duplicate on the outside since we don't output the weighting
  620. > factors. In fact those are not even available to API users.
  621. Queried about the deweighting, Carl says:
  622. > In the SiRF tracking engine, each satellite track is assigned a quality
  623. > value based on the tracker's estimate of that signal. It includes C/No
  624. > estimate, ability to hold onto the phase, stability of the I vs. Q phase
  625. > angle, etc. The navigation algorithm then ranks all the tracks into
  626. > quality order and selects which ones to use in the solution and what
  627. > weight to give those used in the solution. The process is actually a
  628. > bit of a "trial and error" method -- we initially use all available
  629. > tracks in the solution, then we sequentially remove the lowest quality
  630. > ones until the solution stabilizes. The weighting is inherent in the
  631. > Kalman filter algorithm. Once the solution is stable, the DOP is
  632. > computed from those SVs used, and there is an algorithm that looks at
  633. > the quality ratings and determines if we need to deweight any.
  634. > Likewise, if we use altitude hold mode for a 3-SV solution, we deweight
  635. > the phantom satellite at the center of the Earth.
  636. So we cannot exactly duplicate what SiRF does internally. We'll leave
  637. HDOP alone and use our computed values for VDOP and PDOP. Note, this
  638. may have to change in the future if this code is used by a non-SiRF
  639. driver.
  640. ******************************************************************************/
  641. static gps_mask_t fill_dop(const struct gpsd_errout_t *errout,
  642. const struct gps_data_t * gpsdata,
  643. struct dop_t * dop)
  644. {
  645. double prod[4][4];
  646. double inv[4][4];
  647. double satpos[MAXCHANNELS][4];
  648. double xdop, ydop, hdop, vdop, pdop, tdop, gdop;
  649. int i, j, k, n;
  650. memset(satpos, 0, sizeof(satpos));
  651. for (n = k = 0; k < gpsdata->satellites_visible; k++) {
  652. if (!gpsdata->skyview[k].used) {
  653. /* skip unused sats */
  654. continue;
  655. }
  656. if (1 > gpsdata->skyview[k].PRN) {
  657. /* skip bad PRN */
  658. continue;
  659. }
  660. if (0 == isfinite(gpsdata->skyview[k].azimuth) ||
  661. 0 > gpsdata->skyview[k].azimuth ||
  662. 359 < gpsdata->skyview[k].azimuth) {
  663. /* skip bad azimuth */
  664. continue;
  665. }
  666. if (0 == isfinite(gpsdata->skyview[k].elevation) ||
  667. 90 < fabs(gpsdata->skyview[k].elevation)) {
  668. /* skip bad elevation */
  669. continue;
  670. }
  671. const struct satellite_t *sp = &gpsdata->skyview[k];
  672. satpos[n][0] = sin(sp->azimuth * DEG_2_RAD)
  673. * cos(sp->elevation * DEG_2_RAD);
  674. satpos[n][1] = cos(sp->azimuth * DEG_2_RAD)
  675. * cos(sp->elevation * DEG_2_RAD);
  676. satpos[n][2] = sin(sp->elevation * DEG_2_RAD);
  677. satpos[n][3] = 1;
  678. GPSD_LOG(LOG_INF, errout, "PRN=%3d az=%.1f ael%.1f (%f, %f, %f)\n",
  679. gpsdata->skyview[k].PRN,
  680. gpsdata->skyview[k].azimuth,
  681. gpsdata->skyview[k].elevation,
  682. satpos[n][0], satpos[n][1], satpos[n][2]);
  683. n++;
  684. }
  685. /* can't use gpsdata->satellites_used as that is a counter for xxGSA,
  686. * and gets cleared at odd times */
  687. GPSD_LOG(LOG_INF, errout, "Sats used (%d):\n", n);
  688. /* If we don't have 4 satellites then we don't have enough information to calculate DOPS */
  689. if (n < 4) {
  690. #ifdef __UNUSED__
  691. GPSD_LOG(LOG_RAW, errout,
  692. "Not enough satellites available %d < 4:\n",
  693. n);
  694. #endif /* __UNUSED__ */
  695. return 0; /* Is this correct return code here? or should it be ERROR_SET */
  696. }
  697. memset(prod, 0, sizeof(prod));
  698. memset(inv, 0, sizeof(inv));
  699. #ifdef __UNUSED__
  700. GPSD_LOG(LOG_INF, errout, "Line-of-sight matrix:\n");
  701. for (k = 0; k < n; k++) {
  702. GPSD_LOG(LOG_INF, errout, "%f %f %f %f\n",
  703. satpos[k][0], satpos[k][1], satpos[k][2], satpos[k][3]);
  704. }
  705. #endif /* __UNUSED__ */
  706. for (i = 0; i < 4; ++i) { //< rows
  707. for (j = 0; j < 4; ++j) { //< cols
  708. prod[i][j] = 0.0;
  709. for (k = 0; k < n; ++k) {
  710. prod[i][j] += satpos[k][i] * satpos[k][j];
  711. }
  712. }
  713. }
  714. #ifdef __UNUSED__
  715. GPSD_LOG(LOG_INF, errout, "product:\n");
  716. for (k = 0; k < 4; k++) {
  717. GPSD_LOG(LOG_INF, errout, "%f %f %f %f\n",
  718. prod[k][0], prod[k][1], prod[k][2], prod[k][3]);
  719. }
  720. #endif /* __UNUSED__ */
  721. if (matrix_invert(prod, inv)) {
  722. #ifdef __UNUSED__
  723. /*
  724. * Note: this will print garbage unless all the subdeterminants
  725. * are computed in the invert() function.
  726. */
  727. GPSD_LOG(LOG_RAW, errout, "inverse:\n");
  728. for (k = 0; k < 4; k++) {
  729. GPSD_LOG(LOG_RAW, errout,
  730. "%f %f %f %f\n",
  731. inv[k][0], inv[k][1], inv[k][2], inv[k][3]);
  732. }
  733. #endif /* __UNUSED__ */
  734. } else {
  735. #ifndef USE_QT
  736. GPSD_LOG(LOG_DATA, errout,
  737. "LOS matrix is singular, can't calculate DOPs - source '%s'\n",
  738. gpsdata->dev.path);
  739. #endif
  740. return 0;
  741. }
  742. xdop = sqrt(inv[0][0]);
  743. ydop = sqrt(inv[1][1]);
  744. hdop = sqrt(inv[0][0] + inv[1][1]);
  745. vdop = sqrt(inv[2][2]);
  746. pdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2]);
  747. tdop = sqrt(inv[3][3]);
  748. gdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2] + inv[3][3]);
  749. #ifndef USE_QT
  750. GPSD_LOG(LOG_DATA, errout,
  751. "DOPS computed/reported: X=%f/%f, Y=%f/%f, H=%f/%f, V=%f/%f, "
  752. "P=%f/%f, T=%f/%f, G=%f/%f\n",
  753. xdop, dop->xdop, ydop, dop->ydop, hdop, dop->hdop, vdop,
  754. dop->vdop, pdop, dop->pdop, tdop, dop->tdop, gdop, dop->gdop);
  755. #endif
  756. /* Check to see which DOPs we already have. Save values if no value
  757. * from the GPS. Do not overwrite values which came from the GPS */
  758. if (isfinite(dop->xdop) == 0) {
  759. dop->xdop = xdop;
  760. }
  761. if (isfinite(dop->ydop) == 0) {
  762. dop->ydop = ydop;
  763. }
  764. if (isfinite(dop->hdop) == 0) {
  765. dop->hdop = hdop;
  766. }
  767. if (isfinite(dop->vdop) == 0) {
  768. dop->vdop = vdop;
  769. }
  770. if (isfinite(dop->pdop) == 0) {
  771. dop->pdop = pdop;
  772. }
  773. if (isfinite(dop->tdop) == 0) {
  774. dop->tdop = tdop;
  775. }
  776. if (isfinite(dop->gdop) == 0) {
  777. dop->gdop = gdop;
  778. }
  779. return DOP_SET;
  780. }
  781. /* compute errors and derived quantities
  782. * also a handy place to do final sanity checking */
  783. static void gpsd_error_model(struct gps_device_t *session)
  784. {
  785. struct gps_fix_t *fix; /* current fix */
  786. struct gps_fix_t *lastfix; /* last fix, maybe same time stamp */
  787. struct gps_fix_t *oldfix; /* old fix, previsou time stamp */
  788. double deltatime = -1.0; /* time span to compute rates */
  789. /*
  790. * Now we compute derived quantities. This is where the tricky error-
  791. * modeling stuff goes. Presently we don't know how to derive
  792. * time error.
  793. *
  794. * Some drivers set the position-error fields. Only the Zodiacs
  795. * report speed error. No NMEA 183 reports climb error. GPXTE
  796. * and PSRFEPE can report track error, but are rare.
  797. *
  798. * The UERE constants are our assumption about the base error of
  799. * GPS fixes in different directions.
  800. */
  801. #define H_UERE_NO_DGPS 15.0 /* meters, 95% confidence */
  802. #define H_UERE_WITH_DGPS 3.75 /* meters, 95% confidence */
  803. #define V_UERE_NO_DGPS 23.0 /* meters, 95% confidence */
  804. #define V_UERE_WITH_DGPS 5.75 /* meters, 95% confidence */
  805. #define P_UERE_NO_DGPS 19.0 /* meters, 95% confidence */
  806. #define P_UERE_WITH_DGPS 4.75 /* meters, 95% confidence */
  807. double h_uere, v_uere, p_uere;
  808. if (NULL == session)
  809. return;
  810. fix = &session->gpsdata.fix;
  811. lastfix = &session->lastfix;
  812. oldfix = &session->oldfix;
  813. if (0 < fix->time.tv_sec) {
  814. /* we have a time for this merge data */
  815. deltatime = TS_SUB_D(&fix->time, &lastfix->time);
  816. if (0.0099 < fabs(deltatime)) {
  817. /* Time just moved, probably forward at least 10 ms.
  818. * Lastfix is now the previous (old) fix. */
  819. *oldfix = *lastfix;
  820. } else {
  821. // compute delta from old fix
  822. deltatime = TS_SUB_D(&fix->time, &oldfix->time);
  823. }
  824. }
  825. /* Sanity check for negative delta? */
  826. h_uere =
  827. (session->gpsdata.status ==
  828. STATUS_DGPS_FIX ? H_UERE_WITH_DGPS : H_UERE_NO_DGPS);
  829. v_uere =
  830. (session->gpsdata.status ==
  831. STATUS_DGPS_FIX ? V_UERE_WITH_DGPS : V_UERE_NO_DGPS);
  832. p_uere =
  833. (session->gpsdata.status ==
  834. STATUS_DGPS_FIX ? P_UERE_WITH_DGPS : P_UERE_NO_DGPS);
  835. if (0 == isfinite(fix->latitude) ||
  836. 0 == isfinite(fix->longitude) || /* both lat/lon, or none */
  837. 90.0 < fabs(fix->latitude) || /* lat out of range */
  838. 180.0 < fabs(fix->longitude)) { /* lon out of range */
  839. fix->latitude = fix->longitude = NAN;
  840. }
  841. /* validate ECEF */
  842. if (0 == isfinite(fix->ecef.x) ||
  843. 0 == isfinite(fix->ecef.y) ||
  844. 0 == isfinite(fix->ecef.z) ||
  845. 10.0 >= (fabs(fix->ecef.x) +
  846. fabs(fix->ecef.y) +
  847. fabs(fix->ecef.z))) { /* all zeros */
  848. fix->ecef.x = fix->ecef.y = fix->ecef.z = NAN;
  849. }
  850. /* if we have not lat/lon, but do have ECEF, calculate lat/lon */
  851. if ((0 == isfinite(fix->longitude) ||
  852. 0 == isfinite(fix->latitude)) &&
  853. 0 != isfinite(fix->ecef.x)) {
  854. session->gpsdata.set |= ecef_to_wgs84fix(fix,
  855. fix->ecef.x, fix->ecef.y,
  856. fix->ecef.z, fix->ecef.vx,
  857. fix->ecef.vy, fix->ecef.vz);
  858. }
  859. /* If you are in a rocket, and your GPS is ITAR unlocked, then
  860. * triple check these sanity checks.
  861. *
  862. * u-blox 8: Max altitude: 50,000m
  863. * Max horizontal speed: 250 m/s
  864. * Max climb: 100 m/s
  865. *
  866. * u-blox ZED-F9P: Max Velocity: 500 m/s
  867. */
  868. /* sanity check the speed, 10,000 m/s should be a nice max
  869. * Low Earth Orbit (LEO) is about 7,800 m/s */
  870. if (9999.9 < fabs(fix->speed))
  871. fix->speed = NAN;
  872. if (9999.9 < fabs(fix->NED.velN))
  873. fix->NED.velN = NAN;
  874. if (9999.9 < fabs(fix->NED.velE))
  875. fix->NED.velE = NAN;
  876. if (9999.9 < fabs(fix->NED.velD))
  877. fix->NED.velD = NAN;
  878. /* sanity check the climb, 10,000 m/s should be a nice max */
  879. if (9999.9 < fabs(fix->climb))
  880. fix->climb = NAN;
  881. if (0 != isfinite(fix->NED.velD) &&
  882. 0 == isfinite(fix->climb)) {
  883. /* have good velD, use it for climb */
  884. fix->climb = -fix->NED.velD;
  885. }
  886. /* compute speed and track from velN and velE if needed and possible */
  887. if (0 != isfinite(fix->NED.velN) &&
  888. 0 != isfinite(fix->NED.velE)) {
  889. if (0 == isfinite(fix->speed)) {
  890. fix->speed = hypot(fix->NED.velN, fix->NED.velE);
  891. }
  892. if (0 == isfinite(fix->track)) {
  893. fix->track = atan2(fix->NED.velE, fix->NED.velN) * RAD_2_DEG;
  894. // normalized later
  895. }
  896. }
  897. /*
  898. * OK, this is not an error computation, but we're at the right
  899. * place in the architecture for it. Compute geoid separation
  900. * and altHAE and altMSL in the simplest possible way.
  901. */
  902. /* geoid (ellipsoid) separation and variation */
  903. if (0 != isfinite(fix->latitude) &&
  904. 0 != isfinite(fix->longitude)) {
  905. if (0 == isfinite(fix->geoid_sep)) {
  906. fix->geoid_sep = wgs84_separation(fix->latitude,
  907. fix->longitude);
  908. }
  909. if (0 == isfinite(fix->magnetic_var) ||
  910. 0.09 >= fabs(fix->magnetic_var)) {
  911. /* some GPS set 0.0,E, or 0,W instead of blank */
  912. fix->magnetic_var = mag_var(fix->latitude,
  913. fix->longitude);
  914. }
  915. }
  916. if (0 != isfinite(fix->magnetic_var)) {
  917. if (0 == isfinite(fix->magnetic_track) &&
  918. 0 != isfinite(fix->track)) {
  919. // calculate mag track, normalized later
  920. fix->magnetic_track = fix->track + fix->magnetic_var;
  921. } else if (0 != isfinite(fix->magnetic_track) &&
  922. 0 == isfinite(fix->track)) {
  923. // calculate true track, normalized later
  924. fix->track = fix->magnetic_track - fix->magnetic_var;
  925. }
  926. }
  927. if (0 != isfinite(fix->track)) {
  928. // normalize true track
  929. DEG_NORM(fix->track);
  930. }
  931. if (0 != isfinite(fix->magnetic_track)) {
  932. // normalize mag track
  933. DEG_NORM(fix->magnetic_track);
  934. }
  935. if (0 != isfinite(fix->geoid_sep)) {
  936. if (0 != isfinite(fix->altHAE) &&
  937. 0 == isfinite(fix->altMSL)) {
  938. /* compute missing altMSL */
  939. fix->altMSL = fix->altHAE - fix->geoid_sep;
  940. } else if (0 == isfinite(fix->altHAE) &&
  941. 0 != isfinite(fix->altMSL)) {
  942. /* compute missing altHAE */
  943. fix->altHAE = fix->altMSL + fix->geoid_sep;
  944. }
  945. }
  946. /*
  947. * OK, this is not an error computation, but we're at the right
  948. * place in the architecture for it. Compute speed over ground
  949. * and climb/sink in the simplest possible way.
  950. */
  951. #ifdef __UNUSED__
  952. // debug code
  953. {
  954. char tbuf[JSON_DATE_MAX+1];
  955. GPSD_LOG(LOG_SHOUT, &session->context->errout,
  956. "time %s deltatime %f\n",
  957. timespec_to_iso8601(fix->time, tbuf, sizeof(tbuf)),
  958. deltatime);
  959. }
  960. #endif // __UNUSED__
  961. if (0 < deltatime) {
  962. /* have a valid time duration */
  963. /* FIXME! ignore if large. maybe > 1 hour? */
  964. if (MODE_2D <= fix->mode &&
  965. MODE_2D <= oldfix->mode) {
  966. if (0 == isfinite(fix->speed)) {
  967. fix->speed = earth_distance(fix->latitude, fix->longitude,
  968. oldfix->latitude, oldfix->longitude)
  969. / deltatime;
  970. /* sanity check */
  971. if (9999.9 < fabs(fix->speed))
  972. fix->speed = NAN;
  973. }
  974. if (MODE_3D <= fix->mode &&
  975. MODE_3D <= oldfix->mode &&
  976. 0 == isfinite(fix->climb) &&
  977. 0 != isfinite(fix->altHAE) &&
  978. 0 != isfinite(oldfix->altHAE)) {
  979. fix->climb = (fix->altHAE - oldfix->altHAE) / deltatime;
  980. /* sanity check the climb */
  981. if (9999.9 < fabs(fix->climb))
  982. fix->climb = NAN;
  983. }
  984. }
  985. }
  986. /*
  987. * Field reports match the theoretical prediction that
  988. * expected time error should be half the resolution of
  989. * the GPS clock, so we put the bound of the error
  990. * in as a constant pending getting it from each driver.
  991. *
  992. * In an ideal world, we'd increase this if no leap-second has
  993. * been seen and it's less than 750s (one almanac load cycle) from
  994. * device powerup. Alas, we have no way to know when device
  995. * powerup occurred - depending on the receiver design it could be
  996. * when the hardware was first powered up or when it was first
  997. * opened. Also, some devices (notably plain NMEA0183 receivers)
  998. * never ship an indication of when they have valid leap second.
  999. */
  1000. if (0 < fix->time.tv_sec &&
  1001. 0 == isfinite(fix->ept)) {
  1002. /* can we compute ept from tdop? */
  1003. fix->ept = 0.005;
  1004. }
  1005. /* Other error computations depend on having a valid fix */
  1006. if (MODE_2D <= fix->mode) {
  1007. if (0 == isfinite(fix->epx) &&
  1008. 0 != isfinite(session->gpsdata.dop.hdop)) {
  1009. fix->epx = session->gpsdata.dop.xdop * h_uere;
  1010. }
  1011. if (0 == isfinite(fix->epy) &&
  1012. 0 != isfinite(session->gpsdata.dop.hdop)) {
  1013. fix->epy = session->gpsdata.dop.ydop * h_uere;
  1014. }
  1015. if (MODE_3D <= fix->mode &&
  1016. 0 == isfinite(fix->epv) &&
  1017. 0 != isfinite(session->gpsdata.dop.vdop)) {
  1018. fix->epv = session->gpsdata.dop.vdop * v_uere;
  1019. }
  1020. /* 2D error */
  1021. if (0 == isfinite(fix->eph) &&
  1022. 0 != isfinite(session->gpsdata.dop.hdop)) {
  1023. fix->eph = session->gpsdata.dop.hdop * p_uere;
  1024. }
  1025. /* 3D error */
  1026. if (0 == isfinite(fix->sep) &&
  1027. 0 != isfinite(session->gpsdata.dop.pdop)) {
  1028. fix->sep = session->gpsdata.dop.pdop * p_uere;
  1029. }
  1030. /*
  1031. * If we have a current fix and an old fix, and the packet handler
  1032. * didn't set the speed error, climb error or track error members
  1033. * itself, try to compute them now.
  1034. */
  1035. #define EMAX(x, y) (((x) > (y)) ? (x) : (y))
  1036. if (0 < deltatime &&
  1037. MODE_2D <= oldfix->mode) {
  1038. if (0 == isfinite(fix->eps) &&
  1039. 0 != isfinite(oldfix->epx) &&
  1040. 0 != isfinite(oldfix->epy)) {
  1041. fix->eps = (EMAX(oldfix->epx, oldfix->epy) +
  1042. EMAX(fix->epx, fix->epy)) / deltatime;
  1043. }
  1044. if (0 == isfinite(fix->epd)) {
  1045. /*
  1046. * We compute a track error estimate solely from the
  1047. * position of this fix and the last one. The maximum
  1048. * track error, as seen from the position of last fix, is
  1049. * the angle subtended by the two most extreme possible
  1050. * error positions of the current fix; the expected track
  1051. * error is half that. Let the position of the old fix be
  1052. * A and of the new fix B. We model the view from A as
  1053. * two right triangles ABC and ABD with BC and BD both
  1054. * having the length of the new fix's estimated error.
  1055. * adj = len(AB), opp = len(BC) = len(BD), hyp = len(AC) =
  1056. * len(AD). This leads to spurious uncertainties
  1057. * near 180 when we're moving slowly; to avoid reporting
  1058. * garbage, throw back NaN if the distance from the previous
  1059. * fix is less than the error estimate.
  1060. */
  1061. double adj = earth_distance(oldfix->latitude, oldfix->longitude,
  1062. fix->latitude, fix->longitude);
  1063. double opp = EMAX(fix->epx, fix->epy);
  1064. if (isfinite(adj) != 0 && adj > opp) {
  1065. double hyp = sqrt(adj * adj + opp * opp);
  1066. fix->epd = RAD_2_DEG * 2 * asin(opp / hyp);
  1067. }
  1068. }
  1069. if (0 == isfinite(fix->epc) &&
  1070. 0 != isfinite(fix->epv) &&
  1071. 0 != isfinite(oldfix->epv)) {
  1072. /* Is this really valid? */
  1073. /* if vertical uncertainties are zero this will be too */
  1074. fix->epc = (oldfix->epv + fix->epv) / deltatime;
  1075. }
  1076. }
  1077. }
  1078. #ifdef __UNUSED__
  1079. {
  1080. // Debug code.
  1081. char tbuf[JSON_DATE_MAX+1];
  1082. GPSD_LOG(&session->context->errout, 0,
  1083. "DEBUG: %s deltatime %.3f, speed %0.3f climb %.3f "
  1084. "epc %.3f fixHAE %.3f oldHAE %.3f\n",
  1085. timespec_to_iso8601(fix->time, tbuf, sizeof(tbuf)),
  1086. deltatime, fix->speed, fix->climb, fix->epc,
  1087. fix->altHAE, oldfix->altHAE);
  1088. }
  1089. #endif // __UNUSED__
  1090. if (0 < fix->time.tv_sec) {
  1091. /* save lastfix, not yet oldfix, for later error computations */
  1092. *lastfix = *fix;
  1093. }
  1094. }
  1095. int gpsd_await_data(fd_set *rfds,
  1096. fd_set *efds,
  1097. const int maxfd,
  1098. fd_set *all_fds,
  1099. struct gpsd_errout_t *errout)
  1100. /* await data from any socket in the all_fds set */
  1101. {
  1102. int status;
  1103. FD_ZERO(efds);
  1104. *rfds = *all_fds;
  1105. GPSD_LOG(LOG_RAW + 1, errout, "select waits\n");
  1106. /*
  1107. * Poll for user commands or GPS data. The timeout doesn't
  1108. * actually matter here since select returns whenever one of
  1109. * the file descriptors in the set goes ready. The point
  1110. * of tracking maxfd is to keep the set of descriptors that
  1111. * pselect(2) has to poll here as small as possible (for
  1112. * low-clock-rate SBCs and the like).
  1113. *
  1114. * pselect(2) is preferable to vanilla select, to eliminate
  1115. * the once-per-second wakeup when no sensors are attached.
  1116. * This cuts power consumption.
  1117. */
  1118. errno = 0;
  1119. status = pselect(maxfd + 1, rfds, NULL, NULL, NULL, NULL);
  1120. if (status == -1) {
  1121. if (errno == EINTR)
  1122. return AWAIT_NOT_READY;
  1123. else if (errno == EBADF) {
  1124. int fd;
  1125. for (fd = 0; fd < (int)FD_SETSIZE; fd++)
  1126. /*
  1127. * All we care about here is a cheap, fast, uninterruptible
  1128. * way to check if a file descriptor is valid.
  1129. */
  1130. if (FD_ISSET(fd, all_fds) && fcntl(fd, F_GETFL, 0) == -1) {
  1131. FD_CLR(fd, all_fds);
  1132. FD_SET(fd, efds);
  1133. }
  1134. return AWAIT_NOT_READY;
  1135. } else {
  1136. GPSD_LOG(LOG_ERROR, errout, "select: %s\n", strerror(errno));
  1137. return AWAIT_FAILED;
  1138. }
  1139. }
  1140. if (errout->debug >= LOG_SPIN) {
  1141. int i;
  1142. char dbuf[BUFSIZ];
  1143. timespec_t ts_now;
  1144. char ts_str[TIMESPEC_LEN];
  1145. dbuf[0] = '\0';
  1146. for (i = 0; i < (int)FD_SETSIZE; i++)
  1147. if (FD_ISSET(i, all_fds))
  1148. str_appendf(dbuf, sizeof(dbuf), "%d ", i);
  1149. str_rstrip_char(dbuf, ' ');
  1150. (void)strlcat(dbuf, "} -> {", sizeof(dbuf));
  1151. for (i = 0; i < (int)FD_SETSIZE; i++)
  1152. if (FD_ISSET(i, rfds))
  1153. str_appendf(dbuf, sizeof(dbuf), " %d ", i);
  1154. (void)clock_gettime(CLOCK_REALTIME, &ts_now);
  1155. GPSD_LOG(LOG_SPIN, errout,
  1156. "pselect() {%s} at %s (errno %d)\n",
  1157. dbuf,
  1158. timespec_str(&ts_now, ts_str, sizeof(ts_str)),
  1159. errno);
  1160. }
  1161. return AWAIT_GOT_INPUT;
  1162. }
  1163. static bool hunt_failure(struct gps_device_t *session)
  1164. /* after a bad packet, what should cue us to go to next autobaud setting? */
  1165. {
  1166. /*
  1167. * We have tried three different tests here.
  1168. *
  1169. * The first was session->badcount++>1. This worked very well on
  1170. * ttys for years and years, but caused failure to sync on TCP/IP
  1171. * sources, which have I/O boundaries in mid-packet more often
  1172. * than RS232 ones. There's a test for this at
  1173. * test/daemon/tcp-torture.log.
  1174. *
  1175. * The second was session->badcount++>1 && session->lexer.state==0.
  1176. * Fail hunt only if we get a second consecutive bad packet
  1177. * and the lexer is in ground state. We don't want to fail on
  1178. * a first bad packet because the source might have a burst of
  1179. * leading garbage after open. We don't want to fail if the
  1180. * lexer is not in ground state, because that means the read
  1181. * might have picked up a valid partial packet - better to go
  1182. * back around the loop and pick up more data.
  1183. *
  1184. * The "&& session->lexer.state==0" guard causes an intermittent
  1185. * hang while autobauding on SiRF IIIs (but not on SiRF-IIs, oddly
  1186. * enough). Removing this conjunct resurrected the failure
  1187. * of test/daemon/tcp-torture.log.
  1188. *
  1189. * Our third attempt, isatty(session->gpsdata.gps_fd) != 0
  1190. * && session->badcount++>1, reverts to the old test that worked
  1191. * well on ttys for ttys and prevents non-tty devices from *ever*
  1192. * having hunt failures. This has the cost that non-tty devices
  1193. * will never get kicked off for presenting bad packets.
  1194. *
  1195. * This test may need further revision.
  1196. */
  1197. return isatty(session->gpsdata.gps_fd) != 0 && session->badcount++>1;
  1198. }
  1199. gps_mask_t gpsd_poll(struct gps_device_t *session)
  1200. /* update the stuff in the scoreboard structure */
  1201. {
  1202. ssize_t newlen;
  1203. bool driver_change = false;
  1204. timespec_t ts_now;
  1205. timespec_t delta;
  1206. char ts_buf[TIMESPEC_LEN];
  1207. gps_clear_fix(&session->newdata);
  1208. /*
  1209. * Input just became available from a sensor, but no read from the
  1210. * device has yet been done.
  1211. *
  1212. * What we actually do here is trickier. For latency-timing
  1213. * purposes, we want to know the time at the start of the current
  1214. * recording cycle. We rely on the fact that even at 4800bps
  1215. * there's a quiet time perceptible to the human eye in gpsmon
  1216. * between when the last character of the last packet in a
  1217. * 1-second cycle ships and when the next reporting cycle
  1218. * ships. Because the cycle time is fixed, higher baud rates will
  1219. * make this gap larger.
  1220. *
  1221. * Thus, we look for an inter-character delay much larger than an
  1222. * average 4800bps sentence time. How should this delay be set? Well,
  1223. * counting framing bits and erring on the side of caution, it's
  1224. * about 480 characters per second or 2083 microeconds per character;
  1225. * that's almost exactly 0.125 seconds per average 60-char sentence.
  1226. * Doubling this to avoid false positives, we look for an inter-character
  1227. * delay of greater than 0.250s.
  1228. *
  1229. * The above assumes a cycle time of 1 second. To get the minimum size of
  1230. * the quiet period, we multiply by the device cycle time.
  1231. *
  1232. * We can sanity-check these calculation by watching logs. If we have set
  1233. * MINIMUM_QUIET_TIME correctly, the "transmission pause" message below
  1234. * will consistently be emitted just before the sentence that shows up
  1235. * as start-of-cycle in gpsmon, and never emitted at any other point
  1236. * in the cycle.
  1237. *
  1238. * In practice, it seems that edge detection succeeds at 9600bps but
  1239. * fails at 4800bps. This is not surprising, as previous profiling has
  1240. * indicated that at 4800bps some devices overrun a 1-second cycle time
  1241. * with the data they transmit.
  1242. */
  1243. #define MINIMUM_QUIET_TIME 0.25
  1244. if (session->lexer.outbuflen == 0) {
  1245. /* beginning of a new packet */
  1246. (void)clock_gettime(CLOCK_REALTIME, &ts_now);
  1247. if (NULL != session->device_type &&
  1248. (0 < session->lexer.start_time.tv_sec ||
  1249. 0 < session->lexer.start_time.tv_nsec)) {
  1250. #ifdef RECONFIGURE_ENABLE
  1251. const double min_cycle = TSTONS(&session->device_type->min_cycle);
  1252. #else
  1253. // Assume that all GNSS receivers are 1Hz
  1254. const double min_cycle = 1;
  1255. #endif /* RECONFIGURE_ENABLE */
  1256. double quiet_time = (MINIMUM_QUIET_TIME * min_cycle);
  1257. double gap;
  1258. gap = TS_SUB_D(&ts_now, &session->lexer.start_time);
  1259. if (gap > min_cycle)
  1260. GPSD_LOG(LOG_WARN, &session->context->errout,
  1261. "cycle-start detector failed.\n");
  1262. else if (gap > quiet_time) {
  1263. GPSD_LOG(LOG_PROG, &session->context->errout,
  1264. "transmission pause of %f\n", gap);
  1265. session->sor = ts_now;
  1266. session->lexer.start_char = session->lexer.char_counter;
  1267. }
  1268. }
  1269. session->lexer.start_time = ts_now;
  1270. }
  1271. if (session->lexer.type >= COMMENT_PACKET) {
  1272. session->observed |= PACKET_TYPEMASK(session->lexer.type);
  1273. }
  1274. /* can we get a full packet from the device? */
  1275. if (session->device_type != NULL) {
  1276. newlen = session->device_type->get_packet(session);
  1277. /* coverity[deref_ptr] */
  1278. GPSD_LOG(LOG_RAW, &session->context->errout,
  1279. "%s is known to be %s\n",
  1280. session->gpsdata.dev.path,
  1281. session->device_type->type_name);
  1282. } else {
  1283. newlen = generic_get(session);
  1284. }
  1285. /* update the scoreboard structure from the GPS */
  1286. GPSD_LOG(LOG_RAW + 1, &session->context->errout,
  1287. "%s sent %zd new characters\n",
  1288. session->gpsdata.dev.path, newlen);
  1289. (void)clock_gettime(CLOCK_REALTIME, &ts_now);
  1290. TS_SUB(&delta, &ts_now, &session->gpsdata.online);
  1291. if (newlen < 0) { /* read error */
  1292. GPSD_LOG(LOG_INF, &session->context->errout,
  1293. "GPS on %s returned error %zd (%s sec since data)\n",
  1294. session->gpsdata.dev.path, newlen,
  1295. timespec_str(&delta, ts_buf, sizeof(ts_buf)));
  1296. session->gpsdata.online.tv_sec = 0;
  1297. session->gpsdata.online.tv_nsec = 0;
  1298. return ERROR_SET;
  1299. } else if (newlen == 0) { /* zero length read, possible EOF */
  1300. /*
  1301. * Multiplier is 2 to avoid edge effects due to sampling at the exact
  1302. * wrong time...
  1303. */
  1304. if (0 < session->gpsdata.online.tv_sec &&
  1305. // FIXME: do this with integer math...
  1306. TSTONS(&delta) >= (TSTONS(&session->gpsdata.dev.cycle) * 2)) {
  1307. GPSD_LOG(LOG_INF, &session->context->errout,
  1308. "GPS on %s is offline (%s sec since data)\n",
  1309. session->gpsdata.dev.path,
  1310. timespec_str(&delta, ts_buf, sizeof(ts_buf)));
  1311. session->gpsdata.online.tv_sec = 0;
  1312. session->gpsdata.online.tv_nsec = 0;
  1313. }
  1314. return NODATA_IS;
  1315. } else /* (newlen > 0) */ {
  1316. GPSD_LOG(LOG_RAW, &session->context->errout,
  1317. "packet sniff on %s finds type %d\n",
  1318. session->gpsdata.dev.path, session->lexer.type);
  1319. if (session->lexer.type == COMMENT_PACKET) {
  1320. if (strcmp((const char *)session->lexer.outbuffer, "# EOF\n") == 0) {
  1321. GPSD_LOG(LOG_PROG, &session->context->errout,
  1322. "synthetic EOF\n");
  1323. return EOF_IS;
  1324. }
  1325. else
  1326. GPSD_LOG(LOG_PROG, &session->context->errout,
  1327. "comment, sync lock deferred\n");
  1328. /* FALL THROUGH */
  1329. } else if (session->lexer.type > COMMENT_PACKET) {
  1330. if (session->device_type == NULL)
  1331. driver_change = true;
  1332. else {
  1333. int newtype = session->lexer.type;
  1334. /*
  1335. * Are we seeing a new packet type? Then we probably
  1336. * want to change drivers.
  1337. */
  1338. bool new_packet_type =
  1339. (newtype != session->device_type->packet_type);
  1340. /*
  1341. * Possibly the old driver has a mode-switcher method, in
  1342. * which case we know it can handle NMEA itself and may
  1343. * want to do special things (like tracking whether a
  1344. * previous mode switch to binary succeeded in suppressing
  1345. * NMEA).
  1346. */
  1347. #ifdef RECONFIGURE_ENABLE
  1348. bool dependent_nmea = (newtype == NMEA_PACKET &&
  1349. session->device_type->mode_switcher != NULL);
  1350. #else
  1351. bool dependent_nmea = false;
  1352. #endif /* RECONFIGURE_ENABLE */
  1353. /*
  1354. * Compute whether to switch drivers.
  1355. * If the previous driver type was sticky and this one
  1356. * isn't, we'll revert after processing the packet.
  1357. */
  1358. driver_change = new_packet_type && !dependent_nmea;
  1359. }
  1360. if (driver_change) {
  1361. const struct gps_type_t **dp;
  1362. for (dp = gpsd_drivers; *dp; dp++)
  1363. if (session->lexer.type == (*dp)->packet_type) {
  1364. GPSD_LOG(LOG_PROG, &session->context->errout,
  1365. "switching to match packet type %d: %s\n",
  1366. session->lexer.type, gpsd_prettydump(session));
  1367. (void)gpsd_switch_driver(session, (*dp)->type_name);
  1368. break;
  1369. }
  1370. }
  1371. session->badcount = 0;
  1372. session->gpsdata.dev.driver_mode =
  1373. (session->lexer.type > NMEA_PACKET) ? MODE_BINARY : MODE_NMEA;
  1374. /* FALL THROUGH */
  1375. } else if (hunt_failure(session) && !gpsd_next_hunt_setting(session)) {
  1376. (void)clock_gettime(CLOCK_REALTIME, &ts_now);
  1377. TS_SUB(&delta, &ts_now, &session->gpsdata.online);
  1378. GPSD_LOG(LOG_INF, &session->context->errout,
  1379. "hunt on %s failed (%s sec since data)\n",
  1380. session->gpsdata.dev.path,
  1381. timespec_str(&delta, ts_buf, sizeof(ts_buf)));
  1382. return ERROR_SET;
  1383. }
  1384. }
  1385. if (session->lexer.outbuflen == 0) { /* got new data, but no packet */
  1386. GPSD_LOG(LOG_RAW + 1, &session->context->errout,
  1387. "New data on %s, not yet a packet\n",
  1388. session->gpsdata.dev.path);
  1389. return ONLINE_SET;
  1390. } else { /* we have recognized a packet */
  1391. gps_mask_t received = PACKET_SET;
  1392. (void)clock_gettime(CLOCK_REALTIME, &session->gpsdata.online);
  1393. GPSD_LOG(LOG_RAW + 1, &session->context->errout,
  1394. "Accepted packet on %s.\n",
  1395. session->gpsdata.dev.path);
  1396. /* track the packet count since achieving sync on the device */
  1397. if (driver_change &&
  1398. (session->drivers_identified & (1 << session->driver_index)) == 0) {
  1399. speed_t speed = gpsd_get_speed(session);
  1400. /* coverity[var_deref_op] */
  1401. GPSD_LOG(LOG_INF, &session->context->errout,
  1402. "%s identified as type %s, %ld sec @ %ubps\n",
  1403. session->gpsdata.dev.path,
  1404. session->device_type->type_name,
  1405. (long)(time(NULL) - session->opentime),
  1406. (unsigned int)speed);
  1407. /* fire the init_query method */
  1408. if (session->device_type != NULL
  1409. && session->device_type->init_query != NULL) {
  1410. /*
  1411. * We can force readonly off knowing this method does
  1412. * not alter device state.
  1413. */
  1414. bool saved = session->context->readonly;
  1415. session->context->readonly = false;
  1416. session->device_type->init_query(session);
  1417. session->context->readonly = saved;
  1418. }
  1419. /* fire the identified hook */
  1420. if (session->device_type != NULL
  1421. && session->device_type->event_hook != NULL)
  1422. session->device_type->event_hook(session, event_identified);
  1423. session->lexer.counter = 0;
  1424. /* let clients know about this. */
  1425. received |= DRIVER_IS;
  1426. /* mark the fact that this driver has been seen */
  1427. session->drivers_identified |= (1 << session->driver_index);
  1428. } else
  1429. session->lexer.counter++;
  1430. /* fire the configure hook, on every packet. Seems excessive... */
  1431. if (session->device_type != NULL
  1432. && session->device_type->event_hook != NULL)
  1433. session->device_type->event_hook(session, event_configure);
  1434. /*
  1435. * The guard looks superfluous, but it keeps the rather expensive
  1436. * gpsd_packetdump() function from being called even when the debug
  1437. * level does not actually require it.
  1438. */
  1439. if (session->context->errout.debug >= LOG_RAW)
  1440. GPSD_LOG(LOG_RAW, &session->context->errout,
  1441. "raw packet of type %d, %zd:%s\n",
  1442. session->lexer.type,
  1443. session->lexer.outbuflen,
  1444. gpsd_prettydump(session));
  1445. /* Get data from current packet into the fix structure */
  1446. if (session->lexer.type != COMMENT_PACKET)
  1447. if (session->device_type != NULL
  1448. && session->device_type->parse_packet != NULL)
  1449. received |= session->device_type->parse_packet(session);
  1450. #ifdef RECONFIGURE_ENABLE
  1451. /*
  1452. * We may want to revert to the last driver that was marked
  1453. * sticky. What this accomplishes is that if we've just
  1454. * processed something like AIVDM, but a driver with control
  1455. * methods or an event hook had been active before that, we
  1456. * keep the information about those capabilities.
  1457. */
  1458. if (!STICKY(session->device_type)
  1459. && session->last_controller != NULL
  1460. && STICKY(session->last_controller))
  1461. {
  1462. session->device_type = session->last_controller;
  1463. GPSD_LOG(LOG_PROG, &session->context->errout,
  1464. "reverted to %s driver...\n",
  1465. session->device_type->type_name);
  1466. }
  1467. #endif /* RECONFIGURE_ENABLE */
  1468. /* are we going to generate a report? if so, count characters */
  1469. if ((received & REPORT_IS) != 0) {
  1470. session->chars = session->lexer.char_counter - session->lexer.start_char;
  1471. }
  1472. session->gpsdata.set = ONLINE_SET | received;
  1473. /*
  1474. * Compute fix-quality data from the satellite positions.
  1475. * These will not overwrite any DOPs reported from the packet
  1476. * we just got.
  1477. */
  1478. if ((received & SATELLITE_SET) != 0
  1479. && session->gpsdata.satellites_visible > 0) {
  1480. session->gpsdata.set |= fill_dop(&session->context->errout,
  1481. &session->gpsdata,
  1482. &session->gpsdata.dop);
  1483. }
  1484. /* copy/merge device data into staging buffers */
  1485. if ((session->gpsdata.set & CLEAR_IS) != 0) {
  1486. /* CLEAR_IS should only be set on first sentence of cycle */
  1487. gps_clear_fix(&session->gpsdata.fix);
  1488. gps_clear_att(&session->gpsdata.attitude);
  1489. }
  1490. /* GPSD_LOG(LOG_PROG, &session->context->errout,
  1491. "transfer mask: %s\n",
  1492. gps_maskdump(session->gpsdata.set)); */
  1493. gps_merge_fix(&session->gpsdata.fix,
  1494. session->gpsdata.set, &session->newdata);
  1495. gpsd_error_model(session);
  1496. /*
  1497. * Count good fixes. We used to check
  1498. * session->gpsdata.status > STATUS_NO_FIX
  1499. * here, but that wasn't quite right. That tells us whether
  1500. * we think we have a valid fix for the current cycle, but remains
  1501. * true while following non-fix packets are received. What we
  1502. * really want to know is whether the last packet received was a
  1503. * fix packet AND held a valid fix. We must ignore non-fix packets
  1504. * AND packets which have fix data but are flagged as invalid. Some
  1505. * devices output fix packets on a regular basis, even when unable
  1506. * to derive a good fix. Such packets should set STATUS_NO_FIX.
  1507. */
  1508. if (0 != (session->gpsdata.set & (LATLON_SET|ECEF_SET))) {
  1509. if ( session->gpsdata.status > STATUS_NO_FIX) {
  1510. session->context->fixcnt++;
  1511. session->fixcnt++;
  1512. } else {
  1513. session->context->fixcnt = 0;
  1514. session->fixcnt = 0;
  1515. }
  1516. }
  1517. /*
  1518. * Sanity check. This catches a surprising number of port and
  1519. * driver errors, including 32-vs.-64-bit problems.
  1520. */
  1521. if ((session->gpsdata.set & TIME_SET) != 0) {
  1522. if (session->newdata.time.tv_sec >
  1523. (time(NULL) + (60 * 60 * 24 * 365))) {
  1524. GPSD_LOG(LOG_WARN, &session->context->errout,
  1525. "date (%lld) more than a year in the future!\n",
  1526. (long long)session->newdata.time.tv_sec);
  1527. } else if (session->newdata.time.tv_sec < 0) {
  1528. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1529. "date (%lld) is negative!\n",
  1530. (long long)session->newdata.time.tv_sec);
  1531. }
  1532. }
  1533. return session->gpsdata.set;
  1534. }
  1535. /* Should never get here */
  1536. GPSD_LOG(LOG_EMERG, &session->context->errout,
  1537. "fell out of gps_poll()!\n");
  1538. return 0;
  1539. }
  1540. int gpsd_multipoll(const bool data_ready,
  1541. struct gps_device_t *device,
  1542. void (*handler)(struct gps_device_t *, gps_mask_t),
  1543. float reawake_time)
  1544. /* consume and handle packets from a specified device */
  1545. {
  1546. if (data_ready)
  1547. {
  1548. int fragments;
  1549. GPSD_LOG(LOG_RAW + 1, &device->context->errout,
  1550. "polling %d\n", device->gpsdata.gps_fd);
  1551. #ifdef NETFEED_ENABLE
  1552. /*
  1553. * Strange special case - the opening transaction on an NTRIP connection
  1554. * may not yet be completed. Try to ratchet things forward.
  1555. */
  1556. if (device->servicetype == service_ntrip
  1557. && device->ntrip.conn_state != ntrip_conn_established) {
  1558. (void)ntrip_open(device, "");
  1559. if (device->ntrip.conn_state == ntrip_conn_err) {
  1560. GPSD_LOG(LOG_WARN, &device->context->errout,
  1561. "connection to ntrip server failed\n");
  1562. device->ntrip.conn_state = ntrip_conn_init;
  1563. return DEVICE_ERROR;
  1564. } else {
  1565. return DEVICE_READY;
  1566. }
  1567. }
  1568. #endif /* NETFEED_ENABLE */
  1569. for (fragments = 0; ; fragments++) {
  1570. gps_mask_t changed = gpsd_poll(device);
  1571. if (changed == EOF_IS) {
  1572. GPSD_LOG(LOG_WARN, &device->context->errout,
  1573. "device signed off %s\n",
  1574. device->gpsdata.dev.path);
  1575. return DEVICE_EOF;
  1576. } else if (changed == ERROR_SET) {
  1577. GPSD_LOG(LOG_WARN, &device->context->errout,
  1578. "device read of %s returned error or "
  1579. "packet sniffer failed sync (flags %s)\n",
  1580. device->gpsdata.dev.path,
  1581. gps_maskdump(changed));
  1582. return DEVICE_ERROR;
  1583. } else if (changed == NODATA_IS) {
  1584. /*
  1585. * No data on the first fragment read means the device
  1586. * fd may have been in an end-of-file condition on select.
  1587. */
  1588. if (fragments == 0) {
  1589. GPSD_LOG(LOG_DATA, &device->context->errout,
  1590. "%s returned zero bytes\n",
  1591. device->gpsdata.dev.path);
  1592. if (device->zerokill) {
  1593. /* failed timeout-and-reawake, kill it */
  1594. gpsd_deactivate(device);
  1595. if (device->ntrip.works) {
  1596. device->ntrip.works = false; // reset so we try this once only
  1597. if (gpsd_activate(device, O_CONTINUE) < 0) {
  1598. GPSD_LOG(LOG_WARN, &device->context->errout,
  1599. "reconnect to ntrip server failed\n");
  1600. return DEVICE_ERROR;
  1601. } else {
  1602. GPSD_LOG(LOG_INF, &device->context->errout,
  1603. "reconnecting to ntrip server\n");
  1604. return DEVICE_READY;
  1605. }
  1606. }
  1607. } else if (reawake_time == 0) {
  1608. return DEVICE_ERROR;
  1609. } else {
  1610. /*
  1611. * Disable listening to this fd for long enough
  1612. * that the buffer can fill up again.
  1613. */
  1614. GPSD_LOG(LOG_DATA, &device->context->errout,
  1615. "%s will be repolled in %f seconds\n",
  1616. device->gpsdata.dev.path, reawake_time);
  1617. device->reawake = time(NULL) + reawake_time;
  1618. return DEVICE_UNREADY;
  1619. }
  1620. }
  1621. /*
  1622. * No data on later fragment reads just means the
  1623. * input buffer is empty. In this case break out
  1624. * of the fragment-processing loop but consider
  1625. * the device still good.
  1626. */
  1627. break;
  1628. }
  1629. /* we got actual data, head off the reawake special case */
  1630. device->zerokill = false;
  1631. device->reawake = (time_t)0;
  1632. /* must have a full packet to continue */
  1633. if ((changed & PACKET_SET) == 0)
  1634. break;
  1635. /* conditional prevents mask dumper from eating CPU */
  1636. if (device->context->errout.debug >= LOG_DATA) {
  1637. if (device->lexer.type == BAD_PACKET)
  1638. GPSD_LOG(LOG_DATA, &device->context->errout,
  1639. "packet with bad checksum from %s\n",
  1640. device->gpsdata.dev.path);
  1641. else
  1642. GPSD_LOG(LOG_DATA, &device->context->errout,
  1643. "packet type %d from %s with %s\n",
  1644. device->lexer.type,
  1645. device->gpsdata.dev.path,
  1646. gps_maskdump(device->gpsdata.set));
  1647. }
  1648. /* handle data contained in this packet */
  1649. if (device->lexer.type != BAD_PACKET)
  1650. handler(device, changed);
  1651. #ifdef __future__
  1652. // this breaks: test/daemon/passthrough.log ??
  1653. /*
  1654. * Bernd Ocklin suggests:
  1655. * Exit when a full packet was received and parsed.
  1656. * This allows other devices to be serviced even if
  1657. * this device delivers a full packet at every single
  1658. * read.
  1659. * Otherwise we can sit here for a long time without
  1660. * any for-loop exit condition being met.
  1661. * It might also reduce the latency from a received packet to
  1662. * it being output by gpsd.
  1663. */
  1664. if ((changed & PACKET_SET) != 0)
  1665. break;
  1666. #endif /* __future__ */
  1667. }
  1668. }
  1669. else if (device->reawake>0 && time(NULL) >device->reawake) {
  1670. /* device may have had a zero-length read */
  1671. GPSD_LOG(LOG_DATA, &device->context->errout,
  1672. "%s reawakened after zero-length read\n",
  1673. device->gpsdata.dev.path);
  1674. device->reawake = (time_t)0;
  1675. device->zerokill = true;
  1676. return DEVICE_READY;
  1677. }
  1678. /* no change in device descriptor state */
  1679. return DEVICE_UNCHANGED;
  1680. }
  1681. void gpsd_wrap(struct gps_device_t *session)
  1682. /* end-of-session wrapup */
  1683. {
  1684. if (!BAD_SOCKET(session->gpsdata.gps_fd))
  1685. gpsd_deactivate(session);
  1686. }
  1687. void gpsd_zero_satellites( struct gps_data_t *out)
  1688. {
  1689. int sat;
  1690. (void)memset(out->skyview, '\0', sizeof(out->skyview));
  1691. out->satellites_visible = 0;
  1692. /* zero is good inbound data for ss, elevation, and azimuth. */
  1693. /* we need to set them to invalid values */
  1694. for ( sat = 0; sat < MAXCHANNELS; sat++ ) {
  1695. out->skyview[sat].azimuth = NAN;
  1696. out->skyview[sat].elevation = NAN;
  1697. out->skyview[sat].ss = NAN;
  1698. out->skyview[sat].freqid = -1;
  1699. }
  1700. #if 0
  1701. /*
  1702. * We used to clear DOPs here, but this causes misbehavior on some
  1703. * combined GPS/GLONASS/QZSS receivers like the Telit SL869; the
  1704. * symptom is that the "satellites_used" field in a struct gps_data_t
  1705. * filled in by gps_read() is always zero.
  1706. */
  1707. gps_clear_dop(&out->dop);
  1708. #endif
  1709. }
  1710. /* Latch the fact that we've saved a fix.
  1711. * And add in the device fudge */
  1712. void ntp_latch(struct gps_device_t *device, struct timedelta_t *td)
  1713. {
  1714. /* this should be an invariant of the way this function is called */
  1715. if (0 >= device->newdata.time.tv_sec) {
  1716. return;
  1717. }
  1718. (void)clock_gettime(CLOCK_REALTIME, &td->clock);
  1719. /* structure copy of time from GPS */
  1720. td->real = device->newdata.time;
  1721. /* is there an offset method? */
  1722. if (NULL != device->device_type &&
  1723. NULL != device->device_type->time_offset) {
  1724. double integral;
  1725. double offset = device->device_type->time_offset(device);
  1726. /* add in offset which is double */
  1727. td->real.tv_nsec += (long)(modf(offset, &integral) * 1e9);
  1728. td->real.tv_sec += (time_t)integral;
  1729. TS_NORM(&td->real);
  1730. }
  1731. /* thread-safe update */
  1732. pps_thread_fixin(&device->pps_thread, td);
  1733. }
  1734. /* end */