Import CPR changes from experimental branch.
This commit is contained in:
Oliver Jowett
parent
b9b76da02d
commit
ed4d481772
9 changed files with 925 additions and 357 deletions
221
interactive.c
221
interactive.c
|
|
@ -195,13 +195,199 @@ void interactiveUpdateAircraftModeS() {
|
|||
//
|
||||
// Receive new messages and populate the interactive mode with more info
|
||||
//
|
||||
|
||||
// Distance between points on a spherical earth.
|
||||
// This has up to 0.5% error because the earth isn't actually spherical
|
||||
// (but we don't use it in situations where that matters)
|
||||
static double greatcircle(double lat0, double lon0, double lat1, double lon1)
|
||||
{
|
||||
lat0 = lat0 * M_PI / 180.0;
|
||||
lon0 = lon0 * M_PI / 180.0;
|
||||
lat1 = lat1 * M_PI / 180.0;
|
||||
lon1 = lon1 * M_PI / 180.0;
|
||||
return 6371e3 * acos(sin(lat0) * sin(lat1) + cos(lat0) * cos(lat1) * cos(fabs(lon0 - lon1)));
|
||||
}
|
||||
|
||||
static int doGlobalCPR(struct aircraft *a, int fflag, int surface)
|
||||
{
|
||||
int result;
|
||||
double lat=0, lon=0;
|
||||
|
||||
if (surface) {
|
||||
// surface global CPR
|
||||
// find reference location
|
||||
double reflat, reflon;
|
||||
|
||||
if (a->bFlags & MODES_ACFLAGS_LATLON_REL_OK) { // Ok to try aircraft relative first
|
||||
reflat = a->lat;
|
||||
reflon = a->lon;
|
||||
} else if (Modes.bUserFlags & MODES_USER_LATLON_VALID) {
|
||||
reflat = Modes.fUserLat;
|
||||
reflon = Modes.fUserLon;
|
||||
} else {
|
||||
// No local reference, give up
|
||||
return (-1);
|
||||
}
|
||||
|
||||
result = decodeCPRsurface(reflat, reflon,
|
||||
a->even_cprlat, a->even_cprlon,
|
||||
a->odd_cprlat, a->odd_cprlon,
|
||||
fflag,
|
||||
&lat, &lon);
|
||||
} else {
|
||||
// airborne global CPR
|
||||
result = decodeCPRairborne(a->even_cprlat, a->even_cprlon,
|
||||
a->odd_cprlat, a->odd_cprlon,
|
||||
fflag,
|
||||
&lat, &lon);
|
||||
}
|
||||
|
||||
if (result < 0)
|
||||
return result;
|
||||
|
||||
// check max range
|
||||
if (Modes.maxRange > 0 && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) {
|
||||
double range = greatcircle(Modes.fUserLat, Modes.fUserLon, lat, lon);
|
||||
if (range > Modes.maxRange)
|
||||
return (-2); // we consider an out-of-range value to be bad data
|
||||
}
|
||||
|
||||
a->lat = lat;
|
||||
a->lon = lon;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int doLocalCPR(struct aircraft *a, int fflag, int surface, time_t now)
|
||||
{
|
||||
// relative CPR
|
||||
// find reference location
|
||||
double reflat, reflon, lat=0, lon=0;
|
||||
double range_limit = 0;
|
||||
int result;
|
||||
|
||||
if (a->bFlags & MODES_ACFLAGS_LATLON_REL_OK) {
|
||||
int elapsed = (int)(now - a->seenLatLon);
|
||||
if (elapsed < 0) elapsed = 0;
|
||||
|
||||
reflat = a->lat;
|
||||
reflon = a->lon;
|
||||
|
||||
// impose a range limit based on 2000km/h speed
|
||||
range_limit = 5e3 + (2000e3 * elapsed / 3600); // 5km + 2000km/h
|
||||
} else if (!surface && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) {
|
||||
reflat = Modes.fUserLat;
|
||||
reflon = Modes.fUserLon;
|
||||
|
||||
// The cell size is at least 360NM, giving a nominal
|
||||
// max range of 180NM (half a cell).
|
||||
//
|
||||
// If the receiver range is more than half a cell
|
||||
// then we must limit this range further to avoid
|
||||
// ambiguity. (e.g. if we receive a position report
|
||||
// at 200NM distance, this may resolve to a position
|
||||
// at (200-360) = 160NM in the wrong direction)
|
||||
if (Modes.maxRange > 1852*180)
|
||||
range_limit = (1852*360) - Modes.maxRange;
|
||||
} else {
|
||||
// No local reference, give up
|
||||
return (-1);
|
||||
}
|
||||
|
||||
result = decodeCPRrelative(reflat, reflon,
|
||||
fflag ? a->odd_cprlat : a->even_cprlat,
|
||||
fflag ? a->odd_cprlon : a->even_cprlon,
|
||||
fflag, surface,
|
||||
&lat, &lon);
|
||||
if (result < 0)
|
||||
return result;
|
||||
// check range limit
|
||||
if (range_limit > 0) {
|
||||
double range = greatcircle(reflat, reflon, lat, lon);
|
||||
if (range > range_limit)
|
||||
return (-1);
|
||||
}
|
||||
|
||||
// check max range
|
||||
if (Modes.maxRange > 0 && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) {
|
||||
double range = greatcircle(Modes.fUserLat, Modes.fUserLon, lat, lon);
|
||||
if (range > Modes.maxRange)
|
||||
return (-2); // we consider an out-of-range value to be bad data
|
||||
}
|
||||
|
||||
a->lat = lat;
|
||||
a->lon = lon;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void updatePosition(struct aircraft *a, struct modesMessage *mm, time_t now)
|
||||
{
|
||||
int location_result = -1;
|
||||
|
||||
if (mm->bFlags & MODES_ACFLAGS_LLODD_VALID) {
|
||||
a->odd_cprlat = mm->raw_latitude;
|
||||
a->odd_cprlon = mm->raw_longitude;
|
||||
a->odd_cprtime = mstime();
|
||||
} else {
|
||||
a->even_cprlat = mm->raw_latitude;
|
||||
a->even_cprlon = mm->raw_longitude;
|
||||
a->even_cprtime = mstime();
|
||||
}
|
||||
|
||||
// If we have enough recent data, try global CPR
|
||||
if (((mm->bFlags | a->bFlags) & MODES_ACFLAGS_LLEITHER_VALID) == MODES_ACFLAGS_LLBOTH_VALID && abs((int)(a->even_cprtime - a->odd_cprtime)) <= 10000) {
|
||||
location_result = doGlobalCPR(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG));
|
||||
if (location_result == -2) {
|
||||
// Global CPR failed because an airborne position produced implausible results.
|
||||
// This is bad data. Discard both odd and even messages and wait for a fresh pair.
|
||||
// Also disable aircraft-relative positions until we have a new good position (but don't discard the
|
||||
// recorded position itself)
|
||||
Modes.stats_current.cpr_global_bad++;
|
||||
mm->bFlags &= ~(MODES_ACFLAGS_LATLON_VALID | MODES_ACFLAGS_LLODD_VALID | MODES_ACFLAGS_LLEVEN_VALID);
|
||||
a->bFlags &= ~(MODES_ACFLAGS_LATLON_REL_OK | MODES_ACFLAGS_LLODD_VALID | MODES_ACFLAGS_LLEVEN_VALID);
|
||||
return;
|
||||
} else if (location_result == -1) {
|
||||
// No local reference for surface position available, or the two messages crossed a zone.
|
||||
// Nonfatal, try again later.
|
||||
Modes.stats_current.cpr_global_skipped++;
|
||||
} else {
|
||||
Modes.stats_current.cpr_global_ok++;
|
||||
}
|
||||
}
|
||||
|
||||
// Otherwise try relative CPR.
|
||||
if (location_result == -1) {
|
||||
location_result = doLocalCPR(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG), now);
|
||||
if (location_result == -1) {
|
||||
Modes.stats_current.cpr_local_skipped++;
|
||||
} else {
|
||||
Modes.stats_current.cpr_local_ok++;
|
||||
mm->bFlags |= MODES_ACFLAGS_REL_CPR_USED;
|
||||
}
|
||||
}
|
||||
|
||||
if (location_result == 0) {
|
||||
// If we sucessfully decoded, back copy the results to mm so that we can print them in list output
|
||||
mm->bFlags |= MODES_ACFLAGS_LATLON_VALID;
|
||||
mm->fLat = a->lat;
|
||||
mm->fLon = a->lon;
|
||||
|
||||
// Update aircraft state
|
||||
a->bFlags |= (MODES_ACFLAGS_LATLON_VALID | MODES_ACFLAGS_LATLON_REL_OK);
|
||||
a->seenLatLon = a->seen;
|
||||
a->timestampLatLon = a->timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
struct aircraft *interactiveReceiveData(struct modesMessage *mm) {
|
||||
struct aircraft *a, *aux;
|
||||
time_t now;
|
||||
|
||||
// Return if (checking crc) AND (not crcok) AND (not fixed)
|
||||
if (Modes.check_crc && (mm->crcok == 0) && (mm->correctedbits == 0))
|
||||
return NULL;
|
||||
|
||||
now = time(NULL);
|
||||
|
||||
// Lookup our aircraft or create a new one
|
||||
a = interactiveFindAircraft(mm->addr);
|
||||
if (!a) { // If it's a currently unknown aircraft....
|
||||
|
|
@ -216,7 +402,7 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) {
|
|||
* since the aircraft that is currently on head sent a message,
|
||||
* othewise with multiple aircrafts at the same time we have an
|
||||
* useless shuffle of positions on the screen. */
|
||||
if (0 && Modes.aircrafts != a && (time(NULL) - a->seen) >= 1) {
|
||||
if (0 && Modes.aircrafts != a && (now - a->seen) >= 1) {
|
||||
aux = Modes.aircrafts;
|
||||
while(aux->next != a) aux = aux->next;
|
||||
/* Now we are a node before the aircraft to remove. */
|
||||
|
|
@ -228,7 +414,7 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) {
|
|||
}
|
||||
|
||||
a->signalLevel[a->messages & 7] = mm->signalLevel;// replace the 8th oldest signal strength
|
||||
a->seen = time(NULL);
|
||||
a->seen = now;
|
||||
a->timestamp = mm->timestampMsg;
|
||||
a->messages++;
|
||||
|
||||
|
|
@ -282,36 +468,7 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) {
|
|||
|
||||
// If we've got a new cprlat or cprlon
|
||||
if (mm->bFlags & MODES_ACFLAGS_LLEITHER_VALID) {
|
||||
int location_ok = 0;
|
||||
|
||||
if (mm->bFlags & MODES_ACFLAGS_LLODD_VALID) {
|
||||
a->odd_cprlat = mm->raw_latitude;
|
||||
a->odd_cprlon = mm->raw_longitude;
|
||||
a->odd_cprtime = mstime();
|
||||
} else {
|
||||
a->even_cprlat = mm->raw_latitude;
|
||||
a->even_cprlon = mm->raw_longitude;
|
||||
a->even_cprtime = mstime();
|
||||
}
|
||||
|
||||
// If we have enough recent data, try global CPR
|
||||
if (((mm->bFlags | a->bFlags) & MODES_ACFLAGS_LLEITHER_VALID) == MODES_ACFLAGS_LLBOTH_VALID && abs((int)(a->even_cprtime - a->odd_cprtime)) <= 10000) {
|
||||
if (decodeCPR(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG)) == 0) {
|
||||
location_ok = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Otherwise try relative CPR.
|
||||
if (!location_ok && decodeCPRrelative(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG)) == 0) {
|
||||
location_ok = 1;
|
||||
}
|
||||
|
||||
//If we sucessfully decoded, back copy the results to mm so that we can print them in list output
|
||||
if (location_ok) {
|
||||
mm->bFlags |= MODES_ACFLAGS_LATLON_VALID;
|
||||
mm->fLat = a->lat;
|
||||
mm->fLon = a->lon;
|
||||
}
|
||||
updatePosition(a, mm, now);
|
||||
}
|
||||
|
||||
// Update the aircrafts a->bFlags to reflect the newly received mm->bFlags;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue