src/marnav/nmea/nmea.cpp

   1 #include "nmea.hpp"
   2
   3 #include <algorithm>
   4
   5 #include <marnav/nmea/split.hpp>
   6 #include <marnav/nmea/angle.hpp>
   7 #include <marnav/nmea/time.hpp>
   8 #include <marnav/nmea/date.hpp>
   9 #include <marnav/nmea/sentence.hpp>
  10 #include <marnav/nmea/checksum.hpp>
  11 #include <marnav/nmea/aam.hpp>
  12 #include <marnav/nmea/alm.hpp>
  13 #include <marnav/nmea/apb.hpp>
  14 #include <marnav/nmea/bod.hpp>
  15 #include <marnav/nmea/bwc.hpp>
  16 #include <marnav/nmea/bwr.hpp>
  17 #include <marnav/nmea/bww.hpp>
  18 #include <marnav/nmea/dbk.hpp>
  19 #include <marnav/nmea/dbt.hpp>
  20 #include <marnav/nmea/dpt.hpp>
  21 #include <marnav/nmea/dsc.hpp>
  22 #include <marnav/nmea/dse.hpp>
  23 #include <marnav/nmea/dtm.hpp>
  24 #include <marnav/nmea/fsi.hpp>
  25 #include <marnav/nmea/gbs.hpp>
  26 #include <marnav/nmea/gga.hpp>
  27 #include <marnav/nmea/glc.hpp>
  28 #include <marnav/nmea/gll.hpp>
  29 #include <marnav/nmea/gns.hpp>
  30 #include <marnav/nmea/grs.hpp>
  31 #include <marnav/nmea/gsa.hpp>
  32 #include <marnav/nmea/gst.hpp>
  33 #include <marnav/nmea/gsv.hpp>
  34 #include <marnav/nmea/gtd.hpp>
  35 #include <marnav/nmea/hdg.hpp>
  36 #include <marnav/nmea/hfb.hpp>
  37 #include <marnav/nmea/hdm.hpp>
  38 #include <marnav/nmea/hsc.hpp>
  39 #include <marnav/nmea/its.hpp>
  40 #include <marnav/nmea/lcd.hpp>
  41 #include <marnav/nmea/msk.hpp>
  42 #include <marnav/nmea/mss.hpp>
  43 #include <marnav/nmea/mtw.hpp>
  44 #include <marnav/nmea/mwd.hpp>
  45 #include <marnav/nmea/mwv.hpp>
  46 #include <marnav/nmea/osd.hpp>
  47 #include <marnav/nmea/r00.hpp>
  48 #include <marnav/nmea/rma.hpp>
  49 #include <marnav/nmea/rmb.hpp>
  50 #include <marnav/nmea/rmc.hpp>
  51 #include <marnav/nmea/rot.hpp>
  52 #include <marnav/nmea/rpm.hpp>
  53 #include <marnav/nmea/rsa.hpp>
  54 #include <marnav/nmea/rsd.hpp>
  55 #include <marnav/nmea/rte.hpp>
  56 #include <marnav/nmea/sfi.hpp>
  57 #include <marnav/nmea/tds.hpp>
  58 #include <marnav/nmea/tfi.hpp>
  59 #include <marnav/nmea/tll.hpp>
  60 #include <marnav/nmea/tpc.hpp>
  61 #include <marnav/nmea/tpr.hpp>
  62 #include <marnav/nmea/tpt.hpp>
  63 #include <marnav/nmea/ttm.hpp>
  64 #include <marnav/nmea/vbw.hpp>
  65 #include <marnav/nmea/vdm.hpp>
  66 #include <marnav/nmea/vdo.hpp>
  67 #include <marnav/nmea/vdr.hpp>
  68 #include <marnav/nmea/vhw.hpp>
  69 #include <marnav/nmea/vlw.hpp>
  70 #include <marnav/nmea/vpw.hpp>
  71 #include <marnav/nmea/vtg.hpp>
  72 #include <marnav/nmea/vwr.hpp>
  73 #include <marnav/nmea/wcv.hpp>
  74 #include <marnav/nmea/wnc.hpp>
  75 #include <marnav/nmea/wpl.hpp>
  76 #include <marnav/nmea/xdr.hpp>
  77 #include <marnav/nmea/xte.hpp>
  78 #include <marnav/nmea/xtr.hpp>
  79 #include <marnav/nmea/zda.hpp>
  80 #include <marnav/nmea/zdl.hpp>
  81 #include <marnav/nmea/zfo.hpp>
  82 #include <marnav/nmea/ztg.hpp>
  83 #include <marnav/nmea/pgrme.hpp>
  84
  85 /// @example parse_nmea.cpp
  86 /// This is an example on how to parse and handle NMEA sentences from a string.
  87
  88 namespace marnav
  89 {
  90 namespace nmea
  91 {
  92 namespace
  93 {
  94 // local macro, used for convenience while registering sentences
  95 #define REGISTER_SENTENCE(s)             \
  96         {                                    \
  97                 s::TAG, s::ID, detail::parse_##s \
  98         }
  99
 100 struct entry {
 101         const char * TAG;
 102         const sentence_id ID;
 103         const sentence::parse_function parse;
 104 };
 105 static const std::vector<entry> known_sentences = {
 106         // regular
 107         REGISTER_SENTENCE(aam), REGISTER_SENTENCE(alm), REGISTER_SENTENCE(apb),
 108         REGISTER_SENTENCE(bod), REGISTER_SENTENCE(bwc), REGISTER_SENTENCE(bwr),
 109         REGISTER_SENTENCE(bww), REGISTER_SENTENCE(dbk), REGISTER_SENTENCE(dbt),
 110         REGISTER_SENTENCE(dpt), REGISTER_SENTENCE(dsc), REGISTER_SENTENCE(dse),
 111         REGISTER_SENTENCE(dtm), REGISTER_SENTENCE(fsi), REGISTER_SENTENCE(gbs),
 112         REGISTER_SENTENCE(gga), REGISTER_SENTENCE(glc), REGISTER_SENTENCE(gll),
 113         REGISTER_SENTENCE(grs), REGISTER_SENTENCE(gns), REGISTER_SENTENCE(gsa),
 114         REGISTER_SENTENCE(gst), REGISTER_SENTENCE(gsv), REGISTER_SENTENCE(gtd),
 115         REGISTER_SENTENCE(hdg), REGISTER_SENTENCE(hfb), REGISTER_SENTENCE(hdm),
 116         REGISTER_SENTENCE(hsc), REGISTER_SENTENCE(its), REGISTER_SENTENCE(lcd),
 117         REGISTER_SENTENCE(msk), REGISTER_SENTENCE(mss), REGISTER_SENTENCE(mtw),
 118         REGISTER_SENTENCE(mwd), REGISTER_SENTENCE(mwv), REGISTER_SENTENCE(osd),
 119         REGISTER_SENTENCE(r00), REGISTER_SENTENCE(rma), REGISTER_SENTENCE(rmb),
 120         REGISTER_SENTENCE(rmc), REGISTER_SENTENCE(rot), REGISTER_SENTENCE(rpm),
 121         REGISTER_SENTENCE(rsa), REGISTER_SENTENCE(rsd), REGISTER_SENTENCE(rte),
 122         REGISTER_SENTENCE(sfi), REGISTER_SENTENCE(tds), REGISTER_SENTENCE(tfi),
 123         REGISTER_SENTENCE(tll), REGISTER_SENTENCE(tpc), REGISTER_SENTENCE(tpr),
 124         REGISTER_SENTENCE(tpt), REGISTER_SENTENCE(ttm), REGISTER_SENTENCE(vbw),
 125         REGISTER_SENTENCE(vdm), REGISTER_SENTENCE(vdo), REGISTER_SENTENCE(vdr),
 126         REGISTER_SENTENCE(vhw), REGISTER_SENTENCE(vlw), REGISTER_SENTENCE(vpw),
 127         REGISTER_SENTENCE(vtg), REGISTER_SENTENCE(vwr), REGISTER_SENTENCE(wcv),
 128         REGISTER_SENTENCE(wnc), REGISTER_SENTENCE(wpl), REGISTER_SENTENCE(xdr),
 129         REGISTER_SENTENCE(xte), REGISTER_SENTENCE(xtr), REGISTER_SENTENCE(zda),
 130         REGISTER_SENTENCE(zdl), REGISTER_SENTENCE(zfo), REGISTER_SENTENCE(ztg),
 131
 132         // vendor extensions
 133         REGISTER_SENTENCE(pgrme)};
 134
 135 #undef REGISTER_SENTENCE
 136 }
 137
 138 /// @cond DEV
 139 namespace detail
 140 {
 141 /// Returns the parse function of a particular sentence.
 142 ///
 143 /// If an unknown sentence tag is specified, an exception is thrown.
 144 ///
 145 /// @param[in] tag The tag of the sentence to get the parse function for.
 146 /// @return The parse function of the specified sentence.
 147 /// @exception std::unknown_sentence The specified tag could not be found,
 148 ///   the argument cannot be processed.
 149 static sentence::parse_function instantiate_sentence(const std::string & tag)
 150 {
 151         using namespace std;
 152
 153         auto const & i = std::find_if(begin(known_sentences), end(known_sentences),
 154                 [tag](const entry & e) { return e.TAG == tag; });
 155
 156         if (i == end(known_sentences))
 157                 throw unknown_sentence{"unknown sentence in nmea/instantiate_sentence: " + tag};
 158
 159         return i->parse;
 160 }
 161
 162 /// Returns true of the speficied address string indicates a proprietary sentence.
 163 static bool is_proprietary(const std::string & s)
 164 {
 165         if (s.size() < 1)
 166                 return false;
 167         return s[0] == 'P';
 168 }
 169
 170 /// Checks if the address field of the specified sentence is a vendor extension or
 171 /// a regular sentence. It returns the talker ID and tag accordingly.
 172 ///
 173 /// @param[in] address The address field of a sentence.
 174 /// @return The tuple contains talker ID and tag. In case of a vendor extension,
 175 ///   the talker ID may be empty.
 176 /// @exception std::invalid_argument The specified address was probably malformed or
 177 //    empty.
 178 static std::tuple<std::string, std::string> parse_address(const std::string & address)
 179 {
 180         if (address.empty())
 181                 throw std::invalid_argument{"invalid/malformed address in nmea/parse_address"};
 182
 183         // check for vendor extensions
 184         if (is_proprietary(address)) {
 185                 // proprietary extension / vendor extension
 186                 return make_tuple(std::string{}, address);
 187         }
 188
 189         // search in all known sentences
 190         using namespace std;
 191         auto const & index = find_if(begin(known_sentences), end(known_sentences),
 192                 [address](const entry & e) { return e.TAG == address; });
 193         if (index != end(known_sentences))
 194                 throw std::invalid_argument{"invalid address (" + address + ") in nmea/parse_address"};
 195
 196         // found regular sentence
 197         if (address.size() != 5) // talker ID:2 + tag:3
 198                 throw std::invalid_argument{"unknown or malformed address field: [" + address + "]"};
 199
 200         return make_tuple(address.substr(0, 2), address.substr(2, 3));
 201 }
 202
 203 /// Computes and checks the checksum of the specified sentence against the
 204 /// expected checksum.
 205 ///
 206 /// @param[in] s Sentence to check.
 207 /// @param[in] expected The expected checksum to test against.
 208 /// @exception checksum_error Thrown if the checksum does not match.
 209 /// @exception std::invalid_argument Arguments were invalid.
 210 static void ensure_checksum(const std::string & s, const std::string & expected)
 211 {
 212         auto const end_pos = s.find_first_of(sentence::end_token, 1);
 213         if (end_pos == std::string::npos) // end token not found
 214                 throw std::invalid_argument{"invalid format in nmea/make_sentence"};
 215         if (s.size() != end_pos + 3) // short or no checksum
 216                 throw std::invalid_argument{"invalid format in nmea/make_sentence"};
 217         const uint8_t expected_checksum = static_cast<uint8_t>(std::stoul(expected, nullptr, 16));
 218         const uint8_t sum = checksum(begin(s) + 1, begin(s) + end_pos);
 219         if (expected_checksum != sum)
 220                 throw checksum_error{expected_checksum, sum};
 221 }
 222 }
 223 /// @endcond
 224
 225 /// Returns a list of tags of supported sentences.
 226 std::vector<std::string> get_supported_sentences_str()
 227 {
 228         using namespace std;
 229         std::vector<std::string> v;
 230         v.reserve(std::distance(begin(known_sentences), end(known_sentences)));
 231         for (auto const & s : known_sentences) {
 232                 v.push_back(s.TAG);
 233         }
 234         return v;
 235 }
 236
 237 /// Returns a list of IDs of supported sentences.
 238 std::vector<sentence_id> get_supported_sentences_id()
 239 {
 240         using namespace std;
 241         std::vector<sentence_id> v;
 242         v.reserve(std::distance(begin(known_sentences), end(known_sentences)));
 243         for (auto const & s : known_sentences) {
 244                 v.push_back(s.ID);
 245         }
 246         return v;
 247 }
 248
 249 /// Returns the tag of the specified ID. If the sentence is unknown,
 250 /// an exception is thrown.
 251 std::string to_string(sentence_id id)
 252 {
 253         using namespace std;
 254         auto i = find_if(begin(known_sentences), end(known_sentences),
 255                 [id](const entry & e) { return e.ID == id; });
 256         if (i == end(known_sentences))
 257                 throw unknown_sentence{"unknown sentence"};
 258
 259         return i->TAG;
 260 }
 261
 262 /// Returns the ID of the specified tag. If the sentence is unknown,
 263 /// an exceptioni s thrown.
 264 sentence_id tag_to_id(const std::string & tag)
 265 {
 266         using namespace std;
 267         auto i = find_if(begin(known_sentences), end(known_sentences),
 268                 [tag](const entry & e) { return e.TAG == tag; });
 269         if (i == end(known_sentences))
 270                 throw unknown_sentence{"unknown sentence: " + tag};
 271
 272         return i->ID;
 273 }
 274
 275 /// Parses the string and returns the corresponding sentence.
 276 ///
 277 /// @param[in] s The sentence to parse.
 278 /// @param[in] ignore_checksum Option to ignore the checksum.
 279 /// @return The object of the corresponding type.
 280 /// @exception checksum_error Will be thrown if the checksum is wrong.
 281 /// @exception std::invalid_argument Will be thrown if the specified string
 282 ///   is not a NMEA sentence (malformed).
 283 /// @exception unknown_sentence Will be thrown if the sentence is
 284 ///   not supported.
 285 ///
 286 /// Example:
 287 /// @code
 288 ///   auto s =
 289 ///   nmea::make_sentence("$GPRMC,201034,A,4702.4040,N,00818.3281,E,0.0,328.4,260807,0.6,E,A*17");
 290 /// @endcode
 291 std::unique_ptr<sentence> make_sentence(const std::string & s, bool ignore_checksum)
 292 {
 293         using namespace std;
 294
 295         // perform various checks
 296         if (s.empty())
 297                 throw invalid_argument{"empty string in nmea/make_sentence"};
 298         if ((s[0] != sentence::start_token) && (s[0] != sentence::start_token_ais))
 299                 throw invalid_argument{"no start token in nmea/make_sentence"};
 300
 301         // extract all fields, skip start token
 302         std::vector<std::string> fields = detail::parse_fields(s);
 303         if (fields.size() < 2) // at least address and checksum must be present
 304                 throw std::invalid_argument{"malformed sentence in nmea/make_sentence"};
 305
 306         // checksum stuff
 307         if (!ignore_checksum) {
 308                 detail::ensure_checksum(s, fields.back());
 309         }
 310
 311         // extract address and posibly talker_id and tag.
 312         // check for vendor extension is necessary because the address field of this extensions
 313         // to not follow the pattern talker_id/tag
 314         std::string talker;
 315         std::string tag;
 316         std::tie(talker, tag) = detail::parse_address(fields.front());
 317
 318         return detail::instantiate_sentence(tag)(talker, next(begin(fields)), prev(end(fields)));
 319 }
 320 }
 321 }