void background(){
readDatalink();
time = getUTCTime();
writeDatalink(DATALINK_SEND_FREQUENCY);
outboundBufferMaintenance();
inboundBufferMaintenance();
}
STDMETHODIMP HostWrap::COMGETTER(UTCTime)(LONG64 *aUTCTime)
{
LogRelFlow(("{%p} %s: enter aUTCTime=%p\n", this, "Host::getUTCTime", aUTCTime));
VirtualBoxBase::clearError();
HRESULT hrc;
try
{
CheckComArgOutPointerValidThrow(aUTCTime);
AutoCaller autoCaller(this);
if (FAILED(autoCaller.rc()))
throw autoCaller.rc();
hrc = getUTCTime(aUTCTime);
}
catch (HRESULT hrc2)
{
hrc = hrc2;
}
catch (...)
{
hrc = VirtualBoxBase::handleUnexpectedExceptions(this, RT_SRC_POS);
}
LogRelFlow(("{%p} %s: leave *aUTCTime=%RI64 hrc=%Rhrc\n", this, "Host::getUTCTime", *aUTCTime, hrc));
return hrc;
}
static void updateGGA(void* arg) {
argsCarrier *Args = (argsCarrier*)arg;
nmeaINFO *info = Args->info;
char* NMEA2 = Args->NMEA;
GpsLocation newLoc;
GpsStatus gpsStat;
gpsStat.size = sizeof(gpsStat);
if (info->sig == 0) {
LOGV("No valid fix data.");
goto endGGA;
}
newLoc.size = sizeof(GpsLocation);
newLoc.flags = GPS_LOCATION_HAS_LAT_LONG | GPS_LOCATION_HAS_ALTITUDE | GPS_LOCATION_HAS_ACCURACY;
newLoc.accuracy = info->HDOP;
newLoc.altitude = info->elv;
newLoc.latitude = convertCoord(info->lat);
newLoc.longitude = convertCoord(info->lon);
newLoc.timestamp = getUTCTime(&(info->utc));
LOGV("Lat: %lf Long: %lf", newLoc.latitude, newLoc.longitude);
if (adamGpsCallbacks != NULL) {
adamGpsCallbacks->location_cb(&newLoc);
}
endGGA:
free(info);
free(NMEA2);
free(arg);
}
// No real need for this function.
// GGA does the same and more.
static void updateRMC(void* arg) {
argsCarrier *Args = (argsCarrier*)arg;
nmeaINFO *info = Args->info;
char* NMEA2 = Args->NMEA;
GpsLocation newLoc;
if (info->sig == 0) {
LOGV("No valid fix data.");
goto endRMC;
}
newLoc.size = sizeof(GpsLocation);
newLoc.flags = GPS_LOCATION_HAS_LAT_LONG;
newLoc.latitude = convertCoord(info->lat);
newLoc.longitude = convertCoord(info->lon);
newLoc.timestamp = getUTCTime(&(info->utc));
LOGV("Lat: %lf Long: %lf", newLoc.latitude, newLoc.longitude);
if (adamGpsCallbacks != NULL) {
adamGpsCallbacks->location_cb(&newLoc);
}
endRMC:
free(info);
free(NMEA2);
free(arg);
}
PsmAddress postProbeEvent(IonNode *node, Embargo *embargo)
{
PsmPartition ionwm = getIonwm();
PsmAddress addr;
IonProbe *probe;
IonVdb *ionvdb;
IonNeighbor *neighbor;
PsmAddress nextElt;
unsigned int rtlt; /* Round-trip light time. */
int interval = 6; /* Minimum 6-sec interval. */
PsmAddress elt;
IonProbe *pr;
CHKZERO(node);
CHKZERO(embargo);
addr = psm_zalloc(ionwm, sizeof(IonProbe));
if (addr == 0)
{
putErrmsg("Can't create probe event.", NULL);
return 0;
}
probe = (IonProbe *) psp(ionwm, addr);
CHKZERO(probe);
probe->time = getUTCTime();
probe->destNodeNbr = node->nodeNbr;
probe->neighborNodeNbr = embargo->nodeNbr;
/* Schedule next probe of this embargoed neighbor for the
* time that is the current time plus 2x the round-trip
* light time from the local node to the neighbor (but
* at least 6 seconds). */
ionvdb = getIonVdb();
neighbor = findNeighbor(ionvdb, embargo->nodeNbr, &nextElt);
if (neighbor)
{
rtlt = (neighbor->owltOutbound + neighbor->owltInbound) << 1;
if (rtlt > interval)
{
interval = rtlt;
}
}
probe->time += interval;
for (elt = sm_list_last(ionwm, ionvdb->probes); elt;
elt = sm_list_prev(ionwm, elt))
{
pr = (IonProbe *) psp(ionwm, sm_list_data(ionwm, elt));
CHKZERO(pr);
if (pr->time <= probe->time)
{
return sm_list_insert_after(ionwm, elt, addr);
}
}
return sm_list_insert_first(ionwm, ionvdb->probes, addr);
}
static void writeMemoToIonLog(char *text)
{
static ResourceLock logFileLock;
static char ionLogFileName[264] = "";
static int ionLogFile = -1;
time_t currentTime = getUTCTime();
char timestampBuffer[20];
int textLen;
static char msgbuf[256];
if (text == NULL) return;
/* The log file is shared, so access to it must be
* mutexed. */
if (initResourceLock(&logFileLock) < 0)
{
return;
}
lockResource(&logFileLock);
if (ionLogFile == -1)
{
if (ionLogFileName[0] == '\0')
{
isprintf(ionLogFileName, sizeof ionLogFileName,
"%.255s%cion.log",
getIonWorkingDirectory(),
ION_PATH_DELIMITER);
}
ionLogFile = open(ionLogFileName, O_WRONLY | O_APPEND | O_CREAT,
00666);
if (ionLogFile == -1)
{
unlockResource(&logFileLock);
perror("Can't redirect ION error msgs to log");
return;
}
}
writeTimestampLocal(currentTime, timestampBuffer);
isprintf(msgbuf, sizeof msgbuf, "[%s] %s\n", timestampBuffer, text);
textLen = strlen(msgbuf);
if (write(ionLogFile, msgbuf, textLen) < 0)
{
perror("Can't write ION error message to log file");
}
#ifdef TargetFFS
close(ionLogFile);
ionLogFile = -1;
#endif
unlockResource(&logFileLock);
}
static int writeGPSChannels(SampleRecord *sampleRecord, size_t currentTicks, GPSConfig *config){
int rate = SAMPLE_DISABLED;
{
size_t sr = config->dateCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_DateSample.intValue = getDate();
}
}
}
{
size_t sr = config->timeCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_TimeSample.floatValue = getUTCTime();
}
}
}
{
//latitude sample rate is a stand-in for position sample rate
size_t sr = config->latitudeCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_LatitueSample.floatValue = getLatitude();
sampleRecord->GPS_LongitudeSample.floatValue = getLongitude();
}
}
}
{
size_t sr = config->satellitesCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_SatellitesSample.intValue = getSatellitesUsedForPosition();
}
}
}
{
size_t sr = config->speedCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_SpeedSample.floatValue = getGPSSpeed() * 0.621371192; //convert to MPH
}
}
}
return rate;
}
void inferUtcDelta(char *correctUtcTimeStamp)
{
IonVdb *ionvdb = getIonVdb();
time_t correctUtcTime = readTimestampUTC(correctUtcTimeStamp, 0);
time_t clocktime = getUTCTime() + ionvdb->deltaFromUTC;
int delta = clocktime - correctUtcTime;
char buffer[80];
CHKVOID(setDeltaFromUTC(delta) == 0);
sprintf(buffer, "[i] Delta from UTC revised, is now %d.", delta);
writeMemo(buffer);
}
int writeDatalink(float frequency){
if (time - lastTime > frequency) {
lastTime = time;
struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();
if (statusData == 0){
return 0;
}
statusData->lat = getLatitude();
statusData->lon = getLongitude();
statusData->millis = getUTCTime();
statusData->groundSpeed = getSpeed();
statusData->heading = getHeading();
statusData->lastCommandSent = lastCommandSentCode;
statusData->errorCodes = getErrorCodes();
statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
statusData->steeringSetpoint = getPWM(1);
statusData->throttleSetpoint = getPWM(2);
statusData->steeringOutput = sData;
statusData->throttleOutput = tData;
statusData->debugChar = debugChar;
statusData->debugInt = debugInt;
statusData->debugFloat = debugFloat;
if (BLOCKING_MODE) {
sendTelemetryBlock(statusData);
destroyTelemetryBlock(statusData);
} else {
return pushOutboundTelemetryQueue(statusData);
}
}
else if (time < lastTime){
lastTime = time;
return 0;
}
return 0;
}
void ionProd(unsigned long fromNode, unsigned long toNode,
unsigned long xmitRate, unsigned int owlt)
{
Sdr ionsdr = _ionsdr(NULL);
time_t fromTime;
time_t toTime;
Object elt;
char textbuf[RFX_NOTE_LEN];
if (ionsdr == NULL)
{
if (ionAttach() < 0)
{
writeMemo("[?] ionProd: node not initialized yet.");
return;
}
}
fromTime = getUTCTime(); /* The current time. */
toTime = fromTime + 14400; /* Four hours later. */
elt = rfx_insert_range(fromTime, toTime, fromNode, toNode, owlt);
if (elt == 0)
{
writeMemoNote("[?] ionProd: range insertion failed.",
utoa(owlt));
return;
}
writeMemo("ionProd: range inserted.");
writeMemo(rfx_print_range(sdr_list_data(ionsdr, elt), textbuf));
elt = rfx_insert_contact(fromTime, toTime, fromNode, toNode, xmitRate);
if (elt == 0)
{
writeMemoNote("[?] ionProd: contact insertion failed.",
utoa(xmitRate));
return;
}
writeMemo("ionProd: contact inserted.");
writeMemo(rfx_print_contact(sdr_list_data(ionsdr, elt), textbuf));
}
int trySendAcs(SdrAcsPendingCust *custodian,
BpCtReason reasonCode, unsigned char succeeded,
const CtebScratchpad *cteb)
{
Object signalLElt;
Object signalAddr;
SdrAcsSignal signal;
BpEvent timelineEvent;
Object newSerializedZco;
unsigned long newSerializedLength;
int result;
Sdr bpSdr = getIonsdr();
/* To prevent deadlock, take bpSdr before acsSdr. */
CHKERR(sdr_begin_xn(bpSdr));
CHKERR(sdr_begin_xn(acsSdr));
signalLElt = findSdrAcsSignal(custodian->signals, reasonCode, succeeded,
&signalAddr);
if (signalAddr == 0)
{
ACSLOG_ERROR("Can't find ACS signal");
sdr_exit_xn(acsSdr);
sdr_exit_xn(bpSdr);
return -1;
}
sdr_peek(acsSdr, signal, signalAddr);
newSerializedLength = serializeAcs(signalAddr, &newSerializedZco,
signal.serializedZcoLength);
if (newSerializedLength == 0)
{
ACSLOG_ERROR("Can't serialize new ACS (%lu)", signal.serializedZcoLength);
sdr_cancel_xn(acsSdr);
sdr_cancel_xn(bpSdr);
return -1;
}
ACSLOG_DEBUG("Serialized a new ACS to %s that is %lu long (old: %lu)",
custodian->eid, newSerializedLength, signal.serializedZcoLength);
/* If serializeAcs() (which serializes an ACS that covers all the "old"
* custody IDs as well as 1 "new" custody ID that we're trying to append)
* returned an ACS that's larger than the custodian' preferred size, then:
* 1) Send the old ACS (the biggest ACS that's smaller than custodian's
* preferred size), covering all the old custody IDs but not the new
* one.
* 2) Make a new ACS that includes only the new custody ID.
*/
if (custodian->acsSize > 0 && newSerializedLength >= custodian->acsSize)
{
if(signal.serializedZco == 0)
{
/* We don't have an old unserialized ACS to send. This means the
* first custody signal appended to this ACS exceeded the acsSize
* parameter. The best we can do is send this ACS even though it's
* bigger than the recommended acsSize. */
ACSLOG_WARN("Appending first CS to %s was bigger than %lu",
custodian->eid, custodian->acsSize);
signal.serializedZcoLength = newSerializedLength;
signal.serializedZco = newSerializedZco;
sdr_poke(acsSdr, signalAddr, signal);
sendAcs(signalLElt);
if(sdr_end_xn(acsSdr) < 0)
{
ACSLOG_ERROR("Can't serialize ACS bundle.");
sdr_cancel_xn(bpSdr);
return -1;
}
if (sdr_end_xn(bpSdr) < 0)
{
ACSLOG_ERROR("Can't send ACS bundle.");
return -1;
}
return 0;
}
/* Calling this invalidates our signalLElt and signalAddr pointers, so
* we must re-find the signal before using them again. */
sendAcs(signalLElt);
/* Add the one that was uncovered by the serialized payload back in */
result = appendToSdrAcsSignals(custodian->signals,
signal.pendingCustAddr, reasonCode, succeeded,
cteb);
switch (result)
{
case 0:
/* Success; continue processing. */
break;
default:
ACSLOG_ERROR("Can't carry size-limited ID to new ACS");
sdr_cancel_xn(acsSdr);
sdr_cancel_xn(bpSdr);
return -1;
}
/* Find the uncovered one that we just added. */
signalLElt = findSdrAcsSignal(custodian->signals, reasonCode,
succeeded, &signalAddr);
if (signalAddr == 0)
{
ACSLOG_ERROR("Can't find ACS signal");
sdr_cancel_xn(acsSdr);
sdr_cancel_xn(bpSdr);
return -1;
}
sdr_peek(acsSdr, signal, signalAddr);
/* Serialize the new one */
newSerializedLength = serializeAcs(signalAddr, &newSerializedZco, 0);
if (newSerializedLength <= 0)
{
ACSLOG_ERROR("Can't serialize new ACS (%lu)", newSerializedLength);
sdr_cancel_xn(acsSdr);
sdr_cancel_xn(bpSdr);
return -1;
}
} else {
if (signal.serializedZco != 0)
{
/* Free the old payload zco. */
zco_destroy(bpSdr, signal.serializedZco);
}
}
/* Store the new ZCO */
signal.serializedZco = newSerializedZco;
signal.serializedZcoLength = newSerializedLength;
/* If there is not an ACS generation countdown timer, create one. */
if(signal.acsDue == 0)
{
timelineEvent.type = csDue;
if(custodian->acsDelay == 0) {
timelineEvent.time = getUTCTime() + DEFAULT_ACS_DELAY;
} else {
timelineEvent.time = getUTCTime() + custodian->acsDelay;
}
timelineEvent.ref = signalLElt;
signal.acsDue = insertBpTimelineEvent(&timelineEvent);
if (signal.acsDue == 0)
{
ACSLOG_ERROR("Can't add timeline event to generate ACS");
sdr_cancel_xn(acsSdr);
sdr_cancel_xn(bpSdr);
return -1;
}
}
sdr_poke(acsSdr, signalAddr, signal);
if(sdr_end_xn(acsSdr) < 0)
{
ACSLOG_ERROR("Can't track ACS");
sdr_cancel_xn(bpSdr);
return -1;
}
if (sdr_end_xn(bpSdr) < 0)
{
ACSLOG_ERROR("Can't add timeline event to generate ACS");
return -1;
}
return 0;
}
static PsmAddress insertRXref(IonRXref *rxref)
{
PsmPartition ionwm = getIonwm();
IonVdb *vdb = getIonVdb();
IonNode *node;
PsmAddress nextElt;
IonRXref arg;
PsmAddress rxaddr;
PsmAddress rxelt;
PsmAddress addr;
IonEvent *event;
PsmAddress rxaddr2;
IonRXref *rxref2;
time_t currentTime = getUTCTime();
/* Load the affected nodes. */
node = findNode(vdb, rxref->toNode, &nextElt);
if (node == NULL)
{
node = addNode(vdb, rxref->toNode);
if (node == NULL)
{
return 0;
}
}
node = findNode(vdb, rxref->fromNode, &nextElt);
if (node == NULL)
{
node = addNode(vdb, rxref->fromNode);
if (node == NULL)
{
return 0;
}
}
/* Construct the range index entry. */
rxaddr = psm_zalloc(ionwm, sizeof(IonRXref));
if (rxaddr == 0)
{
return 0;
}
memcpy((char *) psp(ionwm, rxaddr), (char *) rxref, sizeof(IonRXref));
rxelt = sm_rbt_insert(ionwm, vdb->rangeIndex, rxaddr, rfx_order_ranges,
rxref);
if (rxelt == 0)
{
psm_free(ionwm, rxaddr);
return 0;
}
/* Insert relevant asserted timeline events. */
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = rxref->fromTime;
event->type = IonStartAssertedRange;
event->ref = rxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events, event)
== 0)
{
return 0;
}
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = rxref->toTime;
event->type = IonStopAssertedRange;
event->ref = rxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events, event)
== 0)
{
return 0;
}
if (rxref->toTime > currentTime) /* Affects routes. */
{
vdb->lastEditTime = currentTime;
}
if (rxref->fromNode > rxref->toNode)
{
/* This is a non-canonical range assertion,
* indicating an override of the normal
* symmetry in the owlt between nodes. The
* reverse range assertion does *not* hold. */
return rxaddr;
}
/* This is a canonical range assertion, so we may need
* to insert the imputed (symmetrical) reverse range
* assertion as well. Is the reverse range already
* asserted? */
arg.fromNode = rxref->toNode;
arg.toNode = rxref->fromNode;
arg.fromTime = rxref->fromTime;
rxelt = sm_rbt_search(ionwm, vdb->rangeIndex, rfx_order_ranges,
&arg, &nextElt);
if (rxelt) /* Asserted range exists; we're done. */
{
return rxaddr;
}
/* Reverse range is not asserted, so it must be imputed.
*
* First, load index entry for the imputed range. */
rxaddr2 = psm_zalloc(ionwm, sizeof(IonRXref));
if (rxaddr2 == 0)
{
return 0;
}
rxref2 = (IonRXref *) psp(ionwm, rxaddr2);
rxref2->fromNode = rxref->toNode; /* Reversed. */
rxref2->toNode = rxref->fromNode; /* Reversed. */
rxref2->fromTime = rxref->fromTime;
rxref2->toTime = rxref->toTime;
rxref2->owlt = rxref->owlt;
rxref2->rangeElt = 0; /* Indicates "imputed". */
memcpy((char *) psp(ionwm, rxaddr2), (char *) rxref2, sizeof(IonRXref));
rxelt = sm_rbt_insert(ionwm, vdb->rangeIndex, rxaddr2, rfx_order_ranges,
rxref2);
if (rxelt == 0)
{
psm_free(ionwm, rxaddr2);
return 0;
}
/* Then insert relevant imputed timeline events. */
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = rxref->fromTime;
event->type = IonStartImputedRange;
event->ref = rxaddr2;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events, event)
== 0)
{
psm_free(ionwm, addr);
return 0;
}
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = rxref->toTime;
event->type = IonStopImputedRange;
event->ref = rxaddr2;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events, event)
== 0)
{
psm_free(ionwm, addr);
return 0;
}
return rxaddr;
}
static void deleteRange(PsmAddress rxaddr, int conditional)
{
Sdr sdr = getIonsdr();
PsmPartition ionwm = getIonwm();
IonVdb *vdb = getIonVdb();
time_t currentTime = getUTCTime();
IonRXref *rxref;
Object obj;
IonEvent event;
IonNeighbor *neighbor;
PsmAddress nextElt;
rxref = (IonRXref *) psp(ionwm, rxaddr);
/* Delete range from non-volatile database. */
if (rxref->rangeElt) /* An asserted range. */
{
if (conditional) /* Delete only if imputed. */
{
return; /* Retain asserted range. */
}
/* Unconditional deletion; remove range from DB. */
obj = sdr_list_data(sdr, rxref->rangeElt);
sdr_free(sdr, obj);
sdr_list_delete(sdr, rxref->rangeElt, NULL, NULL);
}
/* Delete range events from timeline. */
event.ref = rxaddr;
event.time = rxref->fromTime;
if (rxref->rangeElt)
{
event.type = IonStartAssertedRange;
}
else
{
event.type = IonStartImputedRange;
}
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
event.time = rxref->toTime;
if (rxref->rangeElt)
{
event.type = IonStopAssertedRange;
}
else
{
event.type = IonStopImputedRange;
}
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
/* Apply to current state as necessary. */
if (currentTime >= rxref->fromTime && currentTime <= rxref->toTime)
{
if (rxref->fromNode == getOwnNodeNbr())
{
neighbor = findNeighbor(vdb, rxref->toNode, &nextElt);
if (neighbor)
{
neighbor->owltOutbound = 0;
}
}
if (rxref->toNode == getOwnNodeNbr())
{
neighbor = findNeighbor(vdb, rxref->fromNode, &nextElt);
if (neighbor)
{
neighbor->owltInbound = 0;
}
}
}
/* Delete range from index. */
if (rxref->toTime > currentTime) /* Affects routes. */
{
vdb->lastEditTime = currentTime;
}
sm_rbt_delete(ionwm, vdb->rangeIndex, rfx_order_ranges, rxref,
rfx_erase_data, NULL);
}
int bpclock(int a1, int a2, int a3, int a4, int a5,
int a6, int a7, int a8, int a9, int a10)
{
#else
int main(int argc, char *argv[])
{
#endif
Sdr sdr;
BpDB *bpConstants;
int state = 1;
time_t currentTime;
if (bpAttach() < 0)
{
putErrmsg("bpclock can't attach to BP.", NULL);
return 1;
}
sdr = getIonsdr();
bpConstants = getBpConstants();
isignal(SIGTERM, shutDown);
/* Main loop: wait for event occurrence time, then
* execute applicable events. */
oK(_running(&state));
writeMemo("[i] bpclock is running.");
while (_running(NULL))
{
/* Sleep for 1 second, then dispatch all events
* whose executions times have now been reached. */
snooze(1);
currentTime = getUTCTime();
if (dispatchEvents(sdr, bpConstants->timeline, currentTime) < 0)
{
putErrmsg("Can't dispatch events.", NULL);
state = 0; /* Terminate loop. */
oK(_running(&state));
continue;
}
/* Also adjust throttles in response to rate
* changes noted in the shared ION database. */
if (adjustThrottles() < 0)
{
putErrmsg("Can't adjust throttles.", NULL);
state = 0; /* Terminate loop. */
oK(_running(&state));
continue;
}
/* Then apply rate control. */
applyRateControl(sdr);
}
writeErrmsgMemos();
writeMemo("[i] bpclock has ended.");
ionDetach();
return 0;
}
int ltpclock(int a1, int a2, int a3, int a4, int a5,
int a6, int a7, int a8, int a9, int a10)
{
#else
int main(int argc, char *argv[])
{
#endif
Sdr sdr;
LtpDB *ltpConstants;
int state = 1;
time_t currentTime;
if (ltpInit(0, 0) < 0)
{
putErrmsg("ltpclock can't initialize LTP.", NULL);
return 1;
}
sdr = getIonsdr();
ltpConstants = getLtpConstants();
isignal(SIGTERM, shutDown);
/* Main loop: wait for event occurrence time, then
* execute applicable events. */
oK(_running(&state));
writeMemo("[i] ltpclock is running.");
while (_running(NULL))
{
/* Sleep for 1 second, then dispatch all events
* whose executions times have now been reached. */
snooze(1);
currentTime = getUTCTime();
/* Infer link state changes from rate changes
* noted in the shared ION database. */
if (manageLinks(sdr, currentTime) < 0)
{
putErrmsg("Can't manage links.", NULL);
state = 0; /* Terminate loop. */
oK(_running(&state));
continue;
}
/* Then dispatch retransmission events, as
* constrained by the new link state. */
if (dispatchEvents(sdr, ltpConstants->timeline, currentTime)
< 0)
{
putErrmsg("Can't dispatch events.", NULL);
state = 0; /* Terminate loop. */
oK(_running(&state));
continue;
}
}
writeErrmsgMemos();
writeMemo("[i] ltpclock has ended.");
ionDetach();
return 0;
}
void processNMEA() {
argsCarrier *Args = malloc(sizeof(argsCarrier));
nmeaArgs *nArgs = malloc(sizeof(nmeaArgs));
nmeaINFO *info = malloc(sizeof(nmeaINFO));
nmeaPARSER parser;
nmea_zero_INFO(info);
nmea_parser_init(&parser);
char *NMEA2;
nmeaINFO info2;
// Fix up the end so the NMEA parser accepts it
int count = (int)strlen(NMEA);
NMEA[count-1] = '\r';
NMEA[count] = '\n';
NMEA[count+1] = '\0';
// Parse the data
nmea_parse(&parser, NMEA, (int)strlen(NMEA), info);
if (info->smask == 0) {
//Bad data
free(nArgs);
free(info);
free(Args);
return;
}
// NOTE: Make copy of NMEA and Data for these threads - Don't want them to be overwritten by other threads
// Have the individual case function take care of freeing them
Args->NMEA = (char *)malloc((strlen(&NMEA[0])+1)*sizeof(char));
nArgs->NMEA = (char *)malloc((strlen(&NMEA[0])+1)*sizeof(char));
strcpy(Args->NMEA, NMEA);
strcpy(nArgs->NMEA, NMEA);
Args->info = info;
nArgs->time = getUTCTime(&(info->utc));
adamGpsCallbacks->create_thread_cb("adamgps-nmea", updateNMEA, nArgs);
switch (info->smask) {
case 1:
//< GGA - Essential fix data which provide 3D location and accuracy data.
adamGpsCallbacks->create_thread_cb("adamgps-gga", updateGGA, Args);
break;
case 2:
//< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values.
adamGpsCallbacks->create_thread_cb("adamgps-gsa", updateGSA, Args);
break;
case 4:
//< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values.
adamGpsCallbacks->create_thread_cb("adamgps-gsv", updateGSV, Args);
break;
case 8:
//< RMC - Recommended Minimum Specific GPS/TRANSIT Data.
//adamGpsCallbacks->create_thread_cb("adamgps-loc", updateRMC, Args);
free(Args->info);
free(Args->NMEA);
free(Args);
break;
case 16:
//< VTG - Actual track made good and speed over ground.
free(Args->info);
free(Args->NMEA);
free(Args);
break;
default:
free(Args->info);
free(Args->NMEA);
free(Args);
break;
}
//LOGV("Successful read: %i", info->smask);
}
static PsmAddress insertCXref(IonCXref *cxref)
{
PsmPartition ionwm = getIonwm();
IonVdb *vdb = getIonVdb();
IonNode *node;
PsmAddress nextElt;
PsmAddress cxaddr;
Object iondbObj;
IonDB iondb;
PsmAddress cxelt;
PsmAddress addr;
IonEvent *event;
time_t currentTime = getUTCTime();
/* Load the affected nodes. */
node = findNode(vdb, cxref->toNode, &nextElt);
if (node == NULL)
{
node = addNode(vdb, cxref->toNode);
if (node == NULL)
{
return 0;
}
}
node = findNode(vdb, cxref->fromNode, &nextElt);
if (node == NULL)
{
node = addNode(vdb, cxref->fromNode);
if (node == NULL)
{
return 0;
}
}
/* Construct the contact index entry. */
cxaddr = psm_zalloc(ionwm, sizeof(IonCXref));
if (cxaddr == 0)
{
return 0;
}
/* Compute times of relevant events. */
iondbObj = getIonDbObject();
sdr_read(getIonsdr(), (char *) &iondb, iondbObj, sizeof(IonDB));
if (cxref->fromNode == getOwnNodeNbr())
{
/* Be a little slow to start transmission, and
* a little quick to stop, to ensure that
* segments arrive only when neighbor is
* expecting them. */
cxref->startXmit = cxref->fromTime + iondb.maxClockError;
cxref->stopXmit = cxref->toTime - iondb.maxClockError;
}
if (cxref->toNode == getOwnNodeNbr())
{
/* Be a little slow to resume timers, and a
* little quick to suspend them, to minimize the
* chance of premature timeout. */
cxref->startFire = cxref->fromTime + iondb.maxClockError;
cxref->stopFire = cxref->toTime - iondb.maxClockError;
}
else /* Not a transmission to the local node. */
{
cxref->purgeTime = cxref->toTime;
}
memcpy((char *) psp(ionwm, cxaddr), (char *) cxref, sizeof(IonCXref));
cxelt = sm_rbt_insert(ionwm, vdb->contactIndex, cxaddr,
rfx_order_contacts, cxref);
if (cxelt == 0)
{
psm_free(ionwm, cxaddr);
return 0;
}
/* Insert relevant timeline events. */
if (cxref->startXmit)
{
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = cxref->startXmit;
event->type = IonStartXmit;
event->ref = cxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events,
event) == 0)
{
psm_free(ionwm, addr);
return 0;
}
}
if (cxref->stopXmit)
{
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = cxref->stopXmit;
event->type = IonStopXmit;
event->ref = cxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events,
event) == 0)
{
psm_free(ionwm, addr);
return 0;
}
}
if (cxref->startFire)
{
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = cxref->startFire;
event->type = IonStartFire;
event->ref = cxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events,
event) == 0)
{
psm_free(ionwm, addr);
return 0;
}
}
if (cxref->stopFire)
{
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = cxref->stopFire;
event->type = IonStopFire;
event->ref = cxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events,
event) == 0)
{
psm_free(ionwm, addr);
return 0;
}
}
if (cxref->purgeTime)
{
addr = psm_zalloc(ionwm, sizeof(IonEvent));
if (addr == 0)
{
return 0;
}
event = (IonEvent *) psp(ionwm, addr);
event->time = cxref->purgeTime;
event->type = IonPurgeContact;
event->ref = cxaddr;
if (sm_rbt_insert(ionwm, vdb->timeline, addr, rfx_order_events,
event) == 0)
{
psm_free(ionwm, addr);
return 0;
}
}
if (cxref->toTime > currentTime) /* Affects routes. */
{
vdb->lastEditTime = currentTime;
}
return cxaddr;
}
initializeNode(tokenCount, tokens);
return 0;
case '@':
if (tokenCount < 2)
{
printText("Can't set reference time: \
no time.");
}
else if (strcmp(tokens[1], "0") == 0)
{
/* Set reference time to
* the current time. */
currentTime = getUTCTime();
oK(_referenceTime(¤tTime));
}
else
{
/* Get current ref time. */
refTime = _referenceTime(NULL);
/* Get new ref time, which
* may be an offset from
* the current ref time. */
refTime = readTimestampUTC
(tokens[1], refTime);
ctrl_exec_t* ctrl_create(time_t time, mid_t *mid, eid_t sender)
{
ctrl_exec_t *result = NULL;
AMP_DEBUG_ENTRY("ctrl_create","(%d, 0x%x)",
time, (unsigned long) mid);
/* Step 0: Sanity Check. */
if(mid == NULL)
{
AMP_DEBUG_ERR("ctrl_create","Bad Args.",NULL);
AMP_DEBUG_EXIT("ctrl_create","->NULL",NULL);
return NULL;
}
/* Step 1: Allocate the message. */
if((result = (ctrl_exec_t*)STAKE(sizeof(ctrl_exec_t))) == NULL)
{
AMP_DEBUG_ERR("ctrl_create","Can't alloc %d bytes.",
sizeof(ctrl_exec_t));
AMP_DEBUG_EXIT("ctrl_create","->NULL",NULL);
return NULL;
}
/* Step 2: Find the adm_ctrl_t associated with this control. */
if((result->adm_ctrl = adm_find_ctrl(mid)) == NULL)
{
AMP_DEBUG_ERR("ctrl_create","Can't find ADM ctrl.",NULL);
SRELEASE(result);
AMP_DEBUG_EXIT("ctrl_create","->NULL",NULL);
return NULL;
}
/* Step 3: Copy the sender information. */
memcpy(&(result->sender), &sender, sizeof(eid_t));
/* Step 4: Calculate time information for the control. */
result->time = time;
result->mid = mid_copy(mid);
if(result->time <= AMP_RELATIVE_TIME_EPOCH)
{
/* Step 4a: If relative time, that is # seconds. */
result->countdown_ticks = result->time;
}
else
{
/*
* Step 4b: If absolute time, # seconds if difference
* from now until then.
*/
result->countdown_ticks = (result->time - getUTCTime());
}
/* Step 5: Populate dynamic parts of the control. */
result->status = CONTROL_ACTIVE;
AMP_DEBUG_EXIT("ctrl_create","->0x%x",result);
return result;
}
static void deleteContact(PsmAddress cxaddr)
{
Sdr sdr = getIonsdr();
PsmPartition ionwm = getIonwm();
IonVdb *vdb = getIonVdb();
time_t currentTime = getUTCTime();
IonCXref *cxref;
Object obj;
IonEvent event;
IonNeighbor *neighbor;
PsmAddress nextElt;
cxref = (IonCXref *) psp(ionwm, cxaddr);
/* Delete contact events from timeline. */
event.ref = cxaddr;
if (cxref->startXmit)
{
event.time = cxref->startXmit;
event.type = IonStartXmit;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
if (cxref->stopXmit)
{
event.time = cxref->stopXmit;
event.type = IonStopXmit;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
if (cxref->startFire)
{
event.time = cxref->startFire;
event.type = IonStartFire;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
if (cxref->stopFire)
{
event.time = cxref->stopFire;
event.type = IonStopFire;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
if (cxref->startRecv)
{
event.time = cxref->startRecv;
event.type = IonStartRecv;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
if (cxref->stopRecv)
{
event.time = cxref->stopRecv;
event.type = IonStopRecv;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
if (cxref->purgeTime)
{
event.time = cxref->purgeTime;
event.type = IonPurgeContact;
sm_rbt_delete(ionwm, vdb->timeline, rfx_order_events,
&event, rfx_erase_data, NULL);
}
/* Apply to current state as necessary. */
if (currentTime >= cxref->startXmit && currentTime <= cxref->stopXmit)
{
neighbor = findNeighbor(vdb, cxref->toNode, &nextElt);
if (neighbor)
{
neighbor->xmitRate = 0;
}
}
if (currentTime >= cxref->startFire && currentTime <= cxref->stopFire)
{
neighbor = findNeighbor(vdb, cxref->fromNode, &nextElt);
if (neighbor)
{
neighbor->fireRate = 0;
}
}
if (currentTime >= cxref->startRecv && currentTime <= cxref->stopRecv)
{
neighbor = findNeighbor(vdb, cxref->fromNode, &nextElt);
if (neighbor)
{
neighbor->recvRate = 0;
}
}
/* Delete contact from index. */
if (cxref->toTime > currentTime) /* Affects routes. */
{
vdb->lastEditTime = currentTime;
}
sm_rbt_delete(ionwm, vdb->contactIndex, rfx_order_contacts, cxref,
rfx_erase_data, NULL);
/* Delete contact from non-volatile database. */
obj = sdr_list_data(sdr, cxref->contactElt);
sdr_list_delete(sdr, cxref->contactElt, NULL, NULL);
sdr_free(sdr, obj);
}
