kopia lustrzana https://github.com/ogre/pizero_tracker
dynamics
rodzic
6f7ab53dce
commit
f21a521a00
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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) );
|
||||
}
|
|
@ -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);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -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) );
|
||||
}
|
||||
}
|
||||
|
|
Ładowanie…
Reference in New Issue