gpgga.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <novatel_gps_driver/parsers/gpgga.h>
00031 #include <boost/make_shared.hpp>
00032 
00033 #include <swri_string_util/string_util.h>
00034 
00035 const std::string novatel_gps_driver::GpggaParser::MESSAGE_NAME = "GPGGA";
00036 
00037 uint32_t novatel_gps_driver::GpggaParser::GetMessageId() const
00038 {
00039   return 0;
00040 }
00041 
00042 const std::string novatel_gps_driver::GpggaParser::GetMessageName() const
00043 {
00044   return MESSAGE_NAME;
00045 }
00046 
00047 novatel_gps_msgs::GpggaPtr novatel_gps_driver::GpggaParser::ParseAscii(const novatel_gps_driver::NmeaSentence& sentence) throw(ParseException)
00048 {
00049   // Check the length first -- should be 15 elements long
00050   const size_t MAX_LEN = 15;
00051   const size_t MIN_LEN = 14;
00052   if (sentence.body.size() > MAX_LEN || sentence.body.size() < MIN_LEN)
00053   {
00054     std::stringstream error;
00055     error << "Expected GPGGA length " << MIN_LEN << "  <= length <= "
00056           << MAX_LEN << ", actual length = " << sentence.body.size();
00057     throw ParseException(error.str());
00058   }
00059 
00060   novatel_gps_msgs::GpggaPtr msg = boost::make_shared<novatel_gps_msgs::Gpgga>();
00061 
00062   msg->message_id = sentence.body[0];
00063 
00064   if (sentence.body[1].empty() || sentence.body[1] == "0")
00065   {
00066     msg->utc_seconds = 0;
00067   }
00068   else
00069   {
00070     double utc_float;
00071     if (swri_string_util::ToDouble(sentence.body[1], utc_float))
00072     {
00073       msg->utc_seconds = UtcFloatToSeconds(utc_float);
00074     }
00075     else
00076     {
00077       throw ParseException("Error parsing UTC seconds in GPGGA");
00078     }
00079   }
00080 
00081   bool valid = true;
00082 
00083   double latitude = 0.0;
00084   valid = valid && ParseDouble(sentence.body[2], latitude);
00085   msg->lat = ConvertDmsToDegrees(latitude);
00086 
00087   double longitude = 0.0;
00088   valid = valid && ParseDouble(sentence.body[4], longitude);
00089   msg->lon = ConvertDmsToDegrees(longitude);
00090 
00091   msg->lat_dir = sentence.body[3];
00092   msg->lon_dir = sentence.body[5];
00093   valid = valid && ParseUInt32(sentence.body[6], msg->gps_qual);
00094   valid = valid && ParseUInt32(sentence.body[7], msg->num_sats);
00095 
00096   valid = valid && ParseFloat(sentence.body[8], msg->hdop);
00097   valid = valid && ParseFloat(sentence.body[9], msg->alt);
00098   msg->altitude_units = sentence.body[10];
00099   valid = valid && ParseFloat(sentence.body[11], msg->undulation);
00100   msg->undulation_units = sentence.body[12];
00101   valid = valid && ParseUInt32(sentence.body[13], msg->diff_age);
00102   if (sentence.body.size() == MAX_LEN)
00103   {
00104     msg->station_id = sentence.body[14];
00105   }
00106   else
00107   {
00108     msg->station_id = "";
00109   }
00110 
00111   if (!valid)
00112   {
00113     throw ParseException("GPGGA log was invalid.");
00114   }
00115 
00116   // Check for actual lat and lon data
00117   if (sentence.body[2].empty() || sentence.body[4].empty())
00118   {
00119     // No Lat or Lon data, return false;
00120     was_last_gps_valid_ = false;
00121   }
00122 
00123   was_last_gps_valid_ = true;
00124 
00125   return msg;
00126 }
00127 
00128 bool novatel_gps_driver::GpggaParser::WasLastGpsValid() const
00129 {
00130   return was_last_gps_valid_;
00131 }


novatel_gps_driver
Author(s):
autogenerated on Sun Oct 8 2017 02:40:29