trackstat.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 
31 #include <boost/make_shared.hpp>
32 
33 const std::string novatel_gps_driver::TrackstatParser::MESSAGE_NAME = "TRACKSTAT";
34 
36 {
37  return MESSAGE_ID;
38 }
39 
41 {
42  return MESSAGE_NAME;
43 }
44 
45 novatel_gps_msgs::TrackstatPtr
47 {
48  uint32_t num_chans = ParseUInt32(&bin_msg.data_[12]);
49  if (bin_msg.data_.size() != (BINARY_CHANNEL_LENGTH * num_chans) +
51  {
52  std::stringstream error;
53  error << "Unexpected trackstat message size: " << bin_msg.data_.size();
54  throw ParseException(error.str());
55  }
56 
57  uint16_t solution_status = ParseUInt16(&bin_msg.data_[0]);
58  if (solution_status > MAX_SOLUTION_STATUS)
59  {
60  std::stringstream error;
61  error << "Unknown solution status: " << solution_status;
62  throw ParseException(error.str());
63  }
64 
65  novatel_gps_msgs::TrackstatPtr ros_msg = boost::make_shared<novatel_gps_msgs::Trackstat>();
66  ros_msg->solution_status = SOLUTION_STATUSES[solution_status];
67  uint16_t pos_type = ParseUInt16(&bin_msg.data_[4]);
68  if (pos_type > MAX_DATUM)
69  {
70  std::stringstream error;
71  error << "Unknown position type: " << pos_type;
72  throw ParseException(error.str());
73  }
74  ros_msg->position_type = POSITION_TYPES[pos_type];
75  ros_msg->cutoff = ParseFloat(&bin_msg.data_[8]);
76 
77  for (int i = 0; i < num_chans; i++)
78  {
79  size_t chan_offset = BINARY_BODY_LENGTH +
81 
82  novatel_gps_msgs::TrackstatChannel chan;
83  chan.prn = ParseInt16(&bin_msg.data_[chan_offset]);
84  chan.glofreq = ParseInt16(&bin_msg.data_[chan_offset+2]);
85  chan.ch_tr_status = ParseUInt32(&bin_msg.data_[chan_offset+4]);
86  chan.psr = ParseDouble(&bin_msg.data_[chan_offset+8]);
87  chan.doppler = ParseFloat(&bin_msg.data_[chan_offset+16]);
88  chan.c_no = ParseFloat(&bin_msg.data_[chan_offset+20]);
89  chan.locktime = ParseFloat(&bin_msg.data_[chan_offset+24]);
90  chan.psr_res = ParseFloat(&bin_msg.data_[chan_offset+28]);
91  uint32_t reject = ParseUInt32(&bin_msg.data_[chan_offset+32]);
92  switch (reject)
93  {
94  case 0:
95  chan.reject = "GOOD";
96  break;
97  case 1:
98  chan.reject = "BADHEALTH";
99  break;
100  case 2:
101  chan.reject = "OLDEPHEMERIS";
102  break;
103  case 6:
104  chan.reject = "ELEVATIONERROR";
105  break;
106  case 7:
107  chan.reject = "MISCLOSURE";
108  break;
109  case 8:
110  chan.reject = "NODIFFCORR";
111  break;
112  case 9:
113  chan.reject = "NOEPHEMERIS";
114  break;
115  case 10:
116  chan.reject = "INVALIDCODE";
117  break;
118  case 11:
119  chan.reject = "LOCKEDOUT";
120  break;
121  case 12:
122  chan.reject = "LOWPOWER";
123  break;
124  case 13:
125  chan.reject = "OBSL2";
126  break;
127  case 15:
128  chan.reject = "UNKNOWN";
129  break;
130  case 16:
131  chan.reject = "NOIONOCORR";
132  break;
133  case 17:
134  chan.reject = "NOTUSED";
135  break;
136  case 18:
137  chan.reject = "OBSL1";
138  break;
139  case 19:
140  chan.reject = "OBSE1";
141  break;
142  case 20:
143  chan.reject = "OBSL5";
144  break;
145  case 21:
146  chan.reject = "OBSE5";
147  break;
148  case 22:
149  chan.reject = "OBSB2";
150  break;
151  case 23:
152  chan.reject = "OBSB1";
153  break;
154  case 24:
155  chan.reject = "OBSB3";
156  break;
157  case 25:
158  chan.reject = "NOSIGNALMATCH";
159  break;
160  case 26:
161  chan.reject = "SUPPLEMENTARY";
162  break;
163  case 99:
164  chan.reject = "NA";
165  break;
166  case 100:
167  chan.reject = "BAD_INTEGRITY";
168  break;
169  case 101:
170  chan.reject = "LOSSOFLOCK";
171  break;
172  case 102:
173  chan.reject = "NOAMBIGUITY";
174  break;
175  default:
176  {
177  std::stringstream error;
178  error << "Unexpected channel status: " << reject;
179  throw ParseException(error.str());
180  }
181  }
182  chan.psr_weight = ParseFloat(&bin_msg.data_[chan_offset+36]);
183 
184  ros_msg->channels.push_back(chan);
185  }
186 
187  return ros_msg;
188 }
189 
190 novatel_gps_msgs::TrackstatPtr
192 {
193  if (sentence.body.size() < ASCII_BODY_FIELDS)
194  {
195  std::stringstream error;
196  error << "Unexpected number of body fields in TRACKSTAT log: " << sentence.body.size();
197  throw ParseException(error.str());
198  }
199 
200  uint32_t n_channels = 0;
201  ParseUInt32(sentence.body[3], n_channels, 10);
202 
203  if (sentence.body.size() != ASCII_BODY_FIELDS + n_channels * ASCII_CHANNEL_FIELDS)
204  {
205  std::stringstream error;
206  error << "Size of TRACKSTAT log did not match expected size.";
207  throw ParseException(error.str());
208  }
209 
210  bool valid = true;
211  novatel_gps_msgs::TrackstatPtr msg = boost::make_shared<novatel_gps_msgs::Trackstat>();
212  msg->solution_status = sentence.body[0];
213  msg->position_type = sentence.body[1];
214  valid &= ParseFloat(sentence.body[2], msg->cutoff);
215 
216  msg->channels.resize(n_channels);
217  for (size_t i = 0; i < static_cast<size_t>(n_channels); ++i)
218  {
219  size_t offset = 4 + i * ASCII_CHANNEL_FIELDS;
220  novatel_gps_msgs::TrackstatChannel& channel = msg->channels[i];
221  valid &= ParseInt16(sentence.body[offset], channel.prn);
222  valid &= ParseInt16(sentence.body[offset+1], channel.glofreq);
223  valid &= ParseUInt32(sentence.body[offset+2], channel.ch_tr_status, 16);
224  valid &= ParseDouble(sentence.body[offset+3], channel.psr);
225  valid &= ParseFloat(sentence.body[offset+4], channel.doppler);
226  valid &= ParseFloat(sentence.body[offset+5], channel.c_no);
227  valid &= ParseFloat(sentence.body[offset+6], channel.locktime);
228  valid &= ParseFloat(sentence.body[offset+7], channel.psr_res);
229  channel.reject = sentence.body[offset+8];
230  valid &= ParseFloat(sentence.body[offset+9], channel.psr_weight);
231  }
232 
233  if (!valid)
234  {
235  std::stringstream error;
236  error << "Error parsing TRACKSTAT log.";
237  throw ParseException(error.str());
238  }
239  return msg;
240 }
msg
uint16_t ParseUInt16(const uint8_t *buffer)
Converts a buffer containing 2 bytes into an unsigned 16-bit int.
static constexpr uint16_t MESSAGE_ID
Definition: trackstat.h:49
novatel_gps_msgs::TrackstatPtr ParseAscii(const NovatelSentence &sentence) override
Converts sentence into a ROS message pointer and returns it.
Definition: trackstat.cpp:191
double ParseDouble(const uint8_t *buffer)
Converts a buffer containing 8 bytes into a double.
const std::string POSITION_TYPES[]
Definition: parsing_utils.h:57
uint32_t ParseUInt32(const uint8_t *buffer)
Converts a buffer containing 4 bytes into an unsigned 32-bit int.
novatel_gps_msgs::TrackstatPtr ParseBinary(const BinaryMessage &bin_msg) override
Converts bin_msg into a ROS message pointer and returns it.
Definition: trackstat.cpp:46
static constexpr size_t ASCII_BODY_FIELDS
Definition: trackstat.h:50
float ParseFloat(const uint8_t *buffer)
Converts a buffer containing 4 bytes into a float.
uint32_t GetMessageId() const override
Definition: trackstat.cpp:35
const std::string SOLUTION_STATUSES[]
Definition: parsing_utils.h:50
static constexpr size_t BINARY_CHANNEL_LENGTH
Definition: trackstat.h:52
const size_t MAX_SOLUTION_STATUS
Definition: parsing_utils.h:49
static const std::string MESSAGE_NAME
Definition: trackstat.h:54
const size_t MAX_DATUM
Definition: parsing_utils.h:75
static constexpr size_t ASCII_CHANNEL_FIELDS
Definition: trackstat.h:51
static constexpr size_t BINARY_BODY_LENGTH
Definition: trackstat.h:53
int16_t ParseInt16(const uint8_t *buffer)
Converts a buffer containing 2 bytes into a signed 16-bit int.
const std::string GetMessageName() const override
Definition: trackstat.cpp:40


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