mirror of
https://github.com/justcallmekoko/ESP32Marauder.git
synced 2025-12-05 20:40:25 -08:00
673 lines
18 KiB
C++
673 lines
18 KiB
C++
#include "GpsInterface.h"
|
|
|
|
#ifdef HAS_GPS
|
|
|
|
extern GpsInterface gps_obj;
|
|
|
|
char nmeaBuffer[100];
|
|
|
|
MicroNMEA nmea(nmeaBuffer, sizeof(nmeaBuffer));
|
|
|
|
HardwareSerial Serial2(GPS_SERIAL_INDEX);
|
|
|
|
void GpsInterface::begin() {
|
|
|
|
/*#ifdef MARAUDER_MINI
|
|
pinMode(26, OUTPUT);
|
|
|
|
delay(1);
|
|
|
|
analogWrite(26, 243);
|
|
delay(1);
|
|
|
|
Serial.println("Activated GPS");
|
|
delay(100);
|
|
#endif*/
|
|
|
|
|
|
Serial2.begin(9600, SERIAL_8N1, GPS_TX, GPS_RX);
|
|
|
|
MicroNMEA::sendSentence(Serial2, "$PSTMSETPAR,1201,0x00000042");
|
|
MicroNMEA::sendSentence(Serial2, "$PSTMSAVEPAR");
|
|
|
|
MicroNMEA::sendSentence(Serial2, "$PSTMSRR");
|
|
|
|
delay(3900);
|
|
|
|
if (Serial2.available()) {
|
|
Serial.println("GPS Attached Successfully");
|
|
this->gps_enabled = true;
|
|
while (Serial2.available()) {
|
|
//Fetch the character one by one
|
|
char c = Serial2.read();
|
|
//Serial.print(c);
|
|
//Pass the character to the library
|
|
nmea.process(c);
|
|
}
|
|
}
|
|
else {
|
|
this->gps_enabled = false;
|
|
Serial.println("GPS Not Found");
|
|
}
|
|
|
|
|
|
this->type_flag=GPSTYPE_NATIVE; //enforce default
|
|
this->disable_queue(); //init the queue, disabled, kill NULLs
|
|
|
|
nmea.setUnknownSentenceHandler(gps_nmea_notimp);
|
|
|
|
}
|
|
|
|
//passthrough for other objects
|
|
void gps_nmea_notimp(MicroNMEA& nmea){
|
|
gps_obj.enqueue(nmea);
|
|
}
|
|
|
|
void GpsInterface::enqueue(MicroNMEA& nmea){
|
|
std::string nmea_sentence = std::string(nmea.getSentence());
|
|
|
|
if(nmea_sentence.length()){
|
|
this->notimp_nmea_sentence = nmea_sentence.c_str();
|
|
|
|
bool unparsed=1;
|
|
bool enqueue=1;
|
|
|
|
char system=nmea.getTalkerID();
|
|
String msg_id=nmea.getMessageID();
|
|
int length=nmea_sentence.length();
|
|
|
|
if(length>0&&length<256){
|
|
if(system){
|
|
if(msg_id=="TXT"){
|
|
if(length>8){
|
|
std::string content=nmea_sentence.substr(7,std::string::npos);
|
|
|
|
int tot_brk=content.find(',');
|
|
int num_brk=content.find(',',tot_brk+1);
|
|
int txt_brk=content.find(',',num_brk+1);
|
|
int chk_brk=content.rfind('*');
|
|
|
|
if(tot_brk!=std::string::npos && num_brk!=std::string::npos && txt_brk!=std::string::npos && chk_brk!=std::string::npos
|
|
&& chk_brk>txt_brk && txt_brk>num_brk && num_brk>tot_brk && tot_brk>=0){
|
|
std::string total_str=content.substr(0,tot_brk);
|
|
std::string num_str=content.substr(tot_brk+1,num_brk-tot_brk-1);
|
|
std::string type_str=content.substr(num_brk+1,txt_brk-num_brk-1);
|
|
std::string text_str=content.substr(txt_brk+1,chk_brk-txt_brk-1);
|
|
std::string checksum=content.substr(chk_brk+1,std::string::npos);
|
|
|
|
int total=0;
|
|
if(total_str.length()) total=atoi(total_str.c_str());
|
|
|
|
int num=0;
|
|
if(num_str.length()) num=atoi(num_str.c_str());
|
|
|
|
int type=0;
|
|
if(type_str.length()) type=atoi(type_str.c_str());
|
|
|
|
if(text_str.length() && checksum.length()){
|
|
String text=text_str.c_str();
|
|
if(type>1){
|
|
char type_cstr[4];
|
|
snprintf(type_cstr, 4, "%02d ", type);
|
|
type_cstr[3]='\0';
|
|
text=type_cstr+text;
|
|
}
|
|
|
|
if((num<=1||total<=1) && this->queue_enabled_flag){
|
|
if(this->text){
|
|
if(this->text_in){
|
|
int size=text_in->size();
|
|
if(size){
|
|
#ifdef GPS_TEXT_MAXCYCLES
|
|
if(this->text_cycles>=GPS_TEXT_MAXCYCLES){
|
|
#else
|
|
if(this->text_cycles){
|
|
#endif
|
|
if(this->text->size()){
|
|
LinkedList<String> *delme=this->text;
|
|
this->text=new LinkedList<String>;
|
|
delete delme;
|
|
this->text_cycles=0;
|
|
}
|
|
}
|
|
|
|
for(int i=0;i<size;i++){
|
|
this->text->add(this->text_in->get(i));
|
|
}
|
|
LinkedList<String> *delme=this->text_in;
|
|
this->text_in=new LinkedList<String>;
|
|
delete delme;
|
|
this->text_cycles++;
|
|
|
|
this->gps_text=text;
|
|
}
|
|
}
|
|
else
|
|
this->text_in=new LinkedList<String>;
|
|
}
|
|
else{
|
|
if(this->text_in){
|
|
this->text_cycles=0;
|
|
this->text=this->text_in;
|
|
if(this->text->size()){
|
|
if(this->gps_text=="") this->gps_text=this->text->get(0);
|
|
this->text_cycles++;
|
|
}
|
|
this->text_in=new LinkedList<String>;
|
|
}
|
|
else {
|
|
this->text_cycles=0;
|
|
this->text=new LinkedList<String>;
|
|
this->text_in=new LinkedList<String>;
|
|
}
|
|
}
|
|
|
|
this->text_in->add(text);
|
|
}
|
|
else if(this->queue_enabled_flag){
|
|
if(!this->text_in) this->text_in=new LinkedList<String>;
|
|
this->text_in->add(text);
|
|
int size=this->text_in->size();
|
|
|
|
#ifdef GPS_TEXT_MAXLINES
|
|
if(size>=GPS_TEXT_MAXLINES){
|
|
#else
|
|
if(size>=5){
|
|
#endif
|
|
#ifdef GPS_TEXT_MAXCYCLES
|
|
if(this->text_cycles>=GPS_TEXT_MAXCYCLES){
|
|
#else
|
|
if(this->text_cycles){
|
|
#endif
|
|
if(this->text->size()){
|
|
LinkedList<String> *delme=this->text;
|
|
this->text=new LinkedList<String>;
|
|
delete delme;
|
|
this->text_cycles=0;
|
|
}
|
|
}
|
|
|
|
for(int i=0;i<size;i++)
|
|
this->text->add(this->text_in->get(i));
|
|
|
|
LinkedList<String> *delme=this->text_in;
|
|
this->text_in=new LinkedList<String>;
|
|
delete delme;
|
|
this->text_cycles++;
|
|
}
|
|
}
|
|
else
|
|
if(num<=1||total<=1) this->gps_text=text;
|
|
|
|
if(this->gps_text=="") this->gps_text=text;
|
|
unparsed=0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if(unparsed)
|
|
this->notparsed_nmea_sentence = nmea_sentence.c_str();
|
|
|
|
if(this->queue_enabled_flag){
|
|
if(enqueue){
|
|
nmea_sentence_t line = { unparsed, msg_id, nmea_sentence.c_str() };
|
|
|
|
if(this->queue){
|
|
#ifdef GPS_NMEA_MAXQUEUE
|
|
if(this->queue->size()>=GPS_NMEA_MAXQUEUE)
|
|
#else
|
|
if(this->queue->size()>=30)
|
|
#endif
|
|
this->flush_queue();
|
|
}
|
|
else
|
|
this->new_queue();
|
|
|
|
this->queue->add(line);
|
|
}
|
|
else
|
|
if(!this->queue)
|
|
this->new_queue();
|
|
}
|
|
else
|
|
this->flush_queue();
|
|
}
|
|
else
|
|
if(!this->queue_enabled_flag)
|
|
this->flush_queue();
|
|
}
|
|
|
|
void GpsInterface::enable_queue(){
|
|
if(this->queue_enabled_flag){
|
|
if(!this->queue)
|
|
this->new_queue();
|
|
if(!this->text)
|
|
this->text=new LinkedList<String>;
|
|
if(!this->text_in)
|
|
this->text_in=new LinkedList<String>;
|
|
}
|
|
else {
|
|
this->flush_queue();
|
|
this->queue_enabled_flag=1;
|
|
}
|
|
}
|
|
|
|
void GpsInterface::disable_queue(){
|
|
this->queue_enabled_flag=0;
|
|
this->flush_queue();
|
|
}
|
|
|
|
bool GpsInterface::queue_enabled(){
|
|
return this->queue_enabled_flag;
|
|
}
|
|
|
|
LinkedList<nmea_sentence_t>* GpsInterface::get_queue(){
|
|
return this->queue;
|
|
}
|
|
|
|
void GpsInterface::new_queue(){
|
|
this->queue=new LinkedList<nmea_sentence_t>;
|
|
}
|
|
|
|
void GpsInterface::flush_queue(){
|
|
this->flush_queue_nmea();
|
|
this->flush_text();
|
|
}
|
|
|
|
void GpsInterface::flush_queue_nmea(){
|
|
if(this->queue){
|
|
if(this->queue->size()){
|
|
LinkedList<nmea_sentence_t> *delme=this->queue;
|
|
this->new_queue();
|
|
delete delme;
|
|
}
|
|
}
|
|
else
|
|
this->new_queue();
|
|
}
|
|
|
|
void GpsInterface::flush_text(){
|
|
this->flush_queue_text();
|
|
this->flush_queue_textin();
|
|
}
|
|
|
|
void GpsInterface::flush_queue_text(){
|
|
this->text_cycles=0;
|
|
|
|
if(this->text){
|
|
if(this->text->size()){
|
|
LinkedList<String> *delme=this->text;
|
|
this->text=new LinkedList<String>;
|
|
delete delme;
|
|
}
|
|
}
|
|
else
|
|
this->text=new LinkedList<String>;
|
|
}
|
|
|
|
void GpsInterface::flush_queue_textin(){
|
|
if(this->text_in){
|
|
if(this->text_in->size()){
|
|
LinkedList<String> *delme=this->text_in;
|
|
this->text_in=new LinkedList<String>;
|
|
delete delme;
|
|
}
|
|
}
|
|
else
|
|
this->text_in=new LinkedList<String>;
|
|
}
|
|
|
|
void GpsInterface::sendSentence(const char* sentence){
|
|
MicroNMEA::sendSentence(Serial2, sentence);
|
|
}
|
|
|
|
void GpsInterface::sendSentence(Stream &s, const char* sentence){
|
|
MicroNMEA::sendSentence(s, sentence);
|
|
}
|
|
|
|
void GpsInterface::setType(String t){
|
|
if(t == "native")
|
|
this->type_flag=GPSTYPE_NATIVE;
|
|
else if(t == "gps")
|
|
this->type_flag=GPSTYPE_GPS;
|
|
else if(t == "glonass")
|
|
this->type_flag=GPSTYPE_GLONASS;
|
|
else if(t == "galileo")
|
|
this->type_flag=GPSTYPE_GALILEO;
|
|
else if(t == "navic")
|
|
this->type_flag=GPSTYPE_NAVIC;
|
|
else if(t == "qzss")
|
|
this->type_flag=GPSTYPE_QZSS;
|
|
else if(t == "beidou")
|
|
this->type_flag=GPSTYPE_BEIDOU;
|
|
else if(t == "beidou_bd")
|
|
this->type_flag=GPSTYPE_BEIDOU_BD;
|
|
else
|
|
this->type_flag=GPSTYPE_ALL;
|
|
}
|
|
|
|
String GpsInterface::generateGXgga(){
|
|
String msg_type="$"+this->generateType()+"GGA,";
|
|
|
|
char timeStr[8];
|
|
snprintf(timeStr, 8, "%02d%02d%02d,", (int)(nmea.getHour()), (int)(nmea.getMinute()), (int)(nmea.getSecond()));
|
|
|
|
long lat = nmea.getLatitude();
|
|
char latDir = lat < 0 ? 'S' : 'N';
|
|
lat = abs(lat);
|
|
char latStr[12];
|
|
snprintf(latStr, 12, "%02ld%08.5f,", lat / 1000000, ((lat % 1000000)*60) / 1000000.0);
|
|
|
|
long lon = nmea.getLongitude();
|
|
char lonDir = lon < 0 ? 'W' : 'E';
|
|
lon = abs(lon);
|
|
char lonStr[13];
|
|
snprintf(lonStr, 13, "%03ld%08.5f,", lon / 1000000, ((lon % 1000000)*60) / 1000000.0);
|
|
|
|
int fixQuality = nmea.isValid() ? 1 : 0;
|
|
char fixStr[3];
|
|
snprintf(fixStr, 3, "%01d,", fixQuality);
|
|
|
|
int numSatellites = nmea.getNumSatellites();
|
|
char satStr[4];
|
|
snprintf(satStr, 4, "%02d,", numSatellites);
|
|
|
|
unsigned long hdop = nmea.getHDOP();
|
|
char hdopStr[13];
|
|
snprintf(hdopStr, 13, "%01.2f,", 2.5 * (((float)(hdop))/10));
|
|
|
|
long altitude;
|
|
if(!nmea.getAltitude(altitude)) altitude=0;
|
|
char altStr[9];
|
|
snprintf(altStr, 9, "%01.1f,", altitude/1000.0);
|
|
|
|
String message = msg_type + timeStr + latStr + latDir + ',' + lonStr + lonDir +
|
|
',' + fixStr + satStr + hdopStr + altStr + "M,,M,,";
|
|
|
|
return message;
|
|
}
|
|
|
|
String GpsInterface::generateGXrmc(){
|
|
String msg_type="$"+this->generateType()+"RMC,";
|
|
|
|
char timeStr[8];
|
|
snprintf(timeStr, 8, "%02d%02d%02d,", (int)(nmea.getHour()), (int)(nmea.getMinute()), (int)(nmea.getSecond()));
|
|
|
|
char dateStr[8];
|
|
snprintf(dateStr, 8, "%02d%02d%02d,", (int)(nmea.getDay()), (int)(nmea.getMonth()), (int)(nmea.getYear()%100));
|
|
|
|
char status = nmea.isValid() ? 'A' : 'V';
|
|
char mode = nmea.isValid() ? 'A' : 'N';
|
|
|
|
long lat = nmea.getLatitude();
|
|
char latDir = lat < 0 ? 'S' : 'N';
|
|
lat = abs(lat);
|
|
char latStr[12];
|
|
snprintf(latStr, 12, "%02ld%08.5f,", lat / 1000000, ((lat % 1000000)*60) / 1000000.0);
|
|
|
|
long lon = nmea.getLongitude();
|
|
char lonDir = lon < 0 ? 'W' : 'E';
|
|
lon = abs(lon);
|
|
char lonStr[13];
|
|
snprintf(lonStr, 13, "%03ld%08.5f,", lon / 1000000, ((lon % 1000000)*60) / 1000000.0);
|
|
|
|
char speedStr[8];
|
|
snprintf(speedStr, 8, "%01.1f,", nmea.getSpeed() / 1000.0);
|
|
|
|
char courseStr[7];
|
|
snprintf(courseStr, 7, "%01.1f,", nmea.getCourse() / 1000.0);
|
|
|
|
String message = msg_type + timeStr + status + ',' + latStr + latDir + ',' +
|
|
lonStr + lonDir + ',' + speedStr + courseStr + dateStr + ',' + ',' + mode;
|
|
return message;
|
|
}
|
|
|
|
String GpsInterface::generateType(){
|
|
String msg_type="";
|
|
|
|
if(this->type_flag<8) //8=BeiDou in BD mode
|
|
msg_type+='G';
|
|
|
|
if(this->type_flag == GPSTYPE_NATIVE){ //type_flag=0
|
|
char system=this->nav_system;
|
|
if(system)
|
|
msg_type+=system;
|
|
else
|
|
msg_type+='N';
|
|
}
|
|
else if(this->type_flag == GPSTYPE_GPS) //type_flag=2
|
|
msg_type+='P';
|
|
else if(this->type_flag == GPSTYPE_GLONASS) //type_flag=3
|
|
msg_type+='L';
|
|
else if(this->type_flag == GPSTYPE_GALILEO) //type_flag=4
|
|
msg_type+='A';
|
|
else if(this->type_flag == GPSTYPE_NAVIC) //type_flag=5
|
|
msg_type+='I';
|
|
else if(this->type_flag == GPSTYPE_QZSS) //type_flag=6
|
|
msg_type+='Q';
|
|
else if(this->type_flag == GPSTYPE_BEIDOU) //type_flag=7
|
|
msg_type+='B';
|
|
else if(this->type_flag == GPSTYPE_BEIDOU_BD){ //type_flag=8
|
|
msg_type+='B';
|
|
msg_type+='D';
|
|
}
|
|
else{ //type_flag=1=all ... also default if unset/wrong (obj default is type_flag=0=native)
|
|
if(this->type_flag>=8) //catch uncaught first char, assume G if not already output
|
|
msg_type+='G';
|
|
msg_type+='N';
|
|
}
|
|
|
|
return msg_type;
|
|
}
|
|
|
|
// Thanks JosephHewitt
|
|
String GpsInterface::dt_string_from_gps(){
|
|
//Return a datetime String using GPS data only.
|
|
String datetime = "";
|
|
if (nmea.isValid() && nmea.getYear() > 0){
|
|
datetime += nmea.getYear();
|
|
datetime += "-";
|
|
datetime += nmea.getMonth();
|
|
datetime += "-";
|
|
datetime += nmea.getDay();
|
|
datetime += " ";
|
|
datetime += nmea.getHour();
|
|
datetime += ":";
|
|
datetime += nmea.getMinute();
|
|
datetime += ":";
|
|
datetime += nmea.getSecond();
|
|
}
|
|
return datetime;
|
|
}
|
|
|
|
void GpsInterface::setGPSInfo() {
|
|
String nmea_sentence = String(nmea.getSentence());
|
|
if(nmea_sentence != "") this->nmea_sentence = nmea_sentence;
|
|
|
|
this->good_fix = nmea.isValid();
|
|
this->nav_system = nmea.getNavSystem();
|
|
this->num_sats = nmea.getNumSatellites();
|
|
|
|
this->datetime = this->dt_string_from_gps();
|
|
|
|
this->lat = String((float)nmea.getLatitude()/1000000, 7);
|
|
this->lon = String((float)nmea.getLongitude()/1000000, 7);
|
|
long alt = 0;
|
|
if (!nmea.getAltitude(alt)){
|
|
alt = 0;
|
|
}
|
|
this->altf = (float)alt / 1000;
|
|
|
|
this->accuracy = 2.5 * ((float)nmea.getHDOP()/10);
|
|
|
|
//nmea.clear();
|
|
}
|
|
|
|
float GpsInterface::getAccuracy() {
|
|
return this->accuracy;
|
|
}
|
|
|
|
String GpsInterface::getLat() {
|
|
return this->lat;
|
|
}
|
|
|
|
String GpsInterface::getLon() {
|
|
return this->lon;
|
|
}
|
|
|
|
float GpsInterface::getAlt() {
|
|
return this->altf;
|
|
}
|
|
|
|
String GpsInterface::getDatetime() {
|
|
return this->datetime;
|
|
}
|
|
|
|
String GpsInterface::getNumSatsString() {
|
|
return (String)num_sats;
|
|
}
|
|
|
|
int GpsInterface::getNumSats() {
|
|
return num_sats;
|
|
}
|
|
|
|
bool GpsInterface::getFixStatus() {
|
|
return this->good_fix;
|
|
}
|
|
|
|
String GpsInterface::getFixStatusAsString() {
|
|
if (this->getFixStatus())
|
|
return "Yes";
|
|
else
|
|
return "No";
|
|
}
|
|
|
|
bool GpsInterface::getGpsModuleStatus() {
|
|
return this->gps_enabled;
|
|
}
|
|
|
|
String GpsInterface::getText() {
|
|
return this->gps_text;
|
|
}
|
|
|
|
int GpsInterface::getTextQueueSize() {
|
|
if(this->queue_enabled_flag){
|
|
bool exists=0;
|
|
if(this->text){
|
|
int size=this->text->size();
|
|
if(size) return size;
|
|
exists=1;
|
|
}
|
|
if(this->text_in){
|
|
int size=this->text_in->size();
|
|
if(size) return size;
|
|
exists=1;
|
|
}
|
|
if(exists)
|
|
return 0;
|
|
else
|
|
return -2;
|
|
}
|
|
else
|
|
return -1;
|
|
}
|
|
|
|
String GpsInterface::getTextQueue(bool flush) {
|
|
if(this->queue_enabled_flag){
|
|
if(this->text){
|
|
int size=this->text->size();
|
|
if(size){
|
|
String text;
|
|
for(int i=0;i<size;i++){
|
|
String now=this->text_in->get(i);
|
|
if(now!=""){
|
|
if(text!=""){
|
|
text+='\r';
|
|
text+='\n';
|
|
}
|
|
text+=now;
|
|
}
|
|
}
|
|
if(flush){
|
|
LinkedList<String> *delme=this->text;
|
|
this->text_cycles=0;
|
|
this->text=this->text_in;
|
|
if(!this->text) this->text=new LinkedList<String>;
|
|
if(this->text->size()) this->text_cycles++;
|
|
this->text_in=new LinkedList<String>;
|
|
delete delme;
|
|
}
|
|
return text;
|
|
}
|
|
}
|
|
else{
|
|
this->text=new LinkedList<String>;
|
|
this->text_cycles=0;
|
|
}
|
|
|
|
if(this->text_in){
|
|
int size=this->text_in->size();
|
|
if(size){
|
|
LinkedList<String> *buffer=this->text_in;
|
|
if(flush)
|
|
this->text_in=new LinkedList<String>;
|
|
String text;
|
|
for(int i=0;i<size;i++){
|
|
String now=buffer->get(i);
|
|
if(now!=""){
|
|
if(text!=""){
|
|
text+='\r';
|
|
text+='\n';
|
|
}
|
|
text+=now;
|
|
}
|
|
}
|
|
if(flush)
|
|
delete buffer;
|
|
return text;
|
|
}
|
|
}
|
|
else
|
|
this->text_in=new LinkedList<String>;
|
|
|
|
return this->gps_text;
|
|
}
|
|
else
|
|
return this->gps_text;
|
|
}
|
|
|
|
String GpsInterface::getNmea() {
|
|
return this->nmea_sentence;
|
|
}
|
|
|
|
String GpsInterface::getNmeaNotimp() {
|
|
return this->notimp_nmea_sentence;
|
|
}
|
|
|
|
String GpsInterface::getNmeaNotparsed() {
|
|
return this->notparsed_nmea_sentence;
|
|
}
|
|
|
|
void GpsInterface::main() {
|
|
while (Serial2.available()) {
|
|
//Fetch the character one by one
|
|
char c = Serial2.read();
|
|
//Serial.print(c);
|
|
//Pass the character to the library
|
|
nmea.process(c);
|
|
}
|
|
|
|
uint8_t num_sat = nmea.getNumSatellites();
|
|
|
|
if ((nmea.isValid()) && (num_sat > 0))
|
|
this->setGPSInfo();
|
|
|
|
else if ((!nmea.isValid()) && (num_sat <= 0)) {
|
|
this->setGPSInfo();
|
|
}
|
|
}
|
|
#endif
|