kopia lustrzana https://github.com/ogre/pizero_tracker
Added loging
Michal Fratczak
rodzic
b3a4b6d3bb
commit
db9eee21f2
|
|
@ -2,6 +2,7 @@
|
|||
|
||||
#include <sstream>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
|
||||
std::string GLOB::str() const
|
||||
{
|
||||
|
|
@ -17,6 +18,7 @@ std::string GLOB::str() const
|
|||
s<<"\talt="<<cli.alt<<"\n";
|
||||
s<<"\ttestgps="<<cli.testgps<<"\n";
|
||||
s<<"\tport="<<cli.port<<"\n";
|
||||
s<<"\tlogsdir="<<cli.logsdir<<"\n";
|
||||
s<<"\thw_pin_radio_on="<<cli.hw_pin_radio_on<<"\n";
|
||||
s<<"\thw_radio_serial="<<cli.hw_radio_serial<<"\n";
|
||||
s<<"\thw_ublox_device="<<cli.hw_ublox_device<<"\n";
|
||||
|
|
@ -56,3 +58,19 @@ std::vector<std::string> GLOB::dynamics_keys() const
|
|||
ret.push_back(k.first);
|
||||
return ret;
|
||||
}
|
||||
|
||||
std::string GLOB::runtime_str() const
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
const auto run_secs = runtime_secs_;
|
||||
const int secs = run_secs % 60;
|
||||
const int minutes = ((run_secs - secs) / 60) % 60;
|
||||
const int hours = ((run_secs - secs - 60*minutes) / 3600) % 12;
|
||||
|
||||
stringstream ss;
|
||||
ss <<setfill('0')<<setw(2)<<hours
|
||||
<<":"<<setfill('0')<<setw(2)<<minutes
|
||||
<<":"<<setfill('0')<<setw(2)<<secs;
|
||||
return ss.str();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ public:
|
|||
float freqMHz = 0; //MegaHertz
|
||||
baud_t baud = baud_t::kInvalid;
|
||||
std::string ssdv_image; // ssdv encoded image path
|
||||
std::string logsdir{"./"}; // logs dirs
|
||||
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
|
||||
|
|
@ -82,6 +83,7 @@ public:
|
|||
|
||||
// runtime seconds
|
||||
long long runtime_secs_ = 0;
|
||||
std::string runtime_str() const; // return runtime in format HH:MM:SS
|
||||
|
||||
std::string str() const;
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@
|
|||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
#include "GLOB.h"
|
||||
|
||||
class async_log_t
|
||||
{
|
||||
private:
|
||||
|
|
@ -28,7 +30,7 @@ public:
|
|||
if(!insert_ok)
|
||||
return false;
|
||||
}
|
||||
log_stream->second.push_back(msg);
|
||||
log_stream->second.push_back( GLOB::get().runtime_str() + " " + msg);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -51,6 +53,10 @@ public:
|
|||
of.close();
|
||||
}
|
||||
}
|
||||
|
||||
std::string logs_dir() const { return base_dir_; }
|
||||
void logs_dir(const std::string& bdir) { base_dir_ = bdir; }
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ void CLI(int ac, char* av[])
|
|||
("ssdv", po::value<string>(), "ssdv encoded image path")
|
||||
("msg_num", po::value<int>(), "number of telemetry sentences emitted between SSDV packets")
|
||||
("port", po::value<int>(), "zeroMQ server port")
|
||||
("logsdir", po::value<string>(), "logs dir")
|
||||
("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")
|
||||
|
|
@ -72,6 +73,7 @@ void CLI(int ac, char* av[])
|
|||
if (vm.count("freq")) GLOB::get().cli.freqMHz = vm["freq"].as<float>();
|
||||
if (vm.count("msg_num")) GLOB::get().cli.msg_num = vm["msg_num"].as<int>();
|
||||
if (vm.count("port")) GLOB::get().cli.port = vm["port"].as<int>();
|
||||
if (vm.count("logsdir")) GLOB::get().cli.logsdir = vm["logsdir"].as<string>();
|
||||
if (vm.count("hw_pin_radio_on")) GLOB::get().cli.hw_pin_radio_on = vm["hw_pin_radio_on"].as<int>();
|
||||
if (vm.count("hw_radio_serial")) GLOB::get().cli.hw_radio_serial = vm["hw_radio_serial"].as<string>();
|
||||
if (vm.count("hw_ublox_device")) GLOB::get().cli.hw_ublox_device = vm["hw_ublox_device"].as<string>();
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include "cli.h"
|
||||
#include "GLOB.h"
|
||||
#include "gps_distance_t.h"
|
||||
#include "async_log_t.h"
|
||||
|
||||
const char* C_RED = "\033[1;31m";
|
||||
const char* C_GREEN = "\033[1;32m";
|
||||
|
|
@ -196,7 +197,11 @@ int main1(int argc, char** argv)
|
|||
return 1;
|
||||
}
|
||||
|
||||
async_log_t LOG;
|
||||
LOG.logs_dir(G.cli.logsdir);
|
||||
LOG.log("main.log", "___START___");
|
||||
|
||||
LOG.log("main.log", "sudo modprobe w1-gpio");
|
||||
system("sudo modprobe w1-gpio");
|
||||
|
||||
if (gpioInitialise() < 0)
|
||||
|
|
@ -212,6 +217,7 @@ int main1(int argc, char** argv)
|
|||
|
||||
// RADIO
|
||||
//
|
||||
LOG.log("main.log", "radio init");
|
||||
gpioSetPullUpDown( G.cli.hw_pin_radio_on, PI_PUD_DOWN );
|
||||
gpioSetMode( G.cli.hw_pin_radio_on, PI_OUTPUT );
|
||||
gpioWrite ( G.cli.hw_pin_radio_on, 1 );
|
||||
|
|
@ -226,6 +232,7 @@ int main1(int argc, char** argv)
|
|||
|
||||
// uBLOX I2C start and config
|
||||
//
|
||||
LOG.log("main.log", "uBLOX init");
|
||||
int uBlox_i2c_fd = 0;
|
||||
while(uBlox_i2c_fd<1) {
|
||||
cout<<"Opening uBlox I2C "<<G.cli.hw_ublox_device<<endl;
|
||||
|
|
@ -246,7 +253,7 @@ int main1(int argc, char** argv)
|
|||
|
||||
// uBLOX loop
|
||||
//
|
||||
auto ublox_loop = [uBlox_i2c_fd]() {
|
||||
auto ublox_loop = [uBlox_i2c_fd, &LOG]() {
|
||||
while(G_RUN) {
|
||||
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());
|
||||
|
|
@ -255,6 +262,7 @@ int main1(int argc, char** argv)
|
|||
cerr<<C_RED<<"NMEA Checksum Fail: "<<nmea_str<<C_OFF<<endl;
|
||||
continue;
|
||||
}
|
||||
LOG.log("nmea.log", nmea_str);
|
||||
|
||||
nmea_t current_nmea;
|
||||
// REUSE LAT,LON,ALT FROM LAST VALID SENTENCE
|
||||
|
|
@ -308,6 +316,7 @@ int main1(int argc, char** argv)
|
|||
|
||||
// GPS thread. uBLOX or faked
|
||||
//
|
||||
LOG.log("main.log", "GPS thread start");
|
||||
function<void()> gps_loop(ublox_loop);
|
||||
if(G.cli.testgps)
|
||||
gps_loop = function<void()>(fake_gps_loop);
|
||||
|
|
@ -323,10 +332,12 @@ int main1(int argc, char** argv)
|
|||
return 1;
|
||||
}
|
||||
cout<<"ds18b20_device "<<ds18b20_device<<endl;
|
||||
LOG.log("main.log", "ds18b20_device " + ds18b20_device);
|
||||
|
||||
|
||||
// ALL SENSORS THREAD
|
||||
//
|
||||
LOG.log("main.log", "SENSORS thread start");
|
||||
std::thread sensors_thread( [ds18b20_device]() {
|
||||
while(G_RUN) {
|
||||
// internal temp
|
||||
|
|
@ -339,6 +350,7 @@ int main1(int argc, char** argv)
|
|||
// Flight State Thread
|
||||
// try guessing one of these states: kUnknown, kStandBy, kAscend, kDescend, kFreefall, kLanded
|
||||
//
|
||||
LOG.log("main.log", "flight state thread start");
|
||||
std::thread flight_state_thread( []() {
|
||||
const auto START_TIME = chrono::steady_clock::now(); // used to measure running time
|
||||
while(G_RUN) {
|
||||
|
|
@ -364,13 +376,24 @@ int main1(int argc, char** argv)
|
|||
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;
|
||||
// cout<<alt.val()<<" "<<alt.dVdT()<<" "<<alt.dVdT_avg()<<" "<<dist_from_home.dist_line_<<endl;
|
||||
GLOB::get().flight_state_set( flight_state );
|
||||
}
|
||||
});
|
||||
|
||||
// log saving thread
|
||||
//
|
||||
LOG.log("main.log", "log save thread start");
|
||||
std::thread log_save_thread([&LOG]() {
|
||||
while(G_RUN) {
|
||||
this_thread::sleep_for(chrono::seconds(10));
|
||||
LOG.save();
|
||||
}
|
||||
});
|
||||
|
||||
|
||||
// ZeroMQ server
|
||||
LOG.log("main.log", "ZeroMQ thread start");
|
||||
zmq::context_t zmq_context(1);
|
||||
zmq::socket_t zmq_socket(zmq_context, ZMQ_REP);
|
||||
zmq_socket.bind ( string("tcp://*:" + to_string(G.cli.port)).c_str() );
|
||||
|
|
@ -399,6 +422,7 @@ int main1(int argc, char** argv)
|
|||
|
||||
// READ SENSORS, CONSTRUCT TELEMETRY MESSAGE, RF SEND TELEMETRY AND IMAGE
|
||||
//
|
||||
LOG.log("main.log", "main loop");
|
||||
ssdv_t ssdv_packets;
|
||||
while(G_RUN)
|
||||
{
|
||||
|
|
@ -448,6 +472,8 @@ int main1(int argc, char** argv)
|
|||
const string msg_with_crc = string("\0",1) + "$$$" + tlmtr_stream.str() + '*' + CRC(tlmtr_stream.str());
|
||||
cout<<C_GREEN<<msg_with_crc<<C_OFF<<endl;
|
||||
|
||||
LOG.log("sentences.log", msg_with_crc);
|
||||
|
||||
// emit telemetry msg @RF
|
||||
//
|
||||
// mtx2_write(radio_fd, msg_with_crc + '\n');
|
||||
|
|
@ -475,10 +501,6 @@ int main1(int argc, char** argv)
|
|||
{
|
||||
auto tile = ssdv_packets.next_packet();
|
||||
cout<<"Send SSDV @RF. Left tiles: "<<ssdv_packets.size()<<endl;
|
||||
// for(int i=0; i<256; ++i)
|
||||
// cout<<" 0x"<<hex<<(int)(tile[i]);
|
||||
// cout<<dec<<endl;
|
||||
// mtx2_write( radio_fd, tile.data(), sizeof(tile) );
|
||||
auto mtx2_write_future = std::async( std::launch::async, [&]{
|
||||
mtx2_write( radio_fd, tile.data(), sizeof(tile) ); } );
|
||||
while( mtx2_write_future.wait_for(chrono::milliseconds(250)) != future_status::ready )
|
||||
|
|
@ -493,6 +515,8 @@ int main1(int argc, char** argv)
|
|||
|
||||
// RELEASE RESOURCES
|
||||
//
|
||||
LOG.log("main.log", "release resources, close threads");
|
||||
|
||||
if(msgid_fh)
|
||||
fclose(msgid_fh);
|
||||
cout<<"Closing sensors thread"<<endl;
|
||||
|
|
@ -510,10 +534,15 @@ int main1(int argc, char** argv)
|
|||
gpioWrite (G.cli.hw_pin_radio_on, 0);
|
||||
cout<<"Closing gpio"<<endl;
|
||||
gpioTerminate();
|
||||
cout<<"Closing logs thread"<<endl;
|
||||
if(log_save_thread.joinable())
|
||||
log_save_thread.join();
|
||||
cout<<"Closing zmq thread"<<endl;
|
||||
if( zmq_thread.joinable() )
|
||||
zmq_thread.join(); // will return after next received message, or stuck forever if no messages comes in
|
||||
|
||||
LOG.log("main.log", "saving last logs.");
|
||||
LOG.save();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue