ublox.cpp
Go to the documentation of this file.
1 #include "ublox.h"
2 
3 #include <time.h>
4 
6 
7 // A C style callback
8 void cb(uint8_t byte)
9 {
10  gnss_Ptr->read_cb(byte);
11 }
12 
14 
15 // Look for a GNSS receiver, and if found, change its settings
16 void UBLOX::init(UART *uart)
17 {
18  gnss_Ptr = this;
19  // Reset message parser
20  buffer_head_ = 0;
22  message_class_ = 0;
23  message_type_ = 0;
24  length_ = 0;
25  ck_a_ = 0;
26  ck_b_ = 0;
27 
28  // Find the right baudrate
29  looking_for_nmea_ = true;
30  serial_ = uart;
31 
34 
36 }
37 
39 {
40  // Configure the GNSS receiver
43  set_nav_rate(100);
47 
48  // Zeroing the data
49  this->nav_message_ = {};
50  is_initialized_ = true;
51 
53 }
54 
56 {
57  baudrate_search_index_ = BAUDRATE_SEARCH_COUNT - 1; // once incremented, this goes to 0
59 }
60 
62 {
63  searching_baudrate_ = false; // To prevent hits during this function
68  searching_baudrate_ = true;
69 }
70 
72 {
73  if (is_initialized_)
74  {
76  {
77  is_initialized_ = false;
79  }
80  }
81  else
82  {
83  if (got_message_)
84  finish_init();
87  }
88 }
89 
90 bool UBLOX::send_ubx_message(uint8_t msg_class, uint8_t msg_id, UBX_message_t &message, uint16_t len)
91 {
92  // First, calculate the checksum
93  uint8_t ck_a, ck_b;
94  calculate_checksum(msg_class, msg_id, len, message, ck_a, ck_b);
95 
96  // Send message
99  serial_->put_byte(msg_class);
100  serial_->put_byte(msg_id);
101  serial_->put_byte(len & 0xFF);
102  serial_->put_byte((len >> 8) & 0xFF);
103  serial_->write(message.buffer, len);
104  serial_->put_byte(ck_a);
105  serial_->put_byte(ck_b);
106  return true;
107 }
108 
109 // Set the baudrate and other settings on the GNSS receiver
110 // This requires the flight controller to have detected the correct, current baud rate
111 // This also changes the flight controller's baud rate to match the GNSS reciever
112 void UBLOX::set_baudrate(const uint32_t baudrate)
113 {
114  // Now that we have the right baudrate, let's configure the thing
115  memset(&out_message_, 0, sizeof(CFG_PRT_t));
116  out_message_.CFG_PRT.portID = CFG_PRT_t::PORT_UART1;
117  out_message_.CFG_PRT.baudrate = baudrate;
118  out_message_.CFG_PRT.inProtoMask = CFG_PRT_t::IN_UBX | CFG_PRT_t::IN_NMEA | CFG_PRT_t::IN_RTCM;
119  out_message_.CFG_PRT.outProtoMask = CFG_PRT_t::OUT_UBX | CFG_PRT_t::OUT_NMEA;
120  out_message_.CFG_PRT.mode = CFG_PRT_t::CHARLEN_8BIT | CFG_PRT_t::PARITY_NONE | CFG_PRT_t::STOP_BITS_1;
121  out_message_.CFG_PRT.flags = 0;
122  send_ubx_message(CLASS_CFG, CFG_PRT, out_message_, sizeof(CFG_PRT_t));
123  while (!serial_->tx_buffer_empty()) // make sure the message is sent before changing the baud rate
124  {
125  delayMicroseconds(100);
126  }
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 is_initialized_;
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  send_ubx_message(CLASS_CFG, CFG_PRT, out_message_, sizeof(CFG_NAV5_t));
145 }
146 
147 // Set the frequency of nav messages, defined as the period in ms
148 void UBLOX::set_nav_rate(uint8_t period_ms)
149 {
150  memset(&out_message_, 0, sizeof(CFG_RATE_t));
151  out_message_.CFG_RATE.measRate = period_ms;
152  out_message_.CFG_RATE.navRate = 1;
153  out_message_.CFG_RATE.timeRef = CFG_RATE_t::TIME_REF_GPS;
154  send_ubx_message(CLASS_CFG, CFG_RATE, out_message_, sizeof(CFG_RATE_t));
155 }
156 
157 // Enable a specific message from the receiver
158 void UBLOX::enable_message(uint8_t msg_cls, uint8_t msg_id, uint8_t rate)
159 {
160  memset(&out_message_, 0, sizeof(CFG_MSG_t));
161  out_message_.CFG_MSG.msgClass = msg_cls;
162  out_message_.CFG_MSG.msgID = msg_id;
163  out_message_.CFG_MSG.rate = rate;
164  send_ubx_message(CLASS_CFG, CFG_MSG, out_message_, sizeof(CFG_MSG_t));
165 }
166 
167 void UBLOX::read_cb(uint8_t byte)
168 {
169  uint64_t time_recieved = micros();
170  // Look for a valid NMEA packet (do this at the beginning in case
171  // UBX was disabled for some reason) and during autobaud
172  // detection
173  if (looking_for_nmea_)
174  {
175  if (byte == NMEA_START_BYTE2 && prev_byte_ == NMEA_START_BYTE1)
176  {
177  got_message_ = true;
178  looking_for_nmea_ = false;
179  }
180  }
181 
182  // handle the UBX packet
183  switch (parse_state_)
184  {
185  case START:
186  if (byte == START_BYTE_2 && prev_byte_ == START_BYTE_1)
187  {
188  looking_for_nmea_ = false;
189  buffer_head_ = 0;
191  message_class_ = 0;
192  message_type_ = 0;
193  length_ = 0;
194  ck_a_ = 0;
195  ck_b_ = 0;
196  got_message_ = true;
197  }
198  break;
199  case GOT_START_FRAME:
200  message_class_ = byte;
202  break;
203  case GOT_CLASS:
204  message_type_ = byte;
206  break;
207  case GOT_MSG_ID:
208  length_ = byte;
210  break;
211  case GOT_LENGTH1:
212  length_ |= static_cast<uint16_t>(byte) << 8;
215  {
216  num_errors_++;
218  return;
219  }
220  break;
221  case GOT_LENGTH2:
222  if (buffer_head_ < length_)
223  {
224  // push the byte onto the data buffer
226  if (buffer_head_ == length_ - 1)
227  {
229  }
230  buffer_head_++;
231  }
232  break;
233  case GOT_PAYLOAD:
234  ck_a_ = byte;
236  break;
237  case GOT_CK_A:
238  ck_b_ = byte;
240  break;
241  default:
242  num_errors_++;
243  break;
244  }
245 
246  // If we have a complete packet, then try to parse it
247  if (parse_state_ == GOT_CK_B)
248  {
249  if (decode_message())
250  {
253  last_pvt_timestamp_ = time_recieved;
254  if (new_data())
256  }
257  else
258  {
259  // indicate error if it didn't work
260  num_errors_++;
262  }
263  }
264 
265  prev_byte_ = byte;
266 }
267 
268 uint64_t convert_to_unix_time(const UBLOX::GNSS_TIME_T &time)
269 {
270  tm c_time{time.sec,
271  time.min,
272  time.hour,
273  time.day,
274  time.month - 1, // UBX uses 1-indexed months, but c++ uses 0 indexed
275  time.year - 1900, // UBX uses years AD, c++ uses years since 1900
276  0, // ignored
277  0, // also ignored
278  false};
279  return mktime(&c_time);
280 }
281 /* Tells if new data is available
282  * Only returns true if new data has been recieved, and if all three sources match time of week.
283  * This is because if the times of week do not match, new data is still being recieved,
284  * and attempting to read could result in data from different times.
285  */
287 {
288  return this->new_data_ && (this->nav_message_.iTOW == this->pos_ecef_.iTOW)
289  && (this->nav_message_.iTOW == this->vel_ecef_.iTOW);
290 }
292 {
293  GNSSPVT data = {nav_message_.iTOW,
294  static_cast<FixType>(nav_message_.fixType),
296  nav_message_.time.nano,
297  nav_message_.lat,
298  nav_message_.lon,
299  nav_message_.height,
300  nav_message_.velN,
301  nav_message_.velE,
302  nav_message_.velD,
303  nav_message_.hAcc,
304  nav_message_.vAcc,
306  this->new_data_ = false;
307  return data;
308 }
309 
311 {
312  GNSSPosECEF pos = {pos_ecef_.iTOW, pos_ecef_.ecefX, pos_ecef_.ecefY, pos_ecef_.ecefZ, pos_ecef_.pAcc};
313  return pos; // copy elision effectively returns this as a reference without scope issues
314 }
315 
317 {
318  UBLOX::GNSSVelECEF vel = {vel_ecef_.iTOW, vel_ecef_.ecefVX, vel_ecef_.ecefVY, vel_ecef_.ecefVZ, vel_ecef_.sAcc};
319  return vel; // copy elision effectively returns this as a reference without scope issues
320 }
321 
322 const UBLOX::NAV_PVT_t &UBLOX::read_full()
323 {
324  return this->nav_message_;
325 }
326 
328 {
329  // First, check the checksum
330  uint8_t ck_a, ck_b;
332  if (ck_a != ck_a_ || ck_b != ck_b_)
333  return false;
334 
336 
337  // Parse the payload
338  switch (message_class_)
339  {
340  case CLASS_ACK:
341  switch (message_type_)
342  {
343  case ACK_ACK:
344  got_ack_ = true;
345  break;
346  case ACK_NACK:
347  got_nack_ = true;
348  break;
349  default:
350  break;
351  }
352  break;
353 
354  case CLASS_CFG:
355  switch (message_type_)
356  {
357  default:
358  break;
359  }
360  break;
361 
362  case CLASS_NAV:
363  switch (message_type_)
364  {
365  case NAV_PVT:
366  new_data_ = true;
368  break;
369  case NAV_POSECEF:
370  new_data_ = true;
372  break;
373  case NAV_VELECEF:
374  new_data_ = true;
376  break;
377  default:
378  break;
379  }
380  break;
381  default:
382  break;
383  }
384  return true;
385 }
386 
387 void UBLOX::calculate_checksum(const uint8_t msg_cls,
388  const uint8_t msg_id,
389  const uint16_t len,
390  const UBX_message_t &payload,
391  uint8_t &ck_a,
392  uint8_t &ck_b) const
393 {
394  ck_a = ck_b = 0;
395 
396  // Add in class
397  ck_a += msg_cls;
398  ck_b += ck_a;
399 
400  // Id
401  ck_a += msg_id;
402  ck_b += ck_a;
403 
404  // Length
405  ck_a += len & 0xFF;
406  ck_b += ck_a;
407  ck_a += (len >> 8) & 0xFF;
408  ck_b += ck_a;
409 
410  // Payload
411  for (int i = 0; i < len; i++)
412  {
413  ck_a += payload.buffer[i];
414  ck_b += ck_a;
415  }
416 }
CFG_NAV5_t CFG_NAV5
Definition: ublox.h:399
#define UBLOX_BUFFER_SIZE
Definition: ublox.h:4
bool got_ack_
Definition: ublox.h:495
FixType
Definition: ublox.h:19
bool tx_buffer_empty() override
Definition: uart.cpp:272
void set_dynamic_mode()
Definition: ublox.cpp:139
void register_rx_callback(void(*cb)(uint8_t data)) override
Definition: uart.cpp:319
void set_baudrate(const uint32_t baudrate)
Definition: ublox.cpp:112
volatile uint32_t num_messages_received_
Definition: ublox.h:504
bool is_initialized_
Definition: ublox.h:477
NAV_POSECEF_t NAV_POSECEF
Definition: ublox.h:401
uint8_t ck_a_
Definition: ublox.h:501
volatile uint32_t millis(void)
Definition: system.c:50
bool got_message_
Definition: ublox.h:494
uint8_t ck_b_
Definition: ublox.h:502
void increment_detect_baudrate_async()
Definition: ublox.cpp:61
static constexpr uint32_t BAUD_RATE
Definition: ublox.h:441
uint32_t num_errors_
Definition: ublox.h:503
void delayMicroseconds(uint32_t us)
Definition: system.c:94
Definition: ublox.h:14
uint8_t prev_byte_
Definition: ublox.h:510
UBLOX()
Definition: ublox.cpp:13
void enable_message(uint8_t msg_cls, uint8_t msg_id, uint8_t rate)
Definition: ublox.cpp:158
bool looking_for_nmea_
Definition: ublox.h:509
void put_byte(uint8_t ch) override
Definition: uart.cpp:228
uint16_t length_
Definition: ublox.h:500
uint32_t last_baudrate_change_ms_
Definition: ublox.h:485
NAV_VELECEF_t vel_ecef_
Definition: ublox.h:519
uint8_t buffer[UBLOX_BUFFER_SIZE]
Definition: ublox.h:393
UBX_message_t out_message_
Definition: ublox.h:490
volatile uint64_t micros(void)
Definition: system.c:44
NAV_PVT_t NAV_PVT
Definition: ublox.h:400
uint64_t convert_to_unix_time(const UBLOX::GNSS_TIME_T &time)
Definition: ublox.cpp:268
size_t baudrate_search_index_
Definition: ublox.h:484
volatile bool new_data_
Definition: ublox.h:514
bool decode_message()
Definition: ublox.cpp:327
static constexpr uint32_t TIMEOUT_MS
Definition: ublox.h:488
void init(UART *uart)
Definition: ublox.cpp:16
uint16_t buffer_head_
Definition: ublox.h:493
parse_state_t parse_state_
Definition: ublox.h:497
static constexpr UART::uart_mode_t UART_MODE
Definition: ublox.h:442
CFG_RATE_t CFG_RATE
Definition: ublox.h:398
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:387
uint8_t message_type_
Definition: ublox.h:499
bool searching_baudrate_
Definition: ublox.h:483
uint8_t message_class_
Definition: ublox.h:498
uint32_t current_baudrate_
Definition: ublox.h:479
Definition: uart.h:42
void set_nav_rate(uint8_t period_ms)
Definition: ublox.cpp:148
void start_detect_baudrate_async()
Definition: ublox.cpp:55
bool send_ubx_message(uint8_t msg_class, uint8_t msg_id, UBX_message_t &message, uint16_t len)
Definition: ublox.cpp:90
uint32_t last_valid_message_
Definition: ublox.h:487
static constexpr uint32_t BAUDRATE_SEARCH_TIME_MS
Definition: ublox.h:481
bool got_nack_
Definition: ublox.h:496
bool new_data()
Definition: ublox.cpp:286
bool present()
Definition: ublox.cpp:133
NAV_VELECEF_t NAV_VELECEF
Definition: ublox.h:402
GNSSPosECEF read_pos_ecef()
Definition: ublox.cpp:310
bool set_mode(uint32_t baud, uart_mode_t mode)
Definition: uart.cpp:266
void cb(uint8_t byte)
Definition: ublox.cpp:8
const NAV_PVT_t & read_full()
Definition: ublox.cpp:322
void read_cb(uint8_t byte)
Definition: ublox.cpp:167
CFG_MSG_t CFG_MSG
Definition: ublox.h:396
void finish_init()
Definition: ublox.cpp:38
GNSSVelECEF read_vel_ecef()
Definition: ublox.cpp:316
void write(const uint8_t *ch, uint8_t len) override
Definition: uart.cpp:173
void check_connection_status()
Definition: ublox.cpp:71
static constexpr size_t BAUDRATE_SEARCH_COUNT
Definition: ublox.h:480
uint64_t last_pvt_timestamp_
Definition: ublox.h:506
CFG_PRT_t CFG_PRT
Definition: ublox.h:397
NAV_POSECEF_t pos_ecef_
Definition: ublox.h:518
UBLOX * gnss_Ptr
Definition: ublox.cpp:5
const uint32_t baudrates[BAUDRATE_SEARCH_COUNT]
Definition: ublox.h:482
UBX_message_t in_message_
Definition: ublox.h:491
NAV_PVT_t nav_message_
Definition: ublox.h:517
UART * serial_
Definition: ublox.h:512
GNSSPVT read()
Definition: ublox.cpp:291


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:54