ublox.cpp
Go to the documentation of this file.
1 #include "ublox.h"
2 
3 #define DEG2RAD (3.14159 / 180.0)
4 
6 
7 void read_callback(uint8_t byte)
8 {
9  GPSptr->read_cb(byte);
10 }
11 
12 void UBLOX::init(UART* uart_drv)
13 {
14  GPSptr = this;
15  serial_ = uart_drv;
16 
17  // Reset message parser
18  buffer_head_ = 0;
20  message_class_ = 0;
21  message_type_ = 0;
22  length_ = 0;
23  ck_a_ = 0;
24  ck_b_ = 0;
25  memset(debug_buffer_, 0, sizeof(debug_buffer_));
26 
27  // Find the right baudrate
28  uint32_t timeout_ms = 100;
29  looking_for_nmea_ = true;
32  for (size_t i = 0; i < sizeof(baudrates)/sizeof(uint32_t); i++)
33  {
35  uint32_t start = millis();
36  while (millis() - start < timeout_ms)
37  {
38  if (got_message_)
39  {
41  break;
42  }
43  }
44  if (current_baudrate_ != 0)
45  break;
46  }
47 
48  // We didn't find the GPS
49  if (!got_message_)
50  return;
51 
52  // Otherwise, Configure the GPS
53  set_baudrate(115200);
55  set_nav_rate(100);
60 }
61 
62 bool UBLOX::send_message(uint8_t msg_class, uint8_t msg_id, UBX_message_t& message, uint16_t len)
63 {
64  // First, calculate the checksum
65  uint8_t ck_a, ck_b;
66  calculate_checksum(msg_class, msg_id, len, message, ck_a, ck_b);
67 
68  // Send message
71  serial_->put_byte(msg_class);
72  serial_->put_byte(msg_id);
73  serial_->put_byte(len & 0xFF);
74  serial_->put_byte((len >> 8) & 0xFF);
75  serial_->write(message.buffer, len);
76  serial_->put_byte(ck_a);
77  serial_->put_byte(ck_b);
78 
79  delay(20);
80  return true;
81 }
82 
83 void UBLOX::set_baudrate(const uint32_t baudrate)
84 {
85  // Now that we have the right baudrate, let's configure the thing
86  memset(&out_message_, 0, sizeof(CFG_PRT_t));
87  out_message_.CFG_PRT.portID = CFG_PRT_t::PORT_UART1;
88  out_message_.CFG_PRT.baudrate = baudrate;
89  out_message_.CFG_PRT.inProtoMask = CFG_PRT_t::IN_UBX | CFG_PRT_t::IN_NMEA | CFG_PRT_t::IN_RTCM;
90  out_message_.CFG_PRT.outProtoMask = CFG_PRT_t::OUT_UBX | CFG_PRT_t::OUT_NMEA;
91  out_message_.CFG_PRT.mode = CFG_PRT_t::CHARLEN_8BIT | CFG_PRT_t::PARITY_NONE | CFG_PRT_t::STOP_BITS_1;
92  out_message_.CFG_PRT.flags = 0;
93  send_message(CLASS_CFG, CFG_PRT, out_message_, sizeof(CFG_PRT_t));
94  serial_->set_mode(baudrate, UART::MODE_8N1);
95  current_baudrate_ = baudrate;
96 }
97 
99 {
100  memset(&out_message_, 0, sizeof(CFG_NAV5_t));
101  out_message_.CFG_NAV5.mask = CFG_NAV5_t::MASK_DYN;
102  out_message_.CFG_NAV5.dynModel = CFG_NAV5_t::DYNMODE_AIRBORNE_4G;
103  send_message(CLASS_CFG, CFG_PRT, out_message_, sizeof(CFG_NAV5_t));
104 }
105 
106 void UBLOX::set_nav_rate(uint8_t period_ms)
107 {
108  memset(&out_message_, 0, sizeof(CFG_RATE_t));
109  out_message_.CFG_RATE.measRate = period_ms;
110  out_message_.CFG_RATE.navRate = 1;
111  out_message_.CFG_RATE.timeRef = CFG_RATE_t::TIME_REF_UTC;
112  send_message(CLASS_CFG, CFG_RATE, out_message_, sizeof(CFG_RATE_t));
113 }
114 
115 void UBLOX::enable_message(uint8_t msg_cls, uint8_t msg_id, uint8_t rate)
116 {
117  memset(&out_message_, 0, sizeof(CFG_MSG_t));
118  out_message_.CFG_MSG.msgClass = msg_cls;
119  out_message_.CFG_MSG.msgID = msg_id;
120  out_message_.CFG_MSG.rate = rate;
121  send_message(CLASS_CFG, CFG_MSG, out_message_, sizeof(CFG_RATE_t));
122 }
123 
124 
125 void UBLOX::read_cb(uint8_t byte)
126 {
127  debug_buffer_[(debug_buffer_head_++) % sizeof(debug_buffer_)] = byte;
128  // Look for a valid NMEA packet (do this at the beginning in case
129  // UBX was disabled for some reason) and during autobaud
130  // detection
131  if (looking_for_nmea_)
132  {
133  if (byte == NMEA_START_BYTE2 && prev_byte_ == NMEA_START_BYTE1)
134  {
135  got_message_ = true;
136  looking_for_nmea_ = false;
137  }
138  }
139 
140  // handle the UBX packet
141  switch (parse_state_)
142  {
143  case START:
144  if (byte == START_BYTE_2 && prev_byte_ == START_BYTE_1)
145  {
146  looking_for_nmea_ = false;
147  buffer_head_ = 0;
149  message_class_ = 0;
150  message_type_ = 0;
151  length_ = 0;
152  ck_a_ = 0;
153  ck_b_ = 0;
154  got_message_ = true;
155  }
156  break;
157  case GOT_START_FRAME:
158  message_class_ = byte;
160  break;
161  case GOT_CLASS:
162  message_type_ = byte;
164  break;
165  case GOT_MSG_ID:
166  length_ = byte;
168  break;
169  case GOT_LENGTH1:
170  length_ |= static_cast<uint16_t>(byte)<< 8;
173  {
174  num_errors_++;
176  return;
177  }
178  break;
179  case GOT_LENGTH2:
180  if (buffer_head_ <= length_)
181  {
182  // push the byte onto the data buffer
184  if (buffer_head_ == length_)
185  {
187  }
188  buffer_head_++;
189  }
190  break;
191  case GOT_PAYLOAD:
192  ck_a_ = byte;
194  break;
195  case GOT_CK_A:
196  ck_b_ = byte;
198  break;
199  default:
200  num_errors_++;
201  break;
202  }
203 
204  // If we have a complete packet, then try to parse it
205  if (parse_state_ == GOT_CK_B)
206  {
207  if (decode_message())
208  {
210  }
211  else
212  {
213  // indicate error if it didn't work
214  num_errors_++;
216  }
217  }
218 
219  prev_byte_ = byte;
220 }
221 
222 void UBLOX::get_pos_ecef(double* pos_ecef, uint32_t* t_ms)
223 {
224  (void)t_ms;
225  pos_ecef[0] = pos_ecef_message_.ecefX;
226  pos_ecef[1] = pos_ecef_message_.ecefY;
227  pos_ecef[2] = pos_ecef_message_.ecefZ;
228 }
229 
230 void UBLOX::get_vel_ecef(float* vel_ecef, uint32_t* t_ms)
231 {
232  (void)t_ms;
233  vel_ecef[0] = vel_ecef_message_.ecefVX;
234  vel_ecef[1] = vel_ecef_message_.ecefVY;
235  vel_ecef[2] = vel_ecef_message_.ecefVZ;
236 }
237 
238 void UBLOX::read(double* lla, float* vel, uint8_t* fix_type, uint32_t *t_ms)
239 {
240  (void)t_ms;
241  if (new_data_)
242  {
243  convert_data();
244  new_data_ = false;
245  }
246  for (int i = 0; i < 3; i++)
247  {
248  lla[i] = lla_[i];
249  vel[i] = vel_[i];
250  }
251  (*fix_type) = nav_message_.fixType;
252 }
253 
255 {
256  // First, check the checksum
257  uint8_t ck_a, ck_b;
259  if (ck_a != ck_a_ || ck_b != ck_b_)
260  return false;
261 
263 
264  // Parse the payload
265  switch (message_class_)
266  {
267  case CLASS_ACK:
268  switch (message_type_)
269  {
270  case ACK_ACK:
271  got_ack_ = true;
272  break;
273  case ACK_NACK:
274  got_nack_ = true;
275  break;
276  default:
277  break;
278  }
279  break;
280 
281  case CLASS_CFG:
282  switch (message_type_)
283  {
284  case CFG_MSG:
285  break;
286  case CFG_PRT:
287  break;
288  case CFG_NAV5:
289  break;
290  case CFG_RATE:
291  break;
292  default:
293  break;
294  }
295  break;
296 
297  case CLASS_NAV:
298  switch (message_type_)
299  {
300  case NAV_PVT:
301  new_data_ = true;
303  break;
304  case NAV_POSECEF:
305  new_data_ = true;
307  break;
308  case NAV_VELECEF:
309  new_data_ = true;
311  break;
312  default:
313  break;
314  }
315  default:
316  break;
317  }
318  return true;
319 }
320 
322 {
323  double scaling = 1e-7 * 3.14159/180.0;
324  lla_[0] = static_cast<double>(nav_message_.lat) * scaling;
325  lla_[1] = static_cast<double>(nav_message_.lon) * scaling;
326  lla_[2] = nav_message_.height * 1e-3;
327 
328  vel_[0] = nav_message_.velN * 1e-3;
329  vel_[1] = nav_message_.velE * 1e-3;
330  vel_[2] = nav_message_.velD * 1e-3;
331 }
332 
333 void UBLOX::calculate_checksum(const uint8_t msg_cls, const uint8_t msg_id, const uint16_t len, const UBX_message_t payload, uint8_t& ck_a, uint8_t& ck_b) const
334 {
335  ck_a = ck_b = 0;
336 
337  // Add in class
338  ck_a = ck_a + msg_cls;
339  ck_b = ck_b + ck_a;
340 
341  // Id
342  ck_a = ck_a + msg_id;
343  ck_b = ck_b + ck_a;
344 
345  // Length
346  ck_a = ck_a + (len & 0xFF);
347  ck_b = ck_b + ck_a;
348 
349  ck_a = ck_a + ((len >> 8) & 0xFF);
350  ck_b = ck_b + ck_a;
351 
352  // Payload
353  for (int i = 0; i < len; i ++)
354  {
355  ck_a = ck_a + payload.buffer[i];
356  ck_b = ck_b + ck_a;
357  }
358 }
359 
CFG_NAV5_t CFG_NAV5
Definition: ublox.h:345
#define UBLOX_BUFFER_SIZE
Definition: ublox.h:6
bool got_ack_
Definition: ublox.h:382
void set_dynamic_mode()
Definition: ublox.cpp:98
void calculate_checksum(const uint8_t msg_cls, const uint8_t msg_id, const uint16_t len, const UBX_message_t payload, uint8_t &ck_a, uint8_t &ck_b) const
Definition: ublox.cpp:333
void register_rx_callback(void(*cb)(uint8_t data)) override
Definition: uart.cpp:325
void set_baudrate(const uint32_t baudrate)
Definition: ublox.cpp:83
ROSCPP_DECL void start()
NAV_POSECEF_t NAV_POSECEF
Definition: ublox.h:347
uint8_t ck_a_
Definition: ublox.h:388
volatile uint32_t millis(void)
Definition: system.c:50
bool got_message_
Definition: ublox.h:381
uint8_t ck_b_
Definition: ublox.h:389
uint32_t num_errors_
Definition: ublox.h:390
void get_vel_ecef(float *vel_ecef, uint32_t *t_ms)
Definition: ublox.cpp:230
Definition: ublox.h:9
uint8_t prev_byte_
Definition: ublox.h:397
void enable_message(uint8_t msg_cls, uint8_t msg_id, uint8_t rate)
Definition: ublox.cpp:115
bool looking_for_nmea_
Definition: ublox.h:396
void put_byte(uint8_t ch) override
Definition: uart.cpp:235
uint16_t length_
Definition: ublox.h:387
float vel_[3]
Definition: ublox.h:394
uint8_t buffer[UBLOX_BUFFER_SIZE]
Definition: ublox.h:339
UBX_message_t out_message_
Definition: ublox.h:374
NAV_PVT_t NAV_PVT
Definition: ublox.h:346
bool new_data_
Definition: ublox.h:401
bool decode_message()
Definition: ublox.cpp:254
uint16_t buffer_head_
Definition: ublox.h:380
parse_state_t parse_state_
Definition: ublox.h:384
CFG_RATE_t CFG_RATE
Definition: ublox.h:344
NAV_POSECEF_t pos_ecef_message_
Definition: ublox.h:404
UBLOX * GPSptr
Definition: ublox.cpp:5
uint8_t message_type_
Definition: ublox.h:386
NAV_VELECEF_t vel_ecef_message_
Definition: ublox.h:405
uint8_t message_class_
Definition: ublox.h:385
uint32_t current_baudrate_
Definition: ublox.h:371
bool send_message(uint8_t msg_class, uint8_t msg_id, UBX_message_t &message, uint16_t len)
Definition: ublox.cpp:62
char debug_buffer_[15]
Definition: ublox.h:377
Definition: uart.h:43
void set_nav_rate(uint8_t period_ms)
Definition: ublox.cpp:106
void get_pos_ecef(double *pos_ecef, uint32_t *t_ms)
Definition: ublox.cpp:222
double lla_[3]
Definition: ublox.h:393
void read_callback(uint8_t byte)
Definition: ublox.cpp:7
bool got_nack_
Definition: ublox.h:383
NAV_VELECEF_t NAV_VELECEF
Definition: ublox.h:348
bool set_mode(uint32_t baud, uart_mode_t mode)
Definition: uart.cpp:273
void init(UART *uart_drv)
Definition: ublox.cpp:12
uint32_t num_messages_received_
Definition: ublox.h:391
const uint32_t baudrates[5]
Definition: ublox.h:372
void read_cb(uint8_t byte)
Definition: ublox.cpp:125
uint16_t debug_buffer_head_
Definition: ublox.h:378
CFG_MSG_t CFG_MSG
Definition: ublox.h:342
void write(const uint8_t *ch, uint8_t len) override
Definition: uart.cpp:178
CFG_PRT_t CFG_PRT
Definition: ublox.h:343
UBX_message_t in_message_
Definition: ublox.h:375
NAV_PVT_t nav_message_
Definition: ublox.h:403
void delay(uint32_t ms)
Definition: system.c:98
void convert_data()
Definition: ublox.cpp:321
void read(double *lla, float *vel, uint8_t *fix_type, uint32_t *t_ms)
Definition: ublox.cpp:238
UART * serial_
Definition: ublox.h:399


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:20