kopia lustrzana https://github.com/ogre/pizero_tracker
352 wiersze
7.6 KiB
C++
352 wiersze
7.6 KiB
C++
#include "nmea.h"
|
|
|
|
// #include <stdio.h>
|
|
#include <math.h>
|
|
#include <cstring>
|
|
#include <iostream>
|
|
#include <sstream>
|
|
|
|
|
|
namespace
|
|
{
|
|
|
|
float degree_2_decimal(float i_pos)
|
|
{
|
|
float degs = trunc(i_pos / 100);
|
|
float mins = i_pos - 100.0f * degs;
|
|
float res = degs + mins / 60.0f;
|
|
return res;
|
|
}
|
|
|
|
} // ns
|
|
|
|
int NMEA_checksum(const char* buff, const size_t buff_sz)
|
|
{
|
|
int c = 0;
|
|
for (size_t i = 0; i < buff_sz; ++i)
|
|
c ^= *buff++;
|
|
return c;
|
|
}
|
|
|
|
|
|
bool NMEA_msg_checksum_ok(const char* msg, const size_t msg_sz)
|
|
{
|
|
if(msg_sz<5)
|
|
return false;
|
|
int chck = NMEA_checksum(msg+1, msg_sz-4); // without $, up to *
|
|
char chck_str[2];
|
|
sprintf(chck_str, "%02X", chck);
|
|
|
|
bool checksum_is_equal =
|
|
chck_str[0] == *(msg+msg_sz-2) && chck_str[1] == *(msg+msg_sz-1);
|
|
|
|
return checksum_is_equal;
|
|
}
|
|
|
|
|
|
bool NMEA_msg_checksum_ok(const std::string& msg)
|
|
{
|
|
return NMEA_msg_checksum_ok( msg.c_str(), msg.size() );
|
|
}
|
|
|
|
|
|
// $GNGGA,170814.00,,,,,0,00,99.99,,,,,,*73
|
|
// return false if not found
|
|
// output:
|
|
// buff+in points to $
|
|
// buff+out points to second char of checksum (3)
|
|
bool NMEA_get_last_msg(const char* buff, const size_t buff_sz, size_t& in, size_t& out)
|
|
{
|
|
// find end of message: *
|
|
const char* buff_star = buff + buff_sz - 1;
|
|
while ( buff <= buff_star && *buff_star != '*' )
|
|
buff_star--;
|
|
|
|
if(buff <= buff_star)
|
|
out = buff_star - buff + 2;
|
|
else
|
|
return false;
|
|
|
|
const char* buff_dollar = buff_star;
|
|
while ( buff <= buff_dollar && *buff_dollar != '$' )
|
|
buff_dollar--;
|
|
|
|
if(buff <= buff_dollar)
|
|
in = buff_dollar - buff;
|
|
else
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
std::string NMEA_get_last_msg(const char* buff, const size_t buff_sz)
|
|
{
|
|
size_t in;
|
|
size_t out;
|
|
bool has_msg = NMEA_get_last_msg(buff, buff_sz, in, out);
|
|
if(has_msg)
|
|
return std::string(buff+in, buff+out+1);
|
|
else
|
|
return "";
|
|
}
|
|
|
|
|
|
std::string NMEA_get_last_msg(const std::string& msg)
|
|
{
|
|
return NMEA_get_last_msg( msg.c_str(), msg.size() );
|
|
}
|
|
|
|
|
|
bool NMEA_parse(const char* Buffer, nmea_t& o_nmea)
|
|
{
|
|
using namespace std;
|
|
|
|
float utc = 0;
|
|
int date = 0;
|
|
|
|
float lat = 0;
|
|
char ns = 0;
|
|
float lon = 0;
|
|
char ew = 0;
|
|
|
|
float alt = 0;
|
|
char alt_units = 0;
|
|
|
|
// Possible values for status:
|
|
// V = Data invalid
|
|
// A = Data valid
|
|
char status = 0;
|
|
|
|
// Possible values for quality:
|
|
// 0 = No fix
|
|
// 1 = Autonomous GNSS fix
|
|
// 2 = Differential GNSS fix
|
|
// 4 = RTK fixed
|
|
// 5 = RTK float
|
|
// 6 = Estimated/Dead reckoning fix
|
|
int quality = 0;
|
|
|
|
int sats = 0;
|
|
float hdop = 0; // Horizontal Dilution of Precision
|
|
char speedstring[16];
|
|
char coursestring[16];
|
|
|
|
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);
|
|
|
|
if(scanned_positions >= 10)
|
|
{
|
|
lat = degree_2_decimal(lat);
|
|
if(ns == 'S')
|
|
lat = -lat;
|
|
|
|
lon = degree_2_decimal(lon);
|
|
if(ew == 'W')
|
|
lon = -lon;
|
|
|
|
sprintf( o_nmea.utc, "%06d", int(floor(utc)) );
|
|
o_nmea.lat = lat;
|
|
o_nmea.lon = lon;
|
|
o_nmea.alt = alt;
|
|
o_nmea.sats = sats;
|
|
|
|
switch(quality)
|
|
{
|
|
case 0:
|
|
o_nmea.fix_quality = nmea_t::fix_quality_t::kNoFix;
|
|
break;
|
|
case 1:
|
|
o_nmea.fix_quality = nmea_t::fix_quality_t::kAutonomous;
|
|
break;
|
|
case 2:
|
|
o_nmea.fix_quality = nmea_t::fix_quality_t::kDifferential;
|
|
break;
|
|
case 4:
|
|
o_nmea.fix_quality = nmea_t::fix_quality_t::kRtkFixed;
|
|
break;
|
|
case 5:
|
|
o_nmea.fix_quality = nmea_t::fix_quality_t::kRtkFloat;
|
|
break;
|
|
case 6:
|
|
o_nmea.fix_quality = nmea_t::fix_quality_t::kEstimated;
|
|
break;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
else if(scanned_positions > 0 && utc != 0)
|
|
{
|
|
sprintf( o_nmea.utc, "%06d", int(floor(utc)) );
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// cerr<<"GGA scanf err"<<endl; // probably not all fields in sentence
|
|
return false;
|
|
}
|
|
|
|
} // 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 =
|
|
sscanf(Buffer+7, "%f,%c,%f,%c,%f,%c,%[^','],%[^','],%d",
|
|
&utc, &status, &lat, &ns, &lon, &ew, speedstring, coursestring, &date);
|
|
|
|
if(scanned_positions >= 7)
|
|
{
|
|
sprintf( o_nmea.utc, "%06d", int(floor(utc)) );
|
|
|
|
lat = degree_2_decimal(lat);
|
|
if(ns == 'S')
|
|
lat = -lat;
|
|
o_nmea.lat = lat;
|
|
|
|
lon = degree_2_decimal(lon);
|
|
if(ew == 'W')
|
|
lon = -lon;
|
|
o_nmea.lon = lon;
|
|
|
|
// o_nmea.alt = alt; // there is no alt in RMC
|
|
// o_nmea.sats = sats; // the is no sats in RMC
|
|
o_nmea.speed_over_ground_mps = 0.514444444 * atof(speedstring); // knots to m/s
|
|
o_nmea.course_over_ground_deg = atof(coursestring);
|
|
|
|
if(status == 'A')
|
|
o_nmea.fix_status = nmea_t::fix_status_t::kValid;
|
|
else if(status == 'V')
|
|
o_nmea.fix_status = nmea_t::fix_status_t::kInvalid;
|
|
|
|
return true;
|
|
}
|
|
else if(scanned_positions > 0 && utc != 0)
|
|
{
|
|
sprintf( o_nmea.utc, "%06d", int(floor(utc)) );
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// cerr<<"RMC scanf err"<<endl; // probably not all fields in sentence
|
|
return false;
|
|
}
|
|
} // RMC
|
|
else
|
|
{
|
|
// cerr<<"NMEA parser unknown message "<<Buffer<<endl;
|
|
return false;
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
std::string nmea_t::str() const
|
|
{
|
|
std::stringstream s;
|
|
std::string ws(",");
|
|
|
|
s<<"utc:"<<utc<<ws;
|
|
|
|
s<<"lat:"<<lat<<ws;
|
|
s<<"lon:"<<lon<<ws;
|
|
s<<"alt:"<<alt<<ws;
|
|
|
|
s<<"speed_over_ground_mps:"<<speed_over_ground_mps<<ws;
|
|
s<<"course_over_ground_deg:"<<course_over_ground_deg<<ws;
|
|
|
|
s<<"sats:"<<sats<<ws;
|
|
|
|
if(fix_status == nmea_t::fix_status_t::kValid)
|
|
s<<"VALID"<<ws;
|
|
else if(fix_status == nmea_t::fix_status_t::kInvalid)
|
|
s<<"INVALID"<<ws;
|
|
|
|
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";
|
|
|
|
return s.str();
|
|
}
|
|
|
|
|
|
|
|
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
|
|
{
|
|
int H=0;
|
|
int M=0;
|
|
int S=0;
|
|
sscanf(utc, "%02d%02d%02d", &H, &M, &S);
|
|
const int total_seconds = S + 60*M + 60*60*H;
|
|
return total_seconds;
|
|
}
|
|
|
|
bool nmea_t::valid() const
|
|
{
|
|
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;
|
|
}
|