diff --git a/comm_b.c b/comm_b.c index 079b594..9ac9676 100644 --- a/comm_b.c +++ b/comm_b.c @@ -281,6 +281,9 @@ static int decodeBDS40(struct modesMessage *mm, bool store) mcp_alt = mcp_raw * 16; if (mcp_alt >= 1000 && mcp_alt <= 50000) { score += 13; + } else { + // unlikely altitude + score -= 2; } } else if (!mcp_valid && mcp_raw == 0) { score += 1; @@ -293,6 +296,9 @@ static int decodeBDS40(struct modesMessage *mm, bool store) fms_alt = fms_raw * 16; if (fms_alt >= 1000 && fms_alt <= 50000) { score += 13; + } else { + // unlikely altitude + score -= 2; } } else if (!fms_valid && fms_raw == 0) { score += 1; @@ -305,6 +311,9 @@ static int decodeBDS40(struct modesMessage *mm, bool store) baro_setting = 800 + baro_raw * 0.1; if (baro_setting >= 900 && baro_setting <= 1100) { score += 13; + } else { + // unlikely pressure setting + score -= 2; } } else if (!baro_valid && baro_raw == 0) { score += 1; @@ -685,14 +694,8 @@ static int decodeBDS60(struct modesMessage *mm, bool store) } // small bonuses for consistent data - if (ias_valid && mach_valid) { - double delta = fabs(ias / 666.0 - mach); - if (delta < 0.1) { - score += 5; - } else if (delta > 0.25) { - score -= 5; - } - } + + // Should check IAS vs Mach at given altitude, but the maths is a little involved if (baro_rate_valid && inertial_rate_valid) { int delta = abs(baro_rate - inertial_rate); diff --git a/demod_2400.c b/demod_2400.c index 006cd2e..6c0fcbd 100644 --- a/demod_2400.c +++ b/demod_2400.c @@ -311,9 +311,7 @@ void demodulate2400(struct mag_buf *mag) mm.timestampMsg = mag->sampleTimestamp + j*5 + (8 + 56) * 12 + bestphase; // compute message receive time as block-start-time + difference in the 12MHz clock - mm.sysTimestampMsg = mag->sysTimestamp; // start of block time - mm.sysTimestampMsg.tv_nsec += receiveclock_ns_elapsed(mag->sampleTimestamp, mm.timestampMsg); - normalize_timespec(&mm.sysTimestampMsg); + mm.sysTimestampMsg = mag->sysTimestamp + receiveclock_ms_elapsed(mag->sampleTimestamp, mm.timestampMsg); mm.score = bestscore; @@ -646,9 +644,7 @@ void demodulate2400AC(struct mag_buf *mag) mm.timestampMsg = mag->sampleTimestamp + f2_clock / 5; // 60MHz -> 12MHz // compute message receive time as block-start-time + difference in the 12MHz clock - mm.sysTimestampMsg = mag->sysTimestamp; // start of block time - mm.sysTimestampMsg.tv_nsec += receiveclock_ns_elapsed(mag->sampleTimestamp, mm.timestampMsg); - normalize_timespec(&mm.sysTimestampMsg); + mm.sysTimestampMsg = mag->sysTimestamp + receiveclock_ms_elapsed(mag->sampleTimestamp, mm.timestampMsg); decodeModeAMessage(&mm, modeac); diff --git a/dump1090.c b/dump1090.c index 314b85a..eae1223 100644 --- a/dump1090.c +++ b/dump1090.c @@ -381,7 +381,7 @@ void backgroundTasks(void) { } // always update end time so it is current when requests arrive - Modes.stats_current.end = now; + Modes.stats_current.end = mstime(); if (now >= next_stats_update) { int i; diff --git a/dump1090.h b/dump1090.h index b0fbc09..fe95ec2 100644 --- a/dump1090.h +++ b/dump1090.h @@ -272,7 +272,7 @@ struct mag_buf { uint16_t *data; // Magnitude data. Starts with Modes.trailing_samples worth of overlap from the previous block unsigned length; // Number of valid samples _after_ overlap. Total buffer length is buf->length + Modes.trailing_samples. uint64_t sampleTimestamp; // Clock timestamp of the start of this block, 12MHz clock - struct timespec sysTimestamp; // Estimated system time at start of block + uint64_t sysTimestamp; // Estimated system time at start of block uint32_t dropped; // Number of dropped samples preceding this buffer double mean_level; // Mean of normalized (0..1) signal level double mean_power; // Mean of normalized (0..1) power level @@ -392,7 +392,7 @@ struct modesMessage { uint32_t addr; // Address Announced addrtype_t addrtype; // address format / source uint64_t timestampMsg; // Timestamp of the message (12MHz clock) - struct timespec sysTimestampMsg; // Timestamp of the message (system time) + uint64_t sysTimestampMsg; // Timestamp of the message (system time) int remote; // If set this message is from a remote station double signalLevel; // RSSI, in the range [0..1], as a fraction of full-scale power int score; // Scoring from scoreModesMessage, if used diff --git a/mode_s.c b/mode_s.c index 4ace63e..2f13af4 100644 --- a/mode_s.c +++ b/mode_s.c @@ -674,7 +674,6 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg) return 0; } - static void decodeESIdentAndCategory(struct modesMessage *mm) { // Aircraft Identification and Category @@ -690,7 +689,8 @@ static void decodeESIdentAndCategory(struct modesMessage *mm) mm->callsign[5] = ais_charset[getbits(me, 39, 44)]; mm->callsign[6] = ais_charset[getbits(me, 45, 50)]; mm->callsign[7] = ais_charset[getbits(me, 51, 56)]; - + mm->callsign[8] = 0; + // A common failure mode seems to be to intermittently send // all zeros. Catch that here. mm->callsign_valid = (strcmp(mm->callsign, "@@@@@@@@") != 0); diff --git a/net_io.c b/net_io.c index 4119d19..61d2f61 100644 --- a/net_io.c +++ b/net_io.c @@ -53,6 +53,7 @@ #include #include +#include // // ============================= Networking ============================= @@ -565,11 +566,12 @@ static void modesSendSBSOutput(struct modesMessage *mm, struct aircraft *a) { localtime_r(&now.tv_sec, &stTime_now); // Find message reception time - localtime_r(&mm->sysTimestampMsg.tv_sec, &stTime_receive); + time_t received = (time_t) (mm->sysTimestampMsg / 1000); + localtime_r(&received, &stTime_receive); // Fields 7 & 8 are the message reception time and date p += sprintf(p, "%04d/%02d/%02d,", (stTime_receive.tm_year+1900),(stTime_receive.tm_mon+1), stTime_receive.tm_mday); - p += sprintf(p, "%02d:%02d:%02d.%03u,", stTime_receive.tm_hour, stTime_receive.tm_min, stTime_receive.tm_sec, (unsigned) (mm->sysTimestampMsg.tv_nsec / 1000000U)); + p += sprintf(p, "%02d:%02d:%02d.%03u,", stTime_receive.tm_hour, stTime_receive.tm_min, stTime_receive.tm_sec, (unsigned) (mm->sysTimestampMsg / 1000)); // Fields 9 & 10 are the current time and date p += sprintf(p, "%04d/%02d/%02d,", (stTime_now.tm_year+1900),(stTime_now.tm_mon+1), stTime_now.tm_mday); @@ -920,7 +922,7 @@ static int decodeBinMessage(struct client *c, char *p) { } // record reception time as the time we read it. - clock_gettime(CLOCK_REALTIME, &mm.sysTimestampMsg); + mm.sysTimestampMsg = mstime(); ch = *p++; // Grab the signal level mm.signalLevel = ((unsigned char)ch / 255.0); @@ -1048,7 +1050,7 @@ static int decodeHexMessage(struct client *c, char *hex) { } // record reception time as the time we read it. - clock_gettime(CLOCK_REALTIME, &mm.sysTimestampMsg); + mm.sysTimestampMsg = mstime(); if (l == (MODEAC_MSG_BYTES * 2)) { // ModeA or ModeC Modes.stats_current.remote_received_modeac++; @@ -1166,6 +1168,13 @@ static char *append_intent_modes(char *p, char *end, intent_modes_t flags, const return p; } +static const char *intent_modes_string(intent_modes_t flags) { + static char buf[256]; + buf[0] = 0; + append_intent_modes(buf, buf + sizeof(buf), flags, "", " "); + return buf; +} + static const char *addrtype_short_string(addrtype_t type) { switch (type) { case ADDR_ADSB_ICAO: @@ -1748,6 +1757,35 @@ static void modesReadFromClient(struct client *c) { } } +static char *safe_vsnprintf(char *p, char *end, const char *format, va_list ap) +{ + p += vsnprintf(p < end ? p : NULL, p < end ? (size_t)(end - p) : 0, format, ap); + return p; +} + +static char *safe_snprintf(char *p, char *end, const char *format, ...) +{ + va_list ap; + va_start(ap, format); + p += vsnprintf(p < end ? p : NULL, p < end ? (size_t)(end - p) : 0, format, ap); + va_end(ap); + return p; +} + + +static char *appendFATSV(char *p, char *end, const char *field, const char *format, ...) +{ + va_list ap; + va_start(ap, format); + + p = safe_snprintf(p, end, "%s\t", field); + p = safe_vsnprintf(p, end, format, ap); + p = safe_snprintf(p, end, "\t"); + + va_end(ap); + return p; +} + #define TSV_MAX_PACKET_SIZE 400 static void writeFATSVPositionUpdate(float lat, float lon, float alt) @@ -1766,22 +1804,20 @@ static void writeFATSVPositionUpdate(float lat, float lon, float alt) return; char *end = p + TSV_MAX_PACKET_SIZE; -# define bufsize(_p,_e) ((_p) >= (_e) ? (size_t)0 : (size_t)((_e) - (_p))) - p += snprintf(p, bufsize(p, end), "clock\t%" PRIu64, mstime() / 1000); - p += snprintf(p, bufsize(p, end), "\ttype\t%s", "location_update"); - p += snprintf(p, bufsize(p, end), "\tlat\t%.5f", lat); - p += snprintf(p, bufsize(p, end), "\tlon\t%.5f", lon); - p += snprintf(p, bufsize(p, end), "\talt\t%.0f", alt); - p += snprintf(p, bufsize(p, end), "\taltref\t%s", "egm96_meters"); - p += snprintf(p, bufsize(p, end), "\n"); + p = appendFATSV(p, end, "clock", "%" PRIu64, messageNow() / 1000); + p = appendFATSV(p, end, "type", "%s", "location_update"); + p = appendFATSV(p, end, "lat", "%.5f", lat); + p = appendFATSV(p, end, "lon", "%.5f", lon); + p = appendFATSV(p, end, "alt", "%.0f", alt); + p = appendFATSV(p, end, "altref", "%s", "egm96_meters"); + --p; // remove last tab + p = safe_snprintf(p, end, "\n"); if (p <= end) completeWrite(&Modes.fatsv_out, p); else fprintf(stderr, "fatsv: output too large (max %d, overran by %d)\n", TSV_MAX_PACKET_SIZE, (int) (p - end)); - -# undef bufsize } static void writeFATSVEventMessage(struct modesMessage *mm, const char *datafield, unsigned char *data, size_t len) @@ -1791,26 +1827,18 @@ static void writeFATSVEventMessage(struct modesMessage *mm, const char *datafiel return; char *end = p + TSV_MAX_PACKET_SIZE; -# define bufsize(_p,_e) ((_p) >= (_e) ? (size_t)0 : (size_t)((_e) - (_p))) - - p += snprintf(p, bufsize(p, end), "clock\t%" PRIu64, mstime() / 1000); - - if (mm->addr & MODES_NON_ICAO_ADDRESS) { - p += snprintf(p, bufsize(p, end), "\totherid\t%06X", mm->addr & 0xFFFFFF); - } else { - p += snprintf(p, bufsize(p, end), "\thexid\t%06X", mm->addr); - } + p = appendFATSV(p, end, "clock", "%" PRIu64, messageNow() / 1000); + p = appendFATSV(p, end, (mm->addr & MODES_NON_ICAO_ADDRESS) ? "otherid" : "hexid", "%06X", mm->addr & 0xFFFFFF); if (mm->addrtype != ADDR_ADSB_ICAO) { - p += snprintf(p, bufsize(p, end), "\taddrtype\t%s", addrtype_short_string(mm->addrtype)); + p = appendFATSV(p, end, "addrtype", "%s", addrtype_short_string(mm->addrtype)); } - p += snprintf(p, bufsize(p, end), "\t%s\t", datafield); + p = safe_snprintf(p, end, "%s\t", datafield); for (size_t i = 0; i < len; ++i) { - p += snprintf(p, bufsize(p, end), "%02X", data[i]); + p = safe_snprintf(p, end, "%02X", data[i]); } - - p += snprintf(p, bufsize(p, end), "\n"); + p = safe_snprintf(p, end, "\n"); if (p <= end) completeWrite(&Modes.fatsv_out, p); @@ -1874,52 +1902,6 @@ static void writeFATSVEvent(struct modesMessage *mm, struct aircraft *a) } } -typedef enum { - TISB_IDENT = 1, - TISB_SQUAWK = 2, - TISB_ALT = 4, - TISB_ALT_GEOM = 8, - TISB_GS = 16, - TISB_IAS = 32, - TISB_TAS = 64, - TISB_LAT = 128, - TISB_LON = 256, - TISB_TRACK = 512, - TISB_MAG_HEADING = 1024, - TISB_TRUE_HEADING = 2048, - TISB_AIRGROUND = 4096, - TISB_CATEGORY = 8192, - TISB_INTENT_ALT = 16384, - TISB_INTENT_HEADING = 32768, - TISB_ALT_SETTING = 65536, - TISB_INTENT_MODES = 131072 -} tisb_flags; - -struct { - tisb_flags flag; - const char *name; -} tisb_flag_names[] = { - { TISB_IDENT, "ident" }, - { TISB_SQUAWK, "squawk" }, - { TISB_ALT, "alt" }, - { TISB_ALT_GEOM, "alt_geom" }, - { TISB_GS, "gs" }, - { TISB_IAS, "ias" }, - { TISB_TAS, "tas" }, - { TISB_LAT, "lat" }, - { TISB_LON, "lat" }, - { TISB_TRACK, "track" }, - { TISB_MAG_HEADING, "mag_heading" }, - { TISB_TRUE_HEADING, "true_heading" }, - { TISB_AIRGROUND, "airGround" }, - { TISB_CATEGORY, "category" }, - { TISB_INTENT_ALT, "intent_alt" }, - { TISB_INTENT_HEADING, "intent_heading" }, - { TISB_ALT_SETTING, "alt_setting" }, - { TISB_INTENT_MODES, "intent_modes" }, - { 0, NULL } -}; - static inline unsigned unsigned_difference(unsigned v1, unsigned v2) { return (v1 > v2) ? (v1 - v2) : (v2 - v1); @@ -1931,17 +1913,69 @@ static inline float heading_difference(float h1, float h2) return (d < 180) ? d : (360 - d); } +static char *appendFATSVMeta(char *p, char *end, const char *field, struct aircraft *a, const data_validity *source, const char *format, ...) +{ + const char *sourcetype; + switch (source->source) { + case SOURCE_MODE_S: + sourcetype = "U"; + break; + case SOURCE_MODE_S_CHECKED: + sourcetype = "S"; + break; + case SOURCE_TISB: + sourcetype = "T"; + break; + case SOURCE_ADSB: + sourcetype = "A"; + break; + default: + // don't want to forward data sourced from these + return p; + } + + if (!trackDataValid(source)) { + // expired data + return p; + } + + if (source->updated > messageNow()) { + // data in the future + return p; + } + + if (source->updated < a->fatsv_last_emitted) { + // not updated since last time + return p; + } + + uint64_t age = (messageNow() - source->updated) / 1000; + if (age > 255) { + // too old + return p; + } + + p = safe_snprintf(p, end, "%s\t", field); + + va_list ap; + va_start(ap, format); + p = safe_vsnprintf(p, end, format, ap); + va_end(ap); + + p = safe_snprintf(p, end, " %" PRIu64 " %s\t", age, sourcetype); + return p; +} + static void writeFATSV() { struct aircraft *a; - uint64_t now; static uint64_t next_update; if (!Modes.fatsv_out.service || !Modes.fatsv_out.service->connections) { return; // not enabled or no active connections } - now = mstime(); + uint64_t now = mstime(); if (now < next_update) { return; } @@ -1949,14 +1983,9 @@ static void writeFATSV() // scan once a second at most next_update = now + 1000; + // Pretend we are "processing a message" so the validity checks work as expected + _messageNow = now; for (a = Modes.aircrafts; a; a = a->next) { - uint64_t minAge; - - int useful = 0; - tisb_flags tisb = 0; - - char *p, *end; - if (a->messages < 2) // basic filter for bad decodes continue; @@ -1965,27 +1994,12 @@ static void writeFATSV() continue; } - int altValid = trackDataValidEx(&a->altitude_valid, now, 15000, SOURCE_MODE_S); // for non-ADS-B transponders, DF0/4/16/20 are the only sources of altitude data - int altGeomValid = trackDataValidEx(&a->altitude_geom_valid, now, 15000, SOURCE_MODE_S_CHECKED); - int airgroundValid = trackDataValidEx(&a->airground_valid, now, 15000, SOURCE_MODE_S_CHECKED); // for non-ADS-B transponders, only trust DF11 CA field - int baroRateValid = trackDataValidEx(&a->baro_rate_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int geomRateValid = trackDataValidEx(&a->geom_rate_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int positionValid = trackDataValidEx(&a->position_valid, now, 15000, SOURCE_MODE_S_CHECKED); - int trackValid = trackDataValidEx(&a->track_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int trackRateValid = trackDataValidEx(&a->track_rate_valid, now, 15000, SOURCE_MODE_S); // Comm-B - int rollValid = trackDataValidEx(&a->roll_valid, now, 15000, SOURCE_MODE_S); // Comm-B - int trueHeadingValid = trackDataValidEx(&a->true_heading_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int magHeadingValid = trackDataValidEx(&a->mag_heading_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int gsValid = trackDataValidEx(&a->gs_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int iasValid = trackDataValidEx(&a->ias_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int tasValid = trackDataValidEx(&a->tas_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int machValid = trackDataValidEx(&a->mach_valid, now, 15000, SOURCE_MODE_S); // Comm-B - int categoryValid = trackDataValidEx(&a->category_valid, now, 15000, SOURCE_MODE_S_CHECKED); - int intentAltValid = trackDataValidEx(&a->intent_altitude_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int intentHeadingValid = trackDataValidEx(&a->intent_heading_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int intentModesValid = trackDataValidEx(&a->intent_modes_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int altSettingValid = trackDataValidEx(&a->alt_setting_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES - int callsignValid = trackDataValidEx(&a->callsign_valid, now, 15000, SOURCE_MODE_S); // Comm-B or ES + // some special cases: + int altValid = trackDataValid(&a->altitude_valid); + int airgroundValid = trackDataValid(&a->airground_valid) && a->airground_valid.source >= SOURCE_MODE_S_CHECKED; // for non-ADS-B transponders, only trust DF11 CA field + int gsValid = trackDataValid(&a->gs_valid); + int callsignValid = trackDataValid(&a->callsign_valid) && strcmp(a->callsign, " ") != 0; + int positionValid = trackDataValid(&a->position_valid); // If we are definitely on the ground, suppress any unreliable altitude info. // When on the ground, ADS-B transponders don't emit an ADS-B message that includes @@ -1996,68 +2010,31 @@ static void writeFATSV() // if it hasn't changed altitude, heading, or speed much, // don't update so often - int changed = 0; - int immediate = 0; - if (altValid && abs(a->altitude - a->fatsv_emitted_altitude) >= 50) { - changed = 1; - } - if (altGeomValid && abs(a->altitude_geom - a->fatsv_emitted_altitude_gnss) >= 50) { - changed = 1; - } - if (baroRateValid && abs(a->baro_rate - a->fatsv_emitted_baro_rate) > 500) { - changed = 1; - } - if (geomRateValid && abs(a->geom_rate - a->fatsv_emitted_geom_rate) > 500) { - changed = 1; - } - if (trackValid && heading_difference(a->track, a->fatsv_emitted_track) >= 2) { - changed = 1; - } - if (trackRateValid && fabs(a->track_rate - a->fatsv_emitted_track_rate) >= 0.5) { - changed = 1; - } - if (rollValid && fabs(a->roll - a->fatsv_emitted_roll) >= 5.0) { - changed = 1; - } - if (magHeadingValid && heading_difference(a->mag_heading, a->fatsv_emitted_mag_heading) >= 2) { - changed = 1; - } - if (trueHeadingValid && heading_difference(a->true_heading, a->fatsv_emitted_true_heading) >= 2) { - changed = 1; - } - if (gsValid && unsigned_difference(a->gs, a->fatsv_emitted_speed) >= 25) { - changed = 1; - } - if (iasValid && unsigned_difference(a->ias, a->fatsv_emitted_speed_ias) >= 25) { - changed = 1; - } - if (tasValid && unsigned_difference(a->tas, a->fatsv_emitted_speed_tas) >= 25) { - changed = 1; - } - if (machValid && fabs(a->mach - a->fatsv_emitted_mach) >= 0.02) { - changed = 1; - } - if (intentAltValid && unsigned_difference(a->intent_altitude, a->fatsv_emitted_intent_altitude) > 50) { - changed = immediate = 1; - } - if (intentHeadingValid && heading_difference(a->intent_heading, a->fatsv_emitted_intent_heading) > 2) { - changed = immediate = 1; - } - if (intentModesValid && a->intent_modes != a->fatsv_emitted_intent_modes) { - changed = immediate = 1; - } - if (altSettingValid && fabs(a->alt_setting - a->fatsv_emitted_alt_setting) > 0.8) { // 0.8 is the ES message resolution - changed = immediate = 1; - } - if (callsignValid && strcmp(a->callsign, a->fatsv_emitted_callsign) != 0) { - changed = immediate = 1; - } - if (airgroundValid && ((a->airground == AG_AIRBORNE && a->fatsv_emitted_airground == AG_GROUND) || - (a->airground == AG_GROUND && a->fatsv_emitted_airground == AG_AIRBORNE))) { - // Air-ground transition, handle it immediately. - changed = immediate = 1; - } + int changed = + (altValid && abs(a->altitude - a->fatsv_emitted_altitude) >= 50) || + (trackDataValid(&a->altitude_geom_valid) && abs(a->altitude_geom - a->fatsv_emitted_altitude_gnss) >= 50) || + (trackDataValid(&a->baro_rate_valid) && abs(a->baro_rate - a->fatsv_emitted_baro_rate) > 500) || + (trackDataValid(&a->geom_rate_valid) && abs(a->geom_rate - a->fatsv_emitted_geom_rate) > 500) || + (trackDataValid(&a->track_valid) && heading_difference(a->track, a->fatsv_emitted_track) >= 2) || + (trackDataValid(&a->track_rate_valid) && fabs(a->track_rate - a->fatsv_emitted_track_rate) >= 0.5) || + (trackDataValid(&a->roll_valid) && fabs(a->roll - a->fatsv_emitted_roll) >= 5.0) || + (trackDataValid(&a->mag_heading_valid) && heading_difference(a->mag_heading, a->fatsv_emitted_mag_heading) >= 2) || + (trackDataValid(&a->true_heading_valid) && heading_difference(a->true_heading, a->fatsv_emitted_true_heading) >= 2) || + (gsValid && unsigned_difference(a->gs, a->fatsv_emitted_speed) >= 25) || + (trackDataValid(&a->ias_valid) && unsigned_difference(a->ias, a->fatsv_emitted_speed_ias) >= 25) || + (trackDataValid(&a->tas_valid) && unsigned_difference(a->tas, a->fatsv_emitted_speed_tas) >= 25) || + (trackDataValid(&a->mach_valid) && fabs(a->mach - a->fatsv_emitted_mach) >= 0.02); + int immediate = + (trackDataValid(&a->intent_altitude_valid) && unsigned_difference(a->intent_altitude, a->fatsv_emitted_intent_altitude) > 50) || + (trackDataValid(&a->intent_heading_valid) && heading_difference(a->intent_heading, a->fatsv_emitted_intent_heading) > 2) || + (trackDataValid(&a->intent_modes_valid) && a->intent_modes != a->fatsv_emitted_intent_modes) || + (trackDataValid(&a->alt_setting_valid) && fabs(a->alt_setting - a->fatsv_emitted_alt_setting) > 0.8) || // 0.8 is the ES message resolution + (callsignValid && strcmp(a->callsign, a->fatsv_emitted_callsign) != 0) || + (airgroundValid && a->airground == AG_AIRBORNE && a->fatsv_emitted_airground == AG_GROUND) || + (airgroundValid && a->airground == AG_GROUND && a->fatsv_emitted_airground == AG_AIRBORNE); + + uint64_t minAge; if (immediate) { // a change we want to emit right away minAge = 0; @@ -2080,218 +2057,89 @@ static void writeFATSV() if ((now - a->fatsv_last_emitted) < minAge) continue; - p = prepareWrite(&Modes.fatsv_out, TSV_MAX_PACKET_SIZE); + char *p = prepareWrite(&Modes.fatsv_out, TSV_MAX_PACKET_SIZE); if (!p) return; + char *end = p + TSV_MAX_PACKET_SIZE; - end = p + TSV_MAX_PACKET_SIZE; -# define bufsize(_p,_e) ((_p) >= (_e) ? (size_t)0 : (size_t)((_e) - (_p))) - - p += snprintf(p, bufsize(p, end), "clock\t%" PRIu64, (uint64_t)(a->seen / 1000)); - - if (a->addr & MODES_NON_ICAO_ADDRESS) { - p += snprintf(p, bufsize(p, end), "\totherid\t%06X", a->addr & 0xFFFFFF); - } else { - p += snprintf(p, bufsize(p, end), "\thexid\t%06X", a->addr); - } + p = appendFATSV(p, end, "clock", "%" PRIu64, messageNow() / 1000); + p = appendFATSV(p, end, (a->addr & MODES_NON_ICAO_ADDRESS) ? "otherid" : "hexid", "%06X", a->addr & 0xFFFFFF); if (a->addrtype != ADDR_ADSB_ICAO) { - p += snprintf(p, bufsize(p, end), "\taddrtype\t%s", addrtype_short_string(a->addrtype)); + p = appendFATSV(p, end, "addrtype", "%s", addrtype_short_string(a->addrtype)); } if (a->adsb_version >= 0) { - p += snprintf(p, bufsize(p, end), "\tadsb_version\t%d", a->adsb_version); - } - - if (trackDataValidEx(&a->callsign_valid, now, 35000, SOURCE_MODE_S) && strcmp(a->callsign, " ") != 0 && a->callsign_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tident\t%s", a->callsign); - memcpy(a->fatsv_emitted_callsign, a->callsign, sizeof(a->fatsv_emitted_callsign)); - switch (a->callsign_valid.source) { - case SOURCE_MODE_S: - p += snprintf(p, bufsize(p,end), "\tiSource\tmodes"); - break; - case SOURCE_ADSB: - p += snprintf(p, bufsize(p,end), "\tiSource\tadsb"); - break; - case SOURCE_TISB: - p += snprintf(p, bufsize(p,end), "\tiSource\ttisb"); - break; - default: - p += snprintf(p, bufsize(p,end), "\tiSource\tunknown"); - break; - } - - useful = 1; - tisb |= (a->callsign_valid.source == SOURCE_TISB) ? TISB_IDENT : 0; - } - - if (trackDataValidEx(&a->squawk_valid, now, 35000, SOURCE_MODE_S) && a->squawk_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tsquawk\t%04x", a->squawk); - useful = 1; - tisb |= (a->squawk_valid.source == SOURCE_TISB) ? TISB_SQUAWK : 0; + p = appendFATSV(p, end, "adsbVer", "%d", a->adsb_version); } // only emit alt, speed, latlon, track etc if they have been received since the last time // and are not stale - if (altValid && a->altitude_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\talt\t%d", a->altitude); - a->fatsv_emitted_altitude = a->altitude; - useful = 1; - tisb |= (a->altitude_valid.source == SOURCE_TISB) ? TISB_ALT : 0; - } + char *dataStart = p; - if (altGeomValid && a->altitude_geom_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\talt_geom\t%d", a->altitude_geom); - a->fatsv_emitted_altitude_gnss = a->altitude_geom; - useful = 1; - tisb |= (a->altitude_geom_valid.source == SOURCE_TISB) ? TISB_ALT_GEOM : 0; - } - - if (baroRateValid && a->baro_rate_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tbaro_rate\t%d", a->baro_rate); - a->fatsv_emitted_baro_rate = a->baro_rate; - useful = 1; - } - - if (geomRateValid && a->geom_rate_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tgeom_rate\t%d", a->geom_rate); - a->fatsv_emitted_geom_rate = a->geom_rate; - useful = 1; - } - - if (gsValid && a->gs_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tgs\t%u", a->gs); - a->fatsv_emitted_speed = a->gs; - useful = 1; - tisb |= (a->gs_valid.source == SOURCE_TISB) ? TISB_GS : 0; - } - - if (iasValid && a->ias_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tias\t%u", a->ias); - a->fatsv_emitted_speed_ias = a->ias; - useful = 1; - tisb |= (a->ias_valid.source == SOURCE_TISB) ? TISB_IAS : 0; - } - - if (tasValid && a->tas_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\ttas\t%u", a->tas); - a->fatsv_emitted_speed_tas = a->tas; - useful = 1; - tisb |= (a->tas_valid.source == SOURCE_TISB) ? TISB_TAS : 0; - } - - if (machValid && a->mach_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tmach\t%.3f", a->mach); - a->fatsv_emitted_mach = a->mach; - useful = 1; - } - - if (positionValid && a->position_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tlat\t%.5f\tlon\t%.5f", a->lat, a->lon); - useful = 1; - tisb |= (a->position_valid.source == SOURCE_TISB) ? (TISB_LAT | TISB_LON) : 0; - } - - if (trackValid && a->track_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\ttrack\t%.0f", a->track); - a->fatsv_emitted_track = a->track; - useful = 1; - tisb |= (a->track_valid.source == SOURCE_TISB) ? TISB_TRACK : 0; - } - - if (trackRateValid && a->track_rate_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\ttrack_rate\t%.2f", a->track_rate); - a->fatsv_emitted_track_rate = a->track_rate; - useful = 1; - } - - if (rollValid && a->roll_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\troll\t%.1f", a->roll); - a->fatsv_emitted_roll = a->roll; - useful = 1; - } - - if (magHeadingValid && a->mag_heading_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tmag_heading\t%.0f", a->mag_heading); - a->fatsv_emitted_mag_heading = a->mag_heading; - useful = 1; - tisb |= (a->mag_heading_valid.source == SOURCE_TISB) ? TISB_MAG_HEADING : 0; - } - - if (trueHeadingValid && a->true_heading_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\true_heading\t%.0f", a->true_heading); - a->fatsv_emitted_true_heading = a->true_heading; - useful = 1; - tisb |= (a->true_heading_valid.source == SOURCE_TISB) ? TISB_TRUE_HEADING : 0; - } - - if (airgroundValid && (a->airground == AG_GROUND || a->airground == AG_AIRBORNE) && a->airground_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tairGround\t%s", a->airground == AG_GROUND ? "G+" : "A+"); - a->fatsv_emitted_airground = a->airground; - useful = 1; - tisb |= (a->airground_valid.source == SOURCE_TISB) ? TISB_AIRGROUND : 0; - } - - if (categoryValid && (a->category & 0xF0) != 0xA0 && a->category_valid.updated > a->fatsv_last_emitted) { - // interesting category, not a regular aircraft - p += snprintf(p, bufsize(p,end), "\tcategory\t%02X", a->category); - useful = 1; - tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_CATEGORY : 0; - } - - if (intentAltValid && a->intent_altitude_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tintent_alt\t%u", a->intent_altitude); - a->fatsv_emitted_intent_altitude = a->intent_altitude; - useful = 1; - tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_INTENT_ALT : 0; - } - - if (intentHeadingValid && a->intent_heading_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\tintent_heading\t%.0f", a->intent_heading); - a->fatsv_emitted_intent_heading = a->intent_heading; - useful = 1; - tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_INTENT_HEADING : 0; - } - - if (intentModesValid && a->intent_modes_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, end-p, "\tintent_modes\t"); - p = append_intent_modes(p, end, a->intent_modes, "", " "); - a->fatsv_emitted_intent_modes = a->intent_modes; - tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_INTENT_MODES : 0; - } - - if (altSettingValid && a->alt_setting_valid.updated > a->fatsv_last_emitted) { - p += snprintf(p, bufsize(p,end), "\talt_setting\t%.1f", a->alt_setting); - a->fatsv_emitted_alt_setting = a->alt_setting; - useful = 1; - tisb |= (a->category_valid.source == SOURCE_TISB) ? TISB_ALT_SETTING : 0; - } + // special cases + if (altValid) + p = appendFATSVMeta(p, end, "alt", a, &a->altitude_valid, "%d", a->altitude); + if (airgroundValid) + p = appendFATSVMeta(p, end, "ag", a, &a->airground_valid, "%s", a->airground == AG_GROUND ? "G+" : "A+"); + if (strcmp(a->callsign, " ") != 0) + p = appendFATSVMeta(p, end, "ident", a, &a->callsign_valid, "{%s}", a->callsign); + if (positionValid) + p = appendFATSVMeta(p, end, "pos", a, &a->position_valid, "{%.5f %.5f}", a->lat, a->lon); + p = appendFATSVMeta(p, end, "squawk", a, &a->squawk_valid, "%04x", a->squawk); + p = appendFATSVMeta(p, end, "altGeo", a, &a->altitude_geom_valid, "%d", a->altitude_geom); + p = appendFATSVMeta(p, end, "vrate", a, &a->baro_rate_valid, "%d", a->baro_rate); + p = appendFATSVMeta(p, end, "vrateGeo", a, &a->geom_rate_valid, "%d", a->geom_rate); + p = appendFATSVMeta(p, end, "gs", a, &a->gs_valid, "%u", a->gs); + p = appendFATSVMeta(p, end, "ias", a, &a->ias_valid, "%u", a->ias); + p = appendFATSVMeta(p, end, "tas", a, &a->tas_valid, "%u", a->tas); + p = appendFATSVMeta(p, end, "mach", a, &a->mach_valid, "%.3f", a->mach); + p = appendFATSVMeta(p, end, "trk", a, &a->track_valid, "%.0f", a->track); + p = appendFATSVMeta(p, end, "trkRate", a, &a->track_rate_valid, "%.2f", a->track_rate); + p = appendFATSVMeta(p, end, "roll", a, &a->roll_valid, "%.1f", a->roll); + p = appendFATSVMeta(p, end, "hdgMag", a, &a->mag_heading_valid, "%.0f", a->mag_heading); + p = appendFATSVMeta(p, end, "hdgTrue", a, &a->true_heading_valid, "%.0f", a->true_heading); + if (a->category != 0xA0) + p = appendFATSVMeta(p, end, "category", a, &a->category_valid, "%02X", a->category); + p = appendFATSVMeta(p, end, "selAlt", a, &a->intent_altitude_valid,"%u", a->intent_altitude); + p = appendFATSVMeta(p, end, "selHdg", a, &a->intent_heading_valid, "%.0f", a->intent_heading); + p = appendFATSVMeta(p, end, "selModes", a, &a->intent_modes_valid, "{%s}", intent_modes_string(a->intent_modes)); + p = appendFATSVMeta(p, end, "qnh", a, &a->alt_setting_valid, "%.1f", a->alt_setting); // if we didn't get anything interesting, bail out. // We don't need to do anything special to unwind prepareWrite(). - if (!useful) { + if (p == dataStart) { continue; } - if (tisb != 0) { - p += snprintf(p, bufsize(p,end), "\ttisb\t"); - for (int i = 0; tisb_flag_names[i].name; ++i) { - if (tisb & tisb_flag_names[i].flag) { - p += snprintf(p, bufsize(p,end), "%s ", tisb_flag_names[i].name); - } - } - } - - p += snprintf(p, bufsize(p,end), "\n"); + --p; // remove last tab + p = safe_snprintf(p, end, "\n"); if (p <= end) completeWrite(&Modes.fatsv_out, p); else fprintf(stderr, "fatsv: output too large (max %d, overran by %d)\n", TSV_MAX_PACKET_SIZE, (int) (p - end)); -# undef bufsize + a->fatsv_emitted_altitude = a->altitude; + a->fatsv_emitted_altitude_gnss = a->altitude_geom; + a->fatsv_emitted_baro_rate = a->baro_rate; + a->fatsv_emitted_geom_rate = a->geom_rate; + a->fatsv_emitted_speed = a->gs; + a->fatsv_emitted_speed_ias = a->ias; + a->fatsv_emitted_mach = a->mach; + a->fatsv_emitted_track = a->track; + a->fatsv_emitted_track_rate = a->track_rate; + a->fatsv_emitted_roll = a->roll; + a->fatsv_emitted_mag_heading = a->mag_heading; + a->fatsv_emitted_true_heading = a->true_heading; + a->fatsv_emitted_airground = a->airground; + a->fatsv_emitted_intent_altitude = a->intent_altitude; + a->fatsv_emitted_intent_heading = a->intent_heading; + a->fatsv_emitted_intent_modes = a->intent_modes; + a->fatsv_emitted_alt_setting = a->alt_setting; + memcpy(a->fatsv_emitted_callsign, a->callsign, sizeof(a->fatsv_emitted_callsign)); a->fatsv_last_emitted = now; } } diff --git a/sdr_bladerf.c b/sdr_bladerf.c index c849b14..8577b9d 100644 --- a/sdr_bladerf.c +++ b/sdr_bladerf.c @@ -311,8 +311,7 @@ static void *handle_bladerf_samples(struct bladerf *dev, MODES_NOTUSED(num_samples); // record initial time for later sys timestamp calculation - struct timespec entryTimestamp; - clock_gettime(CLOCK_REALTIME, &entryTimestamp); + uint64_t entryTimestamp = mstime(); pthread_mutex_lock(&Modes.data_mutex); if (Modes.exit) { @@ -413,10 +412,8 @@ static void *handle_bladerf_samples(struct bladerf *dev, if (blocks_processed) { // Get the approx system time for the start of this block - unsigned block_duration = 1e9 * outbuf->length / Modes.sample_rate; - outbuf->sysTimestamp = entryTimestamp; - outbuf->sysTimestamp.tv_nsec -= block_duration; - normalize_timespec(&outbuf->sysTimestamp); + unsigned block_duration = 1e3 * outbuf->length / Modes.sample_rate; + outbuf->sysTimestamp = entryTimestamp - block_duration; outbuf->mean_level /= blocks_processed; outbuf->mean_power /= blocks_processed; diff --git a/sdr_ifile.c b/sdr_ifile.c index 771c36e..f615071 100644 --- a/sdr_ifile.c +++ b/sdr_ifile.c @@ -217,7 +217,7 @@ void ifileRun() } // Get the system time for the start of this block - clock_gettime(CLOCK_REALTIME, &outbuf->sysTimestamp); + outbuf->sysTimestamp = mstime(); toread = MODES_MAG_BUF_SAMPLES * ifile.bytes_per_sample; r = ifile.readbuf; diff --git a/sdr_rtlsdr.c b/sdr_rtlsdr.c index 0feb992..979ba38 100644 --- a/sdr_rtlsdr.c +++ b/sdr_rtlsdr.c @@ -313,12 +313,10 @@ void rtlsdrCallback(unsigned char *buf, uint32_t len, void *ctx) { // Compute the sample timestamp and system timestamp for the start of the block outbuf->sampleTimestamp = sampleCounter * 12e6 / Modes.sample_rate; sampleCounter += slen; - block_duration = 1e9 * slen / Modes.sample_rate; // Get the approx system time for the start of this block - clock_gettime(CLOCK_REALTIME, &outbuf->sysTimestamp); - outbuf->sysTimestamp.tv_nsec -= block_duration; - normalize_timespec(&outbuf->sysTimestamp); + block_duration = 1e3 * slen / Modes.sample_rate; + outbuf->sysTimestamp = mstime() - block_duration; // Copy trailing data from last block (or reset if not valid) if (outbuf->dropped == 0) { diff --git a/track.c b/track.c index 55ef7bc..2e39a23 100644 --- a/track.c +++ b/track.c @@ -89,6 +89,34 @@ struct aircraft *trackCreateAircraft(struct modesMessage *mm) { // Copy the first message so we can emit it later when a second message arrives. a->first_message = *mm; + // initialize data validity ages +#define F(f,s,e) do { a->f##_valid.stale_interval = (s) * 1000; a->f##_valid.expire_interval = (e) * 1000; } while (0) + F(callsign, 60, 70); // ADS-B or Comm-B + F(altitude, 15, 70); // ADS-B or Mode S + F(altitude_geom, 60, 70); // ADS-B only + F(geom_delta, 60, 70); // ADS-B only + F(gs, 60, 70); // ADS-B or Comm-B + F(ias, 60, 70); // ADS-B (rare) or Comm-B + F(tas, 60, 70); // ADS-B (rare) or Comm-B + F(mach, 60, 70); // Comm-B only + F(track, 60, 70); // ADS-B or Comm-B + F(track_rate, 60, 70); // Comm-B only + F(roll, 60, 70); // Comm-B only + F(mag_heading, 60, 70); // ADS-B (rare) or Comm-B + F(true_heading, 60, 70); // ADS-B only (rare) + F(baro_rate, 60, 70); // ADS-B or Comm-B + F(geom_rate, 60, 70); // ADS-B or Comm-B + F(squawk, 15, 70); // ADS-B or Mode S + F(category, 60, 70); // ADS-B only + F(airground, 15, 70); // ADS-B or Mode S + F(alt_setting, 60, 70); // Comm-B only + F(intent_altitude, 60, 70); // ADS-B or Comm-B + F(intent_modes, 60, 70); // ADS-B or Comm-B + F(cpr_odd, 60, 70); // ADS-B only + F(cpr_even, 60, 70); // ADS-B only + F(position, 60, 70); // ADS-B only +#undef F + Modes.stats_current.unique_aircraft++; return (a); @@ -112,15 +140,18 @@ struct aircraft *trackFindAircraft(uint32_t addr) { // Should we accept some new data from the given source? // If so, update the validity and return 1 -static int accept_data(data_validity *d, datasource_t source, uint64_t now) +static int accept_data(data_validity *d, datasource_t source) { - if (source < d->source && now < d->stale) + if (messageNow() < d->updated) + return 0; + + if (source < d->source && messageNow() < d->stale) return 0; d->source = source; - d->updated = now; - d->stale = now + 60000; - d->expires = now + 70000; + d->updated = messageNow(); + d->stale = messageNow() + d->stale_interval; + d->expires = messageNow() + d->expire_interval; return 1; } @@ -142,10 +173,10 @@ static void combine_validity(data_validity *to, const data_validity *from1, cons to->expires = (from1->expires < from2->expires) ? from1->expires : from2->expires; // the earlier of the two expiry times } -static int compare_validity(const data_validity *lhs, const data_validity *rhs, uint64_t now) { - if (now < lhs->stale && lhs->source > rhs->source) +static int compare_validity(const data_validity *lhs, const data_validity *rhs) { + if (messageNow() < lhs->stale && lhs->source > rhs->source) return 1; - else if (now < rhs->stale && lhs->source < rhs->source) + else if (messageNow() < rhs->stale && lhs->source < rhs->source) return -1; else if (lhs->updated > rhs->updated) return 1; @@ -201,7 +232,7 @@ static void update_range_histogram(double lat, double lon) // return true if it's OK for the aircraft to have travelled from its last known position // to a new position at (lat,lon,surface) at a time of now. -static int speed_check(struct aircraft *a, double lat, double lon, uint64_t now, int surface) +static int speed_check(struct aircraft *a, double lat, double lon, int surface) { uint64_t elapsed; double distance; @@ -212,7 +243,7 @@ static int speed_check(struct aircraft *a, double lat, double lon, uint64_t now, if (!trackDataValid(&a->position_valid)) return 1; // no reference, assume OK - elapsed = trackDataAge(&a->position_valid, now); + elapsed = trackDataAge(&a->position_valid); if (trackDataValid(&a->gs_valid)) speed = a->gs; @@ -256,7 +287,7 @@ static int speed_check(struct aircraft *a, double lat, double lon, uint64_t now, return inrange; } -static int doGlobalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now, double *lat, double *lon, unsigned *nuc) +static int doGlobalCPR(struct aircraft *a, struct modesMessage *mm, double *lat, double *lon, unsigned *nuc) { int result; int fflag = mm->cpr_odd; @@ -269,7 +300,7 @@ static int doGlobalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now // find reference location double reflat, reflon; - if (trackDataValidEx(&a->position_valid, now, 50000, SOURCE_INVALID)) { // Ok to try aircraft relative first + if (trackDataValid(&a->position_valid)) { // Ok to try aircraft relative first reflat = a->lat; reflon = a->lon; if (a->pos_nuc < *nuc) @@ -325,7 +356,7 @@ static int doGlobalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now return result; // check speed limit - if (trackDataValid(&a->position_valid) && a->pos_nuc >= *nuc && !speed_check(a, *lat, *lon, now, surface)) { + if (trackDataValid(&a->position_valid) && a->pos_nuc >= *nuc && !speed_check(a, *lat, *lon, surface)) { Modes.stats_current.cpr_global_speed_checks++; return -2; } @@ -333,7 +364,7 @@ static int doGlobalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now return result; } -static int doLocalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now, double *lat, double *lon, unsigned *nuc) +static int doLocalCPR(struct aircraft *a, struct modesMessage *mm, double *lat, double *lon, unsigned *nuc) { // relative CPR // find reference location @@ -345,7 +376,7 @@ static int doLocalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now, *nuc = mm->cpr_nucp; - if (trackDataValidEx(&a->position_valid, now, 50000, SOURCE_INVALID)) { + if (trackDataValid(&a->position_valid)) { reflat = a->lat; reflon = a->lon; @@ -399,7 +430,7 @@ static int doLocalCPR(struct aircraft *a, struct modesMessage *mm, uint64_t now, } // check speed limit - if (trackDataValid(&a->position_valid) && a->pos_nuc >= *nuc && !speed_check(a, *lat, *lon, now, surface)) { + if (trackDataValid(&a->position_valid) && a->pos_nuc >= *nuc && !speed_check(a, *lat, *lon, surface)) { #ifdef DEBUG_CPR_CHECKS fprintf(stderr, "Speed check for %06X with local decoding failed\n", a->addr); #endif @@ -418,7 +449,7 @@ static uint64_t time_between(uint64_t t1, uint64_t t2) return t2 - t1; } -static void updatePosition(struct aircraft *a, struct modesMessage *mm, uint64_t now) +static void updatePosition(struct aircraft *a, struct modesMessage *mm) { int location_result = -1; uint64_t max_elapsed; @@ -449,7 +480,7 @@ static void updatePosition(struct aircraft *a, struct modesMessage *mm, uint64_t a->cpr_odd_type == a->cpr_even_type && time_between(a->cpr_odd_valid.updated, a->cpr_even_valid.updated) <= max_elapsed) { - location_result = doGlobalCPR(a, mm, now, &new_lat, &new_lon, &new_nuc); + location_result = doGlobalCPR(a, mm, &new_lat, &new_lon, &new_nuc); if (location_result == -2) { #ifdef DEBUG_CPR_CHECKS @@ -480,7 +511,7 @@ static void updatePosition(struct aircraft *a, struct modesMessage *mm, uint64_t // Otherwise try relative CPR. if (location_result == -1) { - location_result = doLocalCPR(a, mm, now, &new_lat, &new_lon, &new_nuc); + location_result = doLocalCPR(a, mm, &new_lat, &new_lon, &new_nuc); if (location_result < 0) { Modes.stats_current.cpr_local_skipped++; @@ -527,8 +558,8 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) return NULL; } - uint64_t now = mstime(); - + _messageNow = mm->sysTimestampMsg; + // Lookup our aircraft or create a new one a = trackFindAircraft(mm->addr); if (!a) { // If it's a currently unknown aircraft.... @@ -541,7 +572,7 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) a->signalLevel[a->signalNext] = mm->signalLevel; a->signalNext = (a->signalNext + 1) & 7; } - a->seen = now; + a->seen = messageNow(); a->messages++; // update addrtype, we only ever go towards "more direct" types @@ -552,7 +583,7 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) if (mm->source == SOURCE_ADSB && a->adsb_version < 0) a->adsb_version = 0; - if (mm->altitude_valid && mm->altitude_source == ALTITUDE_BARO && accept_data(&a->altitude_valid, mm->source, now)) { + if (mm->altitude_valid && mm->altitude_source == ALTITUDE_BARO && accept_data(&a->altitude_valid, mm->source)) { if (a->modeC_hit) { int new_modeC = (a->altitude + 49) / 100; int old_modeC = (mm->altitude + 49) / 100; @@ -564,18 +595,18 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) a->altitude = mm->altitude; } - if (mm->squawk_valid && accept_data(&a->squawk_valid, mm->source, now)) { + if (mm->squawk_valid && accept_data(&a->squawk_valid, mm->source)) { if (mm->squawk != a->squawk) { a->modeA_hit = 0; } a->squawk = mm->squawk; } - if (mm->altitude_valid && mm->altitude_source == ALTITUDE_GEOM && accept_data(&a->altitude_geom_valid, mm->source, now)) { + if (mm->altitude_valid && mm->altitude_source == ALTITUDE_GEOM && accept_data(&a->altitude_geom_valid, mm->source)) { a->altitude_geom = mm->altitude; } - if (mm->geom_delta_valid && accept_data(&a->geom_delta_valid, mm->source, now)) { + if (mm->geom_delta_valid && accept_data(&a->geom_delta_valid, mm->source)) { a->geom_delta = mm->geom_delta; } @@ -587,81 +618,81 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) htype = a->adsb_tah; } - if (htype == HEADING_GROUND_TRACK && accept_data(&a->track_valid, mm->source, now)) { + if (htype == HEADING_GROUND_TRACK && accept_data(&a->track_valid, mm->source)) { a->track = mm->heading; - } else if (htype == HEADING_MAGNETIC && accept_data(&a->mag_heading_valid, mm->source, now)) { + } else if (htype == HEADING_MAGNETIC && accept_data(&a->mag_heading_valid, mm->source)) { a->mag_heading = mm->heading; - } else if (htype == HEADING_TRUE && accept_data(&a->true_heading_valid, mm->source, now)) { + } else if (htype == HEADING_TRUE && accept_data(&a->true_heading_valid, mm->source)) { a->true_heading = mm->heading; } } - if (mm->track_rate_valid && accept_data(&a->track_rate_valid, mm->source, now)) { + if (mm->track_rate_valid && accept_data(&a->track_rate_valid, mm->source)) { a->track_rate = mm->track_rate; } - if (mm->roll_valid && accept_data(&a->roll_valid, mm->source, now)) { + if (mm->roll_valid && accept_data(&a->roll_valid, mm->source)) { a->roll = mm->roll; } - if (mm->gs_valid && accept_data(&a->gs_valid, mm->source, now)) { + if (mm->gs_valid && accept_data(&a->gs_valid, mm->source)) { a->gs = mm->gs; } - if (mm->ias_valid && accept_data(&a->ias_valid, mm->source, now)) { + if (mm->ias_valid && accept_data(&a->ias_valid, mm->source)) { a->ias = mm->ias; } - if (mm->tas_valid && accept_data(&a->tas_valid, mm->source, now)) { + if (mm->tas_valid && accept_data(&a->tas_valid, mm->source)) { a->tas = mm->tas; } - if (mm->mach_valid && accept_data(&a->mach_valid, mm->source, now)) { + if (mm->mach_valid && accept_data(&a->mach_valid, mm->source)) { a->mach = mm->mach; } - if (mm->baro_rate_valid && accept_data(&a->baro_rate_valid, mm->source, now)) { + if (mm->baro_rate_valid && accept_data(&a->baro_rate_valid, mm->source)) { a->baro_rate = mm->baro_rate; } - if (mm->geom_rate_valid && accept_data(&a->geom_rate_valid, mm->source, now)) { + if (mm->geom_rate_valid && accept_data(&a->geom_rate_valid, mm->source)) { a->geom_rate = mm->geom_rate; } - if (mm->category_valid && accept_data(&a->category_valid, mm->source, now)) { + if (mm->category_valid && accept_data(&a->category_valid, mm->source)) { a->category = mm->category; } - if (mm->airground != AG_INVALID && accept_data(&a->airground_valid, mm->source, now)) { + if (mm->airground != AG_INVALID && accept_data(&a->airground_valid, mm->source)) { a->airground = mm->airground; } - if (mm->callsign_valid && accept_data(&a->callsign_valid, mm->source, now)) { + if (mm->callsign_valid && accept_data(&a->callsign_valid, mm->source)) { memcpy(a->callsign, mm->callsign, sizeof(a->callsign)); } // prefer MCP over FMS // unless the source says otherwise - if (mm->intent.mcp_altitude_valid && mm->intent.altitude_source != INTENT_ALT_FMS && accept_data(&a->intent_altitude_valid, mm->source, now)) { + if (mm->intent.mcp_altitude_valid && mm->intent.altitude_source != INTENT_ALT_FMS && accept_data(&a->intent_altitude_valid, mm->source)) { a->intent_altitude = mm->intent.mcp_altitude; - } else if (mm->intent.fms_altitude_valid && accept_data(&a->intent_altitude_valid, mm->source, now)) { + } else if (mm->intent.fms_altitude_valid && accept_data(&a->intent_altitude_valid, mm->source)) { a->intent_altitude = mm->intent.fms_altitude; } - if (mm->intent.heading_valid && accept_data(&a->intent_heading_valid, mm->source, now)) { + if (mm->intent.heading_valid && accept_data(&a->intent_heading_valid, mm->source)) { a->intent_heading = mm->intent.heading; } - if (mm->intent.modes_valid && accept_data(&a->intent_modes_valid, mm->source, now)) { + if (mm->intent.modes_valid && accept_data(&a->intent_modes_valid, mm->source)) { a->intent_modes = mm->intent.modes; } - if (mm->intent.alt_setting_valid && accept_data(&a->alt_setting_valid, mm->source, now)) { + if (mm->intent.alt_setting_valid && accept_data(&a->alt_setting_valid, mm->source)) { a->alt_setting = mm->intent.alt_setting; } // CPR, even - if (mm->cpr_valid && !mm->cpr_odd && accept_data(&a->cpr_even_valid, mm->source, now)) { + if (mm->cpr_valid && !mm->cpr_odd && accept_data(&a->cpr_even_valid, mm->source)) { a->cpr_even_type = mm->cpr_type; a->cpr_even_lat = mm->cpr_lat; a->cpr_even_lon = mm->cpr_lon; @@ -669,7 +700,7 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) } // CPR, odd - if (mm->cpr_valid && mm->cpr_odd && accept_data(&a->cpr_odd_valid, mm->source, now)) { + if (mm->cpr_valid && mm->cpr_odd && accept_data(&a->cpr_odd_valid, mm->source)) { a->cpr_odd_type = mm->cpr_type; a->cpr_odd_lat = mm->cpr_lat; a->cpr_odd_lon = mm->cpr_lon; @@ -688,8 +719,8 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) // Now handle derived data // derive geometric altitude if we have baro + delta - if (compare_validity(&a->altitude_valid, &a->altitude_geom_valid, now) > 0 && - compare_validity(&a->geom_delta_valid, &a->altitude_geom_valid, now) > 0) { + if (compare_validity(&a->altitude_valid, &a->altitude_geom_valid) > 0 && + compare_validity(&a->geom_delta_valid, &a->altitude_geom_valid) > 0) { // Baro and delta are both more recent than geometric, derive geometric from baro + delta a->altitude_geom = a->altitude + a->geom_delta; combine_validity(&a->altitude_geom_valid, &a->altitude_valid, &a->geom_delta_valid); @@ -697,7 +728,7 @@ struct aircraft *trackUpdateFromMessage(struct modesMessage *mm) // If we've got a new cprlat or cprlon if (mm->cpr_valid) { - updatePosition(a, mm, now); + updatePosition(a, mm); } return (a); @@ -837,7 +868,7 @@ static void trackRemoveStaleAircraft(uint64_t now) EXPIRE(cpr_odd); EXPIRE(cpr_even); EXPIRE(position); - +#undef EXPIRE prev = a; a = a->next; } } diff --git a/track.h b/track.h index c03063f..f8ab089 100644 --- a/track.h +++ b/track.h @@ -64,11 +64,18 @@ */ #define TRACK_MODEAC_MIN_MESSAGES 4 +// data moves through three states: +// fresh: data is valid. Updates from a less reliable source are not accepted. +// stale: data is valid. Updates from a less reliable source are accepted. +// expired: data is not valid. typedef struct { + uint64_t stale_interval; /* how long after an update until the data is stale */ + uint64_t expire_interval; /* how long after an update until the data expires */ + datasource_t source; /* where the data came from */ - uint64_t updated; /* when it arrived */ - uint64_t stale; /* when it will become stale */ - uint64_t expires; /* when it will expire */ + uint64_t updated; /* when it arrived */ + uint64_t stale; /* when it goes stale */ + uint64_t expires; /* when it expires */ } data_validity; /* Structure used to describe the state of one tracked aircraft */ @@ -213,33 +220,17 @@ extern uint32_t modeAC_age[4096]; /* is this bit of data valid? */ static inline int trackDataValid(const data_validity *v) { - return (v->source != SOURCE_INVALID); + return (v->source != SOURCE_INVALID && messageNow() < v->expires); } -/* .. with these constraints? */ -static inline int trackDataValidEx(const data_validity *v, - uint64_t now, - uint64_t maxAge, - datasource_t minSource) -{ - if (v->source == SOURCE_INVALID) - return 0; - if (v->source < minSource) - return 0; - if (v->updated < now && (now - v->updated) > maxAge) - return 0; - return 1; -} - -/* what's the age of this data? */ -static inline uint64_t trackDataAge(const data_validity *v, - uint64_t now) +/* what's the age of this data, in milliseconds? */ +static inline uint64_t trackDataAge(const data_validity *v) { if (v->source == SOURCE_INVALID) return ~(uint64_t)0; - if (v->updated >= now) + if (v->updated >= messageNow()) return 0; - return (now - v->updated); + return (messageNow() - v->updated); } /* Update aircraft state from data in the provided mesage. diff --git a/util.c b/util.c index d7afc07..d5f101c 100644 --- a/util.c +++ b/util.c @@ -52,6 +52,8 @@ #include #include +uint64_t _messageNow = 0; + uint64_t mstime(void) { struct timeval tv; @@ -68,6 +70,11 @@ int64_t receiveclock_ns_elapsed(uint64_t t1, uint64_t t2) return (t2 - t1) * 1000U / 12U; } +int64_t receiveclock_ms_elapsed(uint64_t t1, uint64_t t2) +{ + return (t2 - t1) / 12000U; +} + void normalize_timespec(struct timespec *ts) { if (ts->tv_nsec > 1000000000) { diff --git a/util.h b/util.h index 6030c97..5d32f05 100644 --- a/util.h +++ b/util.h @@ -25,11 +25,20 @@ /* Returns system time in milliseconds */ uint64_t mstime(void); +/* Returns the time for the current message we're dealing with */ +extern uint64_t _messageNow; +static inline uint64_t messageNow() { + return _messageNow; +} + /* Returns the time elapsed, in nanoseconds, from t1 to t2, * where t1 and t2 are 12MHz counters. */ int64_t receiveclock_ns_elapsed(uint64_t t1, uint64_t t2); +/* Same, in milliseconds */ +int64_t receiveclock_ms_elapsed(uint64_t t1, uint64_t t2); + /* Normalize the value in ts so that ts->nsec lies in * [0,999999999] */