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;
}
