kopia lustrzana https://github.com/ogre/pizero_tracker
Michal Fratczak
rodzic
d0407b0056
commit
1b7f91c207
|
|
@ -36,7 +36,7 @@ bool GLOB::dynamics_add(const std::string& name, const dynamics_t::tp timestamp,
|
|||
|
||||
}
|
||||
|
||||
dynamics_t GLOB::dynamics_get(const std::string& name)
|
||||
dynamics_t GLOB::dynamics_get(const std::string& name) const
|
||||
{
|
||||
auto d = dynamics_.find(name);
|
||||
if( d == dynamics_.end() )
|
||||
|
|
@ -44,3 +44,11 @@ dynamics_t GLOB::dynamics_get(const std::string& name)
|
|||
else
|
||||
return d->second;
|
||||
}
|
||||
|
||||
std::vector<std::string> GLOB::dynamics_keys() const
|
||||
{
|
||||
std::vector<std::string> ret;
|
||||
for(const auto& k : dynamics_)
|
||||
ret.push_back(k.first);
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,8 +1,10 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "mtx2/mtx2.h"
|
||||
#include "nmea/nmea.h"
|
||||
|
|
@ -19,7 +21,8 @@ private:
|
|||
GLOB( const GLOB& ) = delete; // non construction-copyable
|
||||
GLOB& operator=( const GLOB& ) = delete; // non copyable
|
||||
|
||||
std::atomic<nmea_t> nmea; // GPS data
|
||||
std::atomic<nmea_t> nmea_; // GPS data
|
||||
std::chrono::steady_clock::time_point gps_fix_timestamp_;
|
||||
|
||||
// sensors dynamics
|
||||
std::map<std::string, dynamics_t> dynamics_; // index: value name (alt, temp1, etc.)
|
||||
|
|
@ -51,11 +54,15 @@ public:
|
|||
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);
|
||||
dynamics_t dynamics_get(const std::string& name) const;
|
||||
std::vector<std::string> dynamics_keys() const; // names in dynamics_
|
||||
|
||||
void nmea_set(const nmea_t& in_nmea) { get().nmea_ = in_nmea; }
|
||||
nmea_t nmea_get() { nmea_t ret = get().nmea_; return ret; }
|
||||
|
||||
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; }
|
||||
|
||||
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;
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -40,7 +40,21 @@ bool dynamics_t::add(const tp timestamp, const float val)
|
|||
return true;
|
||||
}
|
||||
|
||||
std::chrono::system_clock::time_point dynamics_t::utc2tp(const std::string utc)
|
||||
std::string dynamics_t::json() const
|
||||
{
|
||||
std::stringstream ss;
|
||||
std::string sep(",");
|
||||
ss<<"{";
|
||||
ss<<"'initialised':"<<initialised_<<sep;
|
||||
ss<<"'val':"<<val_<<sep;
|
||||
ss<<"'min':"<<val_min_<<sep;
|
||||
ss<<"'max':"<<val_max_<<sep;
|
||||
ss<<"'dVdT':"<<dVdT_;
|
||||
ss<<"}";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
std::chrono::system_clock::time_point dynamics_t::utc2tp(const std::string& utc)
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
|
|
|
|||
|
|
@ -30,7 +30,9 @@ public:
|
|||
float max() const { return val_max_; }
|
||||
float dVdT() const { return dVdT_; }
|
||||
|
||||
static std::chrono::system_clock::time_point utc2tp(const std::string utc);
|
||||
std::string json() const;
|
||||
|
||||
static std::chrono::system_clock::time_point utc2tp(const std::string& utc);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -70,13 +70,19 @@ zmq::message_t make_zmq_reply(const std::string& i_msg_str)
|
|||
{
|
||||
auto& G = GLOB::get();
|
||||
if(i_msg_str == "nmea") {
|
||||
const std::string reply_str( G.nmea_get().str() );
|
||||
std::string reply_str( "{'nmea':" + G.nmea_get().json() );
|
||||
reply_str += ",'fixAge':" + std::to_string(G.gps_fix_age());
|
||||
reply_str += "}";
|
||||
zmq::message_t reply( reply_str.size() );
|
||||
memcpy( (void*) reply.data(), reply_str.c_str(), reply_str.size() );
|
||||
return reply;
|
||||
}
|
||||
else if(i_msg_str == "temp") {
|
||||
std::string reply_str = std::to_string( G.temperature );
|
||||
else if(i_msg_str == "dynamics") {
|
||||
std::string reply_str("{'dynamics':{");
|
||||
for(auto& dyn_name : G.dynamics_keys())
|
||||
reply_str += "'" + dyn_name + "':" + G.dynamics_get(dyn_name).json() + ",";
|
||||
reply_str.pop_back(); // last comma
|
||||
reply_str += "}}";
|
||||
zmq::message_t reply( reply_str.size() );
|
||||
memcpy( (void*) reply.data(), reply_str.c_str(), reply_str.size() );
|
||||
return reply;
|
||||
|
|
@ -189,17 +195,33 @@ int main1(int argc, char** argv)
|
|||
//
|
||||
std::thread ublox_thread( [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);
|
||||
const string nmea_str( NMEA_get_last_msg(ublox_data.data(), ublox_data.size()) );
|
||||
cout<<nmea_str<<C_OFF<<endl;
|
||||
if( !NMEA_msg_checksum_ok(nmea_str) )
|
||||
{
|
||||
if( !NMEA_msg_checksum_ok(nmea_str) ) {
|
||||
cerr<<C_RED<<"NMEA Checksum Fail: "<<nmea_str<<C_OFF<<endl;
|
||||
continue;
|
||||
}
|
||||
nmea_t nmea = GLOB::get().nmea_get();
|
||||
NMEA_parse( nmea_str.c_str(), nmea ); // nmea fields that were not altered by GGA/RMC will remain unmodified
|
||||
GLOB::get().nmea_set(nmea); //update global nmea
|
||||
nmea_t current_nmea;
|
||||
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();
|
||||
}
|
||||
else { // REUSE LAT,LON,ALT FROM LAST VALID SENTENCE
|
||||
nmea_t valid_nmea = GLOB::get().nmea_get();
|
||||
current_nmea.lat = valid_nmea.lat;
|
||||
current_nmea.lon = valid_nmea.lon;
|
||||
current_nmea.alt = valid_nmea.alt;
|
||||
GLOB::get().nmea_set(current_nmea);
|
||||
}
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
|
|
@ -236,7 +258,6 @@ int main1(int argc, char** argv)
|
|||
if(!res.has_value())
|
||||
continue;
|
||||
string msg_str( (char*)msg.data(), msg.size() );
|
||||
cout<<"ZMQ msg: "<<msg_str<<endl;
|
||||
zmq_socket.send( make_zmq_reply(msg_str), zmq::send_flags::none );
|
||||
}
|
||||
});
|
||||
|
|
@ -244,27 +265,20 @@ int main1(int argc, char** argv)
|
|||
|
||||
// READ SENSORS, CONSTRUCT TELEMETRY MESSAGE, RF SEND TEMEMETRY AND IMAGE
|
||||
//
|
||||
nmea_t valid_nmea;
|
||||
ssdv_t ssdv_tiles;
|
||||
int msg_id = 0;
|
||||
while(G_RUN)
|
||||
{
|
||||
int msg_num = 0;
|
||||
while( G_RUN && msg_num++ < G.cli.msg_num )
|
||||
while( G_RUN
|
||||
&& (msg_num++ < G.cli.msg_num || G.gps_fix_age() > 20) )
|
||||
{
|
||||
++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 // at least use time
|
||||
memcpy( valid_nmea.utc, current_nmea.utc, sizeof(current_nmea.utc) );
|
||||
const nmea_t valid_nmea = G.nmea_get();
|
||||
|
||||
|
||||
// dynamics
|
||||
|
|
@ -278,25 +292,22 @@ int main1(int argc, char** argv)
|
|||
// telemetry message
|
||||
//
|
||||
stringstream msg_stream;
|
||||
// Callsign, ID, UTC:
|
||||
msg_stream<<G.cli.callsign;
|
||||
msg_stream<<","<<msg_id;
|
||||
msg_stream<<","<<valid_nmea.utc;
|
||||
// ONLY VALID LAT,LON,ALT ARE BEING SENT. LOOK INTO uBLOX THREAD
|
||||
msg_stream<<","<<valid_nmea.lat<<","<<valid_nmea.lon<<","<<valid_nmea.alt;
|
||||
msg_stream<<","<<valid_nmea.sats<<","<<gps_fix_valid;
|
||||
msg_stream<<","<<valid_nmea.sats<<","<<GLOB::get().gps_fix_age();
|
||||
// Sensors:
|
||||
msg_stream<<","<<setprecision(1)<<fixed<<G.temperature;
|
||||
|
||||
// CRC:
|
||||
const string msg_and_crc = string("\0",1) + "$$$" + msg_stream.str() + '*' + CRC(msg_stream.str());
|
||||
cout<<C_GREEN<<msg_and_crc<<C_OFF<<endl;
|
||||
|
||||
// emit telemetry msg RF
|
||||
// emit telemetry msg @RF
|
||||
//
|
||||
mtx2_write(radio_fd, msg_and_crc + '\n');
|
||||
|
||||
// if no GPS fix, keep sending telemetry instead of SSDV
|
||||
//
|
||||
// for some reason this loop-restart does not work with for() loop
|
||||
if( !gps_fix_valid )
|
||||
msg_num = 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -326,15 +337,15 @@ int main1(int argc, char** argv)
|
|||
//
|
||||
cout<<"Closing sensors thread"<<endl;
|
||||
sensors_thread.join();
|
||||
cout<<"Closing uBlox I2C"<<endl;
|
||||
cout<<"Closing uBlox I2C thread and device"<<endl;
|
||||
ublox_thread.join();
|
||||
close(uBlox_i2c_fd);
|
||||
cout<<"Closing UART Radio"<<endl;
|
||||
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"<<endl;
|
||||
cout<<"Closing zmq thread"<<endl;
|
||||
zmq_thread.join(); // will return after next received message, or stuck forever if no messages come in
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -137,7 +137,8 @@ bool NMEA_parse(const char* Buffer, nmea_t& o_nmea)
|
|||
int scanned_positions =
|
||||
sscanf(Buffer + 7, "%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &
|
||||
utc, &lat, &ns, &lon, &ew, &quality, &sats, &hdop, &alt, &alt_units);
|
||||
if(scanned_positions == 10)
|
||||
|
||||
if(scanned_positions >= 10)
|
||||
{
|
||||
lat = degree_2_decimal(lat);
|
||||
if(ns == 'S')
|
||||
|
|
@ -195,7 +196,8 @@ bool NMEA_parse(const char* Buffer, nmea_t& o_nmea)
|
|||
int scanned_positions =
|
||||
sscanf(Buffer+7, "%f,%c,%f,%c,%f,%c,%[^','],%[^','],%d",
|
||||
&utc, &status, &lat, &ns, &lon, &ew, speedstring, coursestring, &date);
|
||||
if(scanned_positions == 7)
|
||||
|
||||
if(scanned_positions >= 7)
|
||||
{
|
||||
speedOG = atof(speedstring);
|
||||
courseOG = atof(coursestring);
|
||||
|
|
@ -283,6 +285,48 @@ std::string nmea_t::str() const
|
|||
}
|
||||
|
||||
|
||||
|
||||
std::string nmea_t::json() const
|
||||
{
|
||||
std::stringstream s;
|
||||
std::string sep(",");
|
||||
|
||||
s<<"{";
|
||||
|
||||
s<<"'utc':"<<utc<<sep;
|
||||
s<<"'lat':"<<lat<<sep;
|
||||
s<<"'lon':"<<lon<<sep;
|
||||
s<<"'alt':"<<alt<<sep;
|
||||
|
||||
s<<"'speed_over_ground_mps':"<<speed_over_ground_mps<<sep;
|
||||
s<<"'course_over_ground_deg':"<<course_over_ground_deg<<sep;
|
||||
|
||||
s<<"'sats':"<<sats<<sep;
|
||||
|
||||
if(fix_status == nmea_t::fix_status_t::kValid)
|
||||
s<<"'fixStatus':'VALID'"<<sep;
|
||||
else if(fix_status == nmea_t::fix_status_t::kInvalid)
|
||||
s<<"'fixStatus':'INVALID'"<<sep;
|
||||
|
||||
if( fix_quality == nmea_t::fix_quality_t::kNoFix )
|
||||
s<<"'Q':'kNoFix'";
|
||||
else if( fix_quality == nmea_t::fix_quality_t::kAutonomous )
|
||||
s<<"'Q:kAutonomous'";
|
||||
else if( fix_quality == nmea_t::fix_quality_t::kDifferential )
|
||||
s<<"'Q:kDifferential'";
|
||||
else if( fix_quality == nmea_t::fix_quality_t::kRtkFixed )
|
||||
s<<"'Q:kRtkFixed'";
|
||||
else if( fix_quality == nmea_t::fix_quality_t::kRtkFloat )
|
||||
s<<"'Q:kRtkFloat'";
|
||||
else if( fix_quality == nmea_t::fix_quality_t::kEstimated )
|
||||
s<<"'Q:kEstimated'";
|
||||
|
||||
s<<"}";
|
||||
|
||||
return s.str();
|
||||
}
|
||||
|
||||
|
||||
// convert utc HHMMSS to seconds
|
||||
int nmea_t::utc_as_seconds() const
|
||||
{
|
||||
|
|
|
|||
|
|
@ -19,13 +19,13 @@ public:
|
|||
|
||||
int sats = 0;
|
||||
|
||||
enum class fix_status_t {
|
||||
enum class fix_status_t : int {
|
||||
kInvalid=0, // V
|
||||
kValid=1 // A
|
||||
};
|
||||
fix_status_t fix_status = fix_status_t::kInvalid;
|
||||
|
||||
enum class fix_quality_t {
|
||||
enum class fix_quality_t : int {
|
||||
kNoFix = 0,
|
||||
kAutonomous = 1,
|
||||
kDifferential = 2,
|
||||
|
|
@ -36,6 +36,7 @@ public:
|
|||
fix_quality_t fix_quality = fix_quality_t::kNoFix;
|
||||
|
||||
std::string str() const;
|
||||
std::string json() const;
|
||||
|
||||
int utc_as_seconds() const; // convert utc HHMMSS to seconds
|
||||
};
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue