refclock_ubx.c 34.6 KB
Newer Older
1 2 3 4 5 6
#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#if defined(REFCLOCK) && defined(CLOCK_UBX)

7
#include "ntpd.h"
Lukas Senger's avatar
Lukas Senger committed
8
#include "ntp_io.h"
9
#include "ntp_refclock.h"
Lukas Senger's avatar
Lukas Senger committed
10
#include "parse.h"
11
#include "timevalops.h"
12 13

#include <stdio.h>
14
#include <sys/time.h>
Lukas Senger's avatar
Lukas Senger committed
15
#include <unistd.h>
Lukas Senger's avatar
Lukas Senger committed
16
#include <ctype.h>
Lukas Senger's avatar
Lukas Senger committed
17

Lukas Senger's avatar
Lukas Senger committed
18 19 20 21
#ifdef HAVE_PPSAPI
# include "ppsapi_timepps.h"
#endif

Lukas Senger's avatar
Lukas Senger committed
22

23
#define STRING_LEN 64 // used for paths
Lukas Senger's avatar
Lukas Senger committed
24 25
#define CFG_MSG_LEN 1000
#define MAX_POSTMSG_COUNT 20
Lukas Senger's avatar
Lukas Senger committed
26
#define BAUDRATE 9600
Lukas Senger's avatar
Lukas Senger committed
27

Lukas Senger's avatar
Lukas Senger committed
28 29 30
#define UBX_MAX_MSG_LEN 300 // Some messages might theoretically be longer but
                            // for the ones we use this should suffice.
#define UBX_BUF_LEN 1000
31 32
#define UBX_SYNC_CHAR_1 0xb5
#define UBX_SYNC_CHAR_2 0x62
Lukas Senger's avatar
Lukas Senger committed
33
#define UBX_DYN_MODEL_STATIONARY 0x02
Lukas Senger's avatar
Lukas Senger committed
34

Lukas Senger's avatar
Lukas Senger committed
35 36 37 38 39 40
struct instance {
	int	unit; // 127.127.XY.unit
	struct	refclockproc *pp;
	struct	peer *peer;
	pps_handle_t pps_handle;
	pps_seq_t pps_seq;
Lukas Senger's avatar
Lukas Senger committed
41 42
	uint16_t tm_count;
	l_fp pps_q[2];
Lukas Senger's avatar
Lukas Senger committed
43
	FILE *data_fp;
Lukas Senger's avatar
Lukas Senger committed
44 45 46 47

	// config
	int min_elev;
	int pps_on;
48
	int pps_hard;
Lukas Senger's avatar
Lukas Senger committed
49
	int pps_echo;
Lukas Senger's avatar
Lukas Senger committed
50
	int utc_on;
Lukas Senger's avatar
Lukas Senger committed
51 52 53
	u_char tmode;
	uint32_t svin_min_dur;
	uint32_t svin_acc_limit;
Lukas Senger's avatar
Lukas Senger committed
54
	char *pos_file;
55 56 57
	int32_t ecef_x_or_lat;
	int32_t ecef_y_or_lon;
	int32_t ecef_z_or_alt;
Lukas Senger's avatar
Lukas Senger committed
58
	int32_t user_config_delay;
Lukas Senger's avatar
Lukas Senger committed
59
	char *data_file;
60
	int tm_edge;
Lukas Senger's avatar
Lukas Senger committed
61 62
	char postmsg[MAX_POSTMSG_COUNT][CFG_MSG_LEN];
	int postmsg_count;
Lukas Senger's avatar
Lukas Senger committed
63 64 65 66 67 68 69

	// for message receiving
	l_fp recv_time;
	int msg_len;
	unsigned char msg_buf[UBX_MAX_MSG_LEN];
};

70 71 72 73 74
enum ubx_edge {
	UBX_EDGE_RISING,
	UBX_EDGE_FALLING,
};

75 76 77 78 79 80 81 82 83 84
enum ubx_gnss {
	UBX_GNSS_GPS,
	UBX_GNSS_SBAS,
	UBX_GNSS_GALILEO,
	UBX_GNSS_BEIDOU,
	UBX_GNSS_IMES,
	UBX_GNSS_QZSS,
	UBX_GNSS_GLONASS,
};

Lukas Senger's avatar
Lukas Senger committed
85 86 87 88 89
enum ubx_port {
	UBX_PORT_DDC = 0,
	UBX_PORT_UART = 1,
	UBX_PORT_USB = 3,
	UBX_PORT_SPI = 4,
Lukas Senger's avatar
Lukas Senger committed
90
};
91

Lukas Senger's avatar
Lukas Senger committed
92 93 94 95 96 97
enum ubx_tmode {
	UBX_TMODE_DISABLED,
	UBX_TMODE_SURVEY,
	UBX_TMODE_FIXED,
};

Lukas Senger's avatar
Lukas Senger committed
98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
enum ubx_class {
	UBX_CLASS_NAV = 0x01,
	UBX_CLASS_RXM = 0x02,
	UBX_CLASS_INF = 0x04,
	UBX_CLASS_ACK = 0x05,
	UBX_CLASS_CFG = 0x06,
	UBX_CLASS_UPD = 0x09,
	UBX_CLASS_MON = 0x0A,
	UBX_CLASS_AID = 0x0B,
	UBX_CLASS_TIM = 0x0D,
	UBX_CLASS_ESF = 0x10,
	UBX_CLASS_MGA = 0x13,
	UBX_CLASS_LOG = 0x21,
	UBX_CLASS_SEC = 0x27,
	UBX_CLASS_HNR = 0x28,
};

Lukas Senger's avatar
Lukas Senger committed
115
enum ubx_msg {
Lukas Senger's avatar
Lukas Senger committed
116
	UBX_MSG_NAV_TIMEUTC = 0x21,
Lukas Senger's avatar
Lukas Senger committed
117
	UBX_MSG_NAV_TIMELS = 0x26,
Lukas Senger's avatar
Lukas Senger committed
118

Lukas Senger's avatar
Lukas Senger committed
119 120 121 122 123
	UBX_MSG_ACK_NAK = 0x00,
	UBX_MSG_ACK_ACK = 0x01,

	UBX_MSG_CFG_PRT = 0x00,
	UBX_MSG_CFG_MSG = 0x01,
Lukas Senger's avatar
Lukas Senger committed
124
	UBX_MSG_CFG_SBAS = 0x16,
125
	UBX_MSG_CFG_NAV5 = 0x24,
Lukas Senger's avatar
Lukas Senger committed
126
	UBX_MSG_CFG_TP5 = 0x31,
Lukas Senger's avatar
Lukas Senger committed
127
	UBX_MSG_CFG_TMODE2 = 0x3D,
128
	UBX_MSG_CFG_GNSS = 0x3E,
Lukas Senger's avatar
Lukas Senger committed
129

Lukas Senger's avatar
Lukas Senger committed
130
	UBX_MSG_TIM_TM2 = 0x03,
Lukas Senger's avatar
Lukas Senger committed
131
	UBX_MSG_TIM_SVIN = 0x04,
Lukas Senger's avatar
Lukas Senger committed
132 133
};

Lukas Senger's avatar
Lukas Senger committed
134 135 136 137
typedef char bitfield1;
typedef short bitfield2;
typedef int32_t bitfield4;

Lukas Senger's avatar
Lukas Senger committed
138 139 140
// According to the ubx protocol specification, messages are designed so that
// structure packing is not a problem.

Lukas Senger's avatar
Lukas Senger committed
141 142 143 144 145 146 147 148 149 150 151 152 153
struct ubx_nav_timeutc {
	uint32_t i_tow;
	uint32_t t_acc;
	int32_t nano;
	u_short year;
	u_char month;
	u_char day;
	u_char hour;
	u_char min;
	u_char sec;
	bitfield1 valid;
};

Lukas Senger's avatar
Lukas Senger committed
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168
struct ubx_nav_timels {
	uint32_t i_tow;
	u_char version;
	u_char reserved1[3];
	u_char src_of_curr_ls;
	char curr_ls;
	u_char src_of_ls_change;
	char ls_change;
	int32_t time_to_ls_event;
	uint16_t date_of_ls_gps_wn;
	uint16_t date_of_ls_gps_dn;
	u_char reserved2[3];
	bitfield1 valid;
};

Lukas Senger's avatar
Lukas Senger committed
169 170 171 172 173 174 175 176 177 178 179 180
struct ubx_cfg_prt {
	u_char port_id;
	u_char reserved1;
	bitfield2 tx_ready;
	bitfield4 mode;
	uint32_t baud_rate;
	bitfield2 in_proto_mask;
	bitfield2 out_proto_mask;
	bitfield2 flags;
	u_char reserved2[2];
};

Lukas Senger's avatar
Lukas Senger committed
181 182 183 184 185 186
struct ubx_cfg_msg {
	u_char msg_class;
	u_char msg_id;
	u_char rate[6];
};

Lukas Senger's avatar
Lukas Senger committed
187 188 189 190 191 192 193 194
struct ubx_cfg_sbas {
	bitfield1 mode;
	bitfield1 usage;
	u_char max_sbas;
	bitfield1 scanmode2;
	bitfield4 scanmode1;
};

195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216
struct ubx_cfg_nav5 {
	bitfield2 mask;
	u_char dyn_model;
	u_char fix_mode;
	int32_t fixed_alt;
	uint32_t fixed_alt_var;
	char min_elev;
	u_char dr_limit;
	uint16_t p_dop;
	uint16_t t_dop;
	uint16_t p_acc;
	uint16_t t_acc;
	u_char static_hold_thresh;
	u_char dgnss_timeout;
	u_char cno_thresh_num_svs;
	u_char cno_thresh;
	u_char reserved1[2];
	u_int16_t static_hold_max_dist;
	u_char utc_standard;
	u_char reserved2[2];
};

Lukas Senger's avatar
Lukas Senger committed
217 218 219 220 221 222 223 224 225 226 227 228 229 230
struct ubx_cfg_tp5 {
	u_char tp_idx;
	u_char version;
	u_char reserved1[2];
	int16_t ant_cable_delay;
	int16_t rf_group_delay;
	uint32_t freq_period;
	uint32_t freq_period_lock;
	uint32_t pulse_len_ratio;
	uint32_t pulse_len_ratio_lock;
	int32_t user_config_delay;
	bitfield4 flags;
};

Lukas Senger's avatar
Lukas Senger committed
231 232 233 234 235 236 237 238 239 240 241 242
struct ubx_cfg_tmode2 {
	u_char time_mode;
	u_char reserved1;
	bitfield2 flags;
	int32_t ecef_x_or_lat;
	int32_t ecef_y_or_lon;
	int32_t ecef_z_or_alt;
	uint32_t fixed_pos_acc;
	uint32_t svin_min_dur;
	uint32_t svin_acc_limit;
};

Lukas Senger's avatar
Lukas Senger committed
243
// With multiple config blocks multiple GNSS can be configured/queried at a time
244 245 246 247 248 249 250 251 252 253 254 255 256 257
struct ubx_cfg_gnss {
	u_char msg_ver;
	u_char num_trk_ch_hw;
	u_char num_trk_ch_use;
	u_char num_config_blocks;
	struct {
		u_char gnss_id;
		u_char res_trk_ch;
		u_char max_trk_ch;
		u_char reserved1;
		bitfield4 flags;
	} config_blocks[7];
};

Lukas Senger's avatar
Lukas Senger committed
258 259 260 261 262 263 264 265 266 267 268 269 270
struct ubx_tim_tm2 {
	u_char ch;
	bitfield1 flags;
	uint16_t count;
	uint16_t wn_r;
	uint16_t wn_f;
	uint32_t tow_ms_r;
	uint32_t tow_sub_ms_r;
	uint32_t tow_ms_f;
	uint32_t tow_sub_ms_f;
	uint32_t acc_est;
};

Lukas Senger's avatar
Lukas Senger committed
271 272 273 274 275 276 277 278 279 280
struct ubx_tim_svin {
	uint32_t dur;
	int32_t mean_x;
	int32_t mean_y;
	int32_t mean_z;
	uint32_t mean_v;
	uint32_t obs;
	u_char valid;
	u_char active;
	u_char reserved1[2];
Lukas Senger's avatar
Lukas Senger committed
281 282
};

283 284 285 286

static int ubx_start (int, struct peer *);
static void ubx_shutdown (int, struct peer *);
static void ubx_poll(int , struct peer *);
Lukas Senger's avatar
Lukas Senger committed
287
static void ubx_timer(int , struct peer *);
288

289
static void ubx_receive(struct recvbuf *);
Lukas Senger's avatar
Lukas Senger committed
290
static int ubx_write(struct instance *, unsigned int, unsigned int, size_t,
Lukas Senger's avatar
Lukas Senger committed
291
                     void *);
Lukas Senger's avatar
Lukas Senger committed
292
static void ubx_checksum(u_char *, size_t, u_char *, u_char *);
Lukas Senger's avatar
Lukas Senger committed
293
static void ubx_str_msg(struct instance *, char *);
294
static void ubx_msg(struct instance *, char, char, u_char *, size_t);
Lukas Senger's avatar
Lukas Senger committed
295
static void ubx_msg_nav_timeutc(struct instance *, u_char *, size_t);
Lukas Senger's avatar
Lukas Senger committed
296
static void ubx_msg_nav_timels(struct instance *, u_char *, size_t);
Lukas Senger's avatar
Lukas Senger committed
297 298 299
static void ubx_msg_ack_ack(struct instance *, u_char *, size_t);
static void ubx_msg_ack_nak(struct instance *, u_char *, size_t);
static void ubx_msg_cfg_gnss(struct instance *, u_char *, size_t);
Lukas Senger's avatar
Lukas Senger committed
300
static void ubx_msg_tim_tm2(struct instance *, u_char *, size_t);
Lukas Senger's avatar
Lukas Senger committed
301
static void ubx_msg_tim_svin(struct instance *, u_char *, size_t);
302

Lukas Senger's avatar
Lukas Senger committed
303
static void ubx_read_config(struct instance *);
304
static int ubx_read_config_file(struct instance *, char *);
305
static int ubx_setup_pps(struct instance *);
Lukas Senger's avatar
Lukas Senger committed
306
static int ubx_fetch_pps(struct instance *, l_fp *);
Lukas Senger's avatar
Lukas Senger committed
307
static void ubx_configure_receiver(struct instance *);
Lukas Senger's avatar
Lukas Senger committed
308 309
static void ubx_log(struct instance *, int, const char *);
static int ubx_log_f(struct instance *, int, const char *, ...);
Lukas Senger's avatar
Lukas Senger committed
310 311
static void ubx_log_offset_diff(struct instance *, l_fp, l_fp);
static void ubx_log_offset(struct instance *);
Lukas Senger's avatar
Lukas Senger committed
312

313 314 315
struct refclock refclock_ubx = {
	ubx_start,
	ubx_shutdown,
316
	ubx_poll,
317 318 319
	noentry,
	noentry,
	noentry,
Lukas Senger's avatar
Lukas Senger committed
320
	ubx_timer,
321 322 323 324
};


static int ubx_start(int unit, struct peer *peer) {
325
	struct refclockproc *pp;
Lukas Senger's avatar
Lukas Senger committed
326
	struct instance *instance;
327
	char serial_path[STRING_LEN];
328
	int ok;
329 330 331

	pp = peer->procptr;

Lukas Senger's avatar
Lukas Senger committed
332 333 334 335 336 337 338
	instance = emalloc(sizeof(*instance));
	memset(instance, 0, sizeof(*instance));
	instance->unit = unit;
	instance->pp = pp;
	instance->peer = peer;

	pp->unitptr = instance;
339
	pp->io.clock_recv = ubx_receive;
340 341
	pp->io.srcclock = peer;

Lukas Senger's avatar
Lukas Senger committed
342
	ubx_log(instance, LOG_NOTICE, "Hello!\n");
343 344
	snprintf(serial_path, sizeof(serial_path), "/dev/ubx.serial.%d", unit);
	pp->io.fd = refclock_open(serial_path, B9600, LDISC_RAW);
Lukas Senger's avatar
Lukas Senger committed
345 346
	if (pp->io.fd == -1)
		return 0;
347

Lukas Senger's avatar
Lukas Senger committed
348
	if (!io_addclock(&pp->io))
349
		return 0;
Lukas Senger's avatar
Lukas Senger committed
350
	ubx_log_f(instance, LOG_NOTICE, "Communicating on %s\n", serial_path);
Lukas Senger's avatar
Lukas Senger committed
351

Lukas Senger's avatar
Lukas Senger committed
352 353
	instance->min_elev = 0;
	instance->pps_on = 1;
Lukas Senger's avatar
Lukas Senger committed
354
	instance->pps_hard = 0;
Lukas Senger's avatar
Lukas Senger committed
355
	instance->pps_echo = 1;
Lukas Senger's avatar
Lukas Senger committed
356
	instance->utc_on = 1;
Lukas Senger's avatar
Lukas Senger committed
357 358 359
	instance->tmode = UBX_TMODE_DISABLED;
	instance->svin_min_dur = 3600; // one hour
	instance->svin_acc_limit = 10000; // 10m
Lukas Senger's avatar
Lukas Senger committed
360
	instance->user_config_delay = 0;
361
	instance->tm_edge = UBX_EDGE_RISING;
Lukas Senger's avatar
Lukas Senger committed
362 363
	ubx_read_config(instance);

Lukas Senger's avatar
Lukas Senger committed
364 365 366 367 368 369 370 371
	if (instance->data_file != NULL) {
		instance->data_fp = fopen(instance->data_file, "w");
		if (instance->data_fp == NULL) {
			ubx_log_f(instance, LOG_ERR, "Opening data file %m");
			return 0;
		}
	}

372 373 374
	if (ubx_setup_pps(instance) == 0)
		return 0;

Lukas Senger's avatar
Lukas Senger committed
375 376 377
	// Disable all output protocols except ubx. This message may lead to
	// corrupted messages from the receiver, so we do all the other
	// configuration only once we have received an ACK for this one.
Lukas Senger's avatar
Lukas Senger committed
378
	struct ubx_cfg_prt cfg_prt = {};
Lukas Senger's avatar
Lukas Senger committed
379
	cfg_prt.port_id = UBX_PORT_UART;
Lukas Senger's avatar
Lukas Senger committed
380 381
	cfg_prt.tx_ready = 0x00;
	cfg_prt.mode = 0b00000000000000000000100011000000;
Lukas Senger's avatar
Lukas Senger committed
382
	cfg_prt.baud_rate = BAUDRATE;
Lukas Senger's avatar
Lukas Senger committed
383 384 385
	cfg_prt.in_proto_mask = 0x0007;
	cfg_prt.out_proto_mask = 0x0001;
	cfg_prt.flags = 0;
386
	ok = ubx_write(
Lukas Senger's avatar
Lukas Senger committed
387 388 389 390 391
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_PRT,
		sizeof(cfg_prt),
		&cfg_prt);
392
	if (ok == 0)
Lukas Senger's avatar
Lukas Senger committed
393 394
		return 0;

Lukas Senger's avatar
Lukas Senger committed
395 396 397 398 399 400 401 402 403 404 405 406
	cfg_prt.port_id = UBX_PORT_USB;
	ok = ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_PRT,
		sizeof(cfg_prt),
		&cfg_prt);
	if (ok == 0)
		return 0;

	sleep(2);
	ubx_configure_receiver(instance);
Lukas Senger's avatar
Lukas Senger committed
407 408
	for (int i = 0; i < instance->postmsg_count; i++)
		ubx_str_msg(instance, instance->postmsg[i]);
409 410 411 412 413 414 415 416 417 418 419 420
	return 1;
}

static int ubx_setup_pps(struct instance *instance) {
	char pps_path[STRING_LEN];
	int fd;
	pps_params_t params;

	if (instance->pps_on == 0)
		return 1;

	snprintf(pps_path, sizeof(pps_path), "/dev/ubx.pps.%d", instance->unit);
Lukas Senger's avatar
Lukas Senger committed
421 422 423 424 425 426 427 428 429 430 431
	if (instance->pps_on == 1) {
		if ((fd = tty_open(pps_path, O_RDWR, 0777)) < 0) {
			ubx_log_f(instance, LOG_ERR, "Can't open %s", pps_path);
			return 0;
		}
		if (time_pps_create(fd, &instance->pps_handle) < 0) {
			ubx_log(instance, LOG_ERR, "PPSAPI not found in kernel");
			return 0;
		}
		ubx_log(instance, LOG_NOTICE, "PPS enabled");
	}
432 433 434 435 436

	if (time_pps_getparams(instance->pps_handle, &params) < 0) {
		ubx_log_f(instance, LOG_ERR, "Getting PPS params failed: %m");
		return 0;
	}
Lukas Senger's avatar
Lukas Senger committed
437 438 439 440 441 442 443

	if (instance->pps_echo == 1) {
		params.mode |= PPS_ECHOASSERT;
	} else {
		params.mode &= ~PPS_ECHOASSERT;
	}

444 445 446 447 448 449
	params.mode |= PPS_CAPTUREASSERT;
	params.mode &= ~PPS_CAPTURECLEAR;
	if (time_pps_setparams(instance->pps_handle, &params) < 0) {
		ubx_log_f(instance, LOG_ERR, "Setting PPS params failed %m");
		return 0;
	}
Lukas Senger's avatar
Lukas Senger committed
450
	
451 452 453 454 455 456
	if (instance->pps_hard) {
		if (time_pps_kcbind(instance->pps_handle, PPS_KC_HARDPPS,
		    PPS_CAPTUREASSERT, PPS_TSFMT_TSPEC) < 0) {
			ubx_log_f(instance, LOG_ERR, "Enabling kernel PPS failed: %m");
			return 0;
		}
457 458 459 460 461
	} else {
		if (time_pps_kcbind(instance->pps_handle, PPS_KC_HARDPPS,
		    0, PPS_TSFMT_TSPEC) < 0) {
			ubx_log_f(instance, LOG_ERR, "Disabling kernel PPS failed: %m");
		}
462 463
	}

464
	return 1;
465 466 467
}

