Handle GPGSV log with 0 satellites correctly (#76)

If a GPGSV log has 0 satellites, it still has enough empty fields in it for 1 satellite. The size of the message was being calculated incorrectly and ended up throwing an error.
swri-robotics · Dec 5, 2019 · 3005b80 · 3005b80
1 parent 8fbdbbb
commit 3005b80
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 5 deletions.
diff --git a/novatel_gps_driver/src/parsers/gpgsv.cpp b/novatel_gps_driver/src/parsers/gpgsv.cpp
@@ -88,12 +88,14 @@ novatel_gps_msgs::GpgsvPtr novatel_gps_driver::GpgsvParser::ParseAscii(const nov
   {
     n_sats_in_sentence = msg->n_satellites % static_cast<uint8_t>(4);
   }
+  // Check that the sentence is the right length for the number of satellites
+  size_t expected_length = MIN_LENGTH + 4 * n_sats_in_sentence;
   if (n_sats_in_sentence == 0)
   {
-    n_sats_in_sentence = 4;
+    // Even if the number of sats is 0, the message will still have enough
+    // blank fields for 1 satellite.
+    expected_length += 4;
   }
-  // Check that the sentence is the right length for the number of satellites
-  size_t expected_length = MIN_LENGTH + 4 * n_sats_in_sentence;
   if (sentence.body.size() != expected_length && sentence.body.size() != expected_length -1)
   {
     std::stringstream ss;

diff --git a/novatel_gps_driver/test/parser_tests.cpp b/novatel_gps_driver/test/parser_tests.cpp
@@ -180,7 +180,8 @@ TEST(ParserTestSuite, testCorrimudataAsciiParsing)
 TEST(ParserTestSuite, testGpgsvParsing)
 {
   novatel_gps_driver::GpgsvParser parser;
-  std::string sentence_str = "$GPGSV,3,3,11,12,07,00.,32,13,03,227,36,22,0.,041,*4A\r\n";
+  std::string sentence_str = "$GPGSV,3,3,11,12,07,00.,32,13,03,227,36,22,0.,041,*4A\r\n"
+                             "$GPGSV,1,1,00,,,,*79\r\n";
   std::string extracted_str;
 
   novatel_gps_driver::NovatelMessageExtractor extractor;
@@ -193,7 +194,7 @@ TEST(ParserTestSuite, testGpgsvParsing)
   extractor.ExtractCompleteMessages(sentence_str, nmea_sentences, novatel_sentences,
                                     binary_messages, remaining);
 
-  ASSERT_EQ(1, nmea_sentences.size());
+  ASSERT_EQ(2, nmea_sentences.size());
   ASSERT_EQ(0, binary_messages.size());
   ASSERT_EQ(0, novatel_sentences.size());
 
@@ -222,6 +223,11 @@ TEST(ParserTestSuite, testGpgsvParsing)
   ASSERT_EQ(0, msg->satellites[2].elevation);
   ASSERT_EQ(41, msg->satellites[2].azimuth);
   ASSERT_EQ(-1, msg->satellites[2].snr);
+
+  msg = parser.ParseAscii(nmea_sentences.at(1));
+
+  ASSERT_NE(msg.get(), nullptr);
+  ASSERT_EQ(0, msg->satellites.size());
 }
 
 TEST(ParserTestSuite, testGphdtParsing)