From b3a4b6d3bb963b8bd643ec3d608dab5e447cac1f Mon Sep 17 00:00:00 2001
From: Michal Fratczak <michal@cgarea.com>
Date: Fri, 26 Jun 2020 13:38:42 +0100
Subject: [PATCH] Add state server Try predicting one of states: kUnknown,
 kStandBy, kAscend, kDescend, kFreefall, kLanded. Needs launch location lat
 lon alt. Stop Camera on kLand. Stop SSDV on kFreefall and kLand. Add Fake GPS
 for testing.

---
 camera/camera.py                     |  11 +-
 tracker/code/main/CMakeLists.txt     |   1 +
 tracker/code/main/GLOB.cpp           |   4 +
 tracker/code/main/GLOB.h             |  25 ++++-
 tracker/code/main/cli.cpp            |  10 ++
 tracker/code/main/gps_distance_t.cpp |  82 ++++++++++++++
 tracker/code/main/gps_distance_t.h   |  15 +++
 tracker/code/main/main.cpp           | 162 ++++++++++++++++++++++-----
 tracker/code/nmea/nmea.cpp           |   8 ++
 tracker/code/nmea/nmea.h             |   1 +
 10 files changed, 285 insertions(+), 34 deletions(-)
 create mode 100644 tracker/code/main/gps_distance_t.cpp
 create mode 100644 tracker/code/main/gps_distance_t.h

diff --git a/camera/camera.py b/camera/camera.py
index 0f7e003..d3823c4 100644
--- a/camera/camera.py
+++ b/camera/camera.py
@@ -186,8 +186,7 @@ def StateLoop(port):
 	client.connect(SERVER_ENDPOINT)
 	poll = zmq.Poller()
 	poll.register(client, zmq.POLLIN)
-	query_msgs = ['nmea', 'dynamics']
-
+	query_msgs = ['nmea', 'dynamics', 'flight_state']
 
 	retries_left = REQUEST_RETRIES
 	while THREADS_RUN and retries_left:
@@ -228,7 +227,7 @@ def StateLoop(port):
 					client = context.socket(zmq.REQ)
 					client.connect(SERVER_ENDPOINT)
 					poll.register(client, zmq.POLLIN)
-					client.send(qm)
+					client.send_string(qm)
 	context.term()
 
 
@@ -328,6 +327,12 @@ def CameraLoop(session_dir, opts):
 			time.sleep(60)
 			continue
 
+		if 'flight_state' in STATE and STATE['flight_state']['flight_state'] == 'kLanded':
+			CAMERA.stop_preview()
+			print("flight_state::klanded - stop camera and wait.")
+			time.sleep(60)
+			continue
+
 		CAMERA.start_preview()
 
 		# full res photo
diff --git a/tracker/code/main/CMakeLists.txt b/tracker/code/main/CMakeLists.txt
index 2414bea..3426479 100644
--- a/tracker/code/main/CMakeLists.txt
+++ b/tracker/code/main/CMakeLists.txt
@@ -13,6 +13,7 @@ set (tracker_src
 	dynamics_t.h dynamics_t.cpp
 	ssdv_t.cpp
 	main.cpp
+	gps_distance_t.h gps_distance_t.cpp
 	../mtx2/mtx2.cpp
 	../nmea/nmea.cpp
 	../ublox/ublox_cmds.cpp
diff --git a/tracker/code/main/GLOB.cpp b/tracker/code/main/GLOB.cpp
index 9a95668..b1f19ab 100644
--- a/tracker/code/main/GLOB.cpp
+++ b/tracker/code/main/GLOB.cpp
@@ -12,6 +12,10 @@ std::string GLOB::str() const
     s<<"\tbaud="<<static_cast<int>(cli.baud)<<"\n";
     s<<"\tssdv_image="<<cli.ssdv_image<<"\n";
     s<<"\tmsg_num="<<cli.msg_num<<"\n";
+    s<<"\tlat="<<cli.lat<<"\n";
+    s<<"\tlon="<<cli.lon<<"\n";
+    s<<"\talt="<<cli.alt<<"\n";
+    s<<"\ttestgps="<<cli.testgps<<"\n";
     s<<"\tport="<<cli.port<<"\n";
     s<<"\thw_pin_radio_on="<<cli.hw_pin_radio_on<<"\n";
     s<<"\thw_radio_serial="<<cli.hw_radio_serial<<"\n";
diff --git a/tracker/code/main/GLOB.h b/tracker/code/main/GLOB.h
index 039f79d..ae1bab8 100644
--- a/tracker/code/main/GLOB.h
+++ b/tracker/code/main/GLOB.h
@@ -11,8 +11,18 @@
 #include "dynamics_t.h"
 
 
-// global options - singleton
+enum class flight_state_t : int
+{
+	kUnknown = 0,
+	kStandBy = 1, // near launch position and average abs(dAlt/dTime) < 3 m/s
+	kAscend = 2,  // average dAlt/dTime > 3 m/s
+	kDescend = 3, // average dAlt/dTime < -3 m/s
+	kFreefall = 4, // average abs(dAlt/dTime) < -8 m/s
+	kLanded = 5 // far from launch position and average abs(dAlt/dTime) < 3 m/s and alt<2000
+};
 
+
+// global options - singleton
 class GLOB
 {
 
@@ -27,6 +37,9 @@ private:
     // sensors dynamics
     std::map<std::string, dynamics_t>    dynamics_; // index: value name (alt, temp1, etc.)
 
+    // flight state
+    std::atomic<flight_state_t>  flight_state_{flight_state_t::kUnknown};
+
 public:
     static GLOB& get()
     {
@@ -42,6 +55,10 @@ public:
         std::string     ssdv_image;     // ssdv encoded image path
         int             msg_num = 5;    // number of telemetry sentences emitted between SSDV packets
         int             port = 6666;    // zeroMQ server port
+        float           lat = 0;    // launch site latitude deg
+        float           lon = 0;    // launch site longitude deg
+        float           alt = 0;    // launch site altitude meters
+        bool            testgps = false;  // generate fake GPS for testing
 
         // hardware config
         int             hw_pin_radio_on = 0;    // gpio numbered pin for radio enable. current board: 22
@@ -60,6 +77,12 @@ public:
     void gps_fix_now() { gps_fix_timestamp_ = std::chrono::steady_clock::now(); }
     int  gps_fix_age() const { return (std::chrono::steady_clock::now() - gps_fix_timestamp_).count() / 1e9; }
 
+    void flight_state_set(const flight_state_t flight_state) { get().flight_state_ = flight_state; }
+    flight_state_t flight_state_get() const { return get().flight_state_; }
+
+    // runtime seconds
+    long long runtime_secs_ = 0;
+
     std::string str() const;
 
 };
diff --git a/tracker/code/main/cli.cpp b/tracker/code/main/cli.cpp
index acf8ddb..62073b5 100644
--- a/tracker/code/main/cli.cpp
+++ b/tracker/code/main/cli.cpp
@@ -2,6 +2,7 @@
 
 
 #include <string>
+#include <vector>
 #include <iostream>
 #include <fstream>
 #include "glob.h"
@@ -27,6 +28,8 @@ void CLI(int ac, char* av[])
             ("hw_pin_radio_on",	po::value<int>(),       "gpio numbered pin for radio enable. current board: 22")
             ("hw_radio_serial",	po::value<string>(),    "serial device for MTX2 radio. for rPI4: /dev/serial0")
             ("hw_ublox_device",	po::value<string>(),    "I2C device for uBLOX. for rPI4: /dev/i2c-7")
+            ("latlonalt",	po::value< std::vector<float> >()->multitoken(), "Launch site GPS location (decimal) and alt meters")
+            ("testgps",	po::value<bool>(),      "Generate fake GPS for testing")
             ;
 
         po::options_description cli_options("Command Line Interface options");
@@ -74,6 +77,13 @@ void CLI(int ac, char* av[])
         if (vm.count("hw_ublox_device"))    GLOB::get().cli.hw_ublox_device =   vm["hw_ublox_device"].as<string>();
         if (vm.count("ssdv"))               GLOB::get().cli.ssdv_image =        vm["ssdv"].as<string>();
         if (vm.count("baud"))               GLOB::get().cli.baud = static_cast<baud_t>( vm["baud"].as<int>() );
+        if (vm.count("testgps"))            GLOB::get().cli.testgps =           vm["testgps"].as<bool>();
+        if (vm.count("latlonalt")) {
+            vector<float> latlonalt_vec = vm["latlonalt"].as< vector<float> >();
+            GLOB::get().cli.lat = latlonalt_vec[0];
+            GLOB::get().cli.lon = latlonalt_vec[1];
+            GLOB::get().cli.alt = latlonalt_vec[2];
+        }
 
     }
 	catch(exception& e)
diff --git a/tracker/code/main/gps_distance_t.cpp b/tracker/code/main/gps_distance_t.cpp
new file mode 100644
index 0000000..4fe220b
--- /dev/null
+++ b/tracker/code/main/gps_distance_t.cpp
@@ -0,0 +1,82 @@
+#include "gps_distance_t.h"
+#include <cmath>
+
+
+std::ostream& operator<<(std::ostream& o, const gps_distance_t& rhs )
+{
+    o<<"dist_line_: "<<rhs.dist_line_<<"\n"
+     <<"dist_circle_: "<<rhs.dist_circle_<<"\n"
+     <<"dist_radians_: "<<rhs.dist_radians_<<"\n"
+     <<"elevation_: "<<rhs.elevation_<<"\n"
+     <<"bearing_: "<<rhs.bearing_<<"\n";
+
+    return o;
+}
+
+
+// taken from https://github.com/projecthorus/radiosonde_auto_rx/blob/master/auto_rx/autorx/utils.py
+gps_distance_t calc_gps_distance(double lat1, double lon1, double alt1, double lat2, double lon2, double alt2)
+{
+    /*
+    Calculate and return information from 2 (lat, lon, alt) points
+    Input and output latitudes, longitudes, angles, bearings and elevations are
+    in degrees, and input altitudes and output distances are in meters.
+    */
+
+    const double radius = 6371000.0; // Earth radius in meters
+
+    // to radians
+    lat1 *= M_PI / 180.0;
+    lat2 *= M_PI / 180.0;
+    lon1 *= M_PI / 180.0;
+    lon2 *= M_PI / 180.0;
+
+    // Calculate the bearing, the angle at the centre, and the great circle
+    // distance using Vincenty's_formulae with f = 0 (a sphere). See
+    // http://en.wikipedia.org/wiki/Great_circle_distance#Formulas and
+    // http://en.wikipedia.org/wiki/Great-circle_navigation and
+    // http://en.wikipedia.org/wiki/Vincenty%27s_formulae
+    double d_lon = lon2 - lon1;
+    double sa = cos(lat2) * sin(d_lon);
+    double sb = (cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(d_lon));
+    double bearing = atan2(sa, sb);
+    double aa = sqrt((sa*sa) + (sb*sb));
+    double ab = (sin(lat1) * sin(lat2)) + (cos(lat1) * cos(lat2) * cos(d_lon));
+    double angle_at_centre = atan2(aa, ab);
+    double great_circle_distance = angle_at_centre * radius;
+
+    // Armed with the angle at the centre, calculating the remaining items
+    // is a simple 2D triangley circley problem:
+
+    // Use the triangle with sides (r + alt1), (r + alt2), distance in a
+    // straight line. The angle between (r + alt1) and (r + alt2) is the
+    // angle at the centre. The angle between distance in a straight line and
+    // (r + alt1) is the elevation plus pi/2.
+
+    // Use sum of angle in a triangle to express the third angle in terms
+    // of the other two. Use sine rule on sides (r + alt1) and (r + alt2),
+    // expand with compound angle formulae and solve for tan elevation by
+    // dividing both sides by cos elevation
+    double ta = radius + alt1;
+    double tb = radius + alt2;
+    double ea = (cos(angle_at_centre) * tb) - ta;
+    double eb = sin(angle_at_centre) * tb;
+    double elevation = atan2(ea, eb);
+
+    // Use cosine rule to find unknown side.
+    double line_distance = sqrt((ta*ta) + (tb*tb) - 2 * tb * ta * cos(angle_at_centre));
+
+    // Give a bearing in range 0 <= b < 2pi
+    if (bearing < 0)
+        bearing += 2 * M_PI;
+
+
+    gps_distance_t res;
+    res.dist_line_ = line_distance;
+    res.dist_circle_ = great_circle_distance;
+    res.dist_radians_ = angle_at_centre;
+    res.elevation_ = elevation / ( M_PI / 180.0 );
+    res.bearing_ = bearing / ( M_PI / 180.0 );
+    return res;
+
+}
diff --git a/tracker/code/main/gps_distance_t.h b/tracker/code/main/gps_distance_t.h
new file mode 100644
index 0000000..56e9f3f
--- /dev/null
+++ b/tracker/code/main/gps_distance_t.h
@@ -0,0 +1,15 @@
+#include <iostream>
+
+
+struct gps_distance_t
+{
+    double   dist_line_ = 0;
+    double   dist_circle_ = 0;
+    double   dist_radians_ = 0;  // angular distance over Earth surface, in radians
+    double   elevation_ = 0;     // degrees
+    double   bearing_ = 0;       // degrees
+};
+
+std::ostream& operator<<(std::ostream& o, const gps_distance_t& rhs );
+
+gps_distance_t calc_gps_distance(double lat1, double lon1, double alt1, double lat2, double lon2, double alt2);
diff --git a/tracker/code/main/main.cpp b/tracker/code/main/main.cpp
index a8444ec..639c6b4 100644
--- a/tracker/code/main/main.cpp
+++ b/tracker/code/main/main.cpp
@@ -9,6 +9,8 @@
 #include <chrono>
 #include <future>
 #include <atomic>
+#include <functional>
+#include <math.h>
 
 #include "pigpio.h"
 #include <zmq.hpp>
@@ -21,6 +23,7 @@
 #include "ssdv_t.h"
 #include "cli.h"
 #include "GLOB.h"
+#include "gps_distance_t.h"
 
 const char* C_RED = 		"\033[1;31m";
 const char* C_GREEN = 		"\033[1;32m";
@@ -91,6 +94,22 @@ zmq::message_t make_zmq_reply(const std::string& i_msg_str)
 		memcpy( (void*) reply.data(), reply_str.c_str(), reply_str.size() );
 		return reply;
 	}
+	else if(i_msg_str == "flight_state")	{
+		std::string reply_str("{'flight_state':");
+		switch( G.flight_state_get() )
+		{
+			case flight_state_t::kUnknown:		reply_str += "'kUnknown'";		break;
+			case flight_state_t::kStandBy:		reply_str += "'kStandBy'";		break;
+			case flight_state_t::kAscend:		reply_str += "'kAscend'";		break;
+			case flight_state_t::kDescend:		reply_str += "'kDescend'";		break;
+			case flight_state_t::kFreefall:		reply_str += "'kFreefall'";		break;
+			case flight_state_t::kLanded:		reply_str += "'kLanded'";		break;
+		}
+		reply_str += "}";
+		zmq::message_t reply( reply_str.size() );
+		memcpy( (void*) reply.data(), reply_str.c_str(), reply_str.size() );
+		return reply;
+	}
 	else {
 		zmq::message_t reply( 7 );
 		memcpy( (void*) reply.data(), "UNKNOWN", 7 );
@@ -139,8 +158,6 @@ int main1(int argc, char** argv)
 {
     using namespace std;
 
-	const auto START_TIME = chrono::steady_clock::now(); // used to measure running time
-
 	// command line interface
 	CLI(argc, argv);
 
@@ -227,11 +244,10 @@ int main1(int argc, char** argv)
 	sleep(1);
 
 
-	// uBLOX thread
+	// uBLOX loop
 	//
-	std::thread ublox_thread( [uBlox_i2c_fd]() {
+	auto ublox_loop = [uBlox_i2c_fd]() {
 		while(G_RUN) {
-			// std::this_thread::sleep_for( std::chrono::seconds(5) );
 			const vector<char> ublox_data = uBLOX_read_msg(uBlox_i2c_fd); // typical blocking time: 0/1/1.2 seconds
 			const string nmea_str = NMEA_get_last_msg(ublox_data.data(), ublox_data.size());
 			// cout<<nmea_str<<endl;
@@ -250,21 +266,52 @@ int main1(int argc, char** argv)
 			current_nmea.lon = valid_nmea.lon;
 			current_nmea.alt = valid_nmea.alt;
 
-			if( NMEA_parse(nmea_str.c_str(), current_nmea) ) {
-				// only one at a time can be valid.
-				// fix_status is from RMC, fix_quality is from GGA
-				const bool gps_fix_valid =
-								current_nmea.fix_status  == nmea_t::fix_status_t::kValid
-							|| 	current_nmea.fix_quality != nmea_t::fix_quality_t::kNoFix;
-				if(gps_fix_valid) {
-					GLOB::get().nmea_set(current_nmea);
-					GLOB::get().gps_fix_now(); // typical time since uBlox msg read to here is under 1 millisecond
-					GLOB::get().dynamics_add("alt", std::chrono::steady_clock::now(), current_nmea.alt);
-					// cout<<C_MAGENTA<<"alt "<<GLOB::get().dynamics_get("alt").str()<<C_OFF<<endl;
-				}
+			if( NMEA_parse(nmea_str.c_str(), current_nmea) and current_nmea.valid() ) {
+				GLOB::get().nmea_set(current_nmea);
+				GLOB::get().gps_fix_now(); // typical time since uBlox msg read to here is under 1 millisecond
+				GLOB::get().dynamics_add("alt", std::chrono::steady_clock::now(), current_nmea.alt);
+				// cout<<C_MAGENTA<<"alt "<<GLOB::get().dynamics_get("alt").str()<<C_OFF<<endl;
 			}
 		}
-	});
+	};
+
+	// fake GPS loop - usefull for testing
+	//
+	auto fake_gps_loop = [uBlox_i2c_fd]() {
+		cout<<"Using FAKE GPS Coordinates !!!"<<endl;
+		while(G_RUN) {
+			const nmea_t  valid_nmea = GLOB::get().nmea_get();
+			nmea_t  current_nmea;
+			current_nmea.lat = valid_nmea.lat;
+			current_nmea.lon = valid_nmea.lon;
+			current_nmea.alt = valid_nmea.alt;
+			current_nmea.fix_status = nmea_t::fix_status_t::kValid;
+
+			if( GLOB::get().runtime_secs_ > 30 ) {
+				current_nmea.lat = GLOB::get().cli.lat + 0.0001 * (GLOB::get().runtime_secs_ - 30);
+				current_nmea.lon = GLOB::get().cli.lon + 0.0001 * (GLOB::get().runtime_secs_ - 30);
+				current_nmea.alt = GLOB::get().cli.alt + 35000.0 * abs(sin(3.1415 * float(GLOB::get().runtime_secs_-30) / 300));
+			}
+			else {
+				current_nmea.lat = GLOB::get().cli.lat;
+				current_nmea.lon = GLOB::get().cli.lon;
+				current_nmea.alt = GLOB::get().cli.alt;
+			}
+			GLOB::get().nmea_set(current_nmea);
+			GLOB::get().gps_fix_now(); // typical time since uBlox msg read to here is under 1 millisecond
+			GLOB::get().dynamics_add("alt", std::chrono::steady_clock::now(), current_nmea.alt);
+
+			this_thread::sleep_for(chrono::seconds(1));
+		}
+	};
+
+
+	// GPS thread. uBLOX or faked
+	//
+	function<void()> gps_loop(ublox_loop);
+	if(G.cli.testgps)
+		gps_loop = function<void()>(fake_gps_loop);
+	std::thread ublox_thread( gps_loop );
 
 
     // DS18B20 temp sensor
@@ -289,6 +336,39 @@ int main1(int argc, char** argv)
 		}
 	});
 
+	// Flight State Thread
+	// try guessing one of these states: kUnknown, kStandBy, kAscend, kDescend, kFreefall, kLanded
+	//
+	std::thread flight_state_thread( []() {
+		const auto START_TIME = chrono::steady_clock::now(); // used to measure running time
+		while(G_RUN) {
+			this_thread::sleep_for( chrono::seconds(1) );
+
+			GLOB::get().runtime_secs_ = chrono::duration_cast<chrono::seconds>(chrono::steady_clock::now() - START_TIME).count();
+			const nmea_t nmea = GLOB::get().nmea_get();
+			const auto dist_from_home = calc_gps_distance( nmea.lat, nmea.lon, nmea.alt,
+										GLOB::get().cli.lat,  GLOB::get().cli.lon, 0);
+			const auto alt = GLOB::get().dynamics_get("alt");
+			const auto dAltAvg = alt.dVdT_avg();
+
+			flight_state_t flight_state = flight_state_t::kUnknown;
+			if(nmea.valid()) {
+				if( abs(dist_from_home.dist_line_) < 200 and abs(dAltAvg) <= 3 )
+					flight_state = flight_state_t::kStandBy;
+				else if(dAltAvg < -8)
+					flight_state = flight_state_t::kFreefall;
+				else if(dAltAvg < -3)
+					flight_state = flight_state_t::kDescend;
+				else if(dAltAvg > 3)
+					flight_state = flight_state_t::kAscend;
+				else if( abs(dist_from_home.dist_line_) > 2000 and abs(dAltAvg) <= 3 and alt.val() < 2000 )
+					flight_state = flight_state_t::kLanded;
+			}
+			cout<<alt.val()<<" "<<alt.dVdT()<<" "<<alt.dVdT_avg()<<" "<<dist_from_home.dist_line_<<endl;
+			GLOB::get().flight_state_set( flight_state );
+		}
+	});
+
 
 	// ZeroMQ server
 	zmq::context_t zmq_context(1);
@@ -324,7 +404,7 @@ int main1(int argc, char** argv)
 	{
 		// telemetry. G.cli.msg_num sentences before SSDV
 		//
-		for(int __mi=0; __mi<G.cli.msg_num && G_RUN; ++__mi)
+		for(int __mi=0; __mi<G.cli.msg_num and G_RUN; ++__mi)
 		{
 			++msg_id;
 
@@ -332,19 +412,38 @@ int main1(int argc, char** argv)
 			const nmea_t valid_nmea = G.nmea_get();
 
 			// TELEMETRY MESSAGE
+			//
 			stringstream  tlmtr_stream;
+
 			// Callsign, ID, UTC:
 			tlmtr_stream<<G.cli.callsign;
 			tlmtr_stream<<","<<msg_id;
 			tlmtr_stream<<","<<valid_nmea.utc;
+
 			// !! ONLY VALID LAT,LON,ALT ARE BEING SENT. LOOK INTO uBLOX THREAD.
 			tlmtr_stream<<","<<valid_nmea.lat<<","<<valid_nmea.lon<<","<<valid_nmea.alt;
 			tlmtr_stream<<","<<valid_nmea.sats<<","<<GLOB::get().gps_fix_age();
+
+			// runtime
+			tlmtr_stream<<","<<static_cast<int>(GLOB::get().runtime_secs_);
+
 			// Sensors:
 			tlmtr_stream<<","<<setprecision(1)<<fixed<<G.dynamics_get("temp_int").val();
-			// runtime
-			const auto runtime_secs = chrono::duration_cast<chrono::seconds>(chrono::steady_clock::now() - START_TIME).count();
-			tlmtr_stream<<","<<static_cast<int>(runtime_secs);
+
+			// average dAlt/dT
+			tlmtr_stream<<","<<setprecision(1)<<fixed<<G.dynamics_get("alt").dVdT_avg();
+
+			// flight state
+			switch( G.flight_state_get() )
+			{
+				case flight_state_t::kUnknown:		tlmtr_stream<<",U";		break;
+				case flight_state_t::kStandBy:		tlmtr_stream<<",S";		break;
+				case flight_state_t::kAscend:		tlmtr_stream<<",A";		break;
+				case flight_state_t::kDescend:		tlmtr_stream<<",D";		break;
+				case flight_state_t::kFreefall:		tlmtr_stream<<",F";		break;
+				case flight_state_t::kLanded:		tlmtr_stream<<",L";		break;
+			}
+
 			// CRC:
 			const string msg_with_crc = string("\0",1) + "$$$" + tlmtr_stream.str() + '*' + CRC(tlmtr_stream.str());
 			cout<<C_GREEN<<msg_with_crc<<C_OFF<<endl;
@@ -367,12 +466,9 @@ int main1(int argc, char** argv)
 
 		// send SSDV image next packet
 		//
-		if(		true
-			// &&	G.gps_fix_age() < 20
-			// && 	G.dynamics_get("alt").dVdT_avg() > -5  // not falling
-		)
+		if( G.flight_state_get() != flight_state_t::kFreefall and G.flight_state_get() != flight_state_t::kLanded )
 		{
-			if( !ssdv_packets.size() && G.cli.ssdv_image.size() )
+			if( !ssdv_packets.size() and G.cli.ssdv_image.size() )
 				cout<<"SSDV loaded "<<	ssdv_packets.load_file( G.cli.ssdv_image )	<<" packets from disk."<<endl;
 
 			if( ssdv_packets.size() )
@@ -400,17 +496,23 @@ int main1(int argc, char** argv)
 	if(msgid_fh)
 		fclose(msgid_fh);
 	cout<<"Closing sensors thread"<<endl;
-	sensors_thread.join();
+	if( sensors_thread.joinable() )
+		sensors_thread.join();
 	cout<<"Closing uBlox I2C thread and device"<<endl;
-	ublox_thread.join();
+	if( ublox_thread.joinable() )
+		ublox_thread.join();
     close(uBlox_i2c_fd);
+	cout<<"Closing flight state thread"<<endl;
+	if( flight_state_thread.joinable() )
+		flight_state_thread.join();
 	cout<<"Closing UART Radio device"<<endl;
 	close(radio_fd);
 	gpioWrite (G.cli.hw_pin_radio_on, 0);
 	cout<<"Closing gpio"<<endl;
 	gpioTerminate();
 	cout<<"Closing zmq thread"<<endl;
-	zmq_thread.join(); // will return after next received message, or stuck forever if no messages comes in
+	if( zmq_thread.joinable() )
+		zmq_thread.join(); // will return after next received message, or stuck forever if no messages comes in
 
 
     return 0;
diff --git a/tracker/code/nmea/nmea.cpp b/tracker/code/nmea/nmea.cpp
index aef0ab4..fa91953 100644
--- a/tracker/code/nmea/nmea.cpp
+++ b/tracker/code/nmea/nmea.cpp
@@ -333,3 +333,11 @@ int nmea_t::utc_as_seconds() const
 	const int total_seconds = S + 60*M + 60*60*H;
 	return total_seconds;
 }
+
+bool nmea_t::valid() const
+{
+	// only one at a time can be valid.
+	// fix_status is from RMC, fix_quality is from GGA
+	return 		fix_status  == fix_status_t::kValid
+			or 	fix_quality != fix_quality_t::kNoFix;
+}
diff --git a/tracker/code/nmea/nmea.h b/tracker/code/nmea/nmea.h
index f3749d7..c88519b 100644
--- a/tracker/code/nmea/nmea.h
+++ b/tracker/code/nmea/nmea.h
@@ -39,6 +39,7 @@ public:
 	std::string json() const;
 
 	int utc_as_seconds() const;	// convert utc HHMMSS to seconds
+	bool valid() const;
 };