static void ubx_shutdown (int unit, struct peer *peer) {
Lukas Senger's avatar
Lukas Senger committed
468
	struct refclockproc *pp;
Lukas Senger's avatar
Lukas Senger committed
469
	struct instance *instance;
Lukas Senger's avatar
Lukas Senger committed
470 471

	pp = peer->procptr;
Lukas Senger's avatar
Lukas Senger committed
472 473
	instance = pp->unitptr;
	ubx_log(instance, LOG_NOTICE, "Goodbye!");
Lukas Senger's avatar
Lukas Senger committed
474
	close(pp->io.fd);
Lukas Senger's avatar
Lukas Senger committed
475 476
	if (instance->data_fp != NULL)
		fclose(instance->data_fp);
Lukas Senger's avatar
Lukas Senger committed
477 478
	if (instance->pps_on == 1)
		time_pps_destroy(instance->pps_handle);
479 480
}

481 482 483 484 485 486 487
static void ubx_poll(int unit, struct peer *peer){
	struct refclockproc *pp;

	pp = peer->procptr;

	pp->polls++;;
	pp->lastref = pp->lastrec;
Lukas Senger's avatar
Lukas Senger committed
488
	refclock_receive(peer);
489 490
}

Lukas Senger's avatar
Lukas Senger committed
491 492 493 494 495 496 497
static void ubx_timer(int unit , struct peer *peer) {
	struct refclockproc *pp;
	struct instance *instance;

	pp = peer->procptr;
	instance = pp->unitptr;

498 499 500 501 502 503
	ubx_write(
		instance,
		UBX_CLASS_TIM,
		UBX_MSG_TIM_SVIN,
		0,
		NULL);
Lukas Senger's avatar
Lukas Senger committed
504 505
}

506 507 508
static void ubx_receive(struct recvbuf *rbufp) {
	struct peer *peer;
	struct refclockproc *pp;
509
	struct instance *instance;
510
	u_char *p;
511 512 513

	peer = rbufp->recv_peer;
	pp = peer->procptr;
514 515
	instance = pp->unitptr;
	p = (u_char *)&rbufp->recv_space;
516

517
	int i = 0;
Lukas Senger's avatar
Lukas Senger committed
518
	int n = rbufp->recv_length;
519 520 521
	int msg_start = 0;
	int data_start = 0;
	size_t data_len = 0;
Lukas Senger's avatar
Lukas Senger committed
522 523 524 525
	char msg_class = 0;
	char msg_type = 0;
	u_char *buf = instance->msg_buf;
	int len = instance->msg_len;
Lukas Senger's avatar
Lukas Senger committed
526 527
	u_char CK_A;
	u_char CK_B;
528

529 530
	// We set the receive time to the receive time of the first piece of
	// the message to be as close to the navigation solution (on-time) as
531
	// possible. Either here or when an incomplete message is part of
Lukas Senger's avatar
Lukas Senger committed
532 533
	// another chunk.
	if (len == 0)
534
		instance->recv_time = rbufp->recv_time;
535

Lukas Senger's avatar
Lukas Senger committed
536 537
	if (len + n > UBX_BUF_LEN)
		n = UBX_BUF_LEN - len; // losing some characters
Lukas Senger's avatar
Lukas Senger committed
538 539 540 541 542 543 544 545
	instance->msg_len += n;
	memcpy(buf + len, p, n);
	len += n;

	i = 0;
	while (i < len) {
		// If sync char 1 is the last/only char in the buffer, we have
		// to wait until next time to see if sync char 2 will be there.
546 547 548
		if (buf[i] == UBX_SYNC_CHAR_1 && len - i == 1) {
			instance->msg_len = 1;
			buf[0] = UBX_SYNC_CHAR_1;
Lukas Senger's avatar
Lukas Senger committed
549
			return;
550
		}
Lukas Senger's avatar
Lukas Senger committed
551 552 553
		if (buf[i] == UBX_SYNC_CHAR_1 && buf[i + 1] == UBX_SYNC_CHAR_2) {
			// We need at least the first 6 bytes (include length
			// field) of the message available to us.
554
			msg_start = i;
Lukas Senger's avatar
Lukas Senger committed
555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570
			if (len - i >= 6) {
				i += 2;
				msg_class = buf[i++];
				msg_type = buf[i++];
				data_len = buf[i++];
				data_len += buf[i++] << 8;

				// sanity check length
				if (data_len > UBX_MAX_MSG_LEN) {
					ubx_log(instance, LOG_ERR,
						"Sanity check failed, skipping\n");
					i = msg_start + 1;
					continue;
				}
			} else {
				i += 6; // We will fail the length check below
571
			}
Lukas Senger's avatar
Lukas Senger committed
572

573
			// check if message is completely available
Lukas Senger's avatar
Lukas Senger committed
574 575 576 577
			if (((len - i) < 0) || (len - i < data_len + 2)) {
				instance->msg_len = len - msg_start;
				memmove(buf, &buf[msg_start], instance->msg_len);
				return;
578
			}
579 580 581

			data_start = i;
			i += data_len;
Lukas Senger's avatar
Lukas Senger committed
582 583
			ubx_checksum(&buf[msg_start], data_len + 8, &CK_A, &CK_B);
			if (CK_A != buf[i] || CK_B != buf[i + 1]) {
Lukas Senger's avatar
Lukas Senger committed
584
				ubx_log(instance, LOG_ERR,
Lukas Senger's avatar
Lukas Senger committed
585
				        "Checksum not correct, skipping");
Lukas Senger's avatar
Lukas Senger committed
586 587 588 589
				i = msg_start + 1;
				continue;
			}
			i += 2;
590

Lukas Senger's avatar
Lukas Senger committed
591
			ubx_msg(instance, msg_class, msg_type, &buf[data_start],
592
			        data_len);
593
			instance->recv_time = rbufp->recv_time;
594 595 596 597
		} else {
			i++;
		}
	}
Lukas Senger's avatar
Lukas Senger committed
598 599 600

	// No sync chars left in buffer
	instance->msg_len = 0;
601 602
}

Lukas Senger's avatar
Lukas Senger committed
603 604 605 606 607 608 609 610 611 612 613
static int ubx_fetch_pps(struct instance *instance, l_fp *timestamp) {
	pps_info_t pps_info;
	struct timespec timeout;

	timeout.tv_sec = 0;
	timeout.tv_nsec = 0;

	if (time_pps_fetch(instance->pps_handle, PPS_TSFMT_TSPEC,
				&pps_info, &timeout) < 0) {
		ubx_log_f(instance, LOG_ERR, "time_pps_fetch failed %m");
		return 0;
614
	} else if (pps_info.assert_sequence != instance->pps_seq) {
Lukas Senger's avatar
Lukas Senger committed
615 616 617 618 619 620 621 622 623 624 625 626 627
		instance->pps_seq = pps_info.assert_sequence;

		// convert timespec -> ntp l_fp
		timestamp->l_uf = pps_info.assert_timestamp.tv_nsec * 4.294967296;
		timestamp->l_ui = pps_info.assert_timestamp.tv_sec;
		timestamp->l_ui += JAN_1970;
		return 1;
	} else {
		ubx_log(instance, LOG_NOTICE, "No good PPS pulse");
		return 0;
	}
}

628 629
static void ubx_msg(struct instance *instance, char class, char type,
                    u_char *msg, size_t len) {
Lukas Senger's avatar
Lukas Senger committed
630
	switch (class) {
Lukas Senger's avatar
Lukas Senger committed
631 632 633 634 635
		case UBX_CLASS_NAV:
			switch (type) {
				case UBX_MSG_NAV_TIMEUTC:
					ubx_msg_nav_timeutc(instance, msg, len);
					break;
Lukas Senger's avatar
Lukas Senger committed
636 637 638
				case UBX_MSG_NAV_TIMELS:
					ubx_msg_nav_timels(instance, msg, len);
					break;
Lukas Senger's avatar
Lukas Senger committed
639 640
			}
			break;
Lukas Senger's avatar
Lukas Senger committed
641
		case UBX_CLASS_ACK:
Lukas Senger's avatar
Lukas Senger committed
642
			switch (type) {
Lukas Senger's avatar
Lukas Senger committed
643
				case UBX_MSG_ACK_ACK:
644
					ubx_msg_ack_ack(instance, msg, len);
Lukas Senger's avatar
Lukas Senger committed
645
					break;
Lukas Senger's avatar
Lukas Senger committed
646
				case UBX_MSG_ACK_NAK:
647
					ubx_msg_ack_nak(instance, msg, len);
Lukas Senger's avatar
Lukas Senger committed
648 649 650
					break;
			}
			break;
651 652 653
		case UBX_CLASS_CFG:
			switch (type) {
				case UBX_MSG_CFG_GNSS:
654
					ubx_msg_cfg_gnss(instance, msg, len);
655 656 657
					break;

			}
Lukas Senger's avatar
Lukas Senger committed
658
		case UBX_CLASS_TIM:
Lukas Senger's avatar
Lukas Senger committed
659
			switch (type) {
Lukas Senger's avatar
Lukas Senger committed
660 661
				case UBX_MSG_TIM_SVIN:
					ubx_msg_tim_svin(instance, msg, len);
Lukas Senger's avatar
Lukas Senger committed
662
					break;
Lukas Senger's avatar
Lukas Senger committed
663 664 665
				case UBX_MSG_TIM_TM2:
					ubx_msg_tim_tm2(instance, msg, len);
					break;
Lukas Senger's avatar
Lukas Senger committed
666
			}
Lukas Senger's avatar
Lukas Senger committed
667 668
			break;
		default:
669 670 671 672 673 674 675 676
			ubx_log_f(
				instance,
				LOG_NOTICE,
				"Received unhandled message: class %02x, type %02x, length %ld\n",
				class,
				type,
				len
			);
Lukas Senger's avatar
Lukas Senger committed
677 678 679 680
			for (int i = 0; i < len; i++) {
				printf("%02x ", msg[i]);
			}
			printf("\n");
681
	}
682 683
}

Lukas Senger's avatar
Lukas Senger committed
684 685 686 687
static void ubx_msg_nav_timeutc(struct instance *instance,
                                u_char *msg, size_t len) {
	struct peer *peer;
	struct refclockproc *pp;
688
	struct ubx_nav_timeutc time;
689 690
	l_fp recv_pps;
	l_fp diff;
691
	double diff_double;
Lukas Senger's avatar
Lukas Senger committed
692 693 694 695

	peer = instance->peer;
	pp = instance->pp;

696 697
	memcpy(&time, msg, len);
	if ((time.valid & 0b00000100) == 0) {
Lukas Senger's avatar
Lukas Senger committed
698 699 700
		ubx_log(instance, LOG_NOTICE, "GPS timestamp not valid yet");
		return;
	}
701 702 703 704 705 706 707
	pp->year = time.year;
	pp->day = ymd2yd(time.year, time.month, time.day);
	pp->hour = time.hour;
	pp->minute = time.min;
	pp->second = time.sec;
	// time.nano might be negative but ntp handles this properly.
	pp->nsec = time.nano;
Lukas Senger's avatar
Lukas Senger committed
708 709 710
	pp->lastrec = instance->recv_time;

	if (instance->pps_on == 1) {
Lukas Senger's avatar
Lukas Senger committed
711 712
		if (ubx_fetch_pps(instance, &recv_pps) == 0)
			return;
713

Lukas Senger's avatar
Lukas Senger committed
714 715
		diff = instance->recv_time;
		L_SUB(&diff, &recv_pps);
716 717 718
		if (L_ISNEG(&diff) == 1)
			L_NEG(&diff);
		LFPTOD(&diff, diff_double);
Lukas Senger's avatar
Lukas Senger committed
719

720
		if (diff_double < 0.5) {
Lukas Senger's avatar
Lukas Senger committed
721 722 723 724
			pp->nsec = 0; // PPS is aligned with UTC, so no
					// subsecond offset
			peer->flags |= FLAG_PPS;
			pp->lastrec = recv_pps;
Lukas Senger's avatar
Lukas Senger committed
725
		} else {
Lukas Senger's avatar
Lukas Senger committed
726 727
			ubx_log(instance, LOG_NOTICE,
				"Unable to relate PPS pulse to UTC message");
Lukas Senger's avatar
Lukas Senger committed
728 729 730 731 732 733 734
			peer->flags &= ~FLAG_PPS;
		}
	}

	if (!refclock_process(pp)) {
		refclock_report(peer, CEVNT_BADTIME);
		printf("fail\n");
Lukas Senger's avatar
Lukas Senger committed
735
		return;
Lukas Senger's avatar
Lukas Senger committed
736
	}
Lukas Senger's avatar
Lukas Senger committed
737
	ubx_log_offset(instance);
Lukas Senger's avatar
Lukas Senger committed
738 739
}

Lukas Senger's avatar
Lukas Senger committed
740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763
static void ubx_msg_nav_timels(struct instance *instance,
                                u_char *msg, size_t len) {
	struct refclockproc *pp;
	struct ubx_nav_timels leap;

	pp = instance->pp;

	memcpy(&leap, msg, len);
	ubx_log_f(instance, LOG_NOTICE, "leap = %d", leap.ls_change);

	// Check if leap second happens in 27 days to be sure it is this month.
	if (leap.valid & 0x02) {
		if (leap.time_to_ls_event > 0 &&
		    leap.time_to_ls_event < 27*SECSPERDAY) {
			if (leap.ls_change == 1)
				pp->leap = LEAP_ADDSECOND;
			if (leap.ls_change == -1)
				pp->leap = LEAP_DELSECOND;
		} else {
			pp->leap = LEAP_NOWARNING;
		}
	}
}

764 765
static void ubx_msg_ack_ack(struct instance *instance,
                            u_char *msg, size_t len) {
Lukas Senger's avatar
Lukas Senger committed
766
	static int c_sbas = 0;
Lukas Senger's avatar
Lukas Senger committed
767
	static int c_msg = 0;
Lukas Senger's avatar
Lukas Senger committed
768
	static int c_prt = 0;
Lukas Senger's avatar
Lukas Senger committed
769 770
	if (msg[0] != UBX_CLASS_CFG)
		return;
Lukas Senger's avatar
Lukas Senger committed
771

Lukas Senger's avatar
Lukas Senger committed
772 773
	if (msg[1] == UBX_MSG_CFG_NAV5) {
		ubx_log_f(instance, LOG_NOTICE,
Lukas Senger's avatar
Lukas Senger committed
774
		          "Setting stationary mode and min elevation successful");
Lukas Senger's avatar
Lukas Senger committed
775 776 777 778
	} else if (msg[1] == UBX_MSG_CFG_GNSS) {
		c_sbas++;
		if (c_sbas > 1)
			ubx_log_f(instance, LOG_NOTICE,
Lukas Senger's avatar
Lukas Senger committed
779 780 781 782
			          "Turning off SBAS (new way) successful");
	} else if (msg[1] == UBX_MSG_CFG_SBAS) {
		ubx_log_f(instance, LOG_NOTICE,
		          "Turning off SBAS (old way) successful");
Lukas Senger's avatar
Lukas Senger committed
783 784 785
	} else if (msg[1] == UBX_MSG_CFG_TP5) {
		ubx_log_f(instance, LOG_NOTICE, "Configuring PPS successful");
	} else if (msg[1] == UBX_MSG_CFG_MSG) {
Lukas Senger's avatar
Lukas Senger committed
786
		c_msg++;
Lukas Senger's avatar
Lukas Senger committed
787
		ubx_log_f(instance, LOG_NOTICE,
Lukas Senger's avatar
Lukas Senger committed
788
		          "Enabling ubx messages successful (%d/3)", c_msg);
Lukas Senger's avatar
Lukas Senger committed
789 790
	} else if (msg[1] == UBX_MSG_CFG_TMODE2) {
		ubx_log_f(instance, LOG_NOTICE,
Lukas Senger's avatar
Lukas Senger committed
791
		          "Time mode setup successful");
792 793 794
	} else if (msg[1] == UBX_MSG_NAV_TIMELS) {
		ubx_log_f(instance, LOG_NOTICE,
		          "Leap message setup successful");
Lukas Senger's avatar
Lukas Senger committed
795 796 797 798
	} else if (msg[1] == UBX_MSG_CFG_PRT) {
		c_prt++;
		ubx_log_f(instance, LOG_NOTICE,
		          "Enabling UBX protocol successful (%d/2)", c_prt);
Lukas Senger's avatar
Lukas Senger committed
799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814
	}
}

static void ubx_configure_receiver(struct instance *instance) {
	// Set to stationary mode and min elevation
	struct ubx_cfg_nav5 cfg_nav5 = {};
	cfg_nav5.mask = 0x0003; // only set dynamic platform model and elevation
	cfg_nav5.dyn_model = UBX_DYN_MODEL_STATIONARY;
	cfg_nav5.min_elev = instance->min_elev;
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_NAV5,
		sizeof(cfg_nav5),
		&cfg_nav5);

Lukas Senger's avatar
Lukas Senger committed
815
	// Poll GNSS configuration so we can turn of SBAS in the handler
Lukas Senger's avatar
Lukas Senger committed
816 817 818 819 820 821
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_GNSS,
		0,
		NULL);
Lukas Senger's avatar
Lukas Senger committed
822 823 824 825 826 827 828 829 830 831 832

	// Turn off SBAS on older receivers. This is deprecated (in favor of
	// above) but we just do it both ways to support older receivers.
	struct ubx_cfg_sbas cfg_sbas = {};
	cfg_sbas.mode = 0x00;
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_SBAS,
		sizeof(cfg_sbas),
		&cfg_sbas);
Lukas Senger's avatar
Lukas Senger committed
833 834 835 836 837 838 839 840 841 842 843 844

	// Configure PPS
	if (instance->pps_on == 1) {
		struct ubx_cfg_tp5 cfg_tp5 = {};
		cfg_tp5.tp_idx = 0;
		cfg_tp5.version = 0x01;
		cfg_tp5.ant_cable_delay = 0;
		cfg_tp5.rf_group_delay = 0;
		cfg_tp5.freq_period = 1000000;
		cfg_tp5.freq_period_lock = 1000000;
		cfg_tp5.pulse_len_ratio = 0;
		cfg_tp5.pulse_len_ratio_lock = 100000;
Lukas Senger's avatar
Lukas Senger committed
845
		cfg_tp5.user_config_delay = instance->user_config_delay;
Lukas Senger's avatar
Lukas Senger committed
846 847 848 849 850 851 852 853 854
		cfg_tp5.flags = 0b00000000000000000000000001110111;
		ubx_write(
			instance,
			UBX_CLASS_CFG,
			UBX_MSG_CFG_TP5,
			sizeof(cfg_tp5),
			&cfg_tp5);
	}

Lukas Senger's avatar
Lukas Senger committed
855
	// Set survey in parameters and/or position or turn time mode off
856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872
	if (instance->tmode == UBX_TMODE_DISABLED) {
		ubx_log_f(instance, LOG_NOTICE, "Turning time mode off...");
	} else if (instance->tmode == UBX_TMODE_FIXED) {
		ubx_log_f(instance, LOG_NOTICE, "Setting position:");
		ubx_log_f(instance, LOG_NOTICE, "X = %dcm",
		          instance->ecef_x_or_lat);
		ubx_log_f(instance, LOG_NOTICE, "Y = %dcm",
		          instance->ecef_y_or_lon);
		ubx_log_f(instance, LOG_NOTICE, "Z = %dcm",
		          instance->ecef_z_or_alt);
	} else if (instance->tmode == UBX_TMODE_SURVEY) {
		ubx_log_f(instance, LOG_NOTICE, "Starting site survey:");
		ubx_log_f(instance, LOG_NOTICE, "Minimal duration %us",
		          instance->svin_min_dur);
		ubx_log_f(instance, LOG_NOTICE, "Accuracy limit %umm",
		          instance->svin_acc_limit);
	}
Lukas Senger's avatar
Lukas Senger committed
873 874 875
	struct ubx_cfg_tmode2 cfg_tmode2 = {};
	cfg_tmode2.time_mode = instance->tmode;
	cfg_tmode2.flags = 0x00;
876 877 878 879
	cfg_tmode2.ecef_x_or_lat = instance->ecef_x_or_lat;
	cfg_tmode2.ecef_y_or_lon = instance->ecef_y_or_lon;
	cfg_tmode2.ecef_z_or_alt = instance->ecef_z_or_alt;
	cfg_tmode2.fixed_pos_acc = instance->svin_acc_limit;
Lukas Senger's avatar
Lukas Senger committed
880 881 882 883 884 885 886 887 888
	cfg_tmode2.svin_min_dur = instance->svin_min_dur;
	cfg_tmode2.svin_acc_limit = instance->svin_acc_limit;
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_TMODE2,
		sizeof(cfg_tmode2),
		&cfg_tmode2);

Lukas Senger's avatar
Lukas Senger committed
889 890 891
	// Enable/disable UBX-NAV-TIMEUTC messages. The spec says that this
	// should be preferred over messages that output GNSS time directly or
	// messages that output receiver local time.
Lukas Senger's avatar
Lukas Senger committed
892 893 894
	struct ubx_cfg_msg cfg_msg = {};
	cfg_msg.msg_class = UBX_CLASS_NAV;
	cfg_msg.msg_id = UBX_MSG_NAV_TIMEUTC;
Lukas Senger's avatar
Lukas Senger committed
895
	cfg_msg.rate[UBX_PORT_UART] = instance->utc_on;
Lukas Senger's avatar
Lukas Senger committed
896
	cfg_msg.rate[UBX_PORT_USB] = instance->utc_on;
Lukas Senger's avatar
Lukas Senger committed
897 898 899 900 901 902
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_MSG,
		sizeof(cfg_msg),
		&cfg_msg);
Lukas Senger's avatar
Lukas Senger committed
903

Lukas Senger's avatar
Lukas Senger committed
904
	// Enable/disable timemark messages
Lukas Senger's avatar
Lukas Senger committed
905 906
	cfg_msg.msg_class = UBX_CLASS_TIM;
	cfg_msg.msg_id = UBX_MSG_TIM_TM2;
Lukas Senger's avatar
Lukas Senger committed
907
	cfg_msg.rate[UBX_PORT_UART] = instance->pps_echo;
Lukas Senger's avatar
Lukas Senger committed
908
	cfg_msg.rate[UBX_PORT_USB] = instance->pps_echo;
Lukas Senger's avatar
Lukas Senger committed
909 910 911 912 913 914
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_MSG,
		sizeof(cfg_msg),
		&cfg_msg);
915 916 917 918

	// Enable timels
	cfg_msg.msg_class = UBX_CLASS_NAV;
	cfg_msg.msg_id = UBX_MSG_NAV_TIMELS;
Lukas Senger's avatar
Lukas Senger committed
919
	cfg_msg.rate[UBX_PORT_UART] = 0xff;
Lukas Senger's avatar
Lukas Senger committed
920
	cfg_msg.rate[UBX_PORT_USB] = 0xff;
921 922 923 924 925 926
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_MSG,
		sizeof(cfg_msg),
		&cfg_msg);
927 928 929 930 931 932 933 934 935
}

static void ubx_msg_ack_nak(struct instance *instance,
                            u_char *msg, size_t len) {
	ubx_log(instance, LOG_ERR, "Received NAK\n");
}

static void ubx_msg_cfg_gnss(struct instance *instance,
                             u_char *msg, size_t len) {
936
	struct ubx_cfg_gnss cfg_gnss;
937

938 939 940 941
	memcpy(&cfg_gnss, msg, len);
	for (int i = 0; i < cfg_gnss.num_config_blocks; i++) {
		if (cfg_gnss.config_blocks[i].gnss_id == UBX_GNSS_SBAS) {
			cfg_gnss.config_blocks[i].flags = 0;
942 943 944 945 946 947 948 949
		}
	}
	ubx_log_f(instance, LOG_NOTICE, "Turning off SBAS...");
	ubx_write(
		instance,
		UBX_CLASS_CFG,
		UBX_MSG_CFG_GNSS,
		len,
950
		&cfg_gnss);
951 952
}

Lukas Senger's avatar
Lukas Senger committed
953 954
static void ubx_msg_tim_tm2(struct instance *instance,
                            u_char *msg, size_t len) {
955
	struct ubx_tim_tm2 tm2;
Lukas Senger's avatar
Lukas Senger committed
956 957 958 959
	l_fp timemark;
	l_fp ppstime;
	l_fp diff0;
	l_fp diff1;
Lukas Senger's avatar
Lukas Senger committed
960
	uint32_t nsec;
Lukas Senger's avatar
Lukas Senger committed
961
	uint32_t weeks;
962 963
	uint32_t tow_ms;
	uint32_t tow_sub_ms;
Lukas Senger's avatar
Lukas Senger committed
964

965
	memcpy(&tm2, msg, len);
Lukas Senger's avatar
Lukas Senger committed
966 967 968 969 970 971 972 973
	if (tm2.count == instance->tm_count) {
		ubx_log(instance, LOG_NOTICE, "Received same timemark twice");
		return;
	}
	instance->tm_count = tm2.count;
	ubx_log_f(instance, LOG_INFO,
			"count = %d",
			tm2.count);
Lukas Senger's avatar
Lukas Senger committed
974

975 976 977 978 979 980 981 982 983 984
	weeks = tm2.wn_r;
	tow_ms = tm2.tow_ms_r;
	tow_sub_ms = tm2.tow_sub_ms_r;
	if (instance->tm_edge == UBX_EDGE_FALLING) {
		weeks = tm2.wn_f;
		tow_ms = tm2.tow_ms_f;
		tow_sub_ms = tm2.tow_sub_ms_f;
	}

	nsec = tow_ms%1000 * 1e6 + tow_sub_ms; //offset from tos
Lukas Senger's avatar
Lukas Senger committed
985 986 987 988
	if (instance->data_fp != NULL) {
		fprintf(instance->data_fp, "%u\n", nsec);
		fflush(instance->data_fp);
	}
Lukas Senger's avatar
Lukas Senger committed
989 990 991 992 993 994 995 996 997

	// If TIMEUTC numbers the seconds, no need to go further. Else we use
	// TIMEMARK for all syncing.
	if (instance->utc_on == 1)
		return;

	// Convert GPS timestamp to l_fp
	if (weeks < GPSWRAP)
		weeks += GPSWEEKS;
998
	timemark.l_ui = weeks * SECSPERWEEK + tow_ms/1000 + GPSORIGIN;
Lukas Senger's avatar
Lukas Senger committed
999 1000 1001 1002
	timemark.l_uf = nsec * 4.294967296;

	if (ubx_fetch_pps(instance, &ppstime) == 0)
		return;
1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018

	// When the timemark message is sent, the echo for the current PPS
	// pulse might not have reached the receiver yet. So we never know
	// whether we're matching the PPS timestamp to the correct echo
	// timestamp or that of the last epoch. To solve this we always keep
	// two PPS timestamps and check which one is closer to the timemark.
	//
	// Case 1: The echo pulse reaches the receiver before the timemark
	// message is sent. The latest PPS pulse corresponds to the echo
	// timestamp of this message.
	//
	// Case 2: The echo pulse does not reach the receiver in time. The
	// previous (older) PPS timestamp corresponds to the echo timestamp of
	// this message. We are now reporting the times from one second before,
	// but this shouldn't matter.
	instance->pps_q[0] = instance->pps_q[1]; instance->pps_q[1] = ppstime;
Lukas Senger's avatar
Lukas Senger committed
1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034

	diff0 = instance->pps_q[0];
	L_SUB(&diff0, &timemark);
	if (L_ISNEG(&diff0))
		L_NEG(&diff0);
	diff1 = instance->pps_q[1];
	L_SUB(&diff1, &timemark);
	if (L_ISNEG(&diff1))
		L_NEG(&diff1);

	ubx_log_f(instance, LOG_INFO,
			"diff0 = %s",
			lfptoa(&diff0, 10));
	ubx_log_f(instance, LOG_INFO,
			"diff1 = %s",
			lfptoa(&diff1, 10));
1035

Lukas Senger's avatar
Lukas Senger committed
1036 1037 1038 1039 1040 1041
	if (L_ISGT(&diff0, &diff1)) {
		refclock_process_offset(
				instance->pp,
				timemark,
				instance->pps_q[1],
				instance->pp->fudgetime1);
Lukas Senger's avatar
Lukas Senger committed
1042
		ubx_log_offset_diff(instance, timemark, instance->pps_q[1]);
Lukas Senger's avatar
Lukas Senger committed
1043 1044 1045 1046 1047 1048 1049 1050
		return;
	}

	refclock_process_offset(
			instance->pp,
			timemark,
			instance->pps_q[0],
			instance->pp->fudgetime1);
Lukas Senger's avatar
Lukas Senger committed
1051
	ubx_log_offset_diff(instance, timemark, instance->pps_q[0]);
Lukas Senger's avatar
Lukas Senger committed
1052 1053
}

Lukas Senger's avatar
Lukas Senger committed
1054 1055
static void ubx_msg_tim_svin(struct instance *instance,
                             u_char *msg, size_t len) {
1056
	struct ubx_tim_svin svin;
Lukas Senger's avatar
Lukas Senger committed
1057

1058
	memcpy(&svin, msg, len);
Lukas Senger's avatar
Lukas Senger committed
1059 1060
	ubx_log_f(instance, LOG_NOTICE,
	          "SURVEY: dur=%us x=%dcm y=%dcm z=%dcm var=%umm^2 obs=%u valid=%u active=%u",
1061 1062 1063 1064 1065 1066 1067 1068 1069
	          svin.dur,
	          svin.mean_x,
	          svin.mean_y,
	          svin.mean_z,
	          svin.mean_v,
	          svin.obs,
	          svin.valid,
	          svin.active);

1070 1071 1072
	if (svin.active == 0)
		refclock_ubx.clock_timer = NULL;

Lukas Senger's avatar
Lukas Senger committed
1073
	// We have finished a survey, save position
1074
	if (instance->tmode == UBX_TMODE_SURVEY && svin.active == 0) {
Lukas Senger's avatar
Lukas Senger committed
1075 1076 1077 1078 1079 1080 1081 1082
		if (instance->pos_file == NULL || strlen(instance->pos_file) == 0)
			return;
		FILE *f;
		f = fopen(instance->pos_file, "w");
		if (f == NULL) {
			ubx_log_f(instance, LOG_ERR, "Opening position file %m");
			return;
		}
1083 1084 1085
		fprintf(f, "X %d\n", svin.mean_x);
		fprintf(f, "Y %d\n", svin.mean_y);
		fprintf(f, "Z %d\n", svin.mean_z);
Lukas Senger's avatar
Lukas Senger committed
1086
		fclose(f);
1087 1088 1089
	}
}

Lukas Senger's avatar
Lukas Senger committed
1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100
// from doc
static void ubx_checksum(u_char *msg, size_t len, u_char *CK_A, u_char *CK_B) {
	*CK_A = 0;
	*CK_B = 0;
	for(int i = 2; i < len - 2; i++)
	{
		*CK_A = *CK_A + msg[i];
		*CK_B = *CK_B + *CK_A;
	}
}

Lukas Senger's avatar
Lukas Senger committed
1101
// adapted from gpsd
Lukas Senger's avatar
Lukas Senger committed
1102
int ubx_write(struct instance *instance, unsigned int msg_class,
Lukas Senger's avatar
Lukas Senger committed
1103
              unsigned int msg_id, size_t data_len, void *msg) {
Lukas Senger's avatar
Lukas Senger committed
1104 1105 1106 1107 1108 1109 1110
	unsigned char CK_A, CK_B;
	ssize_t count;
	int ok;
	unsigned char msgbuf[UBX_MAX_MSG_LEN];
	size_t msgbuflen;
	int fd;

1111 1112
	msgbuf[0] = UBX_SYNC_CHAR_1;
	msgbuf[1] = UBX_SYNC_CHAR_2;
Lukas Senger's avatar
Lukas Senger committed
1113 1114 1115 1116 1117 1118 1119

	CK_A = CK_B = 0;
	msgbuf[2] = msg_class;
	msgbuf[3] = msg_id;
	msgbuf[4] = data_len & 0xff;
	msgbuf[5] = (data_len >> 8) & 0xff;

1120
	if (msg == NULL && data_len != 0)
Lukas Senger's avatar
Lukas Senger committed
1121
		return 1;
1122 1123
	if (data_len + 8 > UBX_MAX_MSG_LEN)
		return 0;
Lukas Senger's avatar
Lukas Senger committed
1124 1125 1126
	if (msg != NULL)
		(void)memcpy(&msgbuf[6], msg, data_len);

Lukas Senger's avatar
Lukas Senger committed
1127
	ubx_checksum(msgbuf, data_len + 8, &CK_A, &CK_B);
Lukas Senger's avatar
Lukas Senger committed
1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138

	msgbuf[6 + data_len] = CK_A;
	msgbuf[7 + data_len] = CK_B;
	msgbuflen = data_len + 8;


	fd = instance->pp->io.fd;
	count = write(fd, msgbuf, msgbuflen);
	write(fd, "\r\n", (size_t) 2);

	ok = (count == (size_t) msgbuflen);
Lukas Senger's avatar
Lukas Senger committed
1139
	
Lukas Senger's avatar
Lukas Senger committed
1140 1141 1142
	return ok;
}

Lukas Senger's avatar
Lukas Senger committed
1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172
static void ubx_str_msg(struct instance *instance, char *msgstr) {
	char *pos;
	char *end;
	uint32_t part;
	char msg[UBX_MAX_MSG_LEN];
	int i;

	pos = msgstr;
	i = 0;
	while (1) {
		part = strtoul(pos, &end, 16);
		if (part > UCHAR_MAX) {
			if (errno == EINVAL)
				ubx_log_f(instance, LOG_ERR,
				          "Invalid value in MSG %s", msgstr);
			ubx_log_f(instance, LOG_ERR,
					"Out of range value in MSG %s", msgstr);
			return;
		}
		if (pos == end)
			break;
		msg[i] = (char)part;
		pos = end;
		i++;
	}

	if (ubx_write(instance, msg[0], msg[1], i - 4, &msg[4]) == 0)
		ubx_log_f(instance, LOG_ERR, "Failed sending PREMSG %s", msgstr);
}

Lukas Senger's avatar
Lukas Senger committed
1173 1174
// adapted from oncore driver
static void ubx_read_config(struct instance *instance) {