From f21a521a00fbf202103c8270f61a63cd8385e172 Mon Sep 17 00:00:00 2001
From: Michal Fratczak <michal@cgarea.com>
Date: Mon, 13 Apr 2020 19:52:30 +0200
Subject: [PATCH] dynamics

---
 tracker/code/main/CMakeLists.txt |  1 +
 tracker/code/main/GLOB.cpp       | 26 +++++++++++++++
 tracker/code/main/GLOB.h         | 10 ++++++
 tracker/code/main/dynamics_t.cpp | 55 ++++++++++++++++++++++++++++++++
 tracker/code/main/dynamics_t.h   | 37 +++++++++++++++++++++
 tracker/code/main/main.cpp       | 32 ++++++++++++++++---
 6 files changed, 157 insertions(+), 4 deletions(-)
 create mode 100644 tracker/code/main/dynamics_t.cpp
 create mode 100644 tracker/code/main/dynamics_t.h

diff --git a/tracker/code/main/CMakeLists.txt b/tracker/code/main/CMakeLists.txt
index 494dd65..2414bea 100644
--- a/tracker/code/main/CMakeLists.txt
+++ b/tracker/code/main/CMakeLists.txt
@@ -10,6 +10,7 @@ find_package(cppzmq)
 set (tracker_src
 	GLOB.h GLOB.cpp
 	cli.h cli.cpp
+	dynamics_t.h dynamics_t.cpp
 	ssdv_t.cpp
 	main.cpp
 	../mtx2/mtx2.cpp
diff --git a/tracker/code/main/GLOB.cpp b/tracker/code/main/GLOB.cpp
index 261e565..39df4d5 100644
--- a/tracker/code/main/GLOB.cpp
+++ b/tracker/code/main/GLOB.cpp
@@ -1,6 +1,7 @@
 #include "GLOB.h"
 
 #include <sstream>
+#include <iostream>
 
 std::string GLOB::str() const
 {
@@ -18,3 +19,28 @@ std::string GLOB::str() const
 
     return s.str();
 }
+
+
+bool GLOB::dynamics_add(const std::string& name, const dynamics_t::tp timestamp, const float value)
+{
+    auto d = dynamics_.find(name);
+    if( d == dynamics_.end() )
+    {
+        dynamics_[name] = dynamics_t();
+        return dynamics_[name].add(timestamp, value);
+    }
+    else
+    {
+        return d->second.add(timestamp, value);
+    }
+
+}
+
+dynamics_t GLOB::dynamics_get(const std::string& name)
+{
+    auto d = dynamics_.find(name);
+    if( d == dynamics_.end() )
+        return dynamics_t(); // default, uninitialised
+    else
+        return d->second;
+}
diff --git a/tracker/code/main/GLOB.h b/tracker/code/main/GLOB.h
index cebb8fa..067bfc7 100644
--- a/tracker/code/main/GLOB.h
+++ b/tracker/code/main/GLOB.h
@@ -1,10 +1,13 @@
 #pragma once
 
 #include <string>
+#include <map>
 #include <atomic>
 
 #include "mtx2/mtx2.h"
 #include "nmea/nmea.h"
+#include "dynamics_t.h"
+
 
 // global options - singleton
 
@@ -18,6 +21,9 @@ private:
 
     std::atomic<nmea_t> nmea; // GPS data
 
+    // sensors dynamics
+    std::map<std::string, dynamics_t>    dynamics_; // index: value name (alt, temp1, etc.)
+
 public:
     static GLOB& get()
     {
@@ -44,6 +50,10 @@ public:
     // sensors
     std::atomic<float>  temperature{0};
 
+    bool        dynamics_add(const std::string& name, const dynamics_t::tp timestamp, const float value);
+    dynamics_t  dynamics_get(const std::string& name);
+
+
     nmea_t nmea_get() { nmea_t ret = get().nmea; return ret; }
     void   nmea_set(const nmea_t& in_nmea) { get().nmea = in_nmea; }
     std::string str() const;
diff --git a/tracker/code/main/dynamics_t.cpp b/tracker/code/main/dynamics_t.cpp
new file mode 100644
index 0000000..e1ba966
--- /dev/null
+++ b/tracker/code/main/dynamics_t.cpp
@@ -0,0 +1,55 @@
+
+#include "dynamics_t.h"
+
+#include <sstream>
+#include <iomanip>
+
+// utc_seconds is HHMMSS converted to seconds. return true if sample was accepted
+bool dynamics_t::add(const tp timestamp, const float val)
+{
+    if( !initialised_ && timestamp.time_since_epoch().count() )
+    {
+        val_ = val;
+        val_prev_ = val;
+        val_prev_timestamp_ = timestamp;
+        dVdT_ = 0;
+
+        if(val > val_max_)
+            val_max_ = val;
+        if(val < val_min_)
+            val_min_ = val;
+
+        initialised_ = true;
+        return true;
+    }
+
+    if( (timestamp - val_prev_timestamp_).count() < min_dT_ )
+        return false;
+
+    dVdT_ = (val - val_prev_) / float((timestamp - val_prev_timestamp_).count());
+
+    if(val > val_max_)
+        val_max_ = val;
+
+    if(val < val_min_)
+        val_min_ = val;
+
+    val_prev_ = val;
+    val_prev_timestamp_ = timestamp;
+
+    return true;
+}
+
+std::chrono::system_clock::time_point dynamics_t::utc2tp(const std::string utc)
+{
+	using namespace std;
+
+	// init with *now*
+	time_t _now = time(nullptr);   // "now: as long int
+	tm tm = *localtime(&_now); // init all fields
+
+	istringstream ss(utc);
+	ss >> get_time( &tm, "%H%M%S" ); // get HHMMSS
+
+	return	chrono::system_clock::from_time_t( mktime(&tm) );
+}
\ No newline at end of file
diff --git a/tracker/code/main/dynamics_t.h b/tracker/code/main/dynamics_t.h
new file mode 100644
index 0000000..6575d19
--- /dev/null
+++ b/tracker/code/main/dynamics_t.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <string>
+#include <chrono>
+#include <limits>
+
+// record min, max, dV/dT for a value
+class dynamics_t
+{
+public:
+    using tp = std::chrono::system_clock::time_point;
+
+private:
+    bool    initialised_ = false;
+    float   val_ = 0;
+    float   val_prev_ = 0;
+    tp      val_prev_timestamp_; // timestamp of previous value
+    float   val_min_ = std::numeric_limits<float>::max();
+    float   val_max_ = -std::numeric_limits<float>::min();
+    float   dVdT_ = 0;
+
+public:
+    int     min_dT_ = 10; // minimum time difference to compute dV/dT. Sample too close to previous will be rejected
+
+    bool    initialised() const { return initialised_; }
+
+    bool    add(const tp timestamp, const float val); // utc_seconds is HHMMSS converted to seconds. return true if sample was accepted
+
+    float   min()   const { return val_min_; }
+    float   max()   const { return val_max_; }
+    float   dVdT()  const { return dVdT_; }
+
+    static std::chrono::system_clock::time_point utc2tp(const std::string utc);
+
+};
+
+
diff --git a/tracker/code/main/main.cpp b/tracker/code/main/main.cpp
index 91e9447..b10a10d 100644
--- a/tracker/code/main/main.cpp
+++ b/tracker/code/main/main.cpp
@@ -16,6 +16,7 @@
 #include "ublox/ublox_cmds.h"
 #include "ublox/ublox.h"
 #include "ds18b20/ds18b20.h"
+#include "dynamics_t.h"
 
 #include "ssdv_t.h"
 #include "cli.h"
@@ -213,6 +214,7 @@ int main1(int argc, char** argv)
     }
     cout<<"ds18b20_device "<<ds18b20_device<<endl;
 
+
 	// ALL SENSORS THREAD
 	//
 	std::thread sensors_thread( [ds18b20_device]() {
@@ -250,20 +252,34 @@ int main1(int argc, char** argv)
 		int msg_num = 0;
 		while( G_RUN && msg_num++ < G.cli.msg_num )
 		{
+			++msg_id;
+
+
+			// GPS data
+			//
 			const nmea_t current_nmea = G.nmea_get();
 			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)
 				valid_nmea = current_nmea;
-			else
+			else // at least use time
 				memcpy( valid_nmea.utc, current_nmea.utc, sizeof(current_nmea.utc) );
 
+
+			// dynamics
+			//
+			G.dynamics_add("alt", 			dynamics_t::utc2tp(valid_nmea.utc), valid_nmea.alt);
+			G.dynamics_add("temperature", 	dynamics_t::utc2tp(valid_nmea.utc), G.temperature);
+			cout<<C_MAGENTA<<"alt "<<G.dynamics_get("alt").dVdT()<<C_OFF<<endl;
+			cout<<C_MAGENTA<<"temperature "<<G.dynamics_get("temperature").dVdT()<<C_OFF<<endl;
+
+
 			// telemetry message
 			//
 			stringstream  msg_stream;
 			msg_stream<<G.cli.callsign;
-			msg_stream<<","<<msg_id++;
+			msg_stream<<","<<msg_id;
 			msg_stream<<","<<valid_nmea.utc;
 			msg_stream<<","<<valid_nmea.lat<<","<<valid_nmea.lon<<","<<valid_nmea.alt;
 			msg_stream<<","<<valid_nmea.sats<<","<<gps_fix_valid;
@@ -283,6 +299,7 @@ int main1(int argc, char** argv)
 				msg_num = 0;
 		}
 
+
 		// send SSDV image next packet
 		//
 		if( !ssdv_tiles.size() & G.cli.ssdv_image.size() )
@@ -291,8 +308,15 @@ int main1(int argc, char** argv)
 		{
 			auto tile = ssdv_tiles.next_tile();
 			if(!ssdv_tiles.size())	// delete image when done
-				system( (string("rm -f ") + G.cli.ssdv_image).c_str() );
-			cout<<"SSDV"<<endl;
+			{
+				try {
+					system( (string("rm -f ") + G.cli.ssdv_image).c_str() );
+				}
+				catch(exception& e) {
+					cerr<<C_RED<<"Error deleting SSDV file.\n"<<e.what()<<C_OFF<<endl;
+				}
+			}
+			cout<<"Send SSDV"<<endl;
 			mtx2_write( radio_fd, tile.data(), sizeof(tile) );
 		}
 	}