Skip to content

Commit

Permalink
position check ADSB
Browse files Browse the repository at this point in the history
  • Loading branch information
@jvde-github
jvde-github committed Feb 3, 2025
1 parent 6518bd1 commit 648460e
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 32 deletions.
42 changes: 21 additions & 21 deletions Library/ADSB.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -350,19 +350,19 @@ namespace Plane
return std::floor(2 * PI / acos(tmp));
}

bool ADSB::decodeCPR(FLOAT32 lat, FLOAT32 lon, bool use_even, bool &updated)
bool ADSB::decodeCPR(FLOAT32 ref_lat, FLOAT32 ref_lon, bool use_even, bool &updated, FLOAT32 &lt, FLOAT32 &ln)
{
if (!even.Valid() || !odd.Valid() || (even.airborne != odd.airborne))
return false;

if (even.airborne)
{
return decodeCPR_airborne(use_even, updated);
return decodeCPR_airborne(use_even, updated, lt, ln);
}
return decodeCPR_surface(lat, lon, use_even, updated);
return decodeCPR_surface(ref_lat, ref_lon, use_even, updated, lt, ln);
}

bool ADSB::decodeCPR_airborne(bool use_even, bool &updated)
bool ADSB::decodeCPR_airborne(bool use_even, bool &updated, FLOAT32 &lt, FLOAT32 &ln)
{
constexpr double CPR_SCALE = (double)(1 << 17);

Expand All @@ -381,47 +381,47 @@ namespace Plane
if (nl != NL(lat_odd))
return false;

lat = use_even ? lat_even : lat_odd;
lt = use_even ? lat_even : lat_odd;

int ni = MAX(nl - (use_even ? 0 : 1), 1);
int m = std::floor((even.lon * (nl - 1) - odd.lon * nl) / CPR_SCALE + 0.5);

double lon_final = use_even ? even.lon : odd.lon;

lon = (360.0 / ni) * (MOD(m, ni) + lon_final / CPR_SCALE);
ln = (360.0 / ni) * (MOD(m, ni) + lon_final / CPR_SCALE);

if (lon > 180.0)
lon -= 360.0;
if (ln > 180.0)
ln -= 360.0;

latlon_timestamp = use_even ? even.timestamp : odd.timestamp;
updated = true;

return true;
}

bool ADSB::decodeCPR_airborne_reference(bool use_even, FLOAT32 ref_lat, FLOAT32 ref_lon, bool &updated)
bool ADSB::decodeCPR_airborne_reference(bool use_even, FLOAT32 ref_lat, FLOAT32 ref_lon, bool &updated, FLOAT32 &lt, FLOAT32 &ln)
{
constexpr double CPR_SCALE = (double)(1 << 17);

CPR &cpr = use_even ? even : odd;
double d_lat = use_even ? 360.0 / 60 : 360.0 / 59;
int j = std::floor(ref_lat / d_lat) + std::floor(0.5 + MOD(ref_lat, d_lat) / d_lat - cpr.lat / CPR_SCALE + 0.5);

lat = d_lat * (j + cpr.lat / CPR_SCALE);
lt = d_lat * (j + cpr.lat / CPR_SCALE);

int ni = NL(lat) - (use_even ? 0 : 1);
int ni = NL(lt) - (use_even ? 0 : 1);
double d_lon = ni > 0 ? 360 / ni : 360;

int m = std::floor(ref_lon / d_lon) + std::floor(0.5 + (MOD(ref_lon, d_lon) / d_lon) - cpr.lon / CPR_SCALE);

lon = d_lon * (m + cpr.lon / CPR_SCALE);
ln = d_lon * (m + cpr.lon / CPR_SCALE);
latlon_timestamp = cpr.timestamp;
updated = true;
return true;
}

// based on the example from the 1090 riddle
bool ADSB::decodeCPR_surface(FLOAT32 ref_lat, FLOAT32 ref_lon, bool use_even, bool &updated)
bool ADSB::decodeCPR_surface(FLOAT32 ref_lat, FLOAT32 ref_lon, bool use_even, bool &updated, FLOAT32 &lt, FLOAT32 &ln)
{
static bool warning_given = false;

Expand All @@ -447,38 +447,38 @@ namespace Plane
if (nl != NL(lat_odd))
return false;

lat = use_even ? lat_even : lat_odd;
lat -= 90.0 * std::floor((lat - ref_lat + 45.0) / 90.0);
lt = use_even ? lat_even : lat_odd;
lt -= 90.0 * std::floor((lt - ref_lat + 45.0) / 90.0);

int ni = MAX(nl - (use_even ? 0 : 1), 1);

int m = std::floor((even.lon * (nl - 1) - odd.lon * nl) / CPR_SCALE + 0.5);
double lon_final = use_even ? even.lon : odd.lon;

lon = (90.0 / ni) * (MOD(m, ni) + lon_final / CPR_SCALE);
lon -= 90.0 * std::floor((lon - ref_lon + 45.0) / 90.0);
ln = (90.0 / ni) * (MOD(m, ni) + lon_final / CPR_SCALE);
ln -= 90.0 * std::floor((lon - ref_lon + 45.0) / 90.0);

latlon_timestamp = use_even ? even.timestamp : odd.timestamp;
updated = true;
return true;
}

bool ADSB::decodeCPR_surface_reference(bool use_even, FLOAT32 ref_lat, FLOAT32 ref_lon, bool &updated)
bool ADSB::decodeCPR_surface_reference(bool use_even, FLOAT32 ref_lat, FLOAT32 ref_lon, bool &updated, FLOAT32 &lt, FLOAT32 &ln)
{
constexpr double CPR_SCALE = (double)(1 << 17);

CPR &cpr = use_even ? even : odd;
double d_lat = use_even ? 90.0 / 60 : 90.0 / 59;
int j = std::floor(ref_lat / d_lat) + std::floor(0.5 + MOD(ref_lat, d_lat) / d_lat - cpr.lat / CPR_SCALE + 0.5);

lat = d_lat * (j + cpr.lat / CPR_SCALE);
lt = d_lat * (j + cpr.lat / CPR_SCALE);

int ni = NL(lat) - (use_even ? 0 : 1);
int ni = NL(lt) - (use_even ? 0 : 1);
double d_lon = ni > 0 ? 90 / ni : 90;

int m = std::floor(ref_lon / d_lon) + std::floor(0.5 + (MOD(ref_lon, d_lon) / d_lon) - cpr.lon / CPR_SCALE);

lon = d_lon * (m + cpr.lon / CPR_SCALE);
ln = d_lon * (m + cpr.lon / CPR_SCALE);
latlon_timestamp = cpr.timestamp;
updated = true;
return true;
Expand Down
19 changes: 14 additions & 5 deletions Library/ADSB.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,13 @@ namespace Plane

extern uint32_t crc_table[112];

enum class ValueStatus
{
VALID,
INVALID,
UNKNOWN
};

struct CPR
{
int lat, lon;
Expand Down Expand Up @@ -75,6 +82,7 @@ namespace Plane
int hexident_status;
int altitude; // Mode C altitude
FLOAT32 lat, lon; // Position
ValueStatus position_status;
std::time_t latlon_timestamp;
FLOAT32 speed; // Speed over ground
FLOAT32 heading; // Track angle
Expand Down Expand Up @@ -133,6 +141,7 @@ namespace Plane
angle = ANGLE_UNDEFINED;
message_types = 0;
message_subtypes = 0;
position_status = ValueStatus::UNKNOWN;

crc = CRC_UNDEFINED;
status = STATUS_OK;
Expand Down Expand Up @@ -228,11 +237,11 @@ namespace Plane
int MOD(int a, int b);
int NL(double lat);

bool decodeCPR(FLOAT32 lat, FLOAT32 lon, bool is_even, bool &);
bool decodeCPR_airborne(bool is_even, bool &);
bool decodeCPR_airborne_reference(bool is_even, FLOAT32, FLOAT32, bool &);
bool decodeCPR_surface(FLOAT32, FLOAT32, bool, bool &);
bool decodeCPR_surface_reference(bool, FLOAT32, FLOAT32, bool &);
bool decodeCPR(FLOAT32 ref_lat, FLOAT32 ref_lon, bool is_even, bool &, FLOAT32 &lt, FLOAT32 &ln);
bool decodeCPR_airborne(bool is_even, bool &, FLOAT32 &lt, FLOAT32 &ln);
bool decodeCPR_airborne_reference(bool is_even, FLOAT32, FLOAT32, bool &, FLOAT32 &lt, FLOAT32 &ln);
bool decodeCPR_surface(FLOAT32, FLOAT32, bool, bool &, FLOAT32 &lt, FLOAT32 &ln);
bool decodeCPR_surface_reference(bool, FLOAT32, FLOAT32, bool &, FLOAT32 &lt, FLOAT32 &ln);

void setCountryCode();
};
Expand Down
72 changes: 66 additions & 6 deletions Tracking/PlaneDB.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,15 @@ class PlaneDB : public StreamIn<Plane::ADSB>
const int END = -1;
const int FREE = -2;

struct
{
int lat, lon;
std::time_t timestamp;
struct Plane::CPR cpr;
bool even;
} CPR_history[3];
int CPR_history_idx = 0;

private:
int first = -1;
int last = -1;
Expand Down Expand Up @@ -246,6 +255,8 @@ class PlaneDB : public StreamIn<Plane::ADSB>
plane.latlon_timestamp = msg->rxtime;
}

FLOAT32 lat_new = LAT_UNDEFINED, lon_new = LON_UNDEFINED;

if (msg->even.Valid())
{
plane.even.lat = msg->even.lat;
Expand All @@ -257,7 +268,7 @@ class PlaneDB : public StreamIn<Plane::ADSB>
if (!msg->even.airborne)
calcReferencePosition(tag, ptr, ref_lat, ref_lon);

plane.decodeCPR(ref_lat, ref_lon, true, position_updated);
plane.decodeCPR(ref_lat, ref_lon, true, position_updated, lat_new, lon_new);
}

if (msg->odd.Valid())
Expand All @@ -272,7 +283,56 @@ class PlaneDB : public StreamIn<Plane::ADSB>
if (!msg->odd.airborne)
calcReferencePosition(tag, ptr, ref_lat, ref_lon);

plane.decodeCPR(ref_lat, ref_lon, false, position_updated);
plane.decodeCPR(ref_lat, ref_lon, false, position_updated, lat_new, lon_new);
}

if (position_updated)
{
if (plane.position_status == Plane::ValueStatus::VALID)
{
if (std::fabs(plane.lat - lat_new) > 0.1 || std::fabs(plane.lon - lon_new) > 0.1)
{
plane.position_status = Plane::ValueStatus::UNKNOWN;
//std::cerr << "Inconsistent position for " << std::hex << plane.hexident << std::dec << std::endl;
}
}

// store the history of the last 3 CPR positions
CPR_history[CPR_history_idx].lat = lat_new;
CPR_history[CPR_history_idx].lon = lon_new;
CPR_history[CPR_history_idx].even = msg->even.Valid();
CPR_history[CPR_history_idx].cpr = msg->even.Valid() ? plane.even : plane.odd;

if (plane.position_status == Plane::ValueStatus::UNKNOWN)
{
// check for consistency with independent position
int prev = (CPR_history_idx + 2) % 3;
int indepedent = (CPR_history_idx + 1) % 3;

if (CPR_history[prev].cpr.Valid() && CPR_history[CPR_history_idx].cpr.Valid() && CPR_history[CPR_history_idx].even != CPR_history[prev].even)
{
if (CPR_history[indepedent].cpr.Valid())
{
FLOAT32 distance = DISTANCE_UNDEFINED;
int angle = ANGLE_UNDEFINED;
getDistanceAndBearing(CPR_history[indepedent].lat, CPR_history[indepedent].lon, lat_new, lon_new, distance, angle);

if (distance < 500)
{
plane.position_status = Plane::ValueStatus::VALID;
}
}
}
}

CPR_history_idx = (CPR_history_idx + 1) % 3;

if (plane.position_status == Plane::ValueStatus::VALID)
{
plane.lat = lat_new;
plane.lon = lon_new;
plane.latlon_timestamp = msg->rxtime;
}
}

if (position_updated && tag.station_lat != LAT_UNDEFINED && tag.station_lon != LON_UNDEFINED)
Expand Down Expand Up @@ -365,10 +425,10 @@ class PlaneDB : public StreamIn<Plane::ADSB>
(plane.signalLevel != LEVEL_UNDEFINED ? std::to_string(plane.signalLevel) : null_str) + comma +
(plane.country_code[0] != ' ' ? "\"" + std::string(plane.country_code, 2) + "\"" : null_str) + comma +
(plane.distance != DISTANCE_UNDEFINED ? std::to_string(plane.distance) : null_str) + comma +
std::to_string(plane.message_types) + comma + std::to_string(plane.message_subtypes) +comma +
std::to_string(plane.group_mask) + comma + std::to_string(plane.last_group) + comma +
(plane.angle != ANGLE_UNDEFINED ? std::to_string(plane.angle) : null_str) +
"]";
std::to_string(plane.message_types) + comma + std::to_string(plane.message_subtypes) + comma +
std::to_string(plane.group_mask) + comma + std::to_string(plane.last_group) + comma +
(plane.angle != ANGLE_UNDEFINED ? std::to_string(plane.angle) : null_str) +
"]";

delim = comma;
}
Expand Down

0 comments on commit 648460e

Please sign in to comment.