void nmea_zero_INFO(nmeaINFO *info)
{
memset(info, 0, sizeof(nmeaINFO));
nmea_time_now(&info->utc);
info->sig = NMEA_SIG_BAD;
info->fix = NMEA_FIX_BAD;
info->smask = 0;
}
void nmea_zero_GPRMC(nmeaGPRMC *pack)
{
memset(pack, 0, sizeof(nmeaGPRMC));
nmea_time_now(&pack->utc);
pack->status = 'V';
pack->ns = 'N';
pack->ew = 'E';
pack->declin_ew = 'E';
}
void nmea_zero_GPGGA(nmeaGPGGA *pack)
{
memset(pack, 0, sizeof(nmeaGPGGA));
nmea_time_now(&pack->utc);
pack->ns = 'N';
pack->ew = 'E';
pack->elv_units = 'M';
pack->diff_units = 'M';
}
/**
* Clear an info structure.
* Resets the time to now, sets up the signal as BAD, the FIX as BAD, and
* signals presence of these fields.
* Resets all other fields to 0.
*
* @param info a pointer to the structure
*/
void nmea_zero_INFO(nmeaINFO *info) {
if (!info) {
return;
}
memset(info, 0, sizeof(nmeaINFO));
nmea_time_now(&info->utc, &info->present);
info->sig = NMEA_SIG_BAD;
nmea_INFO_set_present(&info->present, SIG);
info->fix = NMEA_FIX_BAD;
nmea_INFO_set_present(&info->present, FIX);
}
int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
int count = info->satinfo.inview;
double deg = 360 / (count?count:1);
double srt = (count?(info->satinfo.sat[0].azimuth):0) + 5;
nmea_time_now(&info->utc);
for(it = 0; it < count; ++it)
{
info->satinfo.sat[it].azimuth =
(int)((srt >= 360)?srt - 360:srt);
srt += deg;
}
return 1;
};
/**
* Sanitise the NMEA info, make sure that:
* - sig is in the range [0, 8],
* - fix is in the range [1, 3],
* - DOPs are positive,
* - latitude is in the range [-9000, 9000],
* - longitude is in the range [-18000, 18000],
* - speed is positive,
* - track is in the range [0, 360>.
* - mtrack is in the range [0, 360>.
* - magvar is in the range [0, 360>.
* - satinfo:
* - inuse and in_use are consistent (w.r.t. count)
* - inview and sat are consistent (w.r.t. count/id)
* - in_use and sat are consistent (w.r.t. count/id)
* - elv is in the range [0, 90]
* - azimuth is in the range [0, 359]
* - sig is in the range [0, 99]
*
* Time is set to the current time when not present.
* Fields are reset to their defaults (0) when not signaled as being present.
*
* @param nmeaInfo
* the NMEA info structure to sanitise
*/
void nmea_INFO_sanitise(nmeaINFO *nmeaInfo) {
double lat = 0;
double lon = 0;
double speed = 0;
double track = 0;
double mtrack = 0;
double magvar = 0;
bool latAdjusted = false;
bool lonAdjusted = false;
bool speedAdjusted = false;
bool trackAdjusted = false;
bool mtrackAdjusted = false;
bool magvarAdjusted = false;
nmeaTIME utc;
int inuseIndex;
int inviewIndex;
if (!nmeaInfo) {
return;
}
nmeaInfo->present = nmeaInfo->present & NMEA_INFO_PRESENT_MASK;
if (!nmea_INFO_is_present(nmeaInfo->present, SMASK)) {
nmeaInfo->smask = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE) || !nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
nmea_time_now(&utc, NULL);
}
if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE)) {
nmeaInfo->utc.year = utc.year;
nmeaInfo->utc.mon = utc.mon;
nmeaInfo->utc.day = utc.day;
}
if (!nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
nmeaInfo->utc.hour = utc.hour;
nmeaInfo->utc.min = utc.min;
nmeaInfo->utc.sec = utc.sec;
nmeaInfo->utc.hsec = utc.hsec;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SIG)) {
nmeaInfo->sig = NMEA_SIG_BAD;
} else {
if ((nmeaInfo->sig < NMEA_SIG_BAD) || (nmeaInfo->sig > NMEA_SIG_SIM)) {
nmeaInfo->sig = NMEA_SIG_BAD;
}
}
if (!nmea_INFO_is_present(nmeaInfo->present, FIX)) {
nmeaInfo->fix = NMEA_FIX_BAD;
} else {
if ((nmeaInfo->fix < NMEA_FIX_BAD) || (nmeaInfo->fix > NMEA_FIX_3D)) {
nmeaInfo->fix = NMEA_FIX_BAD;
}
}
if (!nmea_INFO_is_present(nmeaInfo->present, PDOP)) {
nmeaInfo->PDOP = 0;
} else {
nmeaInfo->PDOP = fabs(nmeaInfo->PDOP);
}
if (!nmea_INFO_is_present(nmeaInfo->present, HDOP)) {
nmeaInfo->HDOP = 0;
} else {
nmeaInfo->HDOP = fabs(nmeaInfo->HDOP);
}
if (!nmea_INFO_is_present(nmeaInfo->present, VDOP)) {
nmeaInfo->VDOP = 0;
} else {
nmeaInfo->VDOP = fabs(nmeaInfo->VDOP);
}
if (!nmea_INFO_is_present(nmeaInfo->present, LAT)) {
nmeaInfo->lat = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, LON)) {
nmeaInfo->lon = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, ELV)) {
nmeaInfo->elv = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SPEED)) {
nmeaInfo->speed = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, TRACK)) {
nmeaInfo->track = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, MTRACK)) {
nmeaInfo->mtrack = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, MAGVAR)) {
nmeaInfo->magvar = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSECOUNT)) {
nmeaInfo->satinfo.inuse = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSE)) {
memset(&nmeaInfo->satinfo.in_use, 0, sizeof(nmeaInfo->satinfo.in_use));
}
if (!nmea_INFO_is_present(nmeaInfo->present, SATINVIEW)) {
nmeaInfo->satinfo.inview = 0;
memset(&nmeaInfo->satinfo.sat, 0, sizeof(nmeaInfo->satinfo.sat));
}
/*
* lat
*/
lat = nmeaInfo->lat;
lon = nmeaInfo->lon;
/* force lat in [-18000, 18000] */
while (lat < -18000.0) {
lat += 36000.0;
latAdjusted = true;
}
while (lat > 18000.0) {
lat -= 36000.0;
latAdjusted = true;
}
/* lat is now in [-18000, 18000] */
/* force lat from <9000, 18000] in [9000, 0] */
if (lat > 9000.0) {
lat = 18000.0 - lat;
lon += 18000.0;
latAdjusted = true;
lonAdjusted = true;
}
/* force lat from [-18000, -9000> in [0, -9000] */
if (lat < -9000.0) {
lat = -18000.0 - lat;
lon += 18000.0;
latAdjusted = true;
lonAdjusted = true;
}
/* lat is now in [-9000, 9000] */
if (latAdjusted) {
nmeaInfo->lat = lat;
}
/*
* lon
*/
/* force lon in [-18000, 18000] */
while (lon < -18000.0) {
lon += 36000.0;
lonAdjusted = true;
}
while (lon > 18000.0) {
lon -= 36000.0;
lonAdjusted = true;
}
/* lon is now in [-18000, 18000] */
if (lonAdjusted) {
nmeaInfo->lon = lon;
}
/*
* speed
*/
speed = nmeaInfo->speed;
track = nmeaInfo->track;
mtrack = nmeaInfo->mtrack;
if (speed < 0.0) {
speed = -speed;
track += 180.0;
mtrack += 180.0;
speedAdjusted = true;
trackAdjusted = true;
mtrackAdjusted = true;
}
/* speed is now in [0, max> */
if (speedAdjusted) {
nmeaInfo->speed = speed;
}
/*
* track
*/
/* force track in [0, 360> */
while (track < 0.0) {
track += 360.0;
trackAdjusted = true;
}
while (track >= 360.0) {
track -= 360.0;
trackAdjusted = true;
}
/* track is now in [0, 360> */
if (trackAdjusted) {
nmeaInfo->track = track;
}
/*
* mtrack
*/
/* force mtrack in [0, 360> */
while (mtrack < 0.0) {
mtrack += 360.0;
mtrackAdjusted = true;
}
while (mtrack >= 360.0) {
mtrack -= 360.0;
mtrackAdjusted = true;
}
/* mtrack is now in [0, 360> */
if (mtrackAdjusted) {
nmeaInfo->mtrack = mtrack;
}
/*
* magvar
*/
magvar = nmeaInfo->magvar;
/* force magvar in [0, 360> */
while (magvar < 0.0) {
magvar += 360.0;
magvarAdjusted = true;
}
while (magvar >= 360.0) {
magvar -= 360.0;
magvarAdjusted = true;
}
/* magvar is now in [0, 360> */
if (magvarAdjusted) {
nmeaInfo->magvar = magvar;
}
/*
* satinfo
*/
nmeaInfo->satinfo.inuse = 0;
for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
if (nmeaInfo->satinfo.in_use[inuseIndex])
nmeaInfo->satinfo.inuse++;
}
nmeaInfo->satinfo.inview = 0;
for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
if (nmeaInfo->satinfo.sat[inviewIndex].id) {
nmeaInfo->satinfo.inview++;
/* force elv in [-180, 180] */
while (nmeaInfo->satinfo.sat[inviewIndex].elv < -180) {
nmeaInfo->satinfo.sat[inviewIndex].elv += 360;
}
while (nmeaInfo->satinfo.sat[inviewIndex].elv > 180) {
nmeaInfo->satinfo.sat[inviewIndex].elv -= 360;
}
/* elv is now in [-180, 180] */
/* force elv from <90, 180] in [90, 0] */
if (nmeaInfo->satinfo.sat[inviewIndex].elv > 90) {
nmeaInfo->satinfo.sat[inviewIndex].elv = 180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
}
/* force elv from [-180, -90> in [0, -90] */
if (nmeaInfo->satinfo.sat[inviewIndex].elv < -90) {
nmeaInfo->satinfo.sat[inviewIndex].elv = -180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
}
/* elv is now in [-90, 90] */
if (nmeaInfo->satinfo.sat[inviewIndex].elv < 0) {
nmeaInfo->satinfo.sat[inviewIndex].elv = -nmeaInfo->satinfo.sat[inviewIndex].elv;
}
/* elv is now in [0, 90] */
/* force azimuth in [0, 360> */
while (nmeaInfo->satinfo.sat[inviewIndex].azimuth < 0) {
nmeaInfo->satinfo.sat[inviewIndex].azimuth += 360;
}
while (nmeaInfo->satinfo.sat[inviewIndex].azimuth >= 360) {
nmeaInfo->satinfo.sat[inviewIndex].azimuth -= 360;
}
/* azimuth is now in [0, 360> */
/* force sig in [0, 99] */
if (nmeaInfo->satinfo.sat[inviewIndex].sig < 0)
nmeaInfo->satinfo.sat[inviewIndex].sig = 0;
if (nmeaInfo->satinfo.sat[inviewIndex].sig > 99)
nmeaInfo->satinfo.sat[inviewIndex].sig = 99;
}
}
/* make sure the in_use IDs map to sat IDs */
for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
int inuseID = nmeaInfo->satinfo.in_use[inuseIndex];
if (inuseID) {
bool found = false;
for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
int inviewID = nmeaInfo->satinfo.sat[inviewIndex].id;
if (inuseID == inviewID) {
found = true;
break;
}
}
if (!found) {
/* clear the id, did not find it */
nmeaInfo->satinfo.in_use[inuseIndex] = 0;
if (nmeaInfo->satinfo.inuse)
nmeaInfo->satinfo.inuse--;
}
}
}
}
int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
nmea_time_now(&info->utc);
return 1;
};
/**
Send the transmit buffer out over all designated interfaces, called as a
timer callback and also immediately on an external state change.
@param interfaces
a bitmap defining which interfaces to send over
*/
static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
/** txBuffer is used to concatenate the position update and cluster leader messages in */
unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
unsigned int txBufferBytesUsed = 0;
#define txBufferBytesFree (sizeof(txBuffer) - txBufferBytesUsed)
/*
* The first message in txBuffer is an OLSR position update.
*
* The position update is always present.
*
* The second message is the cluster leader message, but only when uplink
* was requested and correctly configured.
*/
UplinkMessage * pu_uplink = (UplinkMessage *) &txBuffer[0];
union olsr_message * pu = &pu_uplink->msg.olsrMessage;
unsigned int pu_size = 0;
union olsr_ip_addr gateway;
MovementState externalState;
nmeaINFO nmeaInfo;
externalState = getExternalState();
/* only fixup timestamp when the position is valid _and_ when the position was not updated */
if (positionValid(&transmitGpsInformation.txPosition) && !transmitGpsInformation.positionUpdated) {
nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc, &transmitGpsInformation.txPosition.nmeaInfo.present);
}
nmeaInfo = transmitGpsInformation.txPosition.nmeaInfo;
transmitGpsInformation.positionUpdated = false;
gateway = transmitGpsInformation.txGateway;
/* convert nmeaINFO to wireformat olsr message */
txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
pu_size = gpsToOlsr(&nmeaInfo, pu, txBufferBytesFree,
((externalState == MOVEMENT_STATE_STATIONARY) ? getUpdateIntervalStationary() : getUpdateIntervalMoving()));
txBufferBytesUsed += pu_size;
/*
* push out to all OLSR interfaces
*/
if (((interfaces & TX_INTERFACE_OLSR) != 0) && getOlsrTtl() && (pu_size > 0)) {
int r;
struct interface *ifn;
for (ifn = ifnet; ifn; ifn = ifn->int_next) {
/* force the pending buffer out if there's not enough space for our message */
if ((int)pu_size > net_outbuffer_bytes_left(ifn)) {
net_output(ifn);
}
r = net_outbuffer_push(ifn, pu, pu_size);
if (r != (int) pu_size) {
pudError(
false,
"Could not send to OLSR interface %s: %s (size=%u, r=%d)",
ifn->int_name,
((r == -1) ? "no buffer was found" :
(r == 0) ? "there was not enough room in the buffer" : "unknown reason"), pu_size, r);
}
}
/* loopback to tx interface when so configured */
if (getUseLoopback()) {
(void) packetReceivedFromOlsr(pu, NULL, NULL);
}
}
/* push out over uplink when an uplink is configured */
if (((interfaces & TX_INTERFACE_UPLINK) != 0) && isUplinkAddrSet()) {
int fd = getDownlinkSocketFd();
if (fd != -1) {
union olsr_sockaddr * uplink_addr = getUplinkAddr();
void * addr;
socklen_t addrSize;
UplinkMessage * cl_uplink = (UplinkMessage *) &txBuffer[txBufferBytesUsed];
UplinkClusterLeader * cl = &cl_uplink->msg.clusterLeader;
union olsr_ip_addr * cl_originator = getClusterLeaderOriginator(olsr_cnf->ip_version, cl);
union olsr_ip_addr * cl_clusterLeader = getClusterLeaderClusterLeader(olsr_cnf->ip_version, cl);
unsigned int cl_size =
sizeof(UplinkClusterLeader) - sizeof(cl->leader)
+ ((olsr_cnf->ip_version == AF_INET) ? sizeof(cl->leader.v4) :
sizeof(cl->leader.v6));
unsigned long long uplinkUpdateInterval =
(externalState == MOVEMENT_STATE_STATIONARY) ? getUplinkUpdateIntervalStationary() : getUplinkUpdateIntervalMoving();
if (uplink_addr->in.sa_family == AF_INET) {
addr = &uplink_addr->in4;
addrSize = sizeof(struct sockaddr_in);
} else {
addr = &uplink_addr->in6;
addrSize = sizeof(struct sockaddr_in6);
}
/*
* position update message (pu)
*/
/* set header fields in position update uplink message and adjust
* the validity time to the uplink validity time */
if (pu_size > 0) {
PudOlsrPositionUpdate * pu_gpsMessage = getOlsrMessagePayload(olsr_cnf->ip_version, pu);
setUplinkMessageType(&pu_uplink->header, POSITION);
setUplinkMessageLength(&pu_uplink->header, pu_size);
setUplinkMessageIPv6(&pu_uplink->header, (olsr_cnf->ip_version != AF_INET));
setUplinkMessagePadding(&pu_uplink->header, 0);
/* fixup validity time */
setValidityTime(&pu_gpsMessage->validityTime, uplinkUpdateInterval);
}
/*
* cluster leader message (cl)
*/
/* set cl_uplink header fields */
setUplinkMessageType(&cl_uplink->header, CLUSTERLEADER);
setUplinkMessageLength(&cl_uplink->header, cl_size);
setUplinkMessageIPv6(&cl_uplink->header, (olsr_cnf->ip_version != AF_INET));
setUplinkMessagePadding(&cl_uplink->header, 0);
/* setup cl */
setClusterLeaderVersion(cl, PUD_WIRE_FORMAT_VERSION);
setValidityTime(&cl->validityTime, uplinkUpdateInterval);
/* really need 2 memcpy's here because of olsr_cnf->ipsize */
memcpy(cl_originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
memcpy(cl_clusterLeader, &gateway, olsr_cnf->ipsize);
txBufferBytesUsed += sizeof(UplinkHeader);
txBufferBytesUsed += cl_size;
errno = 0;
if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, addr, addrSize) < 0) {
pudError(true, "Could not send to uplink (size=%u)", txBufferBytesUsed);
}
}
}
}
