Merge pull request #382 from teixeluis/hotfix-adsb-position

Hotfix to issue #362 - ADS-B RX app aircraft position jitter
This commit is contained in:
Erwin Ried 2021-06-25 10:13:21 +02:00 committed by GitHub
commit 010f06c77c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 113 additions and 38 deletions

View File

@ -20,6 +20,8 @@
* Boston, MA 02110-1301, USA.
*/
#include <strings.h>
#include "ui_adsb_rx.hpp"
#include "ui_alphanum.hpp"
@ -205,7 +207,7 @@ void ADSBRxView::on_frame(const ADSBFrameMessage * message) {
auto frame = message->frame;
uint32_t ICAO_address = frame.get_ICAO_address();
if (frame.check_CRC() && frame.get_ICAO_address()) {
if (frame.check_CRC() && ICAO_address) {
rtcGetTime(&RTCD1, &datetime);
auto& entry = ::on_packet(recent, ICAO_address);
frame.set_rx_timestamp(datetime.minute() * 60 + datetime.second());
@ -222,33 +224,49 @@ void ADSBRxView::on_frame(const ADSBFrameMessage * message) {
uint8_t msg_sub = frame.get_msg_sub();
uint8_t * raw_data = frame.get_raw_data();
if ((msg_type >= 1) && (msg_type <= 4)) {
if ((msg_type >= AIRCRAFT_ID_L) && (msg_type <= AIRCRAFT_ID_H)) {
callsign = decode_frame_id(frame);
entry.set_callsign(callsign);
logentry+=callsign+" ";
} else if (((msg_type >= 9) && (msg_type <= 18)) || ((msg_type >= 20) && (msg_type <= 22))) {
}
//
else if (((msg_type >= AIRBORNE_POS_BARO_L) && (msg_type <= AIRBORNE_POS_BARO_H)) ||
((msg_type >= AIRBORNE_POS_GPS_L) && (msg_type <= AIRBORNE_POS_GPS_H))) {
entry.set_frame_pos(frame, raw_data[6] & 4);
if (entry.pos.valid) {
str_info = "Alt:" + to_string_dec_int(entry.pos.altitude) +
" Lat:" + to_string_dec_int(entry.pos.latitude) +
"." + to_string_dec_int((int)abs(entry.pos.latitude * 1000) % 100, 2, '0') +
" Lon:" + to_string_dec_int(entry.pos.longitude) +
"." + to_string_dec_int((int)abs(entry.pos.longitude * 1000) % 100, 2, '0');
entry.set_info_string(str_info);
logentry+=str_info+ " ";
" Lat:" + to_string_decimal(entry.pos.latitude, 2) +
" Lon:" + to_string_decimal(entry.pos.longitude, 2);
if (send_updates)
// printing the coordinates in the log file with more
// resolution, as we are not constrained by screen
// real estate there:
std::string log_info = "Alt:" + to_string_dec_int(entry.pos.altitude) +
" Lat:" + to_string_decimal(entry.pos.latitude, 7) +
" Lon:" + to_string_decimal(entry.pos.longitude, 7);
entry.set_info_string(str_info);
logentry+=log_info + " ";
// we only want to update the details view if the frame
// we received has the same ICAO address, i.e. belongs to
// the same aircraft:
if(send_updates && details_view->get_current_entry().ICAO_address == ICAO_address) {
details_view->update(entry);
}
}
} else if(msg_type == 19 && msg_sub >= 1 && msg_sub <= 4){
} else if(msg_type == AIRBORNE_VEL && msg_sub >= VEL_GND_SUBSONIC && msg_sub <= VEL_AIR_SUPERSONIC){
entry.set_frame_velo(frame);
logentry += "Type:" + to_string_dec_uint(msg_sub) +
" Hdg:" + to_string_dec_uint(entry.velo.heading) +
" Spd: "+ to_string_dec_int(entry.velo.speed);
if (send_updates)
// same here:
if (send_updates && details_view->get_current_entry().ICAO_address == ICAO_address) {
details_view->update(entry);
}
}
}
recent_entries_view.set_dirty();

View File

@ -36,9 +36,33 @@ using namespace adsb;
namespace ui {
#define ADSB_DECAY_A 10 // In seconds
#define ADSB_DECAY_B 30
#define ADSB_DECAY_C 60 // Can be used for removing old entries, RecentEntries already caps to 64
#define ADSB_DECAY_A 10 // In seconds
#define ADSB_DECAY_B 30
#define ADSB_DECAY_C 60 // Can be used for removing old entries, RecentEntries already caps to 64
#define AIRCRAFT_ID_L 1 // aircraft ID message type (lowest type id)
#define AIRCRAFT_ID_H 4 // aircraft ID message type (highest type id)
#define SURFACE_POS_L 5 // surface position (lowest type id)
#define SURFACE_POS_H 8 // surface position (highest type id)
#define AIRBORNE_POS_BARO_L 9 // airborne position (lowest type id)
#define AIRBORNE_POS_BARO_H 18 // airborne position (highest type id)
#define AIRBORNE_VEL 19 // airborne velocities
#define AIRBORNE_POS_GPS_L 20 // airborne position (lowest type id)
#define AIRBORNE_POS_GPS_H 22 // airborne position (highest type id)
#define RESERVED_L 23 // reserved for other uses
#define RESERVED_H 31 // reserved for other uses
#define VEL_GND_SUBSONIC 1
#define VEL_GND_SUPERSONIC 2
#define VEL_AIR_SUBSONIC 3
#define VEL_AIR_SUPERSONIC 4
#define O_E_FRAME_TIMEOUT 20 // timeout between odd and even frames
struct AircraftRecentEntry {
using Key = uint32_t;
@ -82,7 +106,7 @@ struct AircraftRecentEntry {
frame_pos_odd = frame;
if (!frame_pos_even.empty() && !frame_pos_odd.empty()) {
if (abs(frame_pos_even.get_rx_timestamp() - frame_pos_odd.get_rx_timestamp()) < 20)
if (abs(frame_pos_even.get_rx_timestamp() - frame_pos_odd.get_rx_timestamp()) < O_E_FRAME_TIMEOUT)
pos = decode_frame_pos(frame_pos_even, frame_pos_odd);
}
}
@ -137,6 +161,8 @@ public:
void update(const AircraftRecentEntry& entry);
std::string title() const override { return "Details"; };
AircraftRecentEntry get_current_entry() { return entry_copy; }
private:
AircraftRecentEntry entry_copy { 0 };

View File

@ -112,6 +112,23 @@ std::string to_string_dec_int(
return q;
}
std::string to_string_decimal(float decimal, int8_t precision) {
double integer_part;
double fractional_part;
std::string result;
fractional_part = modf(decimal, &integer_part) * pow(10, precision);
if (fractional_part < 0) {
fractional_part = -fractional_part;
}
result = to_string_dec_int(integer_part) + "." + to_string_dec_uint(fractional_part, precision, '0');
return result;
}
std::string to_string_short_freq(const uint64_t f) {
auto final_str = to_string_dec_int(f / 1000000,4) + "." + to_string_dec_int((f / 100) % 10000, 4, '0');
return final_str;

View File

@ -43,6 +43,8 @@ const char unit_prefix[7] { 'n', 'u', 'm', 0, 'k', 'M', 'G' };
std::string to_string_bin(const uint32_t n, const uint8_t l = 0);
std::string to_string_dec_uint(const uint32_t n, const int32_t l = 0, const char fill = ' ');
std::string to_string_dec_int(const int32_t n, const int32_t l = 0, const char fill = 0);
std::string to_string_decimal(float decimal, int8_t precision);
std::string to_string_hex(const uint64_t n, const int32_t l = 0);
std::string to_string_hex_array(uint8_t * const array, const int32_t l = 0);

View File

@ -63,17 +63,9 @@ GeoPos::GeoPos(
const auto changed_fn = [this](int32_t) {
float lat_value = lat();
float lon_value = lon();
double integer_part;
double fractional_part;
fractional_part = modf(lat_value, &integer_part) * 100000;
if (fractional_part < 0)
fractional_part = -fractional_part;
text_lat_decimal.set(to_string_dec_int(integer_part) + "." + to_string_dec_uint(fractional_part, 5));
fractional_part = modf(lon_value, &integer_part) * 100000;
if (fractional_part < 0)
fractional_part = -fractional_part;
text_lon_decimal.set(to_string_dec_int(integer_part) + "." + to_string_dec_uint(fractional_part, 5));
text_lat_decimal.set(to_string_decimal(lat_value, 5));
text_lon_decimal.set(to_string_decimal(lon_value, 5));
if (on_change && report_change)
on_change(altitude(), lat_value, lon_value);

View File

@ -141,7 +141,11 @@ float cpr_mod(float a, float b) {
return a - (b * floor(a / b));
}
int cpr_NL(float lat) {
int cpr_NL_precise(float lat) {
return (int) floor(2 * PI / acos(1 - ((1 - cos(PI / (2 * NZ))) / pow(cos(PI * lat / 180), 2))));
}
int cpr_NL_approx(float lat) {
if (lat < 0)
lat = -lat; // Symmetry
@ -150,7 +154,19 @@ int cpr_NL(float lat) {
return 59 - c;
}
return 1;
return 1;
}
int cpr_NL(float lat) {
// TODO prove that the approximate function is good
// enough for the precision we need. Uncomment if
// that is true. No performance penalty was noticed
// from testing, but if you find it might be an issue,
// switch to cpr_NL_approx() instead:
//return cpr_NL_approx(lat);
return cpr_NL_precise(lat);
}
int cpr_N(float lat, int is_odd) {
@ -185,18 +201,18 @@ void encode_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const int32
// CPR encoding
// Info from: http://antena.fe.uni-lj.si/literatura/Razno/Avionika/modes/CPRencoding.pdf
delta_lat = 360.0 / ((4.0 * 15.0) - time_parity); // NZ = 15
yz = floor(131072.0 * (cpr_mod(latitude, delta_lat) / delta_lat) + 0.5);
rlat = delta_lat * ((yz / 131072.0) + floor(latitude / delta_lat));
delta_lat = 360.0 / ((4.0 * NZ) - time_parity); // NZ = 15
yz = floor(CPR_MAX_VALUE * (cpr_mod(latitude, delta_lat) / delta_lat) + 0.5);
rlat = delta_lat * ((yz / CPR_MAX_VALUE) + floor(latitude / delta_lat));
if ((cpr_NL(rlat) - time_parity) > 0)
delta_lon = 360.0 / cpr_N(rlat, time_parity);
else
delta_lon = 360.0;
xz = floor(131072.0 * (cpr_mod(longitude, delta_lon) / delta_lon) + 0.5);
xz = floor(CPR_MAX_VALUE * (cpr_mod(longitude, delta_lon) / delta_lon) + 0.5);
lat = cpr_mod(yz, 131072.0);
lon = cpr_mod(xz, 131072.0);
lat = cpr_mod(yz, CPR_MAX_VALUE);
lon = cpr_mod(xz, CPR_MAX_VALUE);
frame.push_byte((altitude_coded << 4) | ((uint32_t)time_parity << 2) | (lat >> 15)); // T = 0
frame.push_byte(lat >> 7);
@ -258,7 +274,7 @@ adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) {
// Compute longitude
if (time_even > time_odd) {
// Use even frame
// Use even frame2
ni = cpr_N(latE, 0);
Dlon = 360.0 / ni;
@ -279,7 +295,7 @@ adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) {
position.latitude = latO;
}
if (position.longitude > 180) position.longitude -= 360;
if (position.longitude >= 180) position.longitude -= 360;
position.valid = true;

View File

@ -83,6 +83,10 @@ const float adsb_lat_lut[58] = {
86.53536998, 87.00000000
};
const float PI = 3.14159265358979323846;
const float NZ = 15.0;
void make_frame_adsb(ADSBFrame& frame, const uint32_t ICAO_address);
void encode_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std::string& callsign);