src/main/io/gps_ublox.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdlib.h>
  20 #include <stdint.h>
  21 #include <ctype.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include "platform.h"
  26 #include "build_config.h"
  27 #include "debug.h"
  28
  29 #include "common/maths.h"
  30 #include "common/axis.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/system.h"
  34 #include "drivers/serial.h"
  35 #include "drivers/serial_uart.h"
  36
  37 #include "io/serial.h"
  38 #include "io/gps.h"
  39 #include "io/gps_private.h"
  40
  41 #include "flight/gps_conversion.h"
  42
  43 #include "config/config.h"
  44 #include "config/runtime_config.h"
  45
  46 #if defined(GPS) && defined(GPS_PROTO_UBLOX)
  47
  48 //#define GPS_PROTO_UBLOX_NEO7PLUS
  49 #define GPS_VERSION_DETECTION_TIMEOUT_MS    300
  50
  51 static const char * baudInitData[GPS_BAUDRATE_COUNT] = {
  52     "$PUBX,41,1,0003,0001,115200,0*1E\r\n",     // GPS_BAUDRATE_115200
  53     "$PUBX,41,1,0003,0001,57600,0*2D\r\n",      // GPS_BAUDRATE_57600
  54     "$PUBX,41,1,0003,0001,38400,0*26\r\n",      // GPS_BAUDRATE_38400
  55     "$PUBX,41,1,0003,0001,19200,0*23\r\n",      // GPS_BAUDRATE_19200
  56     "$PUBX,41,1,0003,0001,9600,0*16\r\n"        // GPS_BAUDRATE_9600
  57 };
  58
  59 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
  60 static const uint8_t ubloxVerPoll[] = {
  61     0xB5, 0x62, 0x0A, 0x04, 0x00, 0x00, 0x0E, 0x34,          // MON-VER - Poll version info
  62 };
  63 #endif
  64
  65 static const uint8_t ubloxInit_NAV5_Pedestrian[] = {
  66     0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x03, 0x03, 0x00,           // CFG-NAV5 - Set engine settings (original MWII code)
  67     0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00,           // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
  68     0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00,           // capturing the data from the U-Center binary console.
  69     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xC2
  70 };
  71
  72 static const uint8_t ubloxInit_NAV5_Airborne4G[] = {
  73     0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x02, 0x00,           // CFG-NAV5 - Set engine settings
  74     0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00,           // Airborne <4G 3D fix only
  75     0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
  76     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xFF
  77 };
  78
  79 static const uint8_t ubloxInit_NAVX5[] = {
  80     0xB5, 0x62, 0x06, 0x23, 0x28, 0x00, 0x00, 0x00, 0x4C, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // CFG-NAVX5 min 5 SV
  81     0x05, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  82     0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7C, 0xCD,
  83 };
  84
  85 static const uint8_t ubloxInit_MSG_NMEA[] = {
  86     // DISABLE NMEA messages
  87     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19,           // VGS: Course over ground and Ground speed
  88     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15,           // GSV: GNSS Satellites in View
  89     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11,           // GLL: Latitude and longitude, with time of position fix and status
  90     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F,           // GGA: Global positioning system fix data
  91     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13,           // GSA: GNSS DOP and Active Satellites
  92     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17            // RMC: Recommended Minimum data
  93 };
  94
  95 static const uint8_t ubloxInit_MSG_UBX_POSLLH[] = {
  96     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47,           // set POSLLH MSG rate
  97     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49,           // set STATUS MSG rate
  98     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F,           // set SOL MSG rate
  99     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x05, 0x40, 0xA7,           // set SVINFO MSG rate (evey 5 cycles - low bandwidth)
 100     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67            // set VELNED MSG rate
 101 };
 102
 103 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 104 static const uint8_t ubloxInit_MSG_UBX_PVT[] = {
 105     0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0xB9, // disable POSLLH
 106     0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0xC0, // disable STATUS
 107     0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xD5, // disable SOL
 108     0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x4A, 0x2D, // enable SVINFO 10 cycle
 109     0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x22, 0x29, // disable VELNED
 110     0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x07, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18, 0xE1  // enable PVT 1 cycle
 111 };
 112 #endif
 113
 114 static const uint8_t ubloxInit_RATE_5Hz[] = {
 115     0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A  // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
 116 };
 117
 118 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 119 static const uint8_t ubloxInit_RATE_10Hz[] = {
 120     0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12, // set rate to 10Hz (measurement period: 100ms, navigation rate: 1 cycle)
 121 };
 122 #endif
 123
 124 // UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
 125 // SBAS Configuration Settings Desciption, Page 4/210
 126 // 31.21 CFG-SBAS (0x06 0x16), Page 142/210
 127 // A.10 SBAS Configuration (UBX-CFG-SBAS), Page 198/210 - GPS.G6-SW-10018-F
 128
 129 #define UBLOX_SBAS_MESSAGE_LENGTH 16
 130 typedef struct ubloxSbas_s {
 131     sbasMode_e mode;
 132     uint8_t message[UBLOX_SBAS_MESSAGE_LENGTH];
 133 } ubloxSbas_t;
 134
 135 // Note: these must be defined in the same order is sbasMode_e since no lookup table is used.
 136 static const ubloxSbas_t ubloxSbas[] = {
 137     // NOTE this could be optimized to save a few bytes of flash space since the same prefix is used for each command.
 138     { SBAS_AUTO,  { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5}},
 139     { SBAS_EGNOS, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41}},
 140     { SBAS_WAAS,  { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x04, 0xE0, 0x04, 0x00, 0x19, 0x9D}},
 141     { SBAS_MSAS,  { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
 142     { SBAS_GAGAN, { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
 143 };
 144
 145 // UBX support
 146 typedef struct {
 147     uint8_t preamble1;
 148     uint8_t preamble2;
 149     uint8_t msg_class;
 150     uint8_t msg_id;
 151     uint16_t length;
 152 } ubx_header;
 153
 154 typedef struct {
 155     char swVersion[30];      // Zero-terminated Software Version String
 156     char hwVersion[10];      // Zero-terminated Hardware Version String
 157 } ubx_mon_ver;
 158
 159 typedef struct {
 160     uint32_t time;              // GPS msToW
 161     int32_t longitude;
 162     int32_t latitude;
 163     int32_t altitude_ellipsoid;
 164     int32_t altitude_msl;
 165     uint32_t horizontal_accuracy;
 166     uint32_t vertical_accuracy;
 167 } ubx_nav_posllh;
 168
 169 typedef struct {
 170     uint32_t time;              // GPS msToW
 171     uint8_t fix_type;
 172     uint8_t fix_status;
 173     uint8_t differential_status;
 174     uint8_t res;
 175     uint32_t time_to_first_fix;
 176     uint32_t uptime;            // milliseconds
 177 } ubx_nav_status;
 178
 179 typedef struct {
 180     uint32_t time;
 181     int32_t time_nsec;
 182     int16_t week;
 183     uint8_t fix_type;
 184     uint8_t fix_status;
 185     int32_t ecef_x;
 186     int32_t ecef_y;
 187     int32_t ecef_z;
 188     uint32_t position_accuracy_3d;
 189     int32_t ecef_x_velocity;
 190     int32_t ecef_y_velocity;
 191     int32_t ecef_z_velocity;
 192     uint32_t speed_accuracy;
 193     uint16_t position_DOP;
 194     uint8_t res;
 195     uint8_t satellites;
 196     uint32_t res2;
 197 } ubx_nav_solution;
 198
 199 typedef struct {
 200     uint32_t time;              // GPS msToW
 201     int32_t ned_north;
 202     int32_t ned_east;
 203     int32_t ned_down;
 204     uint32_t speed_3d;
 205     uint32_t speed_2d;
 206     int32_t heading_2d;
 207     uint32_t speed_accuracy;
 208     uint32_t heading_accuracy;
 209 } ubx_nav_velned;
 210
 211 typedef struct {
 212     uint8_t chn;                // Channel number, 255 for SVx not assigned to channel
 213     uint8_t svid;               // Satellite ID
 214     uint8_t flags;              // Bitmask
 215     uint8_t quality;            // Bitfield
 216     uint8_t cno;                // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
 217     uint8_t elev;               // Elevation in integer degrees
 218     int16_t azim;               // Azimuth in integer degrees
 219     int32_t prRes;              // Pseudo range residual in centimetres
 220 } ubx_nav_svinfo_channel;
 221
 222 typedef struct {
 223     uint32_t time;              // GPS Millisecond time of week
 224     uint8_t numCh;              // Number of channels
 225     uint8_t globalFlags;        // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
 226     uint16_t reserved2;         // Reserved
 227     ubx_nav_svinfo_channel channel[16];         // 16 satellites * 12 byte
 228 } ubx_nav_svinfo;
 229
 230 typedef struct {
 231     uint32_t time; // GPS msToW
 232     uint16_t year;
 233     uint8_t month;
 234     uint8_t day;
 235     uint8_t hour;
 236     uint8_t min;
 237     uint8_t sec;
 238     uint8_t valid;
 239     uint32_t tAcc;
 240     int32_t nano;
 241     uint8_t fix_type;
 242     uint8_t fix_status;
 243     uint8_t reserved1;
 244     uint8_t satellites;
 245     int32_t longitude;
 246     int32_t latitude;
 247     int32_t altitude_ellipsoid;
 248     int32_t altitude_msl;
 249     uint32_t horizontal_accuracy;
 250     uint32_t vertical_accuracy;
 251     int32_t ned_north;
 252     int32_t ned_east;
 253     int32_t ned_down;
 254     int32_t speed_2d;
 255     int32_t heading_2d;
 256     uint32_t speed_accuracy;
 257     uint32_t heading_accuracy;
 258     uint16_t position_DOP;
 259     uint16_t reserved2;
 260     uint16_t reserved3;
 261 } ubx_nav_pvt;
 262
 263 enum {
 264     PREAMBLE1 = 0xb5,
 265     PREAMBLE2 = 0x62,
 266     CLASS_NAV = 0x01,
 267     CLASS_ACK = 0x05,
 268     CLASS_CFG = 0x06,
 269     CLASS_MON = 0x0A,
 270     MSG_VER = 0x04,
 271     MSG_ACK_NACK = 0x00,
 272     MSG_ACK_ACK = 0x01,
 273     MSG_POSLLH = 0x2,
 274     MSG_STATUS = 0x3,
 275     MSG_SOL = 0x6,
 276     MSG_PVT = 0x7,
 277     MSG_VELNED = 0x12,
 278     MSG_SVINFO = 0x30,
 279     MSG_CFG_PRT = 0x00,
 280     MSG_CFG_RATE = 0x08,
 281     MSG_CFG_SET_RATE = 0x01,
 282     MSG_CFG_NAV_SETTINGS = 0x24
 283 } ubx_protocol_bytes;
 284
 285 enum {
 286     FIX_NONE = 0,
 287     FIX_DEAD_RECKONING = 1,
 288     FIX_2D = 2,
 289     FIX_3D = 3,
 290     FIX_GPS_DEAD_RECKONING = 4,
 291     FIX_TIME = 5
 292 } ubs_nav_fix_type;
 293
 294 enum {
 295     NAV_STATUS_FIX_VALID = 1
 296 } ubx_nav_status_bits;
 297
 298 // Packet checksum accumulators
 299 static uint8_t _ck_a;
 300 static uint8_t _ck_b;
 301
 302 // State machine state
 303 static bool _skip_packet;
 304 static uint8_t _step;
 305 static uint8_t _msg_id;
 306 static uint16_t _payload_length;
 307 static uint16_t _payload_counter;
 308
 309 static bool next_fix;
 310 static uint8_t _class;
 311
 312 // do we have new position information?
 313 static bool _new_position;
 314
 315 // do we have new speed information?
 316 static bool _new_speed;
 317
 318 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
 319 //15:17:55  R -> UBX NAV-STATUS,  Size  24,  'Navigation Status'
 320 //15:17:55  R -> UBX NAV-POSLLH,  Size  36,  'Geodetic Position'
 321 //15:17:55  R -> UBX NAV-VELNED,  Size  44,  'Velocity in WGS 84'
 322 //15:17:55  R -> UBX NAV-CLOCK,  Size  28,  'Clock Status'
 323 //15:17:55  R -> UBX NAV-AOPSTATUS,  Size  24,  'AOP Status'
 324 //15:17:55  R -> UBX 03-09,  Size 208,  'Unknown'
 325 //15:17:55  R -> UBX 03-10,  Size 336,  'Unknown'
 326 //15:17:55  R -> UBX NAV-SOL,  Size  60,  'Navigation Solution'
 327 //15:17:55  R -> UBX NAV,  Size 100,  'Navigation'
 328 //15:17:55  R -> UBX NAV-SVINFO,  Size 328,  'Satellite Status and Information'
 329
 330 // from the UBlox6 document, the largest payout we receive i the NAV-SVINFO and the payload size
 331 // is calculated as 8 + 12*numCh.  numCh in the case of a Glonass receiver is 28.
 332 #define MAX_UBLOX_PAYLOAD_SIZE 344
 333 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
 334
 335 // Receive buffer
 336 static union {
 337     ubx_nav_posllh posllh;
 338     ubx_nav_status status;
 339     ubx_nav_solution solution;
 340     ubx_nav_velned velned;
 341     ubx_nav_pvt pvt;
 342     ubx_nav_svinfo svinfo;
 343     ubx_mon_ver ver;
 344     uint8_t bytes[UBLOX_BUFFER_SIZE];
 345 } _buffer;
 346
 347 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
 348 {
 349     while (len--) {
 350         *ck_a += *data;
 351         *ck_b += *ck_a;
 352         data++;
 353     }
 354 }
 355
 356
 357 static bool gpsParceFrameUBLOX(void)
 358 {
 359     uint32_t i;
 360
 361     switch (_msg_id) {
 362     case MSG_POSLLH:
 363         //i2c_dataset.time                = _buffer.posllh.time;
 364         gpsSol.llh.lon = _buffer.posllh.longitude;
 365         gpsSol.llh.lat = _buffer.posllh.latitude;
 366         gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
 367         gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy);
 368         gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy);
 369         gpsSol.flags.fix3D = next_fix ? 1 : 0;
 370         _new_position = true;
 371         break;
 372     case MSG_STATUS:
 373         next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
 374         if (!next_fix)
 375             gpsSol.flags.fix3D = 0;
 376         break;
 377     case MSG_SOL:
 378         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
 379         if (!next_fix)
 380             gpsSol.flags.fix3D = 0;
 381         gpsSol.numSat = _buffer.solution.satellites;
 382         break;
 383     case MSG_VELNED:
 384         gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
 385         gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
 386         gpsSol.velNED[0] = _buffer.velned.ned_north;
 387         gpsSol.velNED[1] = _buffer.velned.ned_east;
 388         gpsSol.velNED[2] = _buffer.velned.ned_down;
 389         gpsSol.flags.validVelNE = 1;
 390         gpsSol.flags.validVelD = 1;
 391         _new_speed = true;
 392         break;
 393 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 394     case MSG_PVT:
 395         next_fix = (_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fix_type == FIX_3D);
 396         gpsSol.flags.fix3D = next_fix ? 1 : 0;
 397         gpsSol.llh.lon = _buffer.pvt.longitude;
 398         gpsSol.llh.lat = _buffer.pvt.latitude;
 399         gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10;  //alt in cm
 400         gpsSol.velNED[X]=_buffer.pvt.ned_north / 10;  // to cm/s
 401         gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10;   // to cm/s
 402         gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10;   // to cm/s
 403         gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10;    // to cm/s
 404         gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
 405         gpsSol.numSat = _buffer.pvt.satellites;
 406         gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy);
 407         gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy);
 408         gpsSol.flags.validVelNE = 1;
 409         gpsSol.flags.validVelD = 1;
 410         gpsSol.flags.validEPE = 1;
 411         _new_position = true;
 412         _new_speed = true;
 413         break;
 414     case MSG_VER:
 415         if(_class == CLASS_MON) {
 416             //uint32_t swver = _buffer.ver.swVersion;
 417             gpsState.hwVersion = atoi(_buffer.ver.hwVersion);
 418         }
 419         break;
 420 #endif
 421     case MSG_SVINFO:
 422         gpsSol.numCh = _buffer.svinfo.numCh;
 423         if (gpsSol.numCh > 16)
 424             gpsSol.numCh = 16;
 425         for (i = 0; i < gpsSol.numCh; i++){
 426             gpsSol.svInfo[i].chn = _buffer.svinfo.channel[i].chn;
 427             gpsSol.svInfo[i].svid = _buffer.svinfo.channel[i].svid;
 428             gpsSol.svInfo[i].quality = _buffer.svinfo.channel[i].quality;
 429             gpsSol.svInfo[i].cno =  _buffer.svinfo.channel[i].cno;
 430         }
 431         break;
 432     default:
 433         return false;
 434     }
 435
 436     // we only return true when we get new position and speed data
 437     // this ensures we don't use stale data
 438     if (_new_position && _new_speed) {
 439         gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 440         _new_speed = _new_position = false;
 441         return true;
 442     }
 443
 444     return false;
 445 }
 446
 447 static bool gpsNewFrameUBLOX(uint8_t data)
 448 {
 449     bool parsed = false;
 450
 451     switch (_step) {
 452         case 0: // Sync char 1 (0xB5)
 453             if (PREAMBLE1 == data) {
 454                 _skip_packet = false;
 455                 _step++;
 456             }
 457             break;
 458         case 1: // Sync char 2 (0x62)
 459             if (PREAMBLE2 != data) {
 460                 _step = 0;
 461                 break;
 462             }
 463             _step++;
 464             break;
 465         case 2: // Class
 466             _step++;
 467             _class = data;
 468             _ck_b = _ck_a = data;   // reset the checksum accumulators
 469             break;
 470         case 3: // Id
 471             _step++;
 472             _ck_b += (_ck_a += data);       // checksum byte
 473             _msg_id = data;
 474             break;
 475         case 4: // Payload length (part 1)
 476             _step++;
 477             _ck_b += (_ck_a += data);       // checksum byte
 478             _payload_length = data; // payload length low byte
 479             break;
 480         case 5: // Payload length (part 2)
 481             _step++;
 482             _ck_b += (_ck_a += data);       // checksum byte
 483             _payload_length |= (uint16_t)(data << 8);
 484             if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) {
 485                 // we can't receive the whole packet, just log the error and start searching for the next packet.
 486                 gpsStats.errors++;
 487                 _step = 0;
 488                 break;
 489             }
 490             // prepare to receive payload
 491             _payload_counter = 0;
 492             if (_payload_length == 0) {
 493                 _step = 7;
 494             }
 495             break;
 496         case 6:
 497             _ck_b += (_ck_a += data);       // checksum byte
 498             if (_payload_counter < MAX_UBLOX_PAYLOAD_SIZE) {
 499                 _buffer.bytes[_payload_counter] = data;
 500             }
 501             // NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled.  This can happen if garbage data is received.
 502             if (_payload_counter ==  _payload_length - 1) {
 503                 _step++;
 504             }
 505             _payload_counter++;
 506             break;
 507         case 7:
 508             _step++;
 509             if (_ck_a != data) {
 510                 _skip_packet = true;          // bad checksum
 511                 gpsStats.errors++;
 512             }
 513             break;
 514         case 8:
 515             _step = 0;
 516
 517             if (_ck_b != data) {
 518                 gpsStats.errors++;
 519                 break;              // bad checksum
 520             }
 521
 522             gpsStats.packetCount++;
 523
 524             if (_skip_packet) {
 525                 break;
 526             }
 527
 528             if (gpsParceFrameUBLOX()) {
 529                 parsed = true;
 530             }
 531     }
 532
 533     return parsed;
 534 }
 535
 536 // Send UBLOX binary command data and wait until it is completely transmitted
 537 static bool ubxTransmitAutoConfigCommands(const uint8_t * ubxCmdBuf, uint8_t ubxCmdSize) {
 538     while (serialTxBytesFree(gpsState.gpsPort) > 0) {
 539         if (gpsState.autoConfigPosition < ubxCmdSize) {
 540             serialWrite(gpsState.gpsPort, ubxCmdBuf[gpsState.autoConfigPosition]);
 541             gpsState.autoConfigPosition++;
 542         }
 543         else if (isSerialTransmitBufferEmpty(gpsState.gpsPort)) {
 544             gpsState.autoConfigStep++;
 545             gpsState.autoConfigPosition = 0;
 546             return true;
 547         }
 548         else {
 549             return false;
 550         }
 551     }
 552
 553     return false;
 554 }
 555
 556 static bool gpsConfigure(void)
 557 {
 558     switch (gpsState.autoConfigStep) {
 559     case 0: // NAV5
 560         if (gpsState.gpsConfig->navModel == GPS_MODEL_HIGH_G)
 561             ubxTransmitAutoConfigCommands(ubloxInit_NAV5_Airborne4G, sizeof(ubloxInit_NAV5_Airborne4G));
 562         else
 563             ubxTransmitAutoConfigCommands(ubloxInit_NAV5_Pedestrian, sizeof(ubloxInit_NAV5_Pedestrian));
 564
 565         break;
 566
 567     case 1: // NAVX5 - skip
 568         //ubxTransmitAutoConfigCommands(ubloxInit_NAVX5, sizeof(ubloxInit_NAVX5));
 569         gpsState.autoConfigStep++;
 570         break;
 571
 572     case 2: // Disable NMEA messages
 573         ubxTransmitAutoConfigCommands(ubloxInit_MSG_NMEA, sizeof(ubloxInit_MSG_NMEA));
 574         break;
 575
 576     case 3: // Enable UBX messages
 577 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 578         if (gpsState.hwVersion < 70000) {
 579 #endif
 580             ubxTransmitAutoConfigCommands(ubloxInit_MSG_UBX_POSLLH, sizeof(ubloxInit_MSG_UBX_POSLLH));
 581 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 582         }
 583         else {
 584             ubxTransmitAutoConfigCommands(ubloxInit_MSG_UBX_PVT, sizeof(ubloxInit_MSG_UBX_PVT));
 585         }
 586 #endif
 587         break;
 588
 589     case 4: // Configure RATE
 590 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 591         if (gpsState.hwVersion < 70000) {
 592 #endif
 593             ubxTransmitAutoConfigCommands(ubloxInit_RATE_5Hz, sizeof(ubloxInit_RATE_5Hz));
 594 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 595         }
 596         else {
 597             ubxTransmitAutoConfigCommands(ubloxInit_RATE_10Hz, sizeof(ubloxInit_RATE_10Hz));
 598         }
 599 #endif
 600         break;
 601
 602     case 5: // SBAS
 603         ubxTransmitAutoConfigCommands(ubloxSbas[gpsState.gpsConfig->sbasMode].message, UBLOX_SBAS_MESSAGE_LENGTH);
 604         break;
 605
 606     default:
 607         // ublox should be initialised, try receiving
 608         gpsSetState(GPS_RECEIVING_DATA);
 609         break;
 610     }
 611
 612     return false;
 613 }
 614
 615 static bool gpsCheckVersion(void)
 616 {
 617 #ifdef GPS_PROTO_UBLOX_NEO7PLUS
 618     if (gpsState.autoConfigStep == 0) {
 619         ubxTransmitAutoConfigCommands(ubloxVerPoll, sizeof(ubloxVerPoll));
 620     }
 621     else {
 622         // Wait until version found
 623         if (gpsState.hwVersion != 0) {
 624             gpsState.autoConfigStep = 0;
 625             gpsState.autoConfigPosition = 0;
 626             gpsSetState(GPS_CONFIGURE);
 627         }
 628         else if ((millis() - gpsState.lastStateSwitchMs) >= GPS_VERSION_DETECTION_TIMEOUT_MS) {
 629             gpsState.hwVersion = 0;
 630             gpsState.autoConfigStep = 0;
 631             gpsState.autoConfigPosition = 0;
 632             gpsSetState(GPS_CONFIGURE);
 633         }
 634     }
 635 #else
 636     gpsState.hwVersion = 0;
 637     gpsSetState(GPS_CONFIGURE);
 638 #endif
 639     return false;
 640 }
 641
 642 static bool gpsReceiveData(void)
 643 {
 644     bool hasNewData = false;
 645
 646     if (gpsState.gpsPort) {
 647         while (serialRxBytesWaiting(gpsState.gpsPort)) {
 648             uint8_t newChar = serialRead(gpsState.gpsPort);
 649             if (gpsNewFrameUBLOX(newChar)) {
 650                 hasNewData = true;
 651             }
 652         }
 653     }
 654
 655     return hasNewData;
 656 }
 657
 658 static bool gpsInitialize(void)
 659 {
 660     gpsSetState(GPS_CHANGE_BAUD);
 661     return false;
 662 }
 663
 664 static bool gpsChangeBaud(void)
 665 {
 666     if ((gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) && (gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT)) {
 667         // Do the switch only if TX buffer is empty - make sure all init string was sent at the same baud
 668         if (isSerialTransmitBufferEmpty(gpsState.gpsPort)) {
 669             // Cycle through all possible bauds and send init string
 670             serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
 671             serialPrint(gpsState.gpsPort, baudInitData[gpsState.baudrateIndex]);
 672             gpsState.autoBaudrateIndex++;
 673             gpsSetState(GPS_CHANGE_BAUD);   // switch to the same state to reset state transition time
 674         }
 675     }
 676     else {
 677         gpsFinalizeChangeBaud();
 678     }
 679
 680     return false;
 681 }
 682
 683 bool gpsHandleUBLOX(void)
 684 {
 685     // Receive data
 686     bool hasNewData = gpsReceiveData();
 687
 688     // Process state
 689     switch(gpsState.state) {
 690     default:
 691         return false;
 692
 693     case GPS_INITIALIZING:
 694         return gpsInitialize();
 695
 696     case GPS_CHANGE_BAUD:
 697         return gpsChangeBaud();
 698
 699     case GPS_CHECK_VERSION:
 700         return gpsCheckVersion();
 701
 702     case GPS_CONFIGURE:
 703         // Either use specific config file for GPS or let dynamically upload config
 704         if (gpsState.gpsConfig->autoConfig == GPS_AUTOCONFIG_OFF) {
 705             gpsSetState(GPS_RECEIVING_DATA);
 706             return false;
 707         }
 708         else {
 709             return gpsConfigure();
 710         }
 711
 712     case GPS_RECEIVING_DATA:
 713         return hasNewData;
 714     }
 715 }
 716
 717 #endif