NMEA parser

firmware/libraries/versavis/src/nmea_parser/NmeaParser.cpp

@@ -0,0 +1,190 @@
+#include "NmeaParser.h"
+
+#include <cstring>
+#include <cstdlib>
+
+const char kSentenceStart = '$';
+const char kCheckSumDelim = '*';
+const char kDataFieldDelim = ',';
+const char kSentenceEnd1 = '\r';
+const char kSentenceEnd2 = '\n';
+
+NmeaParser::NmeaParser() { resetSentence(); }
+
+NmeaParser::SentenceType NmeaParser::parseChar(const char c) {
+  // Control state transitions.
+  switch (state_) {
+  case State::kUnknown:
+    if (c == kSentenceStart) { // Start sentence.
+      transitionState(State::kId);
+    }
+    break;
+  case State::kId:
+    addToCheckSum(c);
+    addCharacter(c, id_, kIdSize);  // Fill ID field.
+    if (strlen(id_) == kIdSize) {   // ID complete.
+      transitionState(State::kMsg); // Transition to MSG field.
+    }
+    break;
+  case State::kMsg:
+    addToCheckSum(c);
+    addCharacter(c, msg_, kMsgSize);      // Fill MSG field.
+    if (c == kDataFieldDelim) {           // MSG type complete
+      transitionState(State::kDataField); // Transition to first data field.
+    }
+    break;
+  case State::kDataField:
+    addToCheckSum(c);
+    addCharacter(c, data_field_, kDataFieldSize); // Fill data field.
+    if (c == kDataFieldDelim) { // Data field complete and next data field.
+      transitionState(State::kDataField);
+    } else if (c == kCheckSumDelim) { // Data field complete followed by cs.
+      transitionState(State::kCheckSum);
+    }
+    break;
+  case State::kCheckSum:
+    // TODO(rikba): Catch failure when checksum grows longer than 2 characters.
+    addCharacter(c, cs_, kCsSize);                      // Fill check sum.
+    if ((c == kSentenceEnd1) || (c == kSentenceEnd2)) { // Done!
+      transitionState(State::kSuccess);
+    }
+    break;
+  case State::kSuccess:
+    transitionState(State::kUnknown);
+    break;
+  default:
+    transitionState(State::kUnknown);
+  }
+
+  return sentence_type_;
+}
+
+void NmeaParser::resetSentence() {
+  resetWord();
+
+  memset(id_, '\0', kIdSize + 1);
+  memset(msg_, '\0', kMsgSize + 1);
+  memset(cs_, '\0', kCsSize + 1);
+  cs_calculated_ = 0x00;
+
+  id_type_ = IdType::kUnknown;
+  msg_type_ = MsgType::kUnknown;
+  sentence_type_ = SentenceType::kUnknown;
+  df_idx_ = 0;
+}
+
+void NmeaParser::resetWord() { memset(data_field_, '\0', kDataFieldSize + 1); }
+
+void NmeaParser::transitionState(const State new_state) {
+  // Execute state transitions. If the transition fails the state machine goes
+  // into state Unknown.
+  bool success = true;
+
+  switch (new_state) {
+  case State::kUnknown: // Can be reached from any state. Starting point.
+    resetSentence();
+    success &= true;
+    break;
+  case State::kId:
+    if (state_ == State::kUnknown) { // Transition from kUnknown.
+      success &= true;               // Do nothing.
+    }
+    break;
+  case State::kMsg:
+    if (state_ == State::kId) {   // Transition from kId.
+      success &= processIdType(); // Check valid ID.
+    }
+    break;
+  case State::kDataField:
+    if (state_ == State::kMsg) {              // Transition from kMsg.
+      success &= processMsgType();            // Check valid message.
+    } else if (state_ == State::kDataField) { // Transition from kDataField.
+      success &= processDataField();          // Check valid data.
+    }
+    break;
+  case State::kCheckSum:
+    if (state_ == State::kDataField) { // Transition from kDataField.
+      success &= processDataField();   // Check valid data.
+    }
+    break;
+  case State::kSuccess:
+    if (state_ == State::kCheckSum) {   // Transition from kCheckSum.
+      success &= processCheckSum();     // Check valid check sum.
+      success &= processSentenceType(); // Update sentence type.
+    }
+  default:
+    break;
+  }
+
+  if (success) {
+    resetWord();
+    state_ = new_state;
+  } else {
+    resetSentence();
+    state_ = State::kUnknown;
+  }
+}
+
+bool NmeaParser::processSentenceType() {
+  if ((id_type_ == IdType::kGps) && (msg_type_ == MsgType::kZda)) {
+    sentence_type_ = SentenceType::kGpZda;
+  }
+
+  return sentence_type_ != SentenceType::kUnknown;
+}
+
+void NmeaParser::addCharacter(const char c, char *field, const uint8_t len) {
+  if ((strlen(field) < len) && (c != kDataFieldDelim) &&
+      (c != kCheckSumDelim) && (c != kSentenceEnd1) && (c != kSentenceEnd2)) {
+    strncat(field, &c, 1);
+  }
+}
+
+void NmeaParser::addToCheckSum(const char c) {
+  if (c != kCheckSumDelim)
+    cs_calculated_ ^= c;
+}
+
+bool NmeaParser::processIdType() {
+  const char kGps[3] = "GP";
+
+  if (strcmp(id_, kGps) == 0) {
+    id_type_ = IdType::kGps;
+  } else {
+    id_type_ = IdType::kUnknown;
+  }
+
+  return id_type_ != IdType::kUnknown;
+}
+
+bool NmeaParser::processMsgType() {
+  const char kZda[4] = "ZDA";
+
+  if (strcmp(msg_, kZda) == 0) {
+    msg_type_ = MsgType::kZda;
+  } else {
+    msg_type_ = MsgType::kUnknown;
+  }
+
+  return msg_type_ != MsgType::kUnknown;
+}
+
+bool NmeaParser::processDataField() {
+  bool success = false;
+
+  switch (msg_type_) {
+  case MsgType::kZda:
+    success = gp_zda_message_.update(data_field_, df_idx_);
+    break;
+  default:
+    break;
+  }
+
+  df_idx_++; // Increment data field number.
+  return success;
+}
+
+bool NmeaParser::processCheckSum() {
+  uint8_t cs_received = strtol(cs_, NULL, 16);
+  return cs_received == cs_calculated_;
+}

firmware/libraries/versavis/src/nmea_parser/NmeaParser.h

@@ -0,0 +1,102 @@
+////////////////////////////////////////////////////////////////////////////////
+//  January 2020
+//  Author: Rik Bähnemann <[email protected]>
+////////////////////////////////////////////////////////////////////////////////
+//  NmeaParser.h
+////////////////////////////////////////////////////////////////////////////////
+//
+//  Parse NMEA messages received on Serial.
+//
+// Example:
+// 
+// #include <NmeaParser.h>
+//
+// NmeaParser nmea_parser;
+//
+// void setup() {
+//   while(!SerialUSB);
+//   Serial.begin(115200);
+// }
+//
+// void loop() {
+//   while (Serial.available()) {
+//     auto sentence_type = nmea_parser.parseChar(Serial.read());
+//     if (sentence_type == NmeaParser::SentenceType::kGpZda) {
+//       SerialUSB.println(nmea_parser.getGpZdaMessage().str);
+//     }
+//   }
+//
+//   delay(100);
+// }
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef NmeaParser_h_
+#define NmeaParser_h_
+
+#include <cstdint>
+
+#include "nmea_parser/msg/ZdaMessage.h"
+
+class NmeaParser {
+public:
+  enum class SentenceType { kGpZda, kUnknown };
+
+  NmeaParser();
+  // Parse an individual character from serial buffer. If sentence is finished
+  // return the sentence type.
+  SentenceType parseChar(const char c);
+
+  inline ZdaMessage getGpZdaMessage() { return gp_zda_message_; }
+
+private:
+  // NMEA description https://resources.winsystems.com/software/nmea.pdf
+  // $->ID->MSG->','->Dn->*->CS->[CR][LF]
+  enum class State { kUnknown, kId, kMsg, kDataField, kCheckSum, kSuccess };
+  enum class IdType { kGps, kUnknown };
+  enum class MsgType { kZda, kUnknown };
+
+  // Sentence storage.
+  static const uint8_t kIdSize = 2;
+  static const uint8_t kMsgSize = 3;
+  static const uint8_t kCsSize = 2;
+  // Max size minus minimum info.
+  static const uint8_t kDataFieldSize = 79 - kIdSize - kMsgSize - kCsSize - 1;
+  // +1 for null termination.
+  char id_[kIdSize + 1];
+  char msg_[kMsgSize + 1];
+  char cs_[kCsSize + 1];
+  char data_field_[kDataFieldSize + 1];
+  uint8_t cs_calculated_ = 0x00;
+
+  // State and message info.
+  State state_ = State::kUnknown;
+  IdType id_type_ = IdType::kUnknown;
+  MsgType msg_type_ = MsgType::kUnknown;
+  SentenceType sentence_type_ = SentenceType::kUnknown;
+  uint8_t df_idx_ = 0; // The index of the data field in the current sentence.
+
+  void resetSentence();
+  void resetWord();
+  void transitionState(const State new_state);
+  void addCharacter(const char c, char *field, const uint8_t len);
+  void addToCheckSum(const char c);
+
+  bool terminateId();
+  bool terminateMsg();
+  bool terimateDataFieldAndStartNext();
+  bool terminateDataFieldAndStartCs();
+
+  bool processIdType();
+  bool processMsgType();
+  bool processDataField();
+  bool processCheckSum();
+  bool processSentenceType();
+
+  bool processZdaMessage();
+
+  // Received messages.
+  ZdaMessage gp_zda_message_;
+};
+
+#endif

firmware/libraries/versavis/src/nmea_parser/msg/ZdaMessage.cpp

@@ -0,0 +1,61 @@
+#include "nmea_parser/msg/ZdaMessage.h"
+#include "nmea_parser/msg/helper.h"
+
+bool ZdaMessage::update(const char *data, const uint8_t field) {
+  bool success = true;
+
+  switch (field) {
+  case 0:
+    success &= numFromWord<uint8_t>(data, 0, 2, &hour);
+    success &= numFromWord<uint8_t>(data, 2, 2, &minute);
+    success &= numFromWord<uint8_t>(data, 4, 2, &second);
+    success &= updateHundredths(data);
+    break;
+  case 1:
+    success &= numFromWord<uint8_t>(data, 0, 2, &day);
+    break;
+  case 2:
+    success &= numFromWord<uint8_t>(data, 0, 2, &month);
+    break;
+  case 3:
+    success &= numFromWord<uint16_t>(data, 0, 4, &year);
+    break;
+  case 4:
+    success &= true; // Ignore time zone field.
+    break;
+  case 5:
+    success &= true; // Ignore time zone offset field.
+    break;
+  default:
+    success &= false; // This field is not handled.
+    break;
+  }
+
+  if (!success) {
+    reset();
+  } else {
+    toString();
+  }
+
+  return success;
+}
+
+bool ZdaMessage::updateHundredths(const char *data) {
+  hundreths = 0;
+
+  auto data_len = strlen(data);
+  if (data_len < 7)
+    return true; // No decimal seconds.
+  else if (*(data + 6) != '.')
+    return false; // Missing decimal point.
+  else if (data_len < 8)
+    return true; // No digits.
+
+  uint8_t len = data_len - 7; // Get tail length.
+  return numFromWord<uint32_t>(data, 7, len, &hundreths);
+}
+
+void ZdaMessage::toString() {
+  sprintf(str, "%02d:%02d:%04d:%02d:%02d:%02d.%02d", day, month, year, hour,
+          minute, second, hundreths);
+}

firmware/libraries/versavis/src/nmea_parser/msg/ZdaMessage.h

@@ -0,0 +1,33 @@
+////////////////////////////////////////////////////////////////////////////////
+//  January 2020
+//  Author: Rik Bähnemann <[email protected]>
+////////////////////////////////////////////////////////////////////////////////
+//  NmeaParser.h
+////////////////////////////////////////////////////////////////////////////////
+//
+// A struct representing a ZDA message.
+// NMEA description https://resources.winsystems.com/software/nmea.pdf
+// A GPZDA sentence: $GPZDA,173538.00,14,01,2020,,*69[...]\n
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cstdint>
+
+struct ZdaMessage {
+public:
+  uint8_t hour = 0;
+  uint8_t minute = 0;
+  uint8_t second = 0;
+  uint32_t hundreths = 0;
+  uint8_t day = 0;
+  uint8_t month = 0;
+  uint16_t year = 0;
+  char str[23]; // TODO(rikba): Add this member only for debugging.
+
+  bool update(const char *data, const uint8_t field);
+  inline void reset() { *this = ZdaMessage(); }
+
+private:
+  void toString();
+  bool updateHundredths(const char *data);
+};