receiverstatus_handler.cpp
Go to the documentation of this file.
1 //
3 // Copyright (c) 2020 NovAtel Inc.
4 //
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11 //
12 // The above copyright notice and this permission notice shall be included in all
13 // copies or substantial portions of the Software.
14 //
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 // SOFTWARE.
22 //
25 #include <oem7_ros_publisher.hpp>
26 
27 #include <ros/ros.h>
28 
29 
30 #include <vector>
31 
32 
34 
35 #include "novatel_oem7_msgs/RXSTATUS.h"
36 
37 
38 namespace novatel_oem7_driver
39 {
40  typedef std::vector<std::string> str_vector_t;
41 
42 
43  /*** Oem7 Receiver errors strings - refer to Oem7 manual */
44  const str_vector_t RECEIVER_ERROR_STRS
45  {
46  "DRAM",
47  "Invalid FW",
48  "ROM",
49  "",
50  "ESN Access",
51  "AuthCode",
52  "",
53  "Supply Voltage",
54  "",
55  "Temperature",
56  "MINOS",
57  "PLL RF",
58  "",
59  "",
60  "",
61  "NVM",
62  "Software resource limit exceeded",
63  "Model invalid for this receiver",
64  "",
65  "",
66  "Remote loading has begun",
67  "Export restriction",
68  "Safe Mode",
69  "",
70  "",
71  "",
72  "",
73  "",
74  "",
75  "",
76  "",
77  "Component hardware failure"
78  };
79 
81  const str_vector_t RECEIVER_STATUS_STRS
82  {
83  "Temperature",
84  "Voltage Supply",
85  "Primary antenna not powered",
86  "LNA Failure",
87  "Primary antenna open circuit",
88  "Primary antenna short circuit",
89  "CPU overload",
90  "COM port tx buffer overrun",
91  "",
92  "",
93  "Link overrun",
94  "Input overrun",
95  "Aux transmit overrun",
96  "Antenna gain out of range",
97  "Jammer detected",
98  "INS reset",
99  "IMU communication failure",
100  "GPS almanac invalid",
101  "Position solution invalid",
102  "Position fixed",
103  "Clock steering disabled",
104  "Clock model invalid",
105  "External oscillator locked",
106  "Software resource warning",
107  "",
108  "Interpret Status/Error Bits as Oem7 format",
109  "Tracking mode: HDR",
110  "Digital filtering enabled",
111  "Aux3 event",
112  "Aux2 event",
113  "Aux1 event"
114  };
115 
117  const str_vector_t AUX1_STATUS_STRS
118  {
119  "Jammer detected on RF1",
120  "Jammer detected on RF2",
121  "Jammer detected on RF3",
122  "Position averaging On",
123  "Jammer detected on RF4",
124  "Jammer detected on RF5",
125  "Jammer detected on RF6",
126  "USB not connected",
127  "USB1 buffer overrun",
128  "USB2 buffer overrun",
129  "USB3 buffer overrun",
130  "",
131  "Profile Activation Error",
132  "Throttled Ethernet Reception",
133  "",
134  "",
135  "",
136  "",
137  "Ethernet not connected",
138  "ICOM1 buffer overrun",
139  "ICOM2 buffer overrun",
140  "ICOM3 buffer overrun",
141  "NCOM1 buffer overrun",
142  "NCOM2 buffer overrun",
143  "NCOM3 buffer overrun",
144  "",
145  "",
146  "",
147  "",
148  "",
149  "",
150  "IMU measurement outlier detected"
151  };
152 
154  const str_vector_t AUX2_STATUS_STRS
155  {
156  "SPI Communication Failure",
157  "I2C Communication Failure",
158  "COM4 buffer overrun",
159  "COM5 buffer overrun",
160  "",
161  "",
162  "",
163  "",
164  "",
165  "COM1 buffer overrun",
166  "COM2 buffer overrun",
167  "COM3 buffer overrun",
168  "PLL RF1 unlock",
169  "PLL RF2 unlock",
170  "PLL RF3 unlock",
171  "PLL RF4 unlock",
172  "PLL RF5 unlock",
173  "PLL RF6 unlock",
174  "CCOM1 buffer overrun",
175  "CCOM2 buffer overrun",
176  "CCOM3 buffer overrun",
177  "CCOM4 buffer overrun",
178  "CCOM5 buffer overrun",
179  "CCOM6 buffer overrun",
180  "ICOM4 buffer overrun",
181  "ICOM5 buffer overrun",
182  "ICOM6 buffer overrun",
183  "ICOM7 buffer overrun",
184  "Secondary antenna not powered",
185  "Secondary antenna open circuit",
186  "Secondary antenna short circuit",
187  "Reset loop detected",
188  };
189 
191  const str_vector_t AUX3_STATUS_STRS
192  {
193  "SCOM buffer overrun",
194  "WCOM1 buffer overrun",
195  "FILE buffer overrun",
196  "",
197  "",
198  "",
199  "",
200  "",
201  "",
202  "",
203  "",
204  "",
205  "",
206  "",
207  "",
208  "",
209  "",
210  "",
211  "",
212  "",
213  "",
214  "",
215  "",
216  "",
217  "",
218  "",
219  "",
220  "",
221  "",
222  "Web content is corrupt or does not exist",
223  "RF Calibration Data is present and in error",
224  "RF Calibration data exists and has no errors"
225  };
226 
227  const str_vector_t AUX4_STATUS_STRS
228  {
229  "<60% of available satellites are tracked well",
230  "<15% of available satellites are tracked well",
231  "",
232  "",
233  "",
234  "",
235  "",
236  "",
237  "",
238  "",
239  "",
240  "",
241  "Clock freewheeling due to bad position integrity",
242  "",
243  "Usable RTK Corrections: < 60%",
244  "Usable RTK Corrections: < 15%",
245  "Bad RTK Geometry",
246  "",
247  "",
248  "Long RTK Baseline",
249  "Poor RTK COM link",
250  "Poor ALIGN COM link",
251  "GLIDE Not Active",
252  "Bad PDP Geometry",
253  "No TerraStar Subscription",
254  "",
255  "",
256  "",
257  "Bad PPP Geometry",
258  "",
259  "No INS Alignment",
260  "INS not converged"
261  };
262 
263 
264  void
266  uint32_t bitmask,
267  const str_vector_t& str_map,
268  str_vector_t& str_list,
269  std::vector<uint8_t>& bit_list)
270  {
271  for(int bit = 0;
272  bit < (sizeof(bitmask) * 8);
273  bit++)
274  {
275  if(bitmask & (1 << bit))
276  {
277  bit_list.push_back(bit);
278  if(str_map[bit].length() > 0)
279  {
280  str_list.push_back(str_map[bit]);
281  }
282  }
283  }
284  }
285 
286 
287  /*** Handles RXSTATUS messages */
289  {
291 
292  std::string frame_id_;
293 
294  public:
296  {
297  static const size_t NUM_BITS = sizeof(uint32_t) * 8;
298  assert(RECEIVER_ERROR_STRS.size() == NUM_BITS);
299  assert(AUX1_STATUS_STRS.size() == NUM_BITS);
300  assert(AUX2_STATUS_STRS.size() == NUM_BITS);
301  assert(AUX3_STATUS_STRS.size() == NUM_BITS);
302  assert(AUX4_STATUS_STRS.size() == NUM_BITS);
303  }
304 
306  {
307  }
308 
310  {
311  RXSTATUS_pub_.setup<novatel_oem7_msgs::RXSTATUS>("RXSTATUS", nh);
312  }
313 
314  const std::vector<int>& getMessageIds()
315  {
316  static const std::vector<int> MSG_IDS({RXSTATUS_OEM7_MSGID});
317  return MSG_IDS;
318  }
319 
320  void handleMsg(Oem7RawMessageIf::ConstPtr msg)
321  {
323  MakeROSMessage(msg, rxstatus);
324 
325  // Populate status strings:
326  get_status_info(rxstatus->error, RECEIVER_ERROR_STRS, rxstatus->error_strs, rxstatus->error_bits);
327  get_status_info(rxstatus->rxstat, RECEIVER_STATUS_STRS, rxstatus->rxstat_strs, rxstatus->rxstat_bits);
328  get_status_info(rxstatus->aux1_stat, AUX1_STATUS_STRS, rxstatus->aux1_stat_strs, rxstatus->aux1_stat_bits);
329  get_status_info(rxstatus->aux2_stat, AUX2_STATUS_STRS, rxstatus->aux2_stat_strs, rxstatus->aux2_stat_bits);
330  get_status_info(rxstatus->aux3_stat, AUX3_STATUS_STRS, rxstatus->aux3_stat_strs, rxstatus->aux3_stat_bits);
331  get_status_info(rxstatus->aux4_stat, AUX4_STATUS_STRS, rxstatus->aux4_stat_strs, rxstatus->aux4_stat_bits);
332 
333  RXSTATUS_pub_.publish(rxstatus);
334  }
335  };
336 
337 
338 
339 }
340 
const int RXSTATUS_OEM7_MSGID
void handleMsg(Oem7RawMessageIf::ConstPtr msg)
const str_vector_t AUX2_STATUS_STRS
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
const str_vector_t AUX3_STATUS_STRS
void publish(boost::shared_ptr< M > &msg)
PLUGINLIB_EXPORT_CLASS(novatel_oem7_driver::Oem7ConfigNodelet, nodelet::Nodelet)
const str_vector_t RECEIVER_STATUS_STRS
void setup(const std::string &name, ros::NodeHandle &nh)
const str_vector_t RECEIVER_ERROR_STRS
const str_vector_t AUX1_STATUS_STRS
void MakeROSMessage(const Oem7RawMessageIf::ConstPtr &msg, boost::shared_ptr< T > &rosmsg)
std::vector< std::string > str_vector_t
const str_vector_t AUX4_STATUS_STRS
void get_status_info(uint32_t bitmask, const str_vector_t &str_map, str_vector_t &str_list, std::vector< uint8_t > &bit_list)


novatel_oem7_driver
Author(s):
autogenerated on Tue Mar 9 2021 03:48:00