Note: Goby version 1 (shown here) is now considered obsolete. Please use version 2 for new projects, and consider upgrading old projects.

Goby Underwater Autonomy Project  Series: 1.1, revision: 163, released on 2013-02-06 14:23:27 -0500
util/liblinebasedcomms/nmea_sentence.cpp
00001 // copyright 2009 t. schneider tes@mit.edu
00002 //           2010 c. murphy    cmurphy@whoi.edu
00003 // 
00004 // this file is part of serial, a library for handling serial
00005 // communications.
00006 //
00007 // This program is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // This software is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with this software.  If not, see <http://www.gnu.org/licenses/>.
00019 
00020 #include <boost/algorithm/string.hpp>
00021 
00022 #include "goby/util/binary.h"
00023 
00024 #include "nmea_sentence.h"
00025 
00026 goby::util::NMEASentence::NMEASentence(std::string s, strategy cs_strat /*= VALIDATE*/)
00027 {
00028     bool found_csum = false;
00029     unsigned int cs;
00030     // Silently drop leading/trailing whitespace if present.
00031     boost::trim(s);
00032     // Basic error checks ($, empty)
00033     if (s.empty())
00034       throw bad_nmea_sentence("NMEASentence: no message provided.");
00035     if (s[0] != '$')
00036       throw bad_nmea_sentence("NMEASentence: no $: '" + s + "'.");
00037     // Check if the checksum exists and is correctly placed, and strip it.
00038     // If it's not correctly placed, we'll interpret it as part of message.
00039     // NMEA spec doesn't seem to say that * is forbidden elsewhere? (should be)
00040     if (s.size() > 3 && s.at(s.size()-3) == '*') {
00041       std::string hex_csum = s.substr(s.size()-2);
00042       found_csum = util::hex_string2number(hex_csum, cs);
00043       s = s.substr(0, s.size()-3);
00044     }
00045     // If we require a checksum and haven't found one, fail.
00046     if (cs_strat == REQUIRE and !found_csum)
00047       throw bad_nmea_sentence("NMEASentence: no checksum: '" + s + "'.");
00048     // If we found a bad checksum and we care, fail.
00049     if (found_csum && (cs_strat == REQUIRE || cs_strat == VALIDATE)) {
00050       unsigned char calc_cs = NMEASentence::checksum(s);
00051       if (calc_cs != cs)
00052         throw bad_nmea_sentence("NMEASentence: bad checksum: '" + s + "'.");
00053     }
00054     // Split string into parts.
00055     boost::split(*(std::vector<std::string>*)this, s, boost::is_any_of(","));
00056     // Validate talker size.
00057     if (this->front().size() != 6)
00058       throw bad_nmea_sentence("NMEASentence: bad talker length '" + s + "'.");
00059 }
00060 
00061 unsigned char goby::util::NMEASentence::checksum(const std::string& s) {
00062     unsigned char csum = 0;
00063 
00064     if(s.empty())
00065       throw bad_nmea_sentence("NMEASentence::checksum: no message provided.");
00066     std::string::size_type star = s.find_first_of("*\r\n");
00067     std::string::size_type dollar = s.find('$');
00068     
00069     if(dollar == std::string::npos)
00070       throw bad_nmea_sentence("NMEASentence::checksum: no $ found.");
00071 
00072     if(star == std::string::npos) star = s.length();
00073     
00074     for(std::string::size_type i = dollar+1; i < star; ++i) csum ^= s[i];
00075     return csum;
00076 }
00077 
00078 std::string goby::util::NMEASentence::message_no_cs() const {
00079     std::string message = "";
00080 
00081     for(const_iterator it = begin(), n = end(); it < n; ++it)
00082       message += *it + ",";
00083 
00084     // kill last ","
00085     message.resize(message.size()-1);
00086     return message;
00087 }
00088 
00089 std::string goby::util::NMEASentence::message() const {
00090     std::string bare = message_no_cs();
00091     std::stringstream message;
00092     unsigned char csum = NMEASentence::checksum(bare);
00093     message << bare << "*";
00094     message << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << unsigned(csum);
00095     return message.str();
00096 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends