void get_mbg_utc( unsigned char **buffpp, UTC *utcp ) { utcp->csum = get_lsb_short(buffpp); utcp->valid = get_lsb_short(buffpp); get_mbg_tgps(buffpp, &utcp->t0t); if (fetch_ieee754(buffpp, IEEE_DOUBLE, &utcp->A0, mbg_double) != IEEE_OK) { L_CLR(&utcp->A0); } if (fetch_ieee754(buffpp, IEEE_DOUBLE, &utcp->A1, mbg_double) != IEEE_OK) { L_CLR(&utcp->A1); } utcp->WNlsf = get_lsb_short(buffpp); utcp->DNt = get_lsb_short(buffpp); utcp->delta_tls = *(*buffpp)++; utcp->delta_tlsf = *(*buffpp)++; }
void get_mbg_xyz( unsigned char **buffpp, XYZ xyz ) { int i; for (i = XP; i <= ZP; i++) { if (fetch_ieee754(buffpp, IEEE_DOUBLE, &xyz[i], mbg_double) != IEEE_OK) { L_CLR(&xyz[i]); } } }
void get_mbg_lla( unsigned char **buffpp, LLA lla ) { int i; for (i = LAT; i <= ALT; i++) { if (fetch_ieee754(buffpp, IEEE_DOUBLE, &lla[i], mbg_double) != IEEE_OK) { L_CLR(&lla[i]); } else if (i != ALT) { /* convert to degrees (* 180/PI) */ mfp_mul(&lla[i].l_i, &lla[i].l_uf, lla[i].l_i, lla[i].l_uf, rad2deg_i, rad2deg_f); } } }
/* * cvt_trimtsip * * convert TSIP type format */ static unsigned long cvt_trimtsip( unsigned char *buffer, int size, struct format *format, clocktime_t *clock_time, void *local ) { register struct trimble *t = (struct trimble *)local; /* get local data space */ #define mb(_X_) (buffer[2+(_X_)]) /* shortcut for buffer access */ register u_char cmd; clock_time->flags = 0; if (!t) { return CVT_NONE; /* local data not allocated - sigh! */ } if ((size < 4) || (buffer[0] != DLE) || (buffer[size-1] != ETX) || (buffer[size-2] != DLE)) { printf("TRIMBLE BAD packet, size %d:\n", size); return CVT_NONE; } else { unsigned char *bp; cmd = buffer[1]; switch(cmd) { case CMD_RCURTIME: { /* GPS time */ l_fp secs; int week = getshort((unsigned char *)&mb(4)); l_fp utcoffset; l_fp gpstime; bp = &mb(0); if (fetch_ieee754(&bp, IEEE_SINGLE, &secs, trim_offsets) != IEEE_OK) return CVT_FAIL|CVT_BADFMT; if ((secs.l_i <= 0) || (t->t_utcknown == 0)) { clock_time->flags = PARSEB_POWERUP; return CVT_OK; } if (week < GPSWRAP) { week += GPSWEEKS; } /* time OK */ /* fetch UTC offset */ bp = &mb(6); if (fetch_ieee754(&bp, IEEE_SINGLE, &utcoffset, trim_offsets) != IEEE_OK) return CVT_FAIL|CVT_BADFMT; L_SUB(&secs, &utcoffset); /* adjust GPS time to UTC time */ gpstolfp((unsigned short)week, (unsigned short)0, secs.l_ui, &gpstime); gpstime.l_uf = secs.l_uf; clock_time->utctime = gpstime.l_ui - JAN_1970; TSFTOTVU(gpstime.l_uf, clock_time->usecond); if (t->t_leap == ADDSECOND) clock_time->flags |= PARSEB_LEAPADD; if (t->t_leap == DELSECOND) clock_time->flags |= PARSEB_LEAPDEL; switch (t->t_operable) { case STATUS_SYNC: clock_time->flags &= ~(PARSEB_POWERUP|PARSEB_NOSYNC); break; case STATUS_UNSAFE: clock_time->flags |= PARSEB_NOSYNC; break; case STATUS_BAD: clock_time->flags |= PARSEB_NOSYNC|PARSEB_POWERUP; break; } if (t->t_mode == 0) clock_time->flags |= PARSEB_POSITION; clock_time->flags |= PARSEB_S_LEAP|PARSEB_S_POSITION; return CVT_OK; } /* case 0x41 */ case CMD_RRECVHEALTH: { /* TRIMBLE health */ u_char status = mb(0); switch (status) { case 0x00: /* position fixes */ t->t_operable = STATUS_SYNC; break; case 0x09: /* 1 satellite */ case 0x0A: /* 2 satellites */ case 0x0B: /* 3 satellites */ t->t_operable = STATUS_UNSAFE; break; default: t->t_operable = STATUS_BAD; break; } t->t_mode = status; } break; case CMD_RUTCPARAM: { l_fp t0t; unsigned char *lbp; /* UTC correction data - derive a leap warning */ int tls = t->t_gpsutc = (u_short) getshort((unsigned char *)&mb(12)); /* current leap correction (GPS-UTC) */ int tlsf = t->t_gpsutcleap = (u_short) getshort((unsigned char *)&mb(24)); /* new leap correction */ t->t_weekleap = (u_short) getshort((unsigned char *)&mb(20)); /* week no of leap correction */ if (t->t_weekleap < GPSWRAP) t->t_weekleap = (u_short)(t->t_weekleap + GPSWEEKS); t->t_dayleap = (u_short) getshort((unsigned char *)&mb(22)); /* day in week of leap correction */ t->t_week = (u_short) getshort((unsigned char *)&mb(18)); /* current week no */ if (t->t_week < GPSWRAP) t->t_week = (u_short)(t->t_weekleap + GPSWEEKS); lbp = (unsigned char *)&mb(14); /* last update time */ if (fetch_ieee754(&lbp, IEEE_SINGLE, &t0t, trim_offsets) != IEEE_OK) return CVT_FAIL|CVT_BADFMT; t->t_utcknown = t0t.l_ui != 0; if ((t->t_utcknown) && /* got UTC information */ (tlsf != tls) && /* something will change */ ((t->t_weekleap - t->t_week) < 5)) /* and close in the future */ { /* generate a leap warning */ if (tlsf > tls) t->t_leap = ADDSECOND; else t->t_leap = DELSECOND; } else { t->t_leap = 0; } } break; default: /* it's validly formed, but we don't care about it! */ break; } } return CVT_SKIP; }