ublox.cpp
Go to the documentation of this file.
1 #include "ublox.h"
2 
3 #include <time.h>
4 
5 #define DEG2RAD (3.14159 / 180.0)
6 //#define DBG(...) printf(__VA_ARGS__)
7 #define DBG(...)
8 #ifndef M_PI
9 #define M_PI 3.14159
10 #endif
11 
13 
14 //A C style callback
15 void cb(uint8_t byte)
16 {
17  gnss_Ptr->read_cb(byte);
18 }
19 
21 
22 //Look for a GNSS receiver, and if found, change its settings
23 void UBLOX::init(UART *uart)
24 {
25  gnss_Ptr = this;
26  // Reset message parser
27  buffer_head_ = 0;
29  message_class_ = 0;
30  message_type_ = 0;
31  length_ = 0;
32  ck_a_ = 0;
33  ck_b_ = 0;
34 
35  // Find the right baudrate
36  looking_for_nmea_ = true;
37  serial_ = uart;
38 
39 
42 
43  if (!detect_baudrate())
44  return;
45 
46  // Otherwise, Configure the GNSS receiver
47  set_baudrate(115200);
49  set_nav_rate(100);
53 
54  //Zeroing the data
55  this->nav_message_ = {};
56 }
57 
58 //Attempt to detect the baudrate by trying a rate, waiting 1 s, and
59 //seeing if any messages are recieved.
60 //Attempted baudrates and the order in which they are tried are defined in ublox.h, in the array 'baudrates'
61 //As a consequence of the changing baudrates, the sensor is treated as
62 //disconnected until the procedure finishes
64 {
66  for (uint32_t i = 0; i < sizeof(baudrates)/sizeof(uint32_t); i++)
67  {
68  DBG("Trying %d baudrate\n", baudrates[i]);
70  uint32_t timeout_ms(1000);
71  uint32_t start_ms = millis();
72  uint32_t now_ms = millis();
73  //TREYDO should got_message_ be set to false here?
74  got_message_ = false;
75  while (now_ms < start_ms + timeout_ms)
76  {
77  now_ms = millis();
78  if (got_message_)
79  {
80  DBG("Found UBLOX at %d baud\n", baudrates[i]);
82  break;
83  }
84  }
85  if (current_baudrate_ != 0)
86  break;
87  }
88  return got_message_;
89 }
90 
91 bool UBLOX::send_message(uint8_t msg_class, uint8_t msg_id, UBX_message_t &message, uint16_t len)
92 {
93  // First, calculate the checksum
94  uint8_t ck_a, ck_b;
95  //TREYDO pass by reference, with ints?
96  calculate_checksum(msg_class, msg_id, len, message, ck_a, ck_b);
97 
98  // Send message
101  serial_->put_byte(msg_class);
102  serial_->put_byte(msg_id);
103  serial_->put_byte(len & 0xFF);
104  serial_->put_byte((len >> 8) & 0xFF);
105  serial_->write(message.buffer, len);
106  serial_->put_byte(ck_a);
107  serial_->put_byte(ck_b);
108  return true;
109 }
110 
111 //Set the baudrate and other settings on the GNSS receiver
112 //This requires the flight controller to have detected the correct, current baud rate
113 //This also changes the flight controller's baud rate to match the GNSS reciever
114 void UBLOX::set_baudrate(const uint32_t baudrate)
115 {
116  DBG("Setting baudrate to %d\n", baudrate);
117  // Now that we have the right baudrate, let's configure the thing
118  memset(&out_message_, 0, sizeof(CFG_PRT_t));
119  out_message_.CFG_PRT.portID = CFG_PRT_t::PORT_UART1;
120  out_message_.CFG_PRT.baudrate = baudrate;
121  out_message_.CFG_PRT.inProtoMask = CFG_PRT_t::IN_UBX | CFG_PRT_t::IN_NMEA | CFG_PRT_t::IN_RTCM;
122  out_message_.CFG_PRT.outProtoMask = CFG_PRT_t::OUT_UBX | CFG_PRT_t::OUT_NMEA;
123  out_message_.CFG_PRT.mode = CFG_PRT_t::CHARLEN_8BIT | CFG_PRT_t::PARITY_NONE | CFG_PRT_t::STOP_BITS_1;
124  out_message_.CFG_PRT.flags = 0;
125  send_message(CLASS_CFG, CFG_PRT, out_message_, sizeof(CFG_PRT_t));
126  delayMicroseconds(10000);
127  serial_->set_mode(baudrate, UART::MODE_8N1);
128  current_baudrate_ = baudrate;
129 }
130 
131 //Checks if there is a GNSS reciever present, which is determined by checking if
132 //it has ever recieved a valid message
134 {
135  return got_message_;
136 }
137 
138 //Set the dynamic mode to airborne, with a max acceleration of 4G
140 {
141  memset(&out_message_, 0, sizeof(CFG_NAV5_t));
142  out_message_.CFG_NAV5.mask = CFG_NAV5_t::MASK_DYN;
143  out_message_.CFG_NAV5.dynModel = CFG_NAV5_t::DYNMODE_AIRBORNE_4G;
144  DBG("Setting dynamic mode\n");
145  send_message(CLASS_CFG, CFG_PRT, out_message_, sizeof(CFG_NAV5_t));
146 }
147 
148 //Set the frequency of nav messages, defined as the period in ms
149 void UBLOX::set_nav_rate(uint8_t period_ms)
150 {
151  memset(&out_message_, 0, sizeof(CFG_RATE_t));
152  out_message_.CFG_RATE.measRate = period_ms;
153  out_message_.CFG_RATE.navRate = 1;
154  out_message_.CFG_RATE.timeRef = CFG_RATE_t::TIME_REF_GPS;
155  DBG("Setting nav rate to %d\n", period_ms);
156  send_message(CLASS_CFG, CFG_RATE, out_message_, sizeof(CFG_RATE_t));
157 }
158 
159 void UBLOX::enable_message(uint8_t msg_cls, uint8_t msg_id, uint8_t rate)
160 {
161  memset(&out_message_, 0, sizeof(CFG_MSG_t));
162  out_message_.CFG_MSG.msgClass = msg_cls;
163  out_message_.CFG_MSG.msgID = msg_id;
164  out_message_.CFG_MSG.rate = rate;
165  DBG("Requesting %x:%x message at %d hz\n", msg_cls, msg_id, rate);
166  send_message(CLASS_CFG, CFG_MSG, out_message_, sizeof(CFG_MSG_t));
167 }
168 
169 
170 void UBLOX::read_cb(uint8_t byte)
171 {
172  uint64_t time_recieved = micros();
173  // Look for a valid NMEA packet (do this at the beginning in case
174  // UBX was disabled for some reason) and during autobaud
175  // detection
176  if (looking_for_nmea_)
177  {
178  if (byte == NMEA_START_BYTE2 && prev_byte_ == NMEA_START_BYTE1)
179  {
180  got_message_ = true;
181  looking_for_nmea_ = false;
182  }
183  }
184 
185  // handle the UBX packet
186  switch (parse_state_)
187  {
188  case START:
189  if (byte == START_BYTE_2 && prev_byte_ == START_BYTE_1)
190  {
191  looking_for_nmea_ = false;
192  buffer_head_ = 0;
194  message_class_ = 0;
195  message_type_ = 0;
196  length_ = 0;
197  ck_a_ = 0;
198  ck_b_ = 0;
199  got_message_ = true;
200  }
201  break;
202  case GOT_START_FRAME:
203  message_class_ = byte;
205  break;
206 
207  case GOT_CLASS:
208  message_type_ = byte;
210  break;
211  case GOT_MSG_ID:
212  length_ = byte;
214  break;
215  case GOT_LENGTH1:
216  length_ |= static_cast<uint16_t>(byte) << 8;
219  {
220  num_errors_++;
222  return;
223  }
224  break;
225  case GOT_LENGTH2:
226  if (buffer_head_ < length_)
227  {
228  // push the byte onto the data buffer
230  if (buffer_head_ == length_-1)
231  {
233  }
234  buffer_head_++;
235  }
236  break;
237  case GOT_PAYLOAD:
238  ck_a_ = byte;
240  break;
241  case GOT_CK_A:
242  ck_b_ = byte;
244  break;
245  default:
246  num_errors_++;
247  break;
248  }
249 
250  // If we have a complete packet, then try to parse it
251  if (parse_state_ == GOT_CK_B)
252  {
253  if (decode_message())
254  {
257  last_pvt_timestamp = time_recieved;
258  }
259  else
260  {
261  // indicate error if it didn't work
262  num_errors_++;
263  DBG("failed to parse message\n");
265  }
266  }
267 
268  prev_byte_ = byte;
269 }
270 //convert a time struct to unix time
271 //dumb leap years
272 //This function will break in 2100, because it doesn't take into accoutn that 2100 is not a leap year
273 uint64_t convert_to_unix(UBLOX::GNSS_TIME_T time)
274 {
275  uint32_t day_s = 24*60*60; //seconds per day
276  uint32_t year_s = 365*day_s; //seconds per non-leap day
277  uint32_t elapsed_years = time.year-1970;
278  uint32_t elapsed_leap_days = (elapsed_years+1)/4;
279  if (time.year % 4 == 0) //If currently in a leap year
280  if (time.month>=3) //If past feb 29
281  elapsed_leap_days++;
282  uint32_t elapsed_days;//Days past in the year
283  switch (time.month)
284  {
285  case 1:
286  elapsed_days = 0;
287  break;
288  case 2:
289  elapsed_days = 31;
290  break;
291  case 3:
292  //Ignore leap days, because it is accounted for above
293  elapsed_days = 31+28;
294  break;
295  case 4:
296  elapsed_days = 31+28+31;
297  break;
298  case 5:
299  elapsed_days = 31+28+31+30;
300  break;
301  case 6:
302  elapsed_days = 31+28+31+30+31;
303  break;
304  case 7:
305  elapsed_days = 31+28+31+30+31+30;
306  break;
307  case 8:
308  elapsed_days = 31+28+31+30+31+30+31;
309  break;
310  case 9:
311  elapsed_days = 31+28+31+30+31+30+31+31;
312  break;
313  case 10:
314  elapsed_days = 31+28+31+30+31+30+31+31+30;
315  break;
316  case 11:
317  elapsed_days = 31+28+31+30+31+30+31+31+30+31;
318  break;
319  case 12:
320  elapsed_days = 31+28+31+30+31+30+31+31+30+31+30;
321  break;
322  default://This should never be reached, because there are 12 month / year
323  elapsed_days = 0;
324  }
325  elapsed_days += time.day-1; //Minus 1 because the day is not yet complete
326  return elapsed_years * year_s + (elapsed_days+elapsed_leap_days)*day_s + (time.hour*60+time.min)*60 + time.sec;
327 }
328 /* Tells if new data is available
329  * Only returns true if new data has been recieved, and if all three sources match time of week.
330  * This is because if the times of week do not match, new data is still being recieved,
331  * and attempting to read could result in data from different times.
332  */
334 {
335  return this->new_data_
336  && (this->nav_message_.iTOW == this->pos_ecef_.iTOW)
337  && (this->nav_message_.iTOW == this->vel_ecef_.iTOW);
338 }
339 #pragma GCC diagnostic push
340 #pragma GCC diagnostic ignored "-Wmissing-field-initializers" //Ignore warning about leaving struct fields blank
342 {
343  GNSSPVT data = {};
344  data.time_of_week = this->nav_message_.iTOW;
345  data.time = convert_to_unix(this->nav_message_.time);
346  data.nanos = this->nav_message_.time.nano;
347  data.lat = this->nav_message_.lat;
348  data.lon = this->nav_message_.lon;
349  data.height = this->nav_message_.height;
350  data.vel_n = this->nav_message_.velN;
351  data.vel_e = this->nav_message_.velE;
352  data.vel_d = this->nav_message_.velD;
353  data.h_acc = this->nav_message_.hAcc;
354  data.v_acc = this->nav_message_.vAcc;
356 
357  this->new_data_=false;
358 
359  return data;
360 }
361 
363 {
364  GNSSPosECEF pos = {};
365  pos.x = this->pos_ecef_.ecefX;
366  pos.y = this->pos_ecef_.ecefY;
367  pos.z = this->pos_ecef_.ecefZ;
368  pos.time_of_week = this->pos_ecef_.iTOW;
369  pos.p_acc = this->pos_ecef_.pAcc;
370  return pos;
371 }
372 
374 {
375  UBLOX::GNSSVelECEF vel = {};
376  vel.vx = this->vel_ecef_.ecefVX;
377  vel.vy = this->vel_ecef_.ecefVY;
378  vel.vz = this->vel_ecef_.ecefVZ;
379  vel.time_of_week = this->vel_ecef_.iTOW;
380  vel.s_acc = this->vel_ecef_.sAcc;
381  return vel;
382 }
383 
384 const UBLOX::NAV_PVT_t &UBLOX::read_raw()
385 {
386  return this->nav_message_;
387 }
388 #pragma GCC diagnostic pop //End ignore blank struct initalizers
389 
391 {
392  // First, check the checksum
393  uint8_t ck_a, ck_b;
395  if (ck_a != ck_a_ || ck_b != ck_b_)
396  return false;
397 
399  DBG("recieved message %d: ", num_messages_received_);
400 
401  // Parse the payload
402  switch (message_class_)
403  {
404  case CLASS_ACK:
405  DBG("ACK_");
406  switch (message_type_)
407  {
408  case ACK_ACK:
409  got_ack_ = true;
410  DBG("ACK\n");
411  break;
412  case ACK_NACK:
413  got_nack_ = true;
414  DBG("NACK\n");
415  break;
416  default:
417  DBG("%d\n", message_type_);
418  break;
419  }
420  break;
421 
422 
423  case CLASS_CFG:
424  DBG("CFG_");
425  switch (message_type_)
426  {
427  default:
428  DBG("%d\n", message_type_);
429  break;
430  }
431  break;
432 
433  case CLASS_NAV:
434  DBG("NAV_");
435  switch (message_type_)
436  {
437  case NAV_PVT:
438  new_data_ = true;
440  DBG("PVT\n");
441  break;
442  case NAV_POSECEF:
443  new_data_ = true;
445  DBG("POSECEF\n");
446  break;
447  case NAV_VELECEF:
448  new_data_ = true;
450  DBG("NAVELECEF\n");
451  break;
452  default:
453  DBG("%d\n", message_type_);
454  break;
455  }
456  break;
457  default:
458  DBG("%d_%d\n", message_class_, message_type_);
459  break;
460  }
461  return true;
462 }
463 
465 {
466  lla_[0] = static_cast<double>(nav_message_.lat) * 1e-7L;
467  lla_[1] = static_cast<double>(nav_message_.lon) * 1e-7L;
468  lla_[2] = nav_message_.height * 1e-3;
469 
470  vel_[0] = nav_message_.velN * 1e-3;
471  vel_[1] = nav_message_.velE * 1e-3;
472  vel_[2] = nav_message_.velD * 1e-3;
474 }
475 
476 void UBLOX::calculate_checksum(const uint8_t msg_cls, const uint8_t msg_id, const uint16_t len,
477  const UBX_message_t payload, uint8_t &ck_a, uint8_t &ck_b) const
478 {
479  ck_a = ck_b = 0;
480 
481  // Add in class
482  ck_a += msg_cls;
483  ck_b += ck_a;
484 
485  // Id
486  ck_a += msg_id;
487  ck_b += ck_a;
488 
489  // Length
490  ck_a += len & 0xFF;
491  ck_b += ck_a;
492  ck_a += (len >> 8) & 0xFF;
493  ck_b += ck_a;
494 
495  // Payload
496  for (int i = 0; i < len; i ++)
497  {
498  ck_a += payload.buffer[i];
499  ck_b += ck_a;
500  }
501 }
502 
503 
uint32_t time_of_week
Definition: ublox.h:433
#define DBG(...)
Definition: ublox.cpp:7
uint64_t time_
Definition: ublox.h:500
CFG_NAV5_t CFG_NAV5
Definition: ublox.h:399
#define UBLOX_BUFFER_SIZE
Definition: ublox.h:4
bool got_ack_
Definition: ublox.h:485
int32_t vel_e
Definition: ublox.h:414
void set_dynamic_mode()
Definition: ublox.cpp:139
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:476
void register_rx_callback(void(*cb)(uint8_t data)) override
Definition: uart.cpp:325
void set_baudrate(const uint32_t baudrate)
Definition: ublox.cpp:114
volatile uint32_t num_messages_received_
Definition: ublox.h:494
NAV_POSECEF_t NAV_POSECEF
Definition: ublox.h:401
uint8_t ck_a_
Definition: ublox.h:491
volatile uint32_t millis(void)
Definition: system.c:50
bool got_message_
Definition: ublox.h:484
uint8_t ck_b_
Definition: ublox.h:492
uint32_t time_of_week
Definition: ublox.h:407
uint32_t num_errors_
Definition: ublox.h:493
void delayMicroseconds(uint32_t us)
Definition: system.c:91
Definition: ublox.h:15
uint8_t prev_byte_
Definition: ublox.h:503
UBLOX()
Definition: ublox.cpp:20
void enable_message(uint8_t msg_cls, uint8_t msg_id, uint8_t rate)
Definition: ublox.cpp:159
int32_t vy
Definition: ublox.h:435
bool looking_for_nmea_
Definition: ublox.h:502
uint64_t rosflight_timestamp
Definition: ublox.h:418
void put_byte(uint8_t ch) override
Definition: uart.cpp:235
uint16_t length_
Definition: ublox.h:490
float vel_[3]
Definition: ublox.h:497
NAV_VELECEF_t vel_ecef_
Definition: ublox.h:512
uint8_t buffer[UBLOX_BUFFER_SIZE]
Definition: ublox.h:393
UBX_message_t out_message_
Definition: ublox.h:480
volatile uint64_t micros(void)
Definition: system.c:44
uint32_t s_acc
Definition: ublox.h:437
NAV_PVT_t NAV_PVT
Definition: ublox.h:400
volatile bool new_data_
Definition: ublox.h:507
bool decode_message()
Definition: ublox.cpp:390
void init(UART *uart)
Definition: ublox.cpp:23
uint16_t buffer_head_
Definition: ublox.h:483
parse_state_t parse_state_
Definition: ublox.h:487
CFG_RATE_t CFG_RATE
Definition: ublox.h:398
uint8_t message_type_
Definition: ublox.h:489
uint64_t time
Definition: ublox.h:408
uint8_t message_class_
Definition: ublox.h:488
uint32_t current_baudrate_
Definition: ublox.h:477
int32_t height
Definition: ublox.h:412
uint32_t p_acc
Definition: ublox.h:428
int32_t lat
Definition: ublox.h:410
int32_t vx
Definition: ublox.h:434
const NAV_PVT_t & read_raw()
Definition: ublox.cpp:384
bool send_message(uint8_t msg_class, uint8_t msg_id, UBX_message_t &message, uint16_t len)
Definition: ublox.cpp:91
Definition: uart.h:43
void set_nav_rate(uint8_t period_ms)
Definition: ublox.cpp:149
int32_t lon
Definition: ublox.h:411
double lla_[3]
Definition: ublox.h:496
uint32_t time_of_week
Definition: ublox.h:424
uint32_t v_acc
Definition: ublox.h:417
bool got_nack_
Definition: ublox.h:486
bool new_data()
Definition: ublox.cpp:333
int32_t vel_d
Definition: ublox.h:415
bool present()
Definition: ublox.cpp:133
NAV_VELECEF_t NAV_VELECEF
Definition: ublox.h:402
GNSSPosECEF read_pos_ecef()
Definition: ublox.cpp:362
bool set_mode(uint32_t baud, uart_mode_t mode)
Definition: uart.cpp:273
void cb(uint8_t byte)
Definition: ublox.cpp:15
const uint32_t baudrates[5]
Definition: ublox.h:478
int32_t vel_n
Definition: ublox.h:413
bool detect_baudrate()
Definition: ublox.cpp:63
uint64_t last_pvt_timestamp
Definition: ublox.h:499
uint32_t h_acc
Definition: ublox.h:416
void read_cb(uint8_t byte)
Definition: ublox.cpp:170
CFG_MSG_t CFG_MSG
Definition: ublox.h:396
GNSSVelECEF read_vel_ecef()
Definition: ublox.cpp:373
void write(const uint8_t *ch, uint8_t len) override
Definition: uart.cpp:178
CFG_PRT_t CFG_PRT
Definition: ublox.h:397
NAV_POSECEF_t pos_ecef_
Definition: ublox.h:511
UBLOX * gnss_Ptr
Definition: ublox.cpp:12
int32_t vz
Definition: ublox.h:436
uint64_t convert_to_unix(UBLOX::GNSS_TIME_T time)
Definition: ublox.cpp:273
UBX_message_t in_message_
Definition: ublox.h:481
NAV_PVT_t nav_message_
Definition: ublox.h:510
void convert_data()
Definition: ublox.cpp:464
uint64_t nanos
Definition: ublox.h:409
UART * serial_
Definition: ublox.h:505
GNSSPVT read()
Definition: ublox.cpp:341


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:26