parsing_utils.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <swri_string_util/string_util.h>
31 
32 #include <novatel_gps_driver/parsers/parsing_utils.h>
33 #include <novatel_gps_msgs/NovatelExtendedSolutionStatus.h>
34 #include <novatel_gps_msgs/NovatelSignalMask.h>
35 
36 #include <ros/ros.h>
37 
38 namespace novatel_gps_driver
39 {
40  void GetNovatelReceiverStatusMessage(
41  uint32_t status,
42  novatel_gps_msgs::NovatelReceiverStatus& receiver_status_msg)
43  {
44  receiver_status_msg.original_status_code = status;
45  receiver_status_msg.error_flag = status & 0x00000001;
46  receiver_status_msg.temperature_flag = status & 0x00000002;
47  receiver_status_msg.voltage_supply_flag = status & 0x00000004;
48  receiver_status_msg.antenna_powered = !(status & 0x00000008);
49  receiver_status_msg.antenna_is_open = status & 0x00000020;
50  receiver_status_msg.antenna_is_shorted = status & 0x00000040;
51  receiver_status_msg.cpu_overload_flag = status & 0x00000080;
52  receiver_status_msg.com1_buffer_overrun = status & 0x00000100;
53  receiver_status_msg.com2_buffer_overrun = status & 0x00000200;
54  receiver_status_msg.com3_buffer_overrun = status & 0x00000400;
55  receiver_status_msg.usb_buffer_overrun = status & 0x00000800;
56  receiver_status_msg.rf1_agc_flag = status & 0x00008000;
57  receiver_status_msg.rf2_agc_flag = status & 0x00020000;
58  receiver_status_msg.almanac_flag = status & 0x00040000;
59  receiver_status_msg.position_solution_flag = status & 0x00080000;
60  receiver_status_msg.position_fixed_flag = status & 0x00100000;
61  receiver_status_msg.clock_steering_status_enabled = !(status & 0x00200000);
62  receiver_status_msg.clock_model_flag = status & 0x00400000;
63  receiver_status_msg.oemv_external_oscillator_flag = status & 0x00800000;
64  receiver_status_msg.software_resource_flag = status & 0x01000000;
65  receiver_status_msg.aux3_status_event_flag = status & 0x20000000;
66  receiver_status_msg.aux2_status_event_flag = status & 0x40000000;
67  receiver_status_msg.aux1_status_event_flag = status & 0x80000000;
68  }
69 
70  void GetExtendedSolutionStatusMessage(
71  uint32_t status,
72  novatel_gps_msgs::NovatelExtendedSolutionStatus& msg)
73  {
74  msg.original_mask = status;
75  msg.advance_rtk_verified = 0x01 & status;
76  uint32_t pseudo_iono_correction_mask = (0x0E & status) >> 1;
77  switch(pseudo_iono_correction_mask)
78  {
79  case 0:
80  msg.psuedorange_iono_correction = "Unknown";
81  break;
82  case 1:
83  msg.psuedorange_iono_correction = "Klobuchar Broadcast";
84  break;
85  case 2:
86  msg.psuedorange_iono_correction = "SBAS Broadcast";
87  break;
88  case 3:
89  msg.psuedorange_iono_correction = "Multi-frequency Computed";
90  break;
91  case 4:
92  msg.psuedorange_iono_correction = "PSRDiff Correction";
93  break;
94  case 5:
95  msg.psuedorange_iono_correction = "Novatel Blended Iono Value";
96  break;
97  default:
98  msg.psuedorange_iono_correction = "Unknown";
99  break;
100  }
101  }
102 
103  void GetSignalsUsed(uint32_t mask, novatel_gps_msgs::NovatelSignalMask& msg)
104  {
105  msg.original_mask = mask;
106  msg.gps_L1_used_in_solution = mask & 0x01;
107  msg.gps_L2_used_in_solution = mask & 0x02;
108  msg.gps_L3_used_in_solution = mask & 0x04;
109  msg.glonass_L1_used_in_solution = mask & 0x10;
110  msg.glonass_L2_used_in_solution = mask & 0x20;
111  }
112 
113  double ParseDouble(const uint8_t* buffer)
114  {
115  double x;
116  std::copy(buffer, buffer + sizeof(double), reinterpret_cast<uint8_t*>(&x));
117  return x;
118  }
119 
120  bool ParseDouble(const std::string& string, double& value)
121  {
122  return swri_string_util::ToDouble(string, value) || string.empty();
123  }
124 
125  float ParseFloat(const uint8_t* buffer)
126  {
127  float x;
128  std::copy(buffer, buffer + sizeof(float), reinterpret_cast<uint8_t*>(&x));
129  return x;
130  }
131 
132  bool ParseFloat(const std::string& string, float& value)
133  {
134  return swri_string_util::ToFloat(string, value) || string.empty();
135  }
136 
137  int16_t ParseInt16(const uint8_t* buffer)
138  {
139  int16_t number;
140  std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
141  return number;
142  }
143 
144  bool ParseInt16(const std::string& string, int16_t& value, int32_t base)
145  {
146  value = 0;
147  if (string.empty())
148  {
149  return true;
150  }
151 
152  int32_t tmp;
153  if (swri_string_util::ToInt32(string, tmp, base) &&
154  tmp <= std::numeric_limits<int16_t>::max() &&
155  tmp >= std::numeric_limits<int16_t>::min())
156  {
157  value = static_cast<int16_t>(tmp);
158  return true;
159  }
160 
161  return false;
162  }
163 
164  int32_t ParseInt32(const uint8_t* buffer)
165  {
166  int32_t number;
167  std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&number));
168  return number;
169  }
170 
171  bool ParseInt32(const std::string& string, int32_t& value, int32_t base)
172  {
173  return swri_string_util::ToInt32(string, value, base) || string.empty();
174  }
175 
176  uint32_t ParseUInt32(const uint8_t* buffer)
177  {
178  uint32_t number;
179  std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&number));
180  return number;
181  }
182 
183  bool ParseUInt32(const std::string& string, uint32_t& value, int32_t base)
184  {
185  return swri_string_util::ToUInt32(string, value, base) || string.empty();
186  }
187 
188  bool ParseUInt8(const std::string& string, uint8_t& value, int32_t base)
189  {
190  value = 0;
191  if (string.empty())
192  {
193  return true;
194  }
195 
196  uint32_t tmp;
197  if (swri_string_util::ToUInt32(string, tmp, base) && tmp <= std::numeric_limits<uint8_t>::max())
198  {
199  value = static_cast<uint8_t>(tmp);
200  return true;
201  }
202 
203  return false;
204  }
205 
206  uint16_t ParseUInt16(const uint8_t* buffer)
207  {
208  uint16_t number;
209  std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
210  return number;
211  }
212 
213  bool ParseUInt16(const std::string& string, uint16_t& value, int32_t base)
214  {
215  value = 0;
216  if (string.empty())
217  {
218  return true;
219  }
220 
221  uint32_t tmp;
222  if (swri_string_util::ToUInt32(string, tmp, base) && tmp <= std::numeric_limits<uint16_t>::max())
223  {
224  value = static_cast<uint16_t>(tmp);
225  return true;
226  }
227 
228  return false;
229  }
230 
231  double UtcFloatToSeconds(double utc_float)
232  {
233  uint32_t hours = static_cast<uint32_t>(utc_float) / 10000;
234  uint32_t minutes = (static_cast<uint32_t>(utc_float) - hours * 10000) / 100;
235  double seconds = utc_float -
236  static_cast<double>(hours * 10000 + minutes * 100);
237  seconds += static_cast<double> (hours * 3600 + minutes * 60);
238  return seconds;
239  }
240 
241  double ConvertDmsToDegrees(double dms)
242  {
243  uint32_t whole_degrees = static_cast<uint32_t>(dms) / 100;
244  double minutes = dms - static_cast<double>(whole_degrees * 100);
245  // 60 minutes in a degree
246  double degrees = static_cast<double>(whole_degrees) + minutes / 60.0;
247  return degrees;
248  }
249 
250 
251 }
novatel_gps_driver::GetNovatelReceiverStatusMessage
void GetNovatelReceiverStatusMessage(uint32_t status, novatel_gps_msgs::NovatelReceiverStatus &receiver_status_msg)
Definition: parsing_utils.cpp:40
novatel_gps_driver::ParseUInt16
uint16_t ParseUInt16(const uint8_t *buffer)
Converts a buffer containing 2 bytes into an unsigned 16-bit int.
Definition: parsing_utils.cpp:206
novatel_gps_driver::ParseUInt8
bool ParseUInt8(const std::string &string, uint8_t &value, int32_t base=10)
Parses a string containing an integer number into a uint16_t.
Definition: parsing_utils.cpp:188
parsing_utils.h
swri_string_util::ToInt32
bool ToInt32(const std::string &string, int32_t &value, int32_t base=10)
novatel_gps_driver::ParseDouble
double ParseDouble(const uint8_t *buffer)
Converts a buffer containing 8 bytes into a double.
Definition: parsing_utils.cpp:113
string_util.h
novatel_gps_driver
Definition: binary_header.h:37
novatel_gps_driver::ParseUInt32
uint32_t ParseUInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into an unsigned 32-bit int.
Definition: parsing_utils.cpp:176
novatel_gps_driver::ParseFloat
float ParseFloat(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a float.
Definition: parsing_utils.cpp:125
novatel_gps_driver::GetExtendedSolutionStatusMessage
void GetExtendedSolutionStatusMessage(uint32_t status, novatel_gps_msgs::NovatelExtendedSolutionStatus &msg)
Definition: parsing_utils.cpp:70
novatel_gps_driver::ParseInt32
int32_t ParseInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a signed 32-bit int.
Definition: parsing_utils.cpp:164
novatel_gps_driver::GetSignalsUsed
void GetSignalsUsed(uint32_t mask, novatel_gps_msgs::NovatelSignalMask &msg)
Definition: parsing_utils.cpp:103
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
novatel_gps_driver::UtcFloatToSeconds
double UtcFloatToSeconds(double utc_float)
Definition: parsing_utils.cpp:231
swri_string_util::ToUInt32
bool ToUInt32(const std::string &string, uint32_t &value, int32_t base=10)
novatel_gps_driver::ConvertDmsToDegrees
double ConvertDmsToDegrees(double dms)
Definition: parsing_utils.cpp:241
swri_string_util::ToFloat
bool ToFloat(const std::string &string, float &value)
novatel_gps_driver::ParseInt16
int16_t ParseInt16(const uint8_t *buffer)
Converts a buffer containing 2 bytes into a signed 16-bit int.
Definition: parsing_utils.cpp:137
swri_string_util::ToDouble
bool ToDouble(const std::string &string, double &value)

novatel_gps_driver
Author(s):
autogenerated on Wed Jul 3 2019 19:36:46