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 namespace novatel_gps_driver
37 {
38  void GetNovatelReceiverStatusMessage(
39  uint32_t status,
40  novatel_gps_msgs::NovatelReceiverStatus& receiver_status_msg)
41  {
42  receiver_status_msg.original_status_code = status;
43  receiver_status_msg.error_flag = (status & 0x00000001u) != 0;
44  receiver_status_msg.temperature_flag = (status & 0x00000002u) != 0;
45  receiver_status_msg.voltage_supply_flag = (status & 0x00000004u) != 0;
46  receiver_status_msg.antenna_powered = (status & 0x00000008u) == 0;
47  receiver_status_msg.antenna_is_open = (status & 0x00000020u) != 0;
48  receiver_status_msg.antenna_is_shorted = (status & 0x00000040u) != 0;
49  receiver_status_msg.cpu_overload_flag = (status & 0x00000080u) != 0;
50  receiver_status_msg.com1_buffer_overrun = (status & 0x00000100u) != 0;
51  receiver_status_msg.com2_buffer_overrun = (status & 0x00000200u) != 0;
52  receiver_status_msg.com3_buffer_overrun = (status & 0x00000400u) != 0;
53  receiver_status_msg.usb_buffer_overrun = (status & 0x00000800u) != 0;
54  receiver_status_msg.rf1_agc_flag = (status & 0x00008000u) != 0;
55  receiver_status_msg.rf2_agc_flag = (status & 0x00020000u) != 0;
56  receiver_status_msg.almanac_flag = (status & 0x00040000u) != 0;
57  receiver_status_msg.position_solution_flag = (status & 0x00080000u) != 0;
58  receiver_status_msg.position_fixed_flag = (status & 0x00100000u) != 0;
59  receiver_status_msg.clock_steering_status_enabled = (status & 0x00200000u) == 0;
60  receiver_status_msg.clock_model_flag = (status & 0x00400000u) != 0;
61  receiver_status_msg.oemv_external_oscillator_flag = (status & 0x00800000u) != 0;
62  receiver_status_msg.software_resource_flag = (status & 0x01000000u) != 0;
63  receiver_status_msg.aux3_status_event_flag = (status & 0x20000000u) != 0;
64  receiver_status_msg.aux2_status_event_flag = (status & 0x40000000u) != 0;
65  receiver_status_msg.aux1_status_event_flag = (status & 0x80000000u) != 0;
66  }
67 
68  void GetExtendedSolutionStatusMessage(
69  uint32_t status,
70  novatel_gps_msgs::NovatelExtendedSolutionStatus& msg)
71  {
72  msg.original_mask = status;
73  msg.advance_rtk_verified = 0x01u & status;
74  uint32_t pseudo_iono_correction_mask = (0x0Eu & status) >> 1u;
75  switch(pseudo_iono_correction_mask)
76  {
77  case 0:
78  msg.psuedorange_iono_correction = "Unknown";
79  break;
80  case 1:
81  msg.psuedorange_iono_correction = "Klobuchar Broadcast";
82  break;
83  case 2:
84  msg.psuedorange_iono_correction = "SBAS Broadcast";
85  break;
86  case 3:
87  msg.psuedorange_iono_correction = "Multi-frequency Computed";
88  break;
89  case 4:
90  msg.psuedorange_iono_correction = "PSRDiff Correction";
91  break;
92  case 5:
93  msg.psuedorange_iono_correction = "Novatel Blended Iono Value";
94  break;
95  default:
96  msg.psuedorange_iono_correction = "Unknown";
97  break;
98  }
99  }
100 
101  void GetSignalsUsed(uint32_t mask, novatel_gps_msgs::NovatelSignalMask& msg)
102  {
103  msg.original_mask = mask;
104  msg.gps_L1_used_in_solution = mask & 0x01u;
105  msg.gps_L2_used_in_solution = mask & 0x02u;
106  msg.gps_L3_used_in_solution = mask & 0x04u;
107  msg.glonass_L1_used_in_solution = mask & 0x10u;
108  msg.glonass_L2_used_in_solution = mask & 0x20u;
109  }
110 
111  double ParseDouble(const uint8_t* buffer)
112  {
113  double x;
114  std::copy(buffer, buffer + sizeof(double), reinterpret_cast<uint8_t*>(&x));
115  return x;
116  }
117 
118  bool ParseDouble(const std::string& string, double& value)
119  {
120  return swri_string_util::ToDouble(string, value) || string.empty();
121  }
122 
123  float ParseFloat(const uint8_t* buffer)
124  {
125  float x;
126  std::copy(buffer, buffer + sizeof(float), reinterpret_cast<uint8_t*>(&x));
127  return x;
128  }
129 
130  bool ParseFloat(const std::string& string, float& value)
131  {
132  return swri_string_util::ToFloat(string, value) || string.empty();
133  }
134 
135  int16_t ParseInt16(const uint8_t* buffer)
136  {
137  int16_t number;
138  std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
139  return number;
140  }
141 
142  bool ParseInt16(const std::string& string, int16_t& value, int32_t base)
143  {
144  value = 0;
145  if (string.empty())
146  {
147  return true;
148  }
149 
150  int32_t tmp;
151  if (swri_string_util::ToInt32(string, tmp, base) &&
152  tmp <= std::numeric_limits<int16_t>::max() &&
153  tmp >= std::numeric_limits<int16_t>::min())
154  {
155  value = static_cast<int16_t>(tmp);
156  return true;
157  }
158 
159  return false;
160  }
161 
162  int32_t ParseInt32(const uint8_t* buffer)
163  {
164  int32_t number;
165  std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&number));
166  return number;
167  }
168 
169  bool ParseInt32(const std::string& string, int32_t& value, int32_t base)
170  {
171  return swri_string_util::ToInt32(string, value, base) || string.empty();
172  }
173 
174  uint32_t ParseUInt32(const uint8_t* buffer)
175  {
176  uint32_t number;
177  std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&number));
178  return number;
179  }
180 
181  bool ParseUInt32(const std::string& string, uint32_t& value, int32_t base)
182  {
183  return swri_string_util::ToUInt32(string, value, base) || string.empty();
184  }
185 
186  bool ParseUInt8(const std::string& string, uint8_t& value, int32_t base)
187  {
188  value = 0;
189  if (string.empty())
190  {
191  return true;
192  }
193 
194  uint32_t tmp;
195  if (swri_string_util::ToUInt32(string, tmp, base) && tmp <= std::numeric_limits<uint8_t>::max())
196  {
197  value = static_cast<uint8_t>(tmp);
198  return true;
199  }
200 
201  return false;
202  }
203 
204  uint16_t ParseUInt16(const uint8_t* buffer)
205  {
206  uint16_t number;
207  std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
208  return number;
209  }
210 
211  bool ParseUInt16(const std::string& string, uint16_t& value, int32_t base)
212  {
213  value = 0;
214  if (string.empty())
215  {
216  return true;
217  }
218 
219  uint32_t tmp;
220  if (swri_string_util::ToUInt32(string, tmp, base) && tmp <= std::numeric_limits<uint16_t>::max())
221  {
222  value = static_cast<uint16_t>(tmp);
223  return true;
224  }
225 
226  return false;
227  }
228 
229  double UtcFloatToSeconds(double utc_float)
230  {
231  uint32_t hours = static_cast<uint32_t>(utc_float) / 10000;
232  uint32_t minutes = (static_cast<uint32_t>(utc_float) - hours * 10000) / 100;
233  double seconds = utc_float -
234  static_cast<double>(hours * 10000 + minutes * 100);
235  seconds += static_cast<double> (hours * 3600 + minutes * 60);
236  return seconds;
237  }
238 
239  double ConvertDmsToDegrees(double dms)
240  {
241  uint32_t whole_degrees = static_cast<uint32_t>(dms) / 100;
242  double minutes = dms - static_cast<double>(whole_degrees * 100);
243  // 60 minutes in a degree
244  double degrees = static_cast<double>(whole_degrees) + minutes / 60.0;
245  return degrees;
246  }
247 }
novatel_gps_driver::GetNovatelReceiverStatusMessage
void GetNovatelReceiverStatusMessage(uint32_t status, novatel_gps_msgs::NovatelReceiverStatus &receiver_status_msg)
Definition: parsing_utils.cpp:38
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:204
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:186
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:111
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:174
novatel_gps_driver::ParseFloat
float ParseFloat(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a float.
Definition: parsing_utils.cpp:123
novatel_gps_driver::GetExtendedSolutionStatusMessage
void GetExtendedSolutionStatusMessage(uint32_t status, novatel_gps_msgs::NovatelExtendedSolutionStatus &msg)
Definition: parsing_utils.cpp:68
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:162
novatel_gps_driver::GetSignalsUsed
void GetSignalsUsed(uint32_t mask, novatel_gps_msgs::NovatelSignalMask &msg)
Definition: parsing_utils.cpp:101
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
novatel_gps_driver::UtcFloatToSeconds
double UtcFloatToSeconds(double utc_float)
Definition: parsing_utils.cpp:229
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:239
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:135
swri_string_util::ToDouble
bool ToDouble(const std::string &string, double &value)

novatel_gps_driver
Author(s):
autogenerated on Thu Jul 16 2020 03:17:30