From 9f587e6085597cb7c81ddd6c117e219134da5674 Mon Sep 17 00:00:00 2001 From: Tyler Roussos Date: Tue, 21 May 2019 11:44:14 -0400 Subject: [PATCH] Fix Issue 88, Wrong Longitude in ADSB RX (#242) --- firmware/application/apps/ui_adsb_rx.cpp | 2 +- firmware/common/adsb.cpp | 23 +++++++++++++++-------- firmware/common/adsb.hpp | 2 ++ 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/firmware/application/apps/ui_adsb_rx.cpp b/firmware/application/apps/ui_adsb_rx.cpp index bfa02e77..608af90a 100644 --- a/firmware/application/apps/ui_adsb_rx.cpp +++ b/firmware/application/apps/ui_adsb_rx.cpp @@ -220,7 +220,7 @@ void ADSBRxView::on_frame(const ADSBFrameMessage * message) { callsign = decode_frame_id(frame); entry.set_callsign(callsign); logentry+=callsign+" "; - } else if ((msg_type >= 9) && (msg_type <= 18)) { + } else if (((msg_type >= 9) && (msg_type <= 18)) || ((msg_type >= 20) && (msg_type <= 22))) { entry.set_frame_pos(frame, raw_data[6] & 4); if (entry.pos.valid) { diff --git a/firmware/common/adsb.cpp b/firmware/common/adsb.cpp index f06c064d..c730bbec 100644 --- a/firmware/common/adsb.cpp +++ b/firmware/common/adsb.cpp @@ -211,7 +211,7 @@ void encode_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const int32 adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) { uint8_t * raw_data; uint32_t latcprE, latcprO, loncprE, loncprO; - float latE, latO, m, Dlon; + float latE, latO, m, Dlon, cpr_lon_odd, cpr_lon_even, cpr_lat_odd, cpr_lat_even; int ni; adsb_pos position { false, 0, 0, 0 }; @@ -237,10 +237,17 @@ adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) { latcprO = ((frame_data_odd[6] & 3) << 15) | (frame_data_odd[7] << 7) | (frame_data_odd[8] >> 1); loncprO = ((frame_data_odd[8] & 1) << 16) | (frame_data_odd[9] << 8) | frame_data_odd[10]; + // Calculate the coefficients + cpr_lon_even = loncprE / CPR_MAX_VALUE; + cpr_lon_odd = loncprO / CPR_MAX_VALUE; + + cpr_lat_odd = latcprO / CPR_MAX_VALUE; + cpr_lat_even = latcprE / CPR_MAX_VALUE; + // Compute latitude index - float j = floor((((59.0 * latcprE) - (60.0 * latcprO)) / 131072.0) + 0.5); - latE = (360.0 / 60.0) * (cpr_mod(j, 60) + (latcprE / 131072.0)); - latO = (360.0 / 59.0) * (cpr_mod(j, 59) + (latcprO / 131072.0)); + float j = floor(((59.0 * cpr_lat_even) - (60.0 * cpr_lat_odd)) + 0.5); + latE = (360.0 / 60.0) * (cpr_mod(j, 60) + cpr_lat_even); + latO = (360.0 / 59.0) * (cpr_mod(j, 59) + cpr_lat_odd); if (latE >= 270) latE -= 360; if (latO >= 270) latO -= 360; @@ -255,9 +262,9 @@ adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) { ni = cpr_N(latE, 0); Dlon = 360.0 / ni; - m = floor((((loncprE * (cpr_NL(latE) - 1)) - (loncprO * cpr_NL(latE))) / 131072.0) + 0.5); + m = floor((cpr_lon_even * (cpr_NL(latE) - 1)) - (cpr_lon_odd * cpr_NL(latE)) + 0.5); - position.longitude = Dlon * (cpr_mod(m, ni) + loncprE / 131072.0); + position.longitude = Dlon * (cpr_mod(m, ni) + cpr_lon_even); position.latitude = latE; } else { @@ -265,9 +272,9 @@ adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) { ni = cpr_N(latO, 1); Dlon = 360.0 / ni; - m = floor((((loncprE * (cpr_NL(latO) - 1)) - (loncprO * cpr_NL(latO))) / 131072.0) + 0.5); + m = floor((cpr_lon_even * (cpr_NL(latO) - 1)) - (cpr_lon_odd * cpr_NL(latO)) + 0.5); - position.longitude = Dlon * (cpr_mod(m, ni) + loncprO / 131072.0); + position.longitude = Dlon * (cpr_mod(m, ni) + cpr_lon_odd); position.latitude = latO; } diff --git a/firmware/common/adsb.hpp b/firmware/common/adsb.hpp index 374fa8de..4e042ff2 100644 --- a/firmware/common/adsb.hpp +++ b/firmware/common/adsb.hpp @@ -56,6 +56,8 @@ struct adsb_pos { int32_t altitude; }; +const float CPR_MAX_VALUE = 131072.0; + const float adsb_lat_lut[58] = { 10.47047130, 14.82817437, 18.18626357, 21.02939493, 23.54504487, 25.82924707, 27.93898710, 29.91135686,