Added parsing of GxTXT messages (gps -g text). Added default native mode for nmea local-genned messages, where it claims to be what it hears from your GPS chip. Added support for more spoofed sat types: beidou (BD or GB), navic, qzss, explicit multisat (instead of native). Added UI nicieties. Made screen display one nmea sentence in queue mode, as well as the local-gen ones, and the text buffer. Added GxTXT to gpsdata output, along with accuracy.

esp32_marauder/CommandLine.cpp

@@ -281,10 +281,6 @@ void CommandLine::runCommand(String input) {
 
     wifi_scan_obj.StartScan(WIFI_SCAN_OFF);
 
-    //#ifdef HAS_GPS
-    //  gps_obj.disable_queue();
-    //#endif
-
     if(old_scan_mode == WIFI_SCAN_GPS_NMEA)
       Serial.println("END OF NMEA STREAM");
     else if(old_scan_mode == WIFI_SCAN_GPS_DATA)
@@ -333,17 +329,23 @@ void CommandLine::runCommand(String input) {
             Serial.println("Accuracy: " + (String)gps_obj.getAccuracy());
           else if (gps_info == "date")
             Serial.println("Date/Time: " + gps_obj.getDatetime());
+          else if (gps_info == "text"){
+            Serial.println(gps_obj.getText());
+          }
           else if (gps_info == "nmea"){
-            int notimp_arg = this->argSearch(&cmd_args, "-p");
+            int notparsed_arg = this->argSearch(&cmd_args, "-p");
+            int notimp_arg = this->argSearch(&cmd_args, "-i");
             int recd_arg = this->argSearch(&cmd_args, "-r");
-            if(notimp_arg == -1 && recd_arg == -1){
+            if(notparsed_arg == -1 && notimp_arg == -1 && recd_arg == -1){
               gps_obj.sendSentence(Serial, gps_obj.generateGXgga().c_str());
               gps_obj.sendSentence(Serial, gps_obj.generateGXrmc().c_str());
             }
-            else if(notimp_arg == -1)
+            else if(notparsed_arg == -1 && notimp_arg == -1)
               Serial.println(gps_obj.getNmea());
-            else
+            else if(notparsed_arg == -1)
               Serial.println(gps_obj.getNmeaNotimp());
+            else
+              Serial.println(gps_obj.getNmeaNotparsed());
           }
           else
             Serial.println("You did not provide a valid argument");
@@ -351,8 +353,16 @@ void CommandLine::runCommand(String input) {
         else if(nmea_arg != -1){
           String nmea_type = cmd_args.get(nmea_arg + 1);
 
-          if (nmea_type == "all" || nmea_type == "gps" || nmea_type == "glonass" || nmea_type== "galileo")
+          if (nmea_type == "native" || nmea_type == "all" || nmea_type == "gps" || nmea_type == "glonass"
+              || nmea_type == "galileo" || nmea_type == "navic" || nmea_type == "qzss" || nmea_type == "beidou"){
+            if(nmea_type == "beidou"){
+              int beidou_bd_arg = this->argSearch(&cmd_args, "-b");
+              if(beidou_bd_arg != -1)
+                nmea_type="beidou_bd";
+            }
             gps_obj.setType(nmea_type);
+            Serial.println("GPS Output Type Set To: " + nmea_type);
+          }
           else
             Serial.println("You did not provide a valid argument");
         }
@@ -369,7 +379,7 @@ void CommandLine::runCommand(String input) {
         #ifdef HAS_SCREEN
           menu_function_obj.changeMenu(&menu_function_obj.gpsInfoMenu);
         #endif
-        gps_obj.enable_queue();
+        Serial.println("NMEA STREAM FOLLOWS");
         wifi_scan_obj.currentScanMode = WIFI_SCAN_GPS_NMEA;
         wifi_scan_obj.StartScan(WIFI_SCAN_GPS_NMEA, TFT_CYAN);
       }

esp32_marauder/CommandLine.h

@@ -100,7 +100,7 @@ const char PROGMEM HELP_SETTINGS_CMD[] = "settings [-s <setting> enable/disable>
 const char PROGMEM HELP_LS_CMD[] = "ls <directory>";
 const char PROGMEM HELP_LED_CMD[] = "led -s <hex color>/-p <rainbow>";
 const char PROGMEM HELP_GPS_DATA_CMD[] = "gpsdata";
-const char PROGMEM HELP_GPS_CMD[] = "gps [-g] <fix/sat/lon/lat/alt/date/accuracy/nmea> [-n] <all/gps/glonass/galileo>";
+const char PROGMEM HELP_GPS_CMD[] = "gps [-g] <fix/sat/lon/lat/alt/date/accuracy/text/nmea>\r\n    [-n] <native/all/gps/glonass/galileo/navic/qzss/beidou>\r\n         [-b = use BD vs GB for beidou]";
 const char PROGMEM HELP_NMEA_CMD[] = "nmea";
 
 // WiFi sniff/scan

esp32_marauder/GpsInterface.cpp

@@ -36,9 +36,7 @@ void GpsInterface::begin() {
       Serial2.read();
   }
 
-  this->queue_enabled_flag=0;
+  this->flush_queue(); //init the queue, kill NULLs
-  this->queue=NULL;
-  this->new_queue();
 
   nmea.setUnknownSentenceHandler(gps_nmea_notimp);
 }
@@ -49,26 +47,186 @@ void gps_nmea_notimp(MicroNMEA& nmea){
 }
 
 void GpsInterface::enqueue(MicroNMEA& nmea){
-  String nmea_sentence = String(nmea.getSentence());
+  std::string nmea_sentence = std::string(nmea.getSentence());
 
-  if(nmea_sentence != ""){
+  if(nmea_sentence.length()){
-    this->notimp_nmea_sentence = nmea_sentence;
+    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_MAXCOPIES
+                          if(this->text_cycles>GPS_TEXT_MAXCOPIES){
+                        #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_MAXCOPIES
+                        if(this->text_cycles>GPS_TEXT_MAXCOPIES){
+                      #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(!this->queue) this->new_queue();
-      this->queue->add(String(nmea_sentence));
+      if(enqueue){
+        String enqueue_me=nmea_sentence.c_str();
+        this->queue->add(enqueue_me);
+      }
     }
+    else
+      this->flush_queue();
   }
+  else
+    if(!this->queue_enabled_flag)
+      this->flush_queue();
 }
 
 void GpsInterface::enable_queue(){
-  this->flush_queue();
+  if(this->queue_enabled_flag){
-  this->queue_enabled_flag=1;
+    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();
+  this->queue_enabled_flag=0;
 }
 
 bool GpsInterface::queue_enabled(){
@@ -84,8 +242,51 @@ void GpsInterface::new_queue(){
 }
 
 void GpsInterface::flush_queue(){
-  if(this->queue) delete this->queue;
+  this->flush_queue_nmea();
-  this->new_queue();
+  this->flush_text();
+}
+
+void GpsInterface::flush_queue_nmea(){
+  if(this->queue){
+    if(this->queue->size()){
+      LinkedList<String> *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){
@@ -97,27 +298,28 @@ void GpsInterface::sendSentence(Stream &s, const char* sentence){
 }
 
 void GpsInterface::setType(String t){
-  if(t == "gps")
+  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="$G";
+  String msg_type="$"+this->generateType()+"GGA,";
-  if(this->type_flag == GPSTYPE_GPS)
-    msg_type+='P';
-  else if(this->type_flag == GPSTYPE_GLONASS)
-    msg_type+='L';
-  else if(this->type_flag == GPSTYPE_GALILEO)
-    msg_type+='A';
-  else
-    msg_type+='N';
-  msg_type+="GGA,";
 
   char timeStr[8];
   snprintf(timeStr, 8, "%02d%02d%02d,", (int)(nmea.getHour()), (int)(nmea.getMinute()), (int)(nmea.getSecond()));
@@ -158,16 +360,7 @@ String GpsInterface::generateGXgga(){
 }
 
 String GpsInterface::generateGXrmc(){
-  String msg_type="$G";
+  String msg_type="$"+this->generateType()+"RMC,";
-  if(this->type_flag == GPSTYPE_GPS)
-    msg_type+='P';
-  else if(this->type_flag == GPSTYPE_GLONASS)
-    msg_type+='L';
-  else if(this->type_flag == GPSTYPE_GALILEO)
-    msg_type+='A';
-  else
-    msg_type+='N';
-  msg_type+="RMC,";
 
   char timeStr[8];
   snprintf(timeStr, 8, "%02d%02d%02d,", (int)(nmea.getHour()), (int)(nmea.getMinute()), (int)(nmea.getSecond()));
@@ -201,6 +394,44 @@ String GpsInterface::generateGXrmc(){
   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.
@@ -226,6 +457,7 @@ void GpsInterface::setGPSInfo() {
   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();
@@ -282,6 +514,93 @@ 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+='\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;
 }
@@ -290,6 +609,10 @@ 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

esp32_marauder/GpsInterface.h

@@ -9,6 +9,9 @@
 
 #include "configs.h"
 
+//#define GPS_TEXT_MAXLINES 5 //default:5 lines in the buffer maximum
+//#define GPS_TEXT_MAXCOPIES 1 //default:any nonzero number resets, i.e. one copy
+
 void gps_nmea_notimp(MicroNMEA& nmea);
 
 class GpsInterface {
@@ -25,14 +28,19 @@ class GpsInterface {
     float getAlt();
     float getAccuracy();
     String getDatetime();
+    String getText();
+    int getTextQueueSize();
+    String getTextQueue(bool flush=1);
     String getNmea();
     String getNmeaNotimp();
+    String getNmeaNotparsed();
 
     void setType(String t);
 
     void enqueue(MicroNMEA& nmea);
     LinkedList<String>* get_queue();
     void flush_queue();
+    void flush_text();
     void new_queue();
     void enable_queue();
     void disable_queue();
@@ -44,17 +52,24 @@ class GpsInterface {
     String generateGXgga();
     String generateGXrmc();
 
+  private:
     enum type_t {
-      GPSTYPE_ALL,
+      GPSTYPE_NATIVE = 0,
-      GPSTYPE_GPS,
+      GPSTYPE_ALL = 1,
-      GPSTYPE_GLONASS,
+      GPSTYPE_GPS = 2,
-      GPSTYPE_GALILEO
+      GPSTYPE_GLONASS = 3,
+      GPSTYPE_GALILEO = 4,
+      GPSTYPE_NAVIC = 5,
+      GPSTYPE_QZSS = 6,
+      GPSTYPE_BEIDOU = 7,
+      GPSTYPE_BEIDOU_BD = 8
     };
 
-  private:
     // GPS Info
+    String gps_text = "";
     String nmea_sentence = "";
     String notimp_nmea_sentence = "";
+    String notparsed_nmea_sentence = "";
     String lat = "";
     String lon = "";
     float altf = 0.0;
@@ -63,13 +78,22 @@ class GpsInterface {
     
     bool gps_enabled = false;
     bool good_fix = false;
+    char nav_system='\0';
     uint8_t num_sats = 0;
 
-    type_t type_flag = GPSTYPE_ALL;
+    type_t type_flag = GPSTYPE_NATIVE;
 
     bool queue_enabled_flag=0;
     LinkedList<String> *queue=NULL;
 
+    int text_cycles=0;
+    LinkedList<String> *text_in=NULL;
+    LinkedList<String> *text=NULL;
+
+    String generateType();
+    void flush_queue_text();
+    void flush_queue_textin();
+    void flush_queue_nmea();
     String dt_string_from_gps();
     void setGPSInfo();
 };

esp32_marauder/WiFiScan.cpp

@@ -591,6 +591,11 @@ void WiFiScan::StartScan(uint8_t scan_mode, uint16_t color)
       RunLvJoinWiFi(scan_mode, color);
     #endif
   }
+  else if (scan_mode == WIFI_SCAN_GPS_NMEA){
+    #ifdef HAS_GPS
+      gps_obj.enable_queue();
+    #endif
+  }
 
   WiFiScan::currentScanMode = scan_mode;
 }
@@ -772,8 +777,7 @@ void WiFiScan::StopScan(uint8_t scan_mode)
   #endif
 
   #ifdef HAS_GPS
-    if (gps_obj.queue_enabled())
+    gps_obj.disable_queue();
-      gps_obj.disable_queue();
   #endif
 }
 
@@ -1147,6 +1151,8 @@ void WiFiScan::RunGenerateSSIDs(int count) {
 
 void WiFiScan::RunGPSInfo() {
   #ifdef HAS_GPS
+    String text=gps_obj.getText();
+
     Serial.println("Refreshing GPS Data on screen...");
     #ifdef HAS_SCREEN
 
@@ -1168,7 +1174,10 @@ void WiFiScan::RunGPSInfo() {
       else
         display_obj.tft.println("  Good Fix: No");
         
+      if(text != "") display_obj.tft.println("      Text: " + text);
+
       display_obj.tft.println("Satellites: " + gps_obj.getNumSatsString());
+      display_obj.tft.println("  Accuracy: " + (String)gps_obj.getAccuracy());
       display_obj.tft.println("  Latitude: " + gps_obj.getLat());
       display_obj.tft.println(" Longitude: " + gps_obj.getLon());
       display_obj.tft.println("  Altitude: " + (String)gps_obj.getAlt());
@@ -1182,7 +1191,10 @@ void WiFiScan::RunGPSInfo() {
     else
       Serial.println("  Good Fix: No");
       
+    if(text != "") Serial.println("      Text: " + text);
+
     Serial.println("Satellites: " + gps_obj.getNumSatsString());
+    Serial.println("  Accuracy: " + (String)gps_obj.getAccuracy());
     Serial.println("  Latitude: " + gps_obj.getLat());
     Serial.println(" Longitude: " + gps_obj.getLon());
     Serial.println("  Altitude: " + (String)gps_obj.getAlt());
@@ -1193,9 +1205,14 @@ void WiFiScan::RunGPSInfo() {
 void WiFiScan::RunGPSNmea() {
   #ifdef HAS_GPS
     LinkedList<String> *buffer=gps_obj.get_queue();
+    bool queue_enabled=gps_obj.queue_enabled();
 
-    static String old_nmea_sentence="";
+    if(!buffer||!queue_enabled)
-    #ifdef HAS_SCREEN
+      gps_obj.flush_queue();
+    #ifndef HAS_SCREEN
+      else
+        gps_obj.flush_text();
+    #else
       // Get screen position ready
       display_obj.tft.setTextWrap(true);
       display_obj.tft.setFreeFont(NULL);
@@ -1207,30 +1224,47 @@ void WiFiScan::RunGPSNmea() {
       display_obj.tft.fillRect(0, (SCREEN_HEIGHT / 3) - 6, SCREEN_WIDTH, SCREEN_HEIGHT - ((SCREEN_HEIGHT / 3) - 6), TFT_BLACK);
 
       display_obj.tft.setCursor(0, SCREEN_HEIGHT / 3);
+
+      String text=gps_obj.getText();
+      if(queue_enabled){
+        if(gps_obj.getTextQueueSize()>0)
+          display_obj.tft.print(gps_obj.getTextQueue());
+        else
+          if(text != "") display_obj.tft.print(text);
+      }
+      else
+        if(text != "") display_obj.tft.print(text);
+
+      //This one doesn't contain self-genned GxGGA or GxRMC, nor does it contain GxTXT, processed above
+      String display_nmea_sentence=gps_obj.getNmeaNotparsed();
     #endif
 
-    if(buffer && gps_obj.queue_enabled()){
+    if(buffer && queue_enabled){
-      gps_obj.new_queue();
       int size=buffer->size();
-      for(int i=0;i<size;i++){
+      if(size){
-        Serial.println(buffer->get(i));
-      }
-      delete buffer;
-    } else  {
-      if(buffer){
-        if(buffer->size()>0) gps_obj.flush_queue();
-      } else {
         gps_obj.new_queue();
+        for(int i=0;i<size;i++){
+          Serial.println(buffer->get(i));
+        }
+        delete buffer;
       }
 
+      #ifdef HAS_SCREEN
+        //This matches the else block, but could later display more of the queue...
+        display_obj.tft.print(display_nmea_sentence);
+      #endif
+    } else {
+      static String old_nmea_sentence="";
       String nmea_sentence=gps_obj.getNmeaNotimp();
+
       if(nmea_sentence != "" && nmea_sentence != old_nmea_sentence){
         old_nmea_sentence=nmea_sentence;
         Serial.println(nmea_sentence);
-        #ifdef HAS_SCREEN        
-          display_obj.tft.print(nmea_sentence);
-        #endif
       }
+
+      #ifdef HAS_SCREEN
+        display_obj.tft.print(display_nmea_sentence);
+      #endif
     }
 
     String gxgga = gps_obj.generateGXgga();