header.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/header.h>
00031 
00032 #include <ros/ros.h>
00033 
00034 uint32_t novatel_gps_driver::HeaderParser::GetMessageId() const
00035 {
00036   return 0;
00037 }
00038 
00039 const std::string novatel_gps_driver::HeaderParser::GetMessageName() const
00040 {
00041   return "HEADER";
00042 }
00043 
00044 novatel_gps_msgs::NovatelMessageHeader novatel_gps_driver::HeaderParser::ParseBinary(
00045     const novatel_gps_driver::BinaryMessage& bin_msg) throw(ParseException)
00046 {
00047   // No point in checking whether the port identifier is valid here, because
00048   // the variable's range is 0-255 and this array has 256 values in it.
00049   novatel_gps_msgs::NovatelMessageHeader msg;
00050   msg.port = PORT_IDENTIFIERS[bin_msg.header_.port_address_];
00051   msg.sequence_num = bin_msg.header_.sequence_;
00052   msg.percent_idle_time = bin_msg.header_.idle_time_;
00053   switch (bin_msg.header_.time_status_)
00054   {
00055     case 20:
00056       msg.gps_time_status = "UNKNOWN";
00057       break;
00058     case 60:
00059       msg.gps_time_status = "APPROXIMATE";
00060       break;
00061     case 80:
00062       msg.gps_time_status = "COARSEADJUSTING";
00063       break;
00064     case 100:
00065       msg.gps_time_status = "COARSE";
00066       break;
00067     case 120:
00068       msg.gps_time_status = "COARSESTEERING";
00069       break;
00070     case 130:
00071       msg.gps_time_status = "FREEWHEELING";
00072       break;
00073     case 140:
00074       msg.gps_time_status = "FINEADJUSTING";
00075       break;
00076     case 160:
00077       msg.gps_time_status = "FINE";
00078       break;
00079     case 170:
00080       msg.gps_time_status = "FINEBACKUPSTEERING";
00081       break;
00082     case 180:
00083       msg.gps_time_status = "FINESTEERING";
00084       break;
00085     case 200:
00086       msg.gps_time_status = "SATTIME";
00087       break;
00088     default:
00089     {
00090       std::stringstream error;
00091       error << "Unknown GPS time status: " << bin_msg.header_.time_status_;
00092       throw ParseException(error.str());
00093     }
00094   }
00095   msg.gps_week_num = bin_msg.header_.week_;
00096   msg.gps_seconds = static_cast<double>(bin_msg.header_.gps_ms_) / 1000.0;
00097   GetNovatelReceiverStatusMessage(bin_msg.header_.receiver_status_, msg.receiver_status);
00098   msg.receiver_software_version = bin_msg.header_.receiver_sw_version_;
00099 
00100   return msg;
00101 }
00102 
00103 novatel_gps_msgs::NovatelMessageHeader novatel_gps_driver::HeaderParser::ParseAscii(
00104     const novatel_gps_driver::NovatelSentence& sentence) throw(ParseException)
00105 {
00106   if (sentence.header.size() != NOVATEL_MESSAGE_HEADER_LENGTH)
00107   {
00108     std::stringstream error;
00109     error <<"Novatel message header size wrong: expected "
00110           << NOVATEL_MESSAGE_HEADER_LENGTH
00111           << ", got %zu"<< sentence.header.size();
00112     throw ParseException(error.str());
00113   }
00114 
00115   bool valid = true;
00116 
00117   novatel_gps_msgs::NovatelMessageHeader msg;
00118   msg.message_name = sentence.header[0];
00119   msg.port = sentence.header[1];
00120   valid = valid && ParseUInt32(sentence.header[2], msg.sequence_num);
00121   valid = valid && ParseFloat(sentence.header[3], msg.percent_idle_time);
00122   msg.gps_time_status = sentence.header[4];
00123   valid = valid && ParseUInt32(sentence.header[5], msg.gps_week_num);
00124   valid = valid && ParseDouble(sentence.header[6], msg.gps_seconds);
00125 
00126   uint32_t receiver_status_code = 0;
00127   valid = valid && ParseUInt32(sentence.header[7], receiver_status_code, 16);
00128   GetNovatelReceiverStatusMessage(receiver_status_code, msg.receiver_status);
00129 
00130   valid = valid && ParseUInt32(sentence.header[9], msg.receiver_software_version);
00131 
00132   if (!valid)
00133   {
00134     throw ParseException("Header was invalid.");
00135   }
00136   return msg;
00137 }


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