software/arduino/libraries/gps_ublox/gps_lea6t.cpp

   1 // -*- coding: utf-8 -*-
   2 // LICENSE
   3 //
   4 // This file is part of ruwai.
   5 //
   6 // If you use ruwai in any program or publication, please inform and
   7 // acknowledge its author Stefan Mertl (stefan@mertl-research.at).
   8 //
   9 // pSysmon is free software: you can redistribute it and/or modify
  10 // it under the terms of the GNU General Public License as published by
  11 // the Free Software Foundation, either version 3 of the License, or
  12 // (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful,
  15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 // GNU General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
  21 //
  22 // Parts of this code were adapted from AP_GPS_UBLOX code from ArduPilot. The
  23 // origin of the used code was by Michael Smith, Jordi Munoz and Jose Julio,
  24 // DIYDrones.com and partly rewritten by Andrew Tridgell.
  25
  26
  27 #include <gps_lea6t.h>
  28
  29 extern RuwaiSerial & usb_serial;
  30
  31 GpsLea6T::GpsLea6T(RuwaiSerial &serial_stream) :
  32     serial_port(serial_stream)
  33 {
  34     // Initialize the Arduino pins.
  35     pinMode(GPS_ENABLE_USB, OUTPUT);
  36     digitalWrite(GPS_ENABLE_USB, 1);
  37
  38     init_state();
  39 }
  40
  41 void
  42 GpsLea6T::init_state(void)
  43 {
  44     // Set the read flow state.
  45     read_step = WAITING_FOR_SYNC1;
  46     read_msg_class = 0;
  47     read_msg_id = 0;
  48     read_cka = 0;
  49     read_ckb = 0;
  50     read_payload_length = 0;
  51     read_payload_counter = 0;
  52
  53     // Initialize the state of the GPS module.
  54     timepulse1_cfg.tp_idx = 0;
  55     timepulse2_cfg.tp_idx = 1;
  56     gps_fix = FIX_NO;
  57     gps_fix_ok = false;
  58     nav_status_changed = false;
  59     nav_timeutc_changed = false;
  60     nav_position_changed = false;
  61     cfg_timepulse1_changed = false;
  62     cfg_timepulse2_changed = false;
  63     cfg_prt_uart_changed = false;
  64     msg_ack_changed = false;
  65     timestamp_init = false;
  66     timestamp_empty = true;
  67     timestamp_buf_empty = true;
  68     timestamp_race_error = false;
  69
  70     next_timepulse.tow_ms = 0;
  71     next_timepulse.tow_sub_ms = 0;
  72     next_timepulse.q_err = 0;
  73     next_timepulse.week = 0;
  74     next_timepulse.flags.time_base = 1;
  75     next_timepulse.flags.utc = 0;
  76     next_timepulse.reserved1 = 0;
  77
  78 }
  79
  80 bool
  81 GpsLea6T::start_serial(uint32_t baud_rate)
  82 {
  83     serial_port.begin(baud_rate);
  84     delay(100);
  85     // Test the connection by polling the baud rate.
  86     bool is_working = poll_baud_rate_blocking(baud_rate);
  87     return is_working;
  88 }
  89
  90 void
  91 GpsLea6T::stop_serial()
  92 {
  93     serial_port.end();
  94     delay(100);
  95 }
  96
  97
  98 bool
  99 GpsLea6T::disable_timepulse2_frequency(void)
 100 {
 101     timepulse2_cfg.ant_cable_delay = 0;
 102     timepulse2_cfg.freq_period = 0;
 103     timepulse2_cfg.freq_period_lock = 0;
 104     timepulse2_cfg.pulse_len_ratio = 2147483648;            //Be aware of the scaling factor 0.5^-32: 50% duty cycle = 0.5 * 0.5^-32
 105     timepulse2_cfg.pulse_len_ratio_lock = 2147483648;
 106     timepulse2_cfg.user_config_delay = 0;
 107     timepulse2_cfg.flags.active = 1;
 108     timepulse2_cfg.flags.lock_gps_freq = 1;
 109     timepulse2_cfg.flags.locked_other_set = 0;
 110     timepulse2_cfg.flags.is_freq = 1;
 111     timepulse2_cfg.flags.is_length = 0;
 112     timepulse2_cfg.flags.align_to_tow = 1;
 113     timepulse2_cfg.flags.polarity = 1;
 114     timepulse2_cfg.flags.grid_utc_gps = 0;
 115
 116     return send_cfg_tp5(1);
 117 }
 118
 119 bool
 120 GpsLea6T::set_timepulse2_frequency(uint32_t freq, uint32_t freq_lock)
 121 {
 122     timepulse2_cfg.ant_cable_delay = 0;
 123     timepulse2_cfg.freq_period = freq;
 124     timepulse2_cfg.freq_period_lock = freq_lock;
 125     timepulse2_cfg.pulse_len_ratio = 2147483648;            //Be aware of the scaling factor 0.5^-32: 50% duty cycle = 0.5 * 0.5^-32
 126     timepulse2_cfg.pulse_len_ratio_lock = 2147483648;
 127     timepulse2_cfg.user_config_delay = 0;
 128     timepulse2_cfg.flags.active = 1;
 129     timepulse2_cfg.flags.lock_gps_freq = 1;
 130     timepulse2_cfg.flags.locked_other_set = 0;
 131     timepulse2_cfg.flags.is_freq = 1;
 132     timepulse2_cfg.flags.is_length = 0;
 133     timepulse2_cfg.flags.align_to_tow = 1;
 134     timepulse2_cfg.flags.polarity = 1;
 135     timepulse2_cfg.flags.grid_utc_gps = 0;
 136
 137     return send_cfg_tp5(1);
 138 }
 139
 140
 141 bool
 142 GpsLea6T::set_timepulse1_pps()
 143 {
 144     timepulse1_cfg.ant_cable_delay = 0;
 145     timepulse1_cfg.freq_period = 1;
 146     timepulse1_cfg.freq_period_lock = 1;
 147     timepulse1_cfg.pulse_len_ratio = 500000;            // The scaling only applies to the frequency.
 148     timepulse1_cfg.pulse_len_ratio_lock = 100000;
 149     timepulse1_cfg.user_config_delay = 0;
 150     timepulse1_cfg.flags.active = 1;
 151     timepulse1_cfg.flags.lock_gps_freq = 1;
 152     timepulse1_cfg.flags.locked_other_set = 1;
 153     timepulse1_cfg.flags.is_freq = 1;
 154     timepulse1_cfg.flags.is_length = 1;
 155     timepulse1_cfg.flags.align_to_tow = 1;
 156     timepulse1_cfg.flags.polarity = 1;
 157     timepulse1_cfg.flags.grid_utc_gps = 0;
 158
 159     return send_cfg_tp5(0);
 160 }
 161
 162 void
 163 GpsLea6T::set_uart1_baudrate(uint32_t baudrate)
 164 {
 165     ubx_cfg_prt_uart_t msg;
 166     msg.prt_id = 1;
 167     msg.reserved0 = 0;
 168     msg.tx_ready.en = 0;
 169     msg.tx_ready.pol = 0;
 170     msg.tx_ready.pin = 0;
 171     msg.tx_ready.thres = 0;
 172     msg.mode.reserved1 = 1;
 173     msg.mode.charlen = 3;
 174     msg.mode.parity = 4;
 175     msg.mode.nstopbits = 0;
 176     msg.mode.unused0 = 0;
 177     msg.mode.unused1 = 0;
 178     msg.mode.unused2 = 0;
 179     msg.mode.unused3 = 0;
 180     msg.baud_rate = baudrate;
 181     msg.in_proto_mask.ubx = 1;
 182     msg.in_proto_mask.nmea = 1;
 183     msg.in_proto_mask.rtcm = 1;
 184     msg.in_proto_mask.unused0 = 0;
 185     msg.out_proto_mask.ubx = 1;
 186     msg.out_proto_mask.nmea = 1;
 187     msg.out_proto_mask.rtcm = 1;
 188     msg.out_proto_mask.unused0 = 0;
 189     msg.reserved4 = 0;
 190     msg.reserved5 = 0;
 191
 192     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_PRT, &msg, sizeof(msg));
 193     //confirmed = wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_PRT);
 194 }
 195
 196
 197 uint32_t
 198 GpsLea6T::get_timepulse2_frequency()
 199 {
 200     return timepulse2_cfg.pulse_len_ratio;
 201 }
 202
 203 uint32_t
 204 GpsLea6T::get_timepulse2_frequency_locked()
 205 {
 206     return timepulse2_cfg.freq_period_lock;
 207 }
 208
 209 gpsfix_t
 210 GpsLea6T::get_gps_fix()
 211 {
 212     return (gpsfix_t)navigation_status.gps_fix;
 213 }
 214
 215 bool
 216 GpsLea6T::get_gps_fix_ok()
 217 {
 218     if (navigation_status.flags.gps_fix_ok == 1)
 219     {
 220         return true;
 221     }
 222     else
 223     {
 224         return false;
 225     }
 226 }
 227
 228
 229 bool
 230 GpsLea6T::read()
 231 {
 232     uint8_t data = 0;
 233     uint8_t bytes_available = 0;
 234     bool parsed = false;
 235
 236     bytes_available = serial_port.available();
 237     for (uint16_t k = 0; k < bytes_available; k++)
 238     {
 239         data = serial_port.read();
 240
 241         reset:
 242         switch(read_step)
 243         {
 244             case SYNC1_FOUND:
 245                 //debug_print("SYNC1_FOUND");
 246                 if (data == SYNC_CHAR_2)
 247                 {
 248                     read_step = SYNC2_FOUND;
 249                     //debug_print("SYNC2_FOUND");
 250                     break;
 251                 }
 252                 // If the second sync char is wrong, reset to initial mode and
 253                 // continue the processing of the current data.
 254                 read_step = WAITING_FOR_SYNC1;
 255                 //debug_print("SYNC2 doesn't match.");
 256             case WAITING_FOR_SYNC1:
 257                 if (data == SYNC_CHAR_1)
 258                 {
 259                     read_step = SYNC1_FOUND;
 260                 }
 261                 break;
 262             case SYNC2_FOUND:
 263                 read_msg_class = data;
 264                 read_cka = data;
 265                 read_ckb = data;
 266                 read_step = CLASS_READ;
 267                 //debug_print("CLASS_READ");
 268                 break;
 269             case CLASS_READ:
 270                 read_msg_id = data;
 271                 read_cka += data;
 272                 read_ckb += read_cka;
 273                 read_step = MSGID_READ;
 274                 //debug_print("MSGID_READ");
 275                 break;
 276             case MSGID_READ:
 277                 read_payload_length = data;
 278                 read_cka += data;
 279                 read_ckb += read_cka;
 280                 read_step = LENGTH_BYTE1_READ;
 281                 //debug_print("LENGTH_BYTE1_READ");
 282                 break;
 283             case LENGTH_BYTE1_READ:
 284                 read_payload_length += (uint16_t)(data << 8);
 285                 read_cka += data;
 286                 read_ckb += read_cka;
 287                 read_payload_counter = 0;
 288                 read_step = LENGTH_BYTE2_READ;
 289                 //debug_print("LENGTH_BYTE2_READ");
 290                 if(read_payload_length > 256) {
 291                     //debug_print("Too large payload");
 292                     read_payload_length = 0;
 293                     read_step = WAITING_FOR_SYNC1;
 294                     goto reset;
 295                 }
 296                 break;
 297             case LENGTH_BYTE2_READ:
 298                 //debug_print("LENGTH_BYTE2_READ");
 299                 if (read_payload_counter < sizeof(read_buffer))
 300                 {
 301                     read_buffer.bytes[read_payload_counter] = data;
 302                 }
 303                 if (++read_payload_counter == read_payload_length)
 304                 {
 305                     read_step = PAYLOAD_READ;
 306                     //debug_print("PAYLOAD_READ");
 307                 }
 308                 read_cka += data;
 309                 read_ckb += read_cka;
 310                 break;
 311             case PAYLOAD_READ:
 312                 if (read_cka != data)
 313                 {
 314                     //debug_print("Bad cka.");
 315                     read_step = WAITING_FOR_SYNC1;
 316                     goto reset;
 317                 }
 318                 read_step = CKA_OK;
 319                 //debug_print("CKA_OK");
 320                 break;
 321             case CKA_OK:
 322                 read_step = WAITING_FOR_SYNC1;
 323                 if(read_ckb != data)
 324                 {
 325                     //debug_print("Bad ckb.");
 326                     break;
 327                 }
 328                 //debug_print("CKB_OK");
 329                 parsed = parse_ubx();
 330         }
 331     }
 332     return parsed;
 333 }
 334
 335
 336 bool
 337 GpsLea6T::parse_ubx()
 338 {
 339     bool parsed = false;
 340     if (read_msg_class == UBX_CLASS_ACK)
 341     {
 342         debug_print("ACK ");
 343         switch (read_msg_id)
 344         {
 345             case UBX_ACK_ID_ACK:
 346                 msg_ack = read_buffer.ack_ack;
 347                 msg_ack_changed = true;
 348                 /*
 349                 usb_serial.println("Received UBX_ACK_ID_ACK.");
 350                 usb_serial.println(msg_ack.cls_id);
 351                 usb_serial.println(msg_ack.msg_id);
 352                 usb_serial.println("----");
 353                 */
 354                 break;
 355             default:
 356                 handle_unexpected_msg();
 357         }
 358     }
 359     else if (read_msg_class == UBX_CLASS_CFG)
 360     {
 361         switch (read_msg_id)
 362         {
 363             case UBX_CFG_ID_TP5:
 364                 //debug_print("Parsing UBX_CFG_ID_TP5");
 365                 //timepulse2_cfg.tp_idx = read_buffer.cfg_tp5.tp_idx;
 366                 if (read_buffer.cfg_tp5.tp_idx == 0)
 367                 {
 368                     timepulse_cfg = read_buffer.cfg_tp5;
 369                     parsed = true;
 370                     cfg_timepulse1_changed = true;
 371                 }
 372                 else if (read_buffer.cfg_tp5.tp_idx == 1)
 373                 {
 374                     timepulse2_cfg = read_buffer.cfg_tp5;
 375                     parsed = true;
 376                     cfg_timepulse2_changed = true;
 377                 }
 378                 break;
 379             case UBX_CFG_ID_PRT:
 380                 //usb_serial.println("Parsing UBX_CFG_ID_PRT");
 381                 cfg_prt_uart = read_buffer.cfg_prt_uart;
 382                 parsed = true;
 383                 cfg_prt_uart_changed = true;
 384                 break;
 385             default:
 386                 handle_unexpected_msg();
 387         }
 388     }
 389     else if (read_msg_class == UBX_CLASS_NAV)
 390     {
 391         switch (read_msg_id)
 392         {
 393             case UBX_NAV_ID_STATUS:
 394                 //debug_print("Parsing UBX_NAV_STATUS");
 395                 navigation_status = read_buffer.nav_status;
 396                 nav_status_changed = true;
 397                 break;
 398
 399             case UBX_NAV_ID_TIMEUTC:
 400                 //debug_print("Parsing UBX_NAV_TIMEUTC");
 401                 timeutc = read_buffer.nav_timeutc;
 402                 nav_timeutc_changed = true;
 403                 break;
 404
 405             case UBX_NAV_ID_POSLLH:
 406                 //debug_print("Parsing UBX_NAV_TIMEUTC");
 407                 nav_position = read_buffer.nav_posllh;
 408                 nav_position_changed = true;
 409                 break;
 410
 411             default:
 412                 handle_unexpected_msg();
 413         }
 414
 415     }
 416     else if (read_msg_class == UBX_CLASS_MON)
 417     {
 418
 419     }
 420     else if (read_msg_class == UBX_CLASS_TIM)
 421     {
 422         switch (read_msg_id)
 423         {
 424             case UBX_TIM_ID_TP:
 425                 //debug_print("Parsing UBX_TIM_TP");
 426                 if (!timestamp_buf_empty)
 427                 {
 428                     timestamp_race_error = true;
 429                 }
 430                 next_timepulse_buf = read_buffer.tim_tp;
 431                 timestamp_buf_empty = false;
 432                 if (!timestamp_init)
 433                 {
 434                     timestamp_init = true;
 435                 }
 436                 //usb_serial.println(read_buffer.tim_tp.week);
 437                 //usb_serial.println(read_buffer.tim_tp.tow_ms);
 438                 //usb_serial.println(read_buffer.tim_tp.tow_sub_ms);
 439                 break;
 440
 441             default:
 442                 handle_unexpected_msg();
 443         }
 444     }
 445     else
 446     {
 447         handle_unexpected_msg();
 448     }
 449
 450     return parsed;
 451 }
 452
 453
 454 void
 455 GpsLea6T::handle_unexpected_msg()
 456 {
 457     debug_print("Handling an unexpected UBX message.");
 458 }
 459
 460
 461 void
 462 GpsLea6T::send_ublox_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t length)
 463 {
 464     ubx_header_t header;
 465     uint8_t ck_a=0, ck_b=0;
 466     header.sync_char1 = SYNC_CHAR_1;
 467     header.sync_char2 = SYNC_CHAR_2;
 468     header.msg_class = msg_class;
 469     header.msg_id = msg_id;
 470     header.payload_length = length;
 471
 472     // Update the checksum using the relevant header part.
 473     update_ubx_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 474
 475     // Update the checksum using the payload.
 476     update_ubx_checksum((uint8_t *)msg, length, ck_a, ck_b);
 477
 478     // Send the paket to the serial port.
 479     serial_port.write((const uint8_t *)&header, sizeof(header));
 480     serial_port.write((const uint8_t *)msg, length);
 481     serial_port.write((const uint8_t *)&ck_a, 1);
 482     serial_port.write((const uint8_t *)&ck_b, 1);
 483 }
 484
 485
 486 uint8_t
 487 GpsLea6T::disable_default_messages()
 488 {
 489     uint8_t ack_status = 0;
 490     // Disable all default messages on the target which is used to send this
 491     // message.
 492     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GGA, 0))
 493         ack_status |= 1UL << 0;
 494
 495     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GLL, 0))
 496         ack_status |= 1UL << 1;
 497
 498     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GSA, 0))
 499         ack_status |= 1UL << 2;
 500
 501     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GSV, 0))
 502         ack_status |= 1UL << 3;
 503
 504     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_RMC, 0))
 505         ack_status |= 1UL << 4;
 506
 507     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_VTG, 0))
 508         ack_status |= 1UL << 5;
 509
 510     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_ZDA, 0))
 511         ack_status |= 1UL << 6;
 512
 513     // Disable the NMEA_STD_ID_TXT output using the CFG-INF message.
 514     ubx_cfg_inf_t msg;
 515     msg.protocol_id = 1;
 516     msg.reserved_0 = 0;
 517     msg.reserved_1 = 0;
 518     msg.inf_msg_mask.serial_1.error = 0;
 519     msg.inf_msg_mask.serial_1.warning = 0;
 520     msg.inf_msg_mask.serial_1.notice = 0;
 521     msg.inf_msg_mask.serial_1.debug = 0;
 522     msg.inf_msg_mask.serial_1.test = 0;
 523     msg.inf_msg_mask.serial_1.reserved = 0;
 524     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_INF, &msg, sizeof(msg));
 525     if(wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_INF))
 526         ack_status |= 1UL << 7;
 527
 528     return ack_status;
 529 }
 530
 531 bool GpsLea6T::enable_message(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 532 {
 533     // Enable a message on the current target which is used to send this
 534     // message.
 535     bool confirmed = send_cfg_msg(msg_class, msg_id, rate);
 536     return confirmed;
 537 }
 538
 539 void
 540 GpsLea6T::poll_cfg_prt_uart()
 541 {
 542     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_PRT, NULL, 0);
 543 }
 544
 545 void
 546 GpsLea6T::poll_cfg_msg(uint8_t msg_class, uint8_t msg_id)
 547 {
 548     ubx_cfg_msg_rate_poll_t msg;
 549     msg.msg_class = msg_class;
 550     msg.msg_id = msg_id;
 551     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_MSG, &msg, sizeof(msg));
 552 }
 553
 554 bool
 555 GpsLea6T::send_cfg_msg(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 556 {
 557     bool confirmed = false;
 558     ubx_cfg_msg_rate_t msg;
 559     msg.msg_class = msg_class;
 560     msg.msg_id = msg_id;
 561     msg.rate = rate;
 562     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_MSG, &msg, sizeof(msg));
 563     confirmed = wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_MSG);
 564     return confirmed;
 565 }
 566
 567 void
 568 GpsLea6T::poll_cfg_tp5(uint8_t timepulse_id)
 569 {
 570     ubx_cfg_tp5_poll_t msg;
 571     msg.tp_idx = timepulse_id;
 572     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_TP5, &msg, sizeof(msg));
 573 }
 574
 575 bool
 576 GpsLea6T::send_cfg_tp5(uint8_t tp_idx)
 577 {
 578     if (tp_idx == 0)
 579     {
 580         send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_TP5, &timepulse1_cfg, sizeof(timepulse1_cfg));
 581     }
 582     else if (tp_idx == 1)
 583     {
 584         send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_TP5, &timepulse2_cfg, sizeof(timepulse2_cfg));
 585     }
 586
 587     return wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_TP5);
 588 }
 589
 590
 591 void
 592 GpsLea6T::poll_nav_status()
 593 {
 594     send_ublox_message(UBX_CLASS_NAV, UBX_NAV_ID_STATUS, NULL, 0);
 595 }
 596
 597
 598 void
 599 GpsLea6T::poll_nav_posllh()
 600 {
 601     send_ublox_message(UBX_CLASS_NAV, UBX_NAV_ID_POSLLH, NULL, 0);
 602 }
 603
 604
 605 bool
 606 GpsLea6T::wait_for_cfg_ack(uint8_t msg_class, uint8_t msg_id)
 607 {
 608     unsigned timeout = 1000;          // The timeout in milliseconds.
 609     unsigned long start = 0;
 610     unsigned long current = 0;
 611     unsigned long elapsed = 0;
 612     uint8_t gps_byte_available = 0;
 613
 614
 615     start = millis();
 616     while (elapsed < timeout)
 617     {
 618         gps_byte_available = serial_port.available();
 619         if (gps_byte_available > 0)
 620         {
 621             read();
 622         }
 623
 624         if (msg_ack_changed)
 625         {
 626             if ((msg_ack.cls_id == msg_class) && (msg_ack.msg_id == msg_id))
 627             {
 628                 usb_serial.println("Message acknowledged.");
 629                 msg_ack_changed = false;
 630                 return true;
 631             }
 632             else
 633             {
 634                 // TODO: Handle the reception of ACK messages not fitting the
 635                 // required msg_class and msg_id.
 636                 msg_ack_changed = false;
 637             }
 638         }
 639         current = millis();
 640         elapsed = current - start;
 641     }
 642     usb_serial.println("GPS ACK Timeout");
 643     usb_serial.println(msg_class);
 644     usb_serial.println(msg_id);
 645     usb_serial.println("#####");
 646     return false;
 647 }
 648
 649 uint32_t
 650 GpsLea6T::detect_baud_rate(uint32_t baud_1, uint32_t baud_2)
 651 {
 652     uint32_t active_baud = 0;
 653
 654     // Try the first baud rate.
 655     if (test_baud_rate(baud_1))
 656         active_baud = cfg_prt_uart.baud_rate;
 657     else if (test_baud_rate(baud_2))
 658         active_baud = cfg_prt_uart.baud_rate;
 659
 660     return active_baud;
 661 }
 662
 663 bool
 664 GpsLea6T::test_baud_rate(uint32_t baud_rate)
 665 {
 666     bool confirmed = false;
 667     serial_port.end();
 668     delay(100);
 669     serial_port.begin(baud_rate);
 670     delay(100);
 671     confirmed = poll_baud_rate_blocking(baud_rate);
 672     serial_port.end();
 673     delay(100);
 674     return confirmed;
 675 }
 676
 677 bool
 678 GpsLea6T::poll_baud_rate_blocking(uint32_t baud_rate)
 679 {
 680     unsigned timeout = 3000;          // The timeout in milliseconds.
 681     unsigned long start = 0;
 682     unsigned long current = 0;
 683     unsigned long elapsed = 0;
 684     uint8_t gps_byte_available = 0;
 685
 686     poll_cfg_prt_uart();
 687     start = millis();
 688     while (elapsed < timeout)
 689     {
 690         gps_byte_available = serial_port.available();
 691         if (gps_byte_available > 0)
 692         {
 693             read();
 694         }
 695         if (cfg_prt_uart_changed)
 696         {
 697             cfg_prt_uart_changed = false;
 698             usb_serial.println("Working baud rate found.");
 699             usb_serial.println(baud_rate);
 700             return true;
 701         }
 702         current = millis();
 703         elapsed = current - start;
 704     }
 705     usb_serial.println("Baud Timeout");
 706     usb_serial.println(baud_rate);
 707     usb_serial.println("#####");
 708     return false;
 709 }