parsing_utils.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 <swri_string_util/string_util.h>
00031 
00032 #include <novatel_gps_driver/parsers/parsing_utils.h>
00033 #include <novatel_gps_msgs/NovatelExtendedSolutionStatus.h>
00034 #include <novatel_gps_msgs/NovatelSignalMask.h>
00035 
00036 #include <ros/ros.h>
00037 
00038 namespace novatel_gps_driver
00039 {
00040   void GetNovatelReceiverStatusMessage(
00041       uint32_t status,
00042       novatel_gps_msgs::NovatelReceiverStatus& receiver_status_msg)
00043   {
00044     receiver_status_msg.original_status_code = status;
00045     receiver_status_msg.error_flag = status & 0x00000001;
00046     receiver_status_msg.temperature_flag = status & 0x00000002;
00047     receiver_status_msg.voltage_supply_flag = status & 0x00000004;
00048     receiver_status_msg.antenna_powered = !(status & 0x00000008);
00049     receiver_status_msg.antenna_is_open = status & 0x00000020;
00050     receiver_status_msg.antenna_is_shorted = status & 0x00000040;
00051     receiver_status_msg.cpu_overload_flag = status & 0x00000080;
00052     receiver_status_msg.com1_buffer_overrun = status & 0x00000100;
00053     receiver_status_msg.com2_buffer_overrun = status & 0x00000200;
00054     receiver_status_msg.com3_buffer_overrun = status & 0x00000400;
00055     receiver_status_msg.usb_buffer_overrun = status & 0x00000800;
00056     receiver_status_msg.rf1_agc_flag = status & 0x00008000;
00057     receiver_status_msg.rf2_agc_flag = status & 0x00020000;
00058     receiver_status_msg.almanac_flag = status & 0x00040000;
00059     receiver_status_msg.position_solution_flag = status & 0x00080000;
00060     receiver_status_msg.position_fixed_flag = status & 0x00100000;
00061     receiver_status_msg.clock_steering_status_enabled = !(status & 0x00200000);
00062     receiver_status_msg.clock_model_flag = status & 0x00400000;
00063     receiver_status_msg.oemv_external_oscillator_flag = status & 0x00800000;
00064     receiver_status_msg.software_resource_flag = status & 0x01000000;
00065     receiver_status_msg.aux3_status_event_flag = status & 0x20000000;
00066     receiver_status_msg.aux2_status_event_flag = status & 0x40000000;
00067     receiver_status_msg.aux1_status_event_flag = status & 0x80000000;
00068   }
00069 
00070   void GetExtendedSolutionStatusMessage(
00071       uint32_t status,
00072       novatel_gps_msgs::NovatelExtendedSolutionStatus& msg)
00073   {
00074     msg.original_mask = status;
00075     msg.advance_rtk_verified = 0x01 & status;
00076     uint32_t pseudo_iono_correction_mask = (0x0E & status) >> 1;
00077     switch(pseudo_iono_correction_mask)
00078     {
00079       case 0:
00080         msg.psuedorange_iono_correction = "Unknown";
00081         break;
00082       case 1:
00083         msg.psuedorange_iono_correction = "Klobuchar Broadcast";
00084         break;
00085       case 2:
00086         msg.psuedorange_iono_correction = "SBAS Broadcast";
00087         break;
00088       case 3:
00089         msg.psuedorange_iono_correction = "Multi-frequency Computed";
00090         break;
00091       case 4:
00092         msg.psuedorange_iono_correction = "PSRDiff Correction";
00093         break;
00094       case 5:
00095         msg.psuedorange_iono_correction = "Novatel Blended Iono Value";
00096         break;
00097       default:
00098         msg.psuedorange_iono_correction = "Unknown";
00099         break;
00100     }
00101   }
00102 
00103   void GetSignalsUsed(uint32_t mask, novatel_gps_msgs::NovatelSignalMask& msg)
00104   {
00105     msg.original_mask = mask;
00106     msg.gps_L1_used_in_solution = mask & 0x01;
00107     msg.gps_L2_used_in_solution = mask & 0x02;
00108     msg.gps_L3_used_in_solution = mask & 0x04;
00109     msg.glonass_L1_used_in_solution = mask & 0x10;
00110     msg.glonass_L2_used_in_solution = mask & 0x20;
00111   }
00112 
00113   double ParseDouble(const uint8_t* buffer)
00114   {
00115     double x;
00116     std::copy(buffer, buffer + sizeof(double), reinterpret_cast<uint8_t*>(&x));
00117     return x;
00118   }
00119 
00120   bool ParseDouble(const std::string& string, double& value)
00121   {
00122     return swri_string_util::ToDouble(string, value) || string.empty();
00123   }
00124 
00125   float ParseFloat(const uint8_t* buffer)
00126   {
00127     float x;
00128     std::copy(buffer, buffer + sizeof(float), reinterpret_cast<uint8_t*>(&x));
00129     return x;
00130   }
00131 
00132   bool ParseFloat(const std::string& string, float& value)
00133   {
00134     return swri_string_util::ToFloat(string, value) || string.empty();
00135   }
00136 
00137   int16_t ParseInt16(const uint8_t* buffer)
00138   {
00139     int16_t number;
00140     std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
00141     return number;
00142   }
00143 
00144   bool ParseInt16(const std::string& string, int16_t& value, int32_t base)
00145   {
00146     value = 0;
00147     if (string.empty())
00148     {
00149       return true;
00150     }
00151 
00152     int32_t tmp;
00153     if (swri_string_util::ToInt32(string, tmp, base) &&
00154         tmp <= std::numeric_limits<int16_t>::max() &&
00155         tmp >= std::numeric_limits<int16_t>::min())
00156     {
00157       value = static_cast<int16_t>(tmp);
00158       return true;
00159     }
00160 
00161     return false;
00162   }
00163 
00164   int32_t ParseInt32(const uint8_t* buffer)
00165   {
00166     int32_t number;
00167     std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&number));
00168     return number;
00169   }
00170 
00171   bool ParseInt32(const std::string& string, int32_t& value, int32_t base)
00172   {
00173     return swri_string_util::ToInt32(string, value, base) || string.empty();
00174   }
00175 
00176   uint32_t ParseUInt32(const uint8_t* buffer)
00177   {
00178     uint32_t number;
00179     std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&number));
00180     return number;
00181   }
00182 
00183   bool ParseUInt32(const std::string& string, uint32_t& value, int32_t base)
00184   {
00185     return swri_string_util::ToUInt32(string, value, base) || string.empty();
00186   }
00187 
00188   bool ParseUInt8(const std::string& string, uint8_t& value, int32_t base)
00189   {
00190     value = 0;
00191     if (string.empty())
00192     {
00193       return true;
00194     }
00195 
00196     uint32_t tmp;
00197     if (swri_string_util::ToUInt32(string, tmp, base) && tmp <= std::numeric_limits<uint8_t>::max())
00198     {
00199       value = static_cast<uint8_t>(tmp);
00200       return true;
00201     }
00202 
00203     return false;
00204   }
00205 
00206   uint16_t ParseUInt16(const uint8_t* buffer)
00207   {
00208     uint16_t number;
00209     std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
00210     return number;
00211   }
00212 
00213   bool ParseUInt16(const std::string& string, uint16_t& value, int32_t base)
00214   {
00215     value = 0;
00216     if (string.empty())
00217     {
00218       return true;
00219     }
00220 
00221     uint32_t tmp;
00222     if (swri_string_util::ToUInt32(string, tmp, base) && tmp <= std::numeric_limits<uint16_t>::max())
00223     {
00224       value = static_cast<uint16_t>(tmp);
00225       return true;
00226     }
00227 
00228     return false;
00229   }
00230 
00231   double UtcFloatToSeconds(double utc_float)
00232   {
00233     uint32_t hours = static_cast<uint32_t>(utc_float) / 10000;
00234     uint32_t minutes = (static_cast<uint32_t>(utc_float) - hours * 10000) / 100;
00235     double seconds = utc_float -
00236                      static_cast<double>(hours * 10000 + minutes * 100);
00237     seconds += static_cast<double> (hours * 3600 + minutes * 60);
00238     return seconds;
00239   }
00240 
00241   double ConvertDmsToDegrees(double dms)
00242   {
00243     uint32_t whole_degrees = static_cast<uint32_t>(dms) / 100;
00244     double minutes = dms - static_cast<double>(whole_degrees * 100);
00245     // 60 minutes in a degree
00246     double degrees = static_cast<double>(whole_degrees) + minutes / 60.0;
00247     return degrees;
00248   }
00249 
00250 
00251 }


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