trackstat.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/trackstat.h>
00031 #include <boost/make_shared.hpp>
00032 
00033 const std::string novatel_gps_driver::TrackstatParser::MESSAGE_NAME = "TRACKSTAT";
00034 
00035 uint32_t novatel_gps_driver::TrackstatParser::GetMessageId() const
00036 {
00037   return MESSAGE_ID;
00038 }
00039 
00040 const std::string novatel_gps_driver::TrackstatParser::GetMessageName() const
00041 {
00042   return MESSAGE_NAME;
00043 }
00044 
00045 novatel_gps_msgs::TrackstatPtr
00046 novatel_gps_driver::TrackstatParser::ParseBinary(const novatel_gps_driver::BinaryMessage& bin_msg) throw(ParseException)
00047 {
00048   uint32_t num_chans = ParseUInt32(&bin_msg.data_[12]);
00049   if (bin_msg.data_.size() != (BINARY_CHANNEL_LENGTH * num_chans) +
00050                               BINARY_BODY_LENGTH)
00051   {
00052     std::stringstream error;
00053     error << "Unexpected trackstat message size: " << bin_msg.data_.size();
00054     throw ParseException(error.str());
00055   }
00056 
00057   uint16_t solution_status = ParseUInt16(&bin_msg.data_[0]);
00058   if (solution_status > MAX_SOLUTION_STATUS)
00059   {
00060     std::stringstream error;
00061     error << "Unknown solution status: " << solution_status;
00062     throw ParseException(error.str());
00063   }
00064 
00065   novatel_gps_msgs::TrackstatPtr ros_msg = boost::make_shared<novatel_gps_msgs::Trackstat>();
00066   ros_msg->solution_status = SOLUTION_STATUSES[solution_status];
00067   uint16_t pos_type = ParseUInt16(&bin_msg.data_[4]);
00068   if (pos_type > MAX_DATUM)
00069   {
00070     std::stringstream error;
00071     error << "Unknown position type: " << pos_type;
00072     throw ParseException(error.str());
00073   }
00074   ros_msg->position_type = POSITION_TYPES[pos_type];
00075   ros_msg->cutoff = ParseFloat(&bin_msg.data_[8]);
00076 
00077   for (int i = 0; i < num_chans; i++)
00078   {
00079     size_t chan_offset = BINARY_BODY_LENGTH +
00080                          i * BINARY_CHANNEL_LENGTH;
00081 
00082     novatel_gps_msgs::TrackstatChannel chan;
00083     chan.prn = ParseInt16(&bin_msg.data_[chan_offset]);
00084     chan.glofreq = ParseInt16(&bin_msg.data_[chan_offset+2]);
00085     chan.ch_tr_status = ParseUInt32(&bin_msg.data_[chan_offset+4]);
00086     chan.psr = ParseDouble(&bin_msg.data_[chan_offset+8]);
00087     chan.doppler = ParseFloat(&bin_msg.data_[chan_offset+16]);
00088     chan.c_no = ParseFloat(&bin_msg.data_[chan_offset+20]);
00089     chan.locktime = ParseFloat(&bin_msg.data_[chan_offset+24]);
00090     chan.psr_res = ParseFloat(&bin_msg.data_[chan_offset+28]);
00091     uint32_t reject = ParseUInt32(&bin_msg.data_[chan_offset+32]);
00092     switch (reject)
00093     {
00094       case 0:
00095         chan.reject = "GOOD";
00096         break;
00097       case 1:
00098         chan.reject = "BADHEALTH";
00099         break;
00100       case 2:
00101         chan.reject = "OLDEPHEMERIS";
00102         break;
00103       case 6:
00104         chan.reject = "ELEVATIONERROR";
00105         break;
00106       case 7:
00107         chan.reject = "MISCLOSURE";
00108         break;
00109       case 8:
00110         chan.reject = "NODIFFCORR";
00111         break;
00112       case 9:
00113         chan.reject = "NOEPHEMERIS";
00114         break;
00115       case 10:
00116         chan.reject = "INVALIDCODE";
00117         break;
00118       case 11:
00119         chan.reject = "LOCKEDOUT";
00120         break;
00121       case 12:
00122         chan.reject = "LOWPOWER";
00123         break;
00124       case 13:
00125         chan.reject = "OBSL2";
00126         break;
00127       case 15:
00128         chan.reject = "UNKNOWN";
00129         break;
00130       case 16:
00131         chan.reject = "NOIONOCORR";
00132         break;
00133       case 17:
00134         chan.reject = "NOTUSED";
00135         break;
00136       case 18:
00137         chan.reject = "OBSL1";
00138         break;
00139       case 19:
00140         chan.reject = "OBSE1";
00141         break;
00142       case 20:
00143         chan.reject = "OBSL5";
00144         break;
00145       case 21:
00146         chan.reject = "OBSE5";
00147         break;
00148       case 22:
00149         chan.reject = "OBSB2";
00150         break;
00151       case 23:
00152         chan.reject = "OBSB1";
00153         break;
00154       case 24:
00155         chan.reject = "OBSB3";
00156         break;
00157       case 25:
00158         chan.reject = "NOSIGNALMATCH";
00159         break;
00160       case 26:
00161         chan.reject = "SUPPLEMENTARY";
00162         break;
00163       case 99:
00164         chan.reject = "NA";
00165         break;
00166       case 100:
00167         chan.reject = "BAD_INTEGRITY";
00168         break;
00169       case 101:
00170         chan.reject = "LOSSOFLOCK";
00171         break;
00172       case 102:
00173         chan.reject = "NOAMBIGUITY";
00174         break;
00175       default:
00176       {
00177         std::stringstream error;
00178         error << "Unexpected channel status: " << reject;
00179         throw ParseException(error.str());
00180       }
00181     }
00182     chan.psr_weight = ParseFloat(&bin_msg.data_[chan_offset+36]);
00183 
00184     ros_msg->channels.push_back(chan);
00185   }
00186 
00187   return ros_msg;
00188 }
00189 
00190 novatel_gps_msgs::TrackstatPtr
00191 novatel_gps_driver::TrackstatParser::ParseAscii(const novatel_gps_driver::NovatelSentence& sentence) throw(ParseException)
00192 {
00193   if (sentence.body.size() < ASCII_BODY_FIELDS)
00194   {
00195     std::stringstream error;
00196     error << "Unexpected number of body fields in TRACKSTAT log: " << sentence.body.size();
00197     throw ParseException(error.str());
00198   }
00199 
00200   uint32_t n_channels = 0;
00201   ParseUInt32(sentence.body[3], n_channels, 10);
00202 
00203   if (sentence.body.size() != ASCII_BODY_FIELDS + n_channels * ASCII_CHANNEL_FIELDS)
00204   {
00205     std::stringstream error;
00206     error << "Size of TRACKSTAT log did not match expected size.";
00207     throw ParseException(error.str());
00208   }
00209 
00210   bool valid = true;
00211   novatel_gps_msgs::TrackstatPtr msg = boost::make_shared<novatel_gps_msgs::Trackstat>();
00212   msg->solution_status = sentence.body[0];
00213   msg->position_type = sentence.body[1];
00214   valid &= ParseFloat(sentence.body[2], msg->cutoff);
00215 
00216   msg->channels.resize(n_channels);
00217   for (size_t i = 0; i < static_cast<size_t>(n_channels); ++i)
00218   {
00219     size_t offset = 4 + i * ASCII_CHANNEL_FIELDS;
00220     novatel_gps_msgs::TrackstatChannel& channel = msg->channels[i];
00221     valid &= ParseInt16(sentence.body[offset], channel.prn);
00222     valid &= ParseInt16(sentence.body[offset+1], channel.glofreq);
00223     valid &= ParseUInt32(sentence.body[offset+2], channel.ch_tr_status, 16);
00224     valid &= ParseDouble(sentence.body[offset+3], channel.psr);
00225     valid &= ParseFloat(sentence.body[offset+4], channel.doppler);
00226     valid &= ParseFloat(sentence.body[offset+5], channel.c_no);
00227     valid &= ParseFloat(sentence.body[offset+6], channel.locktime);
00228     valid &= ParseFloat(sentence.body[offset+7], channel.psr_res);
00229     channel.reject = sentence.body[offset+8];
00230     valid &= ParseFloat(sentence.body[offset+9], channel.psr_weight);
00231   }
00232 
00233   if (!valid)
00234   {
00235     std::stringstream error;
00236     error << "Error parsing TRACKSTAT log.";
00237     throw ParseException(error.str());
00238   }
00239   return msg;
00240 }


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