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

Hotfix to issue #362 - ADS-B RX app aircraft position jitter

firmware/application/apps/ui_adsb_rx.cpp

@@ -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) +
+						" Lat:" + to_string_decimal(entry.pos.latitude, 2) +
-						"." + to_string_dec_int((int)abs(entry.pos.latitude * 1000) % 100, 2, '0') +
+						" Lon:" + to_string_decimal(entry.pos.longitude, 2);
-						" 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+ " ";
 
-					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(); 

firmware/application/apps/ui_adsb_rx.hpp

@@ -36,9 +36,33 @@ using namespace adsb;
 
 namespace ui {
 
-#define ADSB_DECAY_A 10		// In seconds
+#define ADSB_DECAY_A 		10		// In seconds
-#define ADSB_DECAY_B 30
+#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_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 };

firmware/application/string_format.cpp

@@ -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;

firmware/application/string_format.hpp

@@ -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);
 

firmware/application/ui/ui_geomap.cpp

@@ -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;
+		text_lat_decimal.set(to_string_decimal(lat_value, 5));
-		
+		text_lon_decimal.set(to_string_decimal(lon_value, 5));
-		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));
 	
 		if (on_change && report_change)
 			on_change(altitude(), lat_value, lon_value);

firmware/common/adsb.cpp

@@ -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
+	delta_lat = 360.0 / ((4.0 * NZ) - time_parity);		// NZ = 15
-	yz = floor(131072.0 * (cpr_mod(latitude, delta_lat) / delta_lat) + 0.5);
+	yz = floor(CPR_MAX_VALUE * (cpr_mod(latitude, delta_lat) / delta_lat) + 0.5);
-	rlat = delta_lat * ((yz / 131072.0) + floor(latitude / delta_lat));
+	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);
+	lat = cpr_mod(yz, CPR_MAX_VALUE);
-	lon = cpr_mod(xz, 131072.0);
+	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;
 

firmware/common/adsb.hpp

@@ -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);