kopia lustrzana https://github.com/ogre/pizero_tracker
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.master
Michal Fratczak
rodzic
096d428413
commit
b3a4b6d3bb
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@ public:
|
|||
std::string json() const;
|
||||
|
||||
int utc_as_seconds() const; // convert utc HHMMSS to seconds
|
||||
bool valid() const;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue