gpgsv.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
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 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. 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 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
32 
39 const std::string GpgsvParser::MESSAGE_ID = "$GPGSV";
40 
41 const std::string GpgsvParser::getMessageID() const
42 {
44 }
45 
54  const std::string& frame_id, bool /*use_gnss_time*/,
55  Timestamp /*time_obj*/) noexcept(false)
56 {
57 
58  const size_t MIN_LENGTH = 4;
59  // Checking that the message is at least as long as a GPGSV with no satellites
60  if (sentence.get_body().size() < MIN_LENGTH)
61  {
62  std::stringstream error;
63  error << "Expected GSV length is at least " << MIN_LENGTH
64  << ". The actual length is " << sentence.get_body().size();
65  throw ParseException(error.str());
66  }
67  GpgsvMsg msg;
68  msg.header.frame_id = frame_id;
69  msg.message_id = sentence.get_body()[0];
70  if (!parsing_utilities::parseUInt8(sentence.get_body()[1], msg.n_msgs))
71  {
72  throw ParseException("Error parsing n_msgs in GSV.");
73  }
74  if (msg.n_msgs >
75  9) // Checking that the number of messages is smaller or equal to 9
76  {
77  std::stringstream error;
78  error << "n_msgs in GSV is too large: " << msg.n_msgs << ".";
79  throw ParseException(error.str());
80  }
81 
82  if (!parsing_utilities::parseUInt8(sentence.get_body()[2], msg.msg_number))
83  {
84  throw ParseException("Error parsing msg_number in GSV.");
85  }
86  if (msg.msg_number >
87  msg.n_msgs) // Checking that this message is within the sequence range
88  {
89  std::stringstream error;
90  error << "msg_number in GSV is larger than n_msgs: " << msg.msg_number
91  << " > " << msg.n_msgs << ".";
92  throw ParseException(error.str());
93  }
94  if (!parsing_utilities::parseUInt8(sentence.get_body()[3], msg.n_satellites))
95  {
96  throw ParseException("Error parsing n_satellites in GSV.");
97  }
98  // Figuring out how many satellites should be described in this sentence
99  size_t n_sats_in_sentence = 4;
100  if (msg.msg_number == msg.n_msgs)
101  {
102  n_sats_in_sentence = msg.n_satellites % static_cast<uint8_t>(4);
103  if (msg.n_satellites % static_cast<uint8_t>(4) == 0)
104  {
105  n_sats_in_sentence = 4;
106  }
107  if (msg.n_satellites == 0)
108  {
109  n_sats_in_sentence = 0;
110  }
111  if (msg.msg_number == 1)
112  {
113  n_sats_in_sentence = msg.n_satellites;
114  }
115  }
116  // Checking that the sentence is the right length for the number of satellites
117  size_t expected_length = MIN_LENGTH + 4 * n_sats_in_sentence + 1;
118  // Note that we add +1 due to the checksum data being part of the argument
119  // "sentence".
120  if (n_sats_in_sentence == 0)
121  {
122  // Even if the number of sats is 0, the message will still have enough
123  // blank fields for 1 satellite.
124  expected_length += 4;
125  }
126  // ROS_DEBUG("number of sats is %u but nsats in sentence if msg_number = max is
127  // %u and msg.msg_number == msg.n_msgs is %s and nsats in sentence is %li",
128  // msg.n_satellites, msg.n_satellites % static_cast<uint8_t>(4),
129  // msg.msg_number
130  // == msg.n_msgs ? "true" : "false", n_sats_in_sentence);
131  if (sentence.get_body().size() != expected_length &&
132  sentence.get_body().size() != expected_length - 1)
133  {
134  std::stringstream ss;
135  for (size_t i = 0; i < sentence.get_body().size(); ++i)
136  {
137  ss << sentence.get_body()[i];
138  if ((i + 1) < sentence.get_body().size())
139  {
140  ss << ",";
141  }
142  }
143  std::stringstream error;
144  error << "Expected GSV length is " << expected_length << " for message with "
145  << n_sats_in_sentence << " satellites. The actual length is "
146  << sentence.get_body().size() << ".\n"
147  << ss.str().c_str();
148  throw ParseException(error.str());
149  }
150 
151  // Parsing information about n_sats_in_sentence SVs..
152  msg.satellites.resize(n_sats_in_sentence);
153  for (size_t sat = 0, index = MIN_LENGTH; sat < n_sats_in_sentence;
154  ++sat, index += 4)
155  {
156  if (!parsing_utilities::parseUInt8(sentence.get_body()[index],
157  msg.satellites[sat].prn))
158  {
159  std::stringstream error;
160  error << "Error parsing PRN for satellite " << sat << " in GSV.";
161  throw ParseException(error.str());
162  }
163  float elevation;
164  if (!parsing_utilities::parseFloat(sentence.get_body()[index + 1],
165  elevation))
166  {
167  std::stringstream error;
168  error << "Error parsing elevation for satellite " << sat << " in GSV.";
169  throw ParseException(error.str());
170  }
171  msg.satellites[sat].elevation = static_cast<uint8_t>(elevation);
172 
173  float azimuth;
174  if (!parsing_utilities::parseFloat(sentence.get_body()[index + 2], azimuth))
175  {
176  std::stringstream error;
177  error << "Error parsing azimuth for satellite " << sat << " in GSV.";
178  throw ParseException(error.str());
179  }
180  msg.satellites[sat].azimuth = static_cast<uint16_t>(azimuth);
181 
182  if ((index + 3) >= sentence.get_body().size() ||
183  sentence.get_body()[index + 3].empty())
184  {
185  msg.satellites[sat].snr = -1;
186  } else
187  {
188  uint8_t snr;
189  if (!parsing_utilities::parseUInt8(sentence.get_body()[index + 3], snr))
190  {
191  std::stringstream error;
192  error << "Error parsing snr for satellite " << sat << " in GSV.";
193  throw ParseException(error.str());
194  }
195  msg.satellites[sat].snr = static_cast<int8_t>(snr);
196  }
197  }
198  return msg;
199 }
Timestamp
uint64_t Timestamp
Definition: typedefs.hpp:92
GpgsvMsg
nmea_msgs::Gpgsv GpgsvMsg
Definition: typedefs.hpp:137
frame_id
std::string frame_id
parsing_utilities::parseFloat
float parseFloat(const uint8_t *buffer)
Converts a 4-byte-buffer into a float.
Definition: parsing_utilities.cpp:73
NMEASentence
Struct to split an NMEA sentence into its ID and its body, the latter tokenized into a vector of stri...
Definition: nmea_sentence.hpp:55
GpgsvParser::MESSAGE_ID
static const std::string MESSAGE_ID
Declares the string MESSAGE_ID.
Definition: gpgsv.hpp:102
parsing_utilities::parseUInt8
bool parseUInt8(const std::string &string, uint8_t &value, int32_t base=10)
Interprets the contents of "string" as a unsigned integer number of type uint8_t.
Definition: parsing_utilities.cpp:153
gpgsv.hpp
Derived class for parsing GSV messages.
GpgsvParser::getMessageID
const std::string getMessageID() const override
Returns the ASCII message ID, here "$GPGSV".
Definition: gpgsv.cpp:41
GpgsvParser::parseASCII
GpgsvMsg parseASCII(const NMEASentence &sentence, const std::string &frame_id, bool use_gnss_time, Timestamp time_obj) noexcept(false) override
Parses one GSV message.
Definition: gpgsv.cpp:53
ParseException
Class to declare error message format when parsing, derived from the public class "std::runtime_error...
Definition: parse_exception.hpp:86


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Wed Nov 22 2023 04:04:27