kopia lustrzana https://github.com/ogre/pizero_tracker
Improve nmea_t type and fix altitude not being recorded. Adjust flight state logic.
Michal Fratczak
rodzic
1d7058455b
commit
cb6bb631e2
|
|
@ -7,7 +7,6 @@
|
|||
#include <algorithm>
|
||||
|
||||
|
||||
// utc_seconds is HHMMSS converted to seconds. return true if sample was accepted
|
||||
void dynamics_t::add(const tp timestamp, const float val)
|
||||
{
|
||||
val_ = val;
|
||||
|
|
|
|||
|
|
@ -82,7 +82,7 @@ public:
|
|||
|
||||
bool initialised() const { return initialised_; }
|
||||
|
||||
void add(const tp timestamp, const float val); // utc_seconds is HHMMSS converted to seconds. return true if sample was accepted
|
||||
void add(const tp timestamp, const float val);
|
||||
|
||||
float val() const { return val_; }
|
||||
float min() const { return val_min_; }
|
||||
|
|
|
|||
|
|
@ -278,23 +278,18 @@ int main1(int argc, char** argv)
|
|||
LOG.log("nmea.log", nmea_str);
|
||||
|
||||
nmea_t current_nmea;
|
||||
/*
|
||||
// REUSE LAT,LON,ALT FROM LAST VALID SENTENCE
|
||||
// if currently parsed NMEA string has valid lat/lon/alt
|
||||
// they will be stored in current_nmea
|
||||
// otherwise we keep last valid
|
||||
const 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;
|
||||
*/
|
||||
|
||||
if( NMEA_parse(nmea_str.c_str(), current_nmea) /*and current_nmea.valid()*/ ) {
|
||||
// RMC has no altitude, copy from last known value
|
||||
if( current_nmea.nmea_msg_type_ == nmea_t::nmea_msg_type_t::kRMC )
|
||||
current_nmea.alt = GLOB::get().nmea_current().alt;
|
||||
|
||||
GLOB::get().nmea_set(current_nmea);
|
||||
if (current_nmea.valid())
|
||||
|
||||
if(current_nmea.valid())
|
||||
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;
|
||||
}
|
||||
// else - parse error or no lock (even no time)
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -383,14 +378,14 @@ int main1(int argc, char** argv)
|
|||
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;
|
||||
else if(dAltAvg < -15)
|
||||
flight_state = flight_state_t::kFreefall;
|
||||
else if(dAltAvg < 0)
|
||||
flight_state = flight_state_t::kDescend;
|
||||
else if(dAltAvg >= 0)
|
||||
flight_state = flight_state_t::kAscend;
|
||||
}
|
||||
// cout<<alt.val()<<" "<<alt.dVdT()<<" "<<alt.dVdT_avg()<<" "<<dist_from_home.dist_line_<<endl;
|
||||
GLOB::get().flight_state_set( flight_state );
|
||||
|
|
|
|||
|
|
@ -134,6 +134,8 @@ bool NMEA_parse(const char* Buffer, nmea_t& o_nmea)
|
|||
|
||||
if (strncmp(Buffer + 3, "GGA", 3) == 0) // Global positioning system fix data
|
||||
{
|
||||
o_nmea.nmea_msg_type_ = nmea_t::nmea_msg_type_t::kGGA;
|
||||
|
||||
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);
|
||||
|
|
@ -175,6 +177,7 @@ bool NMEA_parse(const char* Buffer, nmea_t& o_nmea)
|
|||
o_nmea.fix_quality = nmea_t::fix_quality_t::kEstimated;
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else if(scanned_positions > 0 && utc != 0)
|
||||
|
|
@ -191,6 +194,8 @@ bool NMEA_parse(const char* Buffer, nmea_t& o_nmea)
|
|||
} // GGA
|
||||
else if (strncmp(Buffer+3, "RMC", 3) == 0)
|
||||
{
|
||||
o_nmea.nmea_msg_type_ = nmea_t::nmea_msg_type_t::kRMC;
|
||||
|
||||
speedstring[0] = '\0';
|
||||
coursestring[0] = '\0';
|
||||
int scanned_positions =
|
||||
|
|
@ -338,8 +343,9 @@ int nmea_t::utc_as_seconds() const
|
|||
|
||||
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;
|
||||
if(nmea_msg_type_ == nmea_msg_type_t::kGGA and fix_quality != fix_quality_t::kNoFix)
|
||||
return true;
|
||||
if(nmea_msg_type_ == nmea_msg_type_t::kRMC and fix_status == fix_status_t::kValid)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,26 +5,23 @@
|
|||
|
||||
|
||||
// keep data parsed from GGA __and__ RMC messages
|
||||
// http://aprs.gids.nl/nmea/#rmc
|
||||
// http://aprs.gids.nl/nmea/#gga
|
||||
|
||||
class nmea_t
|
||||
{
|
||||
public:
|
||||
// fields provided by both GGA and RMC messages:
|
||||
//
|
||||
char utc[6] = {'0', '0', '0', '0', '0', '0'}; // HHMMSS
|
||||
|
||||
float lat = 0; // degree
|
||||
float lon = 0; // degree
|
||||
|
||||
// GGA message fields:
|
||||
//
|
||||
float alt = 0; // meters
|
||||
|
||||
float speed_over_ground_mps = 0;
|
||||
float course_over_ground_deg = 0;
|
||||
|
||||
int sats = 0;
|
||||
|
||||
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 : int {
|
||||
kNoFix = 0,
|
||||
kAutonomous = 1,
|
||||
|
|
@ -35,6 +32,25 @@ public:
|
|||
};
|
||||
fix_quality_t fix_quality = fix_quality_t::kNoFix;
|
||||
|
||||
|
||||
// RMC message fields:
|
||||
//
|
||||
float speed_over_ground_mps = 0;
|
||||
float course_over_ground_deg = 0;
|
||||
enum class fix_status_t : int {
|
||||
kInvalid=0, // V
|
||||
kValid=1 // A
|
||||
};
|
||||
fix_status_t fix_status = fix_status_t::kInvalid;
|
||||
|
||||
enum class nmea_msg_type_t : int {
|
||||
kUnknown = 0,
|
||||
kGGA = 1,
|
||||
kRMC = 2
|
||||
};
|
||||
nmea_msg_type_t nmea_msg_type_ = nmea_msg_type_t::kUnknown;
|
||||
|
||||
|
||||
std::string str() const;
|
||||
std::string json() const;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,17 +1,22 @@
|
|||
# tracker options
|
||||
#
|
||||
callsign = fro
|
||||
callsign = setcallsign
|
||||
freq = 434.5
|
||||
baud = 300
|
||||
port = 6666
|
||||
ssdv = /tracker/ssdv.bin
|
||||
logsdir = /tracker/logs/
|
||||
# number of telemetry packets to send before SSDV packet
|
||||
msg_num = 3
|
||||
# launch location
|
||||
latlonalt = 52.1120009
|
||||
latlonalt = 19.957789
|
||||
latlonalt = 100
|
||||
# enable watchdog
|
||||
watchdog = 1
|
||||
|
||||
# hardware configuration
|
||||
#
|
||||
hw_pin_radio_on = 22
|
||||
hw_radio_serial = /dev/serial0
|
||||
#pi4
|
||||
|
|
@ -20,9 +25,13 @@ hw_radio_serial = /dev/serial0
|
|||
hw_ublox_device = /dev/i2c-3
|
||||
|
||||
# camera
|
||||
#
|
||||
cam_dir = /tracker/camera
|
||||
cam_flip_h =
|
||||
cam_flip_h = 0
|
||||
cam_flip_v = 0
|
||||
# SSDV image resolution
|
||||
cam_ssdv_res = 128
|
||||
# video clip duration in minutes
|
||||
cam_video_dur = 15
|
||||
# photo snapshot interval in seconds
|
||||
cam_snapshot_interval = 30
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue