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
  37     // Disable the usb port power supply by default.
  38     digitalWrite(GPS_ENABLE_USB, 1);
  39
  40     init_state();
  41 }
  42
  43 void
  44 GpsLea6T::init_state(void)
  45 {
  46     // Set the read flow state.
  47     read_step = WAITING_FOR_SYNC1;
  48     read_msg_class = 0;
  49     read_msg_id = 0;
  50     read_cka = 0;
  51     read_ckb = 0;
  52     read_payload_length = 0;
  53     read_payload_counter = 0;
  54
  55     // Initialize the state of the GPS module.
  56     timepulse1_cfg.tp_idx = 0;
  57     timepulse2_cfg.tp_idx = 1;
  58     gps_fix = FIX_NO;
  59     gps_fix_ok = false;
  60     nav_status_changed = false;
  61     nav_timeutc_changed = false;
  62     nav_position_changed = false;
  63     cfg_timepulse1_changed = false;
  64     cfg_timepulse2_changed = false;
  65     cfg_prt_uart_changed = false;
  66     msg_ack_changed = false;
  67     timestamp_init = false;
  68     timestamp_empty = true;
  69     timestamp_buf_empty = true;
  70     timestamp_race_error = false;
  71
  72     next_timepulse.tow_ms = 0;
  73     next_timepulse.tow_sub_ms = 0;
  74     next_timepulse.q_err = 0;
  75     next_timepulse.week = 0;
  76     next_timepulse.flags.time_base = 1;
  77     next_timepulse.flags.utc = 0;
  78     next_timepulse.reserved1 = 0;
  79
  80 }
  81
  82 bool
  83 GpsLea6T::start_serial(uint32_t baud_rate)
  84 {
  85     serial_port.begin(baud_rate);
  86     delay(100);
  87     // Test the connection by polling the baud rate.
  88     bool is_working = poll_baud_rate_blocking(baud_rate);
  89     return is_working;
  90 }
  91
  92 void
  93 GpsLea6T::stop_serial()
  94 {
  95     serial_port.end();
  96     delay(100);
  97 }
  98
  99
 100 bool
 101 GpsLea6T::disable_timepulse2_frequency(void)
 102 {
 103     timepulse2_cfg.ant_cable_delay = 0;
 104     timepulse2_cfg.freq_period = 0;
 105     timepulse2_cfg.freq_period_lock = 0;
 106     timepulse2_cfg.pulse_len_ratio = 2147483648;            //Be aware of the scaling factor 0.5^-32: 50% duty cycle = 0.5 * 0.5^-32
 107     timepulse2_cfg.pulse_len_ratio_lock = 2147483648;
 108     timepulse2_cfg.user_config_delay = 0;
 109     timepulse2_cfg.flags.active = 1;
 110     timepulse2_cfg.flags.lock_gps_freq = 1;
 111     timepulse2_cfg.flags.locked_other_set = 0;
 112     timepulse2_cfg.flags.is_freq = 1;
 113     timepulse2_cfg.flags.is_length = 0;
 114     timepulse2_cfg.flags.align_to_tow = 1;
 115     timepulse2_cfg.flags.polarity = 1;
 116     timepulse2_cfg.flags.grid_utc_gps = 0;
 117
 118     return send_cfg_tp5(1);
 119 }
 120
 121 bool
 122 GpsLea6T::set_timepulse2_frequency(uint32_t freq, uint32_t freq_lock)
 123 {
 124     timepulse2_cfg.ant_cable_delay = 0;
 125     timepulse2_cfg.freq_period = freq;
 126     timepulse2_cfg.freq_period_lock = freq_lock;
 127     timepulse2_cfg.pulse_len_ratio = 2147483648;            //Be aware of the scaling factor 0.5^-32: 50% duty cycle = 0.5 * 0.5^-32
 128     timepulse2_cfg.pulse_len_ratio_lock = 2147483648;
 129     timepulse2_cfg.user_config_delay = 0;
 130     timepulse2_cfg.flags.active = 1;
 131     timepulse2_cfg.flags.lock_gps_freq = 1;
 132     timepulse2_cfg.flags.locked_other_set = 0;
 133     timepulse2_cfg.flags.is_freq = 1;
 134     timepulse2_cfg.flags.is_length = 0;
 135     timepulse2_cfg.flags.align_to_tow = 1;
 136     timepulse2_cfg.flags.polarity = 1;
 137     timepulse2_cfg.flags.grid_utc_gps = 0;
 138
 139     return send_cfg_tp5(1);
 140 }
 141
 142
 143 bool
 144 GpsLea6T::set_timepulse1_pps()
 145 {
 146     timepulse1_cfg.ant_cable_delay = 0;
 147     timepulse1_cfg.freq_period = 1;
 148     timepulse1_cfg.freq_period_lock = 1;
 149     timepulse1_cfg.pulse_len_ratio = 500000;            // The scaling only applies to the frequency.
 150     timepulse1_cfg.pulse_len_ratio_lock = 100000;
 151     timepulse1_cfg.user_config_delay = 0;
 152     timepulse1_cfg.flags.active = 1;
 153     timepulse1_cfg.flags.lock_gps_freq = 1;
 154     timepulse1_cfg.flags.locked_other_set = 1;
 155     timepulse1_cfg.flags.is_freq = 1;
 156     timepulse1_cfg.flags.is_length = 1;
 157     timepulse1_cfg.flags.align_to_tow = 1;
 158     timepulse1_cfg.flags.polarity = 1;
 159     timepulse1_cfg.flags.grid_utc_gps = 0;
 160
 161     return send_cfg_tp5(0);
 162 }
 163
 164 void
 165 GpsLea6T::set_uart1_baudrate(uint32_t baudrate)
 166 {
 167     ubx_cfg_prt_uart_t msg;
 168     msg.prt_id = 1;
 169     msg.reserved0 = 0;
 170     msg.tx_ready.en = 0;
 171     msg.tx_ready.pol = 0;
 172     msg.tx_ready.pin = 0;
 173     msg.tx_ready.thres = 0;
 174     msg.mode.reserved1 = 1;
 175     msg.mode.charlen = 3;
 176     msg.mode.parity = 4;
 177     msg.mode.nstopbits = 0;
 178     msg.mode.unused0 = 0;
 179     msg.mode.unused1 = 0;
 180     msg.mode.unused2 = 0;
 181     msg.mode.unused3 = 0;
 182     msg.baud_rate = baudrate;
 183     msg.in_proto_mask.ubx = 1;
 184     msg.in_proto_mask.nmea = 1;
 185     msg.in_proto_mask.rtcm = 1;
 186     msg.in_proto_mask.unused0 = 0;
 187     msg.out_proto_mask.ubx = 1;
 188     msg.out_proto_mask.nmea = 1;
 189     msg.out_proto_mask.rtcm = 1;
 190     msg.out_proto_mask.unused0 = 0;
 191     msg.reserved4 = 0;
 192     msg.reserved5 = 0;
 193
 194     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_PRT, &msg, sizeof(msg));
 195     //confirmed = wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_PRT);
 196 }
 197
 198
 199 void
 200 GpsLea6T::set_usb_baudrate(uint32_t baudrate)
 201 {
 202     ubx_cfg_prt_uart_t msg;
 203     msg.prt_id = 3;
 204     msg.reserved0 = 0;
 205     msg.tx_ready.en = 0;
 206     msg.tx_ready.pol = 0;
 207     msg.tx_ready.pin = 0;
 208     msg.tx_ready.thres = 0;
 209     msg.mode.reserved1 = 1;
 210     msg.mode.charlen = 3;
 211     msg.mode.parity = 4;
 212     msg.mode.nstopbits = 0;
 213     msg.mode.unused0 = 0;
 214     msg.mode.unused1 = 0;
 215     msg.mode.unused2 = 0;
 216     msg.mode.unused3 = 0;
 217     msg.baud_rate = baudrate;
 218     msg.in_proto_mask.ubx = 1;
 219     msg.in_proto_mask.nmea = 1;
 220     msg.in_proto_mask.rtcm = 1;
 221     msg.in_proto_mask.unused0 = 0;
 222     msg.out_proto_mask.ubx = 1;
 223     msg.out_proto_mask.nmea = 1;
 224     msg.out_proto_mask.rtcm = 1;
 225     msg.out_proto_mask.unused0 = 0;
 226     msg.reserved4 = 0;
 227     msg.reserved5 = 0;
 228
 229     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_PRT, &msg, sizeof(msg));
 230     //confirmed = wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_PRT);
 231 }
 232
 233
 234 uint32_t
 235 GpsLea6T::get_timepulse2_frequency()
 236 {
 237     return timepulse2_cfg.pulse_len_ratio;
 238 }
 239
 240 uint32_t
 241 GpsLea6T::get_timepulse2_frequency_locked()
 242 {
 243     return timepulse2_cfg.freq_period_lock;
 244 }
 245
 246 gpsfix_t
 247 GpsLea6T::get_gps_fix()
 248 {
 249     return (gpsfix_t)navigation_status.gps_fix;
 250 }
 251
 252 bool
 253 GpsLea6T::get_gps_fix_ok()
 254 {
 255     if (navigation_status.flags.gps_fix_ok == 1)
 256     {
 257         return true;
 258     }
 259     else
 260     {
 261         return false;
 262     }
 263 }
 264
 265
 266 bool
 267 GpsLea6T::read()
 268 {
 269     uint8_t data = 0;
 270     uint8_t bytes_available = 0;
 271     bool parsed = false;
 272
 273     bytes_available = serial_port.available();
 274     for (uint16_t k = 0; k < bytes_available; k++)
 275     {
 276         data = serial_port.read();
 277
 278         reset:
 279         switch(read_step)
 280         {
 281             case SYNC1_FOUND:
 282                 //debug_print("SYNC1_FOUND");
 283                 if (data == SYNC_CHAR_2)
 284                 {
 285                     read_step = SYNC2_FOUND;
 286                     //debug_print("SYNC2_FOUND");
 287                     break;
 288                 }
 289                 // If the second sync char is wrong, reset to initial mode and
 290                 // continue the processing of the current data.
 291                 read_step = WAITING_FOR_SYNC1;
 292                 //debug_print("SYNC2 doesn't match.");
 293             case WAITING_FOR_SYNC1:
 294                 if (data == SYNC_CHAR_1)
 295                 {
 296                     read_step = SYNC1_FOUND;
 297                 }
 298                 break;
 299             case SYNC2_FOUND:
 300                 read_msg_class = data;
 301                 read_cka = data;
 302                 read_ckb = data;
 303                 read_step = CLASS_READ;
 304                 //debug_print("CLASS_READ");
 305                 break;
 306             case CLASS_READ:
 307                 read_msg_id = data;
 308                 read_cka += data;
 309                 read_ckb += read_cka;
 310                 read_step = MSGID_READ;
 311                 //debug_print("MSGID_READ");
 312                 break;
 313             case MSGID_READ:
 314                 read_payload_length = data;
 315                 read_cka += data;
 316                 read_ckb += read_cka;
 317                 read_step = LENGTH_BYTE1_READ;
 318                 //debug_print("LENGTH_BYTE1_READ");
 319                 break;
 320             case LENGTH_BYTE1_READ:
 321                 read_payload_length += (uint16_t)(data << 8);
 322                 read_cka += data;
 323                 read_ckb += read_cka;
 324                 read_payload_counter = 0;
 325                 read_step = LENGTH_BYTE2_READ;
 326                 //debug_print("LENGTH_BYTE2_READ");
 327                 if(read_payload_length > 256) {
 328                     //debug_print("Too large payload");
 329                     read_payload_length = 0;
 330                     read_step = WAITING_FOR_SYNC1;
 331                     goto reset;
 332                 }
 333                 break;
 334             case LENGTH_BYTE2_READ:
 335                 //debug_print("LENGTH_BYTE2_READ");
 336                 if (read_payload_counter < sizeof(read_buffer))
 337                 {
 338                     read_buffer.bytes[read_payload_counter] = data;
 339                 }
 340                 if (++read_payload_counter == read_payload_length)
 341                 {
 342                     read_step = PAYLOAD_READ;
 343                     //debug_print("PAYLOAD_READ");
 344                 }
 345                 read_cka += data;
 346                 read_ckb += read_cka;
 347                 break;
 348             case PAYLOAD_READ:
 349                 if (read_cka != data)
 350                 {
 351                     //debug_print("Bad cka.");
 352                     read_step = WAITING_FOR_SYNC1;
 353                     goto reset;
 354                 }
 355                 read_step = CKA_OK;
 356                 //debug_print("CKA_OK");
 357                 break;
 358             case CKA_OK:
 359                 read_step = WAITING_FOR_SYNC1;
 360                 if(read_ckb != data)
 361                 {
 362                     //debug_print("Bad ckb.");
 363                     break;
 364                 }
 365                 //debug_print("CKB_OK");
 366                 parsed = parse_ubx();
 367         }
 368     }
 369     return parsed;
 370 }
 371
 372
 373 bool
 374 GpsLea6T::parse_ubx()
 375 {
 376     bool parsed = false;
 377     if (read_msg_class == UBX_CLASS_ACK)
 378     {
 379         debug_print("ACK ");
 380         switch (read_msg_id)
 381         {
 382             case UBX_ACK_ID_ACK:
 383                 msg_ack = read_buffer.ack_ack;
 384                 msg_ack_changed = true;
 385                 /*
 386                 usb_serial.println("Received UBX_ACK_ID_ACK.");
 387                 usb_serial.println(msg_ack.cls_id);
 388                 usb_serial.println(msg_ack.msg_id);
 389                 usb_serial.println("----");
 390                 */
 391                 break;
 392             default:
 393                 handle_unexpected_msg();
 394         }
 395     }
 396     else if (read_msg_class == UBX_CLASS_CFG)
 397     {
 398         switch (read_msg_id)
 399         {
 400             case UBX_CFG_ID_TP5:
 401                 //debug_print("Parsing UBX_CFG_ID_TP5");
 402                 //timepulse2_cfg.tp_idx = read_buffer.cfg_tp5.tp_idx;
 403                 if (read_buffer.cfg_tp5.tp_idx == 0)
 404                 {
 405                     timepulse_cfg = read_buffer.cfg_tp5;
 406                     parsed = true;
 407                     cfg_timepulse1_changed = true;
 408                 }
 409                 else if (read_buffer.cfg_tp5.tp_idx == 1)
 410                 {
 411                     timepulse2_cfg = read_buffer.cfg_tp5;
 412                     parsed = true;
 413                     cfg_timepulse2_changed = true;
 414                 }
 415                 break;
 416             case UBX_CFG_ID_PRT:
 417                 //usb_serial.println("Parsing UBX_CFG_ID_PRT");
 418                 cfg_prt_uart = read_buffer.cfg_prt_uart;
 419                 parsed = true;
 420                 cfg_prt_uart_changed = true;
 421                 break;
 422             default:
 423                 handle_unexpected_msg();
 424         }
 425     }
 426     else if (read_msg_class == UBX_CLASS_NAV)
 427     {
 428         switch (read_msg_id)
 429         {
 430             case UBX_NAV_ID_STATUS:
 431                 //debug_print("Parsing UBX_NAV_STATUS");
 432                 navigation_status = read_buffer.nav_status;
 433                 nav_status_changed = true;
 434                 break;
 435
 436             case UBX_NAV_ID_TIMEUTC:
 437                 //debug_print("Parsing UBX_NAV_TIMEUTC");
 438                 timeutc = read_buffer.nav_timeutc;
 439                 nav_timeutc_changed = true;
 440                 break;
 441
 442             case UBX_NAV_ID_POSLLH:
 443                 //debug_print("Parsing UBX_NAV_TIMEUTC");
 444                 nav_position = read_buffer.nav_posllh;
 445                 nav_position_changed = true;
 446                 break;
 447
 448             default:
 449                 handle_unexpected_msg();
 450         }
 451
 452     }
 453     else if (read_msg_class == UBX_CLASS_MON)
 454     {
 455
 456     }
 457     else if (read_msg_class == UBX_CLASS_TIM)
 458     {
 459         switch (read_msg_id)
 460         {
 461             case UBX_TIM_ID_TP:
 462                 //debug_print("Parsing UBX_TIM_TP");
 463                 if (!timestamp_buf_empty)
 464                 {
 465                     timestamp_race_error = true;
 466                 }
 467                 next_timepulse_buf = read_buffer.tim_tp;
 468                 timestamp_buf_empty = false;
 469                 if (!timestamp_init)
 470                 {
 471                     timestamp_init = true;
 472                 }
 473                 //usb_serial.println(read_buffer.tim_tp.week);
 474                 //usb_serial.println(read_buffer.tim_tp.tow_ms);
 475                 //usb_serial.println(read_buffer.tim_tp.tow_sub_ms);
 476                 break;
 477
 478             default:
 479                 handle_unexpected_msg();
 480         }
 481     }
 482     else
 483     {
 484         handle_unexpected_msg();
 485     }
 486
 487     return parsed;
 488 }
 489
 490
 491 void
 492 GpsLea6T::handle_unexpected_msg()
 493 {
 494     debug_print("Handling an unexpected UBX message.");
 495 }
 496
 497
 498 void
 499 GpsLea6T::send_ublox_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t length)
 500 {
 501     ubx_header_t header;
 502     uint8_t ck_a=0, ck_b=0;
 503     header.sync_char1 = SYNC_CHAR_1;
 504     header.sync_char2 = SYNC_CHAR_2;
 505     header.msg_class = msg_class;
 506     header.msg_id = msg_id;
 507     header.payload_length = length;
 508
 509     // Update the checksum using the relevant header part.
 510     update_ubx_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 511
 512     // Update the checksum using the payload.
 513     update_ubx_checksum((uint8_t *)msg, length, ck_a, ck_b);
 514
 515     // Send the paket to the serial port.
 516     serial_port.write((const uint8_t *)&header, sizeof(header));
 517     serial_port.write((const uint8_t *)msg, length);
 518     serial_port.write((const uint8_t *)&ck_a, 1);
 519     serial_port.write((const uint8_t *)&ck_b, 1);
 520 }
 521
 522
 523 uint8_t
 524 GpsLea6T::disable_default_messages()
 525 {
 526     uint8_t ack_status = 0;
 527     uint8_t targets[2] = {1, 3};
 528     uint8_t n_targets = 2;
 529
 530     // Disable the default messages on the used targest.
 531     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GGA, 0, targets, n_targets))
 532         ack_status |= 1UL << 0;
 533
 534     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GLL, 0, targets, n_targets))
 535         ack_status |= 1UL << 1;
 536
 537     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GSA, 0, targets, n_targets))
 538         ack_status |= 1UL << 2;
 539
 540     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_GSV, 0, targets, n_targets))
 541         ack_status |= 1UL << 3;
 542
 543     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_RMC, 0, targets, n_targets))
 544         ack_status |= 1UL << 4;
 545
 546     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_VTG, 0, targets, n_targets))
 547         ack_status |= 1UL << 5;
 548
 549     if (send_cfg_msg(NMEA_CLASS_STD, NMEA_STD_ID_ZDA, 0, targets, n_targets))
 550         ack_status |= 1UL << 6;
 551
 552     // Disable the NMEA_STD_ID_TXT output using the CFG-INF message.
 553     // Disable the output for the USB port as well.
 554     ubx_cfg_inf_t msg;
 555     msg.protocol_id = 1;
 556     msg.reserved_0 = 0;
 557     msg.reserved_1 = 0;
 558     msg.inf_msg_mask.serial_1.error = 0;
 559     msg.inf_msg_mask.serial_1.warning = 0;
 560     msg.inf_msg_mask.serial_1.notice = 0;
 561     msg.inf_msg_mask.serial_1.debug = 0;
 562     msg.inf_msg_mask.serial_1.test = 0;
 563     msg.inf_msg_mask.serial_1.reserved = 0;
 564     msg.inf_msg_mask.usb.error = 0;
 565     msg.inf_msg_mask.usb.warning = 0;
 566     msg.inf_msg_mask.usb.notice = 0;
 567     msg.inf_msg_mask.usb.debug = 0;
 568     msg.inf_msg_mask.usb.test = 0;
 569     msg.inf_msg_mask.usb.reserved = 0;
 570     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_INF, &msg, sizeof(msg));
 571     if(wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_INF))
 572         ack_status |= 1UL << 7;
 573
 574     return ack_status;
 575 }
 576
 577
 578 bool GpsLea6T::enable_message(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 579 {
 580     // Enable a message on the current target which is used to send this
 581     // message.
 582     bool confirmed = send_cfg_msg(msg_class, msg_id, rate);
 583     return confirmed;
 584 }
 585
 586 bool GpsLea6T::enable_message(uint8_t msg_class, uint8_t msg_id, uint8_t rate, uint8_t target)
 587 {
 588     // Enable a message on the specified target.
 589     uint8_t targets[1] = {target};
 590     uint8_t n_targets = 1;
 591     bool confirmed = send_cfg_msg(msg_class, msg_id, rate, targets, n_targets);
 592     return confirmed;
 593 }
 594
 595 bool GpsLea6T::enable_message(uint8_t msg_class, uint8_t msg_id, uint8_t rate, uint8_t* targets, uint8_t n_targets)
 596 {
 597     // Enable a message on the specified target.
 598     bool confirmed = send_cfg_msg(msg_class, msg_id, rate, targets, n_targets);
 599     return confirmed;
 600 }
 601
 602 void
 603 GpsLea6T::poll_cfg_prt_uart()
 604 {
 605     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_PRT, NULL, 0);
 606 }
 607
 608 void
 609 GpsLea6T::poll_cfg_msg(uint8_t msg_class, uint8_t msg_id)
 610 {
 611     ubx_cfg_msg_rate_poll_t msg;
 612     msg.msg_class = msg_class;
 613     msg.msg_id = msg_id;
 614     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_MSG, &msg, sizeof(msg));
 615 }
 616
 617 bool
 618 GpsLea6T::send_cfg_msg(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 619 {
 620     bool confirmed = false;
 621     ubx_cfg_msg_rate_t msg;
 622     msg.msg_class = msg_class;
 623     msg.msg_id = msg_id;
 624     msg.rate = rate;
 625     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_MSG, &msg, sizeof(msg));
 626     confirmed = wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_MSG);
 627     return confirmed;
 628 }
 629
 630 bool
 631 GpsLea6T::send_cfg_msg(uint8_t msg_class, uint8_t msg_id, uint8_t rate, uint8_t* targets, uint8_t n_targets)
 632 {
 633     bool confirmed = false;
 634     ubx_cfg_msg_rate_all_t msg;
 635     msg.msg_class = msg_class;
 636     msg.msg_id = msg_id;
 637     msg.rate_ddc = 0;
 638     msg.rate_uart1 = 0;
 639     msg.rate_uart2 = 0;
 640     msg.rate_usb = 0;
 641     msg.rate_spi = 0;
 642     msg.rate_reserved = 0;
 643
 644     for (int k = 0; k < n_targets; k++)
 645     {
 646         switch(targets[k])
 647         {
 648             case 1:
 649                 msg.rate_uart1 = rate;
 650                 break;
 651             case 3:
 652                 msg.rate_usb = rate;
 653                 break;
 654         }
 655     }
 656     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_MSG, &msg, sizeof(msg));
 657     confirmed = wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_MSG);
 658     return confirmed;
 659 }
 660
 661 void
 662 GpsLea6T::poll_cfg_tp5(uint8_t timepulse_id)
 663 {
 664     ubx_cfg_tp5_poll_t msg;
 665     msg.tp_idx = timepulse_id;
 666     send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_TP5, &msg, sizeof(msg));
 667 }
 668
 669 bool
 670 GpsLea6T::send_cfg_tp5(uint8_t tp_idx)
 671 {
 672     if (tp_idx == 0)
 673     {
 674         send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_TP5, &timepulse1_cfg, sizeof(timepulse1_cfg));
 675     }
 676     else if (tp_idx == 1)
 677     {
 678         send_ublox_message(UBX_CLASS_CFG, UBX_CFG_ID_TP5, &timepulse2_cfg, sizeof(timepulse2_cfg));
 679     }
 680
 681     return wait_for_cfg_ack(UBX_CLASS_CFG, UBX_CFG_ID_TP5);
 682 }
 683
 684
 685 void
 686 GpsLea6T::poll_nav_status()
 687 {
 688     send_ublox_message(UBX_CLASS_NAV, UBX_NAV_ID_STATUS, NULL, 0);
 689 }
 690
 691
 692 void
 693 GpsLea6T::poll_nav_posllh()
 694 {
 695     send_ublox_message(UBX_CLASS_NAV, UBX_NAV_ID_POSLLH, NULL, 0);
 696 }
 697
 698
 699 bool
 700 GpsLea6T::wait_for_cfg_ack(uint8_t msg_class, uint8_t msg_id)
 701 {
 702     unsigned timeout = 1000;          // The timeout in milliseconds.
 703     unsigned long start = 0;
 704     unsigned long current = 0;
 705     unsigned long elapsed = 0;
 706     uint8_t gps_byte_available = 0;
 707
 708
 709     start = millis();
 710     while (elapsed < timeout)
 711     {
 712         gps_byte_available = serial_port.available();
 713         if (gps_byte_available > 0)
 714         {
 715             read();
 716         }
 717
 718         if (msg_ack_changed)
 719         {
 720             if ((msg_ack.cls_id == msg_class) && (msg_ack.msg_id == msg_id))
 721             {
 722                 usb_serial.println("Message acknowledged.");
 723                 msg_ack_changed = false;
 724                 return true;
 725             }
 726             else
 727             {
 728                 // TODO: Handle the reception of ACK messages not fitting the
 729                 // required msg_class and msg_id.
 730                 msg_ack_changed = false;
 731             }
 732         }
 733         current = millis();
 734         elapsed = current - start;
 735     }
 736     usb_serial.println("GPS ACK Timeout");
 737     usb_serial.println(msg_class);
 738     usb_serial.println(msg_id);
 739     usb_serial.println("#####");
 740     return false;
 741 }
 742
 743 uint32_t
 744 GpsLea6T::detect_baud_rate(uint32_t baud_1, uint32_t baud_2)
 745 {
 746     uint32_t active_baud = 0;
 747
 748     // Try the first baud rate.
 749     if (test_baud_rate(baud_1))
 750         active_baud = cfg_prt_uart.baud_rate;
 751     else if (test_baud_rate(baud_2))
 752         active_baud = cfg_prt_uart.baud_rate;
 753
 754     return active_baud;
 755 }
 756
 757 bool
 758 GpsLea6T::test_baud_rate(uint32_t baud_rate)
 759 {
 760     bool confirmed = false;
 761     serial_port.end();
 762     delay(100);
 763     serial_port.begin(baud_rate);
 764     delay(100);
 765     confirmed = poll_baud_rate_blocking(baud_rate);
 766     serial_port.end();
 767     delay(100);
 768     return confirmed;
 769 }
 770
 771 bool
 772 GpsLea6T::poll_baud_rate_blocking(uint32_t baud_rate)
 773 {
 774     unsigned timeout = 3000;          // The timeout in milliseconds.
 775     unsigned long start = 0;
 776     unsigned long current = 0;
 777     unsigned long elapsed = 0;
 778     uint8_t gps_byte_available = 0;
 779
 780     poll_cfg_prt_uart();
 781     start = millis();
 782     while (elapsed < timeout)
 783     {
 784         gps_byte_available = serial_port.available();
 785         if (gps_byte_available > 0)
 786         {
 787             read();
 788         }
 789         if (cfg_prt_uart_changed)
 790         {
 791             cfg_prt_uart_changed = false;
 792             usb_serial.println("Working baud rate found.");
 793             usb_serial.println(baud_rate);
 794             return true;
 795         }
 796         current = millis();
 797         elapsed = current - start;
 798     }
 799     usb_serial.println("Baud Timeout");
 800     usb_serial.println(baud_rate);
 801     usb_serial.println("#####");
 802     return false;
 803 }
 804
 805
 806 void
 807 GpsLea6T::enable_usb()
 808 {
 809     // Enable the usb power supply. The pin is active low.
 810     digitalWrite(GPS_ENABLE_USB, 0);
 811
 812     // Configure the port baudrate.
 813     set_usb_baudrate(GPS_USB_BAUDRATE);
 814
 815     // Enable the raw messages needed for post processing with rtklib.
 816     enable_message(UBX_CLASS_RXM, UBX_RXM_ID_RAW, 1, 3);
 817     enable_message(UBX_CLASS_RXM, UBX_RXM_ID_SFRB, 1, 3);
 818
 819 }
 820
 821
 822 void
 823 GpsLea6T::disable_usb()
 824 {
 825     // Disable the usb power supply. The pin is active low.
 826     digitalWrite(GPS_ENABLE_USB, 1);
 827
 828     // Setting the rate to 0 disables the message.
 829     enable_message(UBX_CLASS_RXM, UBX_RXM_ID_RAW, 0, 3);
 830     enable_message(UBX_CLASS_RXM, UBX_RXM_ID_SFRB, 0, 3);
 831
 832 }