CoCalc -- AP_GPS

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_GPS/AP_GPS_UBLOX.cpp
⁹⁵⁶⁵ views
1
/*
2
   This program is free software: you can redistribute it and/or modify
3
   it under the terms of the GNU General Public License as published by
4
   the Free Software Foundation, either version 3 of the License, or
5
   (at your option) any later version.
6

7
   This program is distributed in the hope that it will be useful,
8
   but WITHOUT ANY WARRANTY; without even the implied warranty of
9
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10
   GNU General Public License for more details.
11

12
   You should have received a copy of the GNU General Public License
13
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
14
 */
15

16
//
17
//  u-blox GPS driver for ArduPilot
18
//	Origin code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
19
//  Substantially rewritten for new GPS driver structure by Andrew Tridgell
20
//
21
#include "AP_GPS_UBLOX.h"
22

23
#if AP_GPS_UBLOX_ENABLED
24

25
#include "AP_GPS.h"
26
#include <AP_HAL/Util.h>
27
#include <AP_Logger/AP_Logger.h>
28
#include <GCS_MAVLink/GCS.h>
29
#include "RTCM3_Parser.h"
30
#include <stdio.h>
31

32
#ifndef UBLOX_SPEED_CHANGE
33
    #define UBLOX_SPEED_CHANGE 0
34
#endif
35

36

37
#define UBLOX_DEBUGGING 0
38
#define UBLOX_FAKE_3DLOCK 0
39
#ifndef CONFIGURE_PPS_PIN
40
#define CONFIGURE_PPS_PIN 0
41
#endif
42

43
// this is number of epochs per output. A higher value will reduce
44
// the uart bandwidth needed and allow for higher latency
45
#define RTK_MB_RTCM_RATE 1
46

47
// use this to enable debugging of moving baseline configs
48
#define UBLOX_MB_DEBUGGING 0
49

50
// debug VALGET/VALSET configuration
51
#define UBLOX_CFG_DEBUGGING 0
52

53
extern const AP_HAL::HAL& hal;
54

55
#if UBLOX_DEBUGGING
56
#if defined(HAL_BUILD_AP_PERIPH)
57
 extern "C" {
58
   void can_printf(const char *fmt, ...);
59
 }
60
 # define Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
61
#else
62
 # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
63
#endif
64
#else
65
 # define Debug(fmt, args ...)
66
#endif
67

68
#if UBLOX_MB_DEBUGGING
69
#if defined(HAL_BUILD_AP_PERIPH)
70
 extern "C" {
71
   void can_printf(const char *fmt, ...);
72
 }
73
 # define MB_Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
74
#else
75
 # define MB_Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
76
#endif
77
#else
78
 # define MB_Debug(fmt, args ...)
79
#endif
80

81
#if UBLOX_CFG_DEBUGGING
82
#if defined(HAL_BUILD_AP_PERIPH)
83
 extern "C" {
84
   void can_printf(const char *fmt, ...);
85
 }
86
 # define CFG_Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
87
#else
88
 # define CFG_Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
89
#endif
90
#else
91
 # define CFG_Debug(fmt, args ...)
92
#endif
93

94
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps,
95
                           AP_GPS::Params &_params,
96
                           AP_GPS::GPS_State &_state,
97
                           AP_HAL::UARTDriver *_port,
98
                           AP_GPS::GPS_Role _role) :
99
    AP_GPS_Backend(_gps, _params, _state, _port),
100
    role(_role)
101
{
102
    // stop any config strings that are pending
103
    gps.send_blob_start(state.instance, nullptr, 0);
104

105
    // start the process of updating the GPS rates
106
    _request_next_config();
107

108
#if CONFIGURE_PPS_PIN
109
    _unconfigured_messages |= CONFIG_TP5;
110
#endif
111

112
#if GPS_MOVING_BASELINE
113
    if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) {
114
        rtcm3_parser = NEW_NOTHROW RTCM3_Parser;
115
        if (rtcm3_parser == nullptr) {
116
            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1);
117
        }
118
        _unconfigured_messages |= CONFIG_RTK_MOVBASE;
119
    }
120
    if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
121
        _unconfigured_messages |= CONFIG_RTK_MOVBASE;
122
        state.gps_yaw_configured = true;
123
    }
124
#endif
125
}
126

127
AP_GPS_UBLOX::~AP_GPS_UBLOX()
128
{
129
#if GPS_MOVING_BASELINE
130
    delete rtcm3_parser;
131
#endif
132

133
    free(config_GNSS);
134
}
135

136
#if GPS_MOVING_BASELINE
137
/*
138
  config for F9 GPS in moving baseline base role
139
  See ZED-F9P integration manual section 3.1.5.6.1
140
 */
141
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart1[] {
142
 { ConfigKey::CFG_UART1OUTPROT_RTCM3X, 1},
143
 { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
144
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0},
145
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
146
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, RTK_MB_RTCM_RATE},
147
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, RTK_MB_RTCM_RATE},
148
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, RTK_MB_RTCM_RATE},
149
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, RTK_MB_RTCM_RATE},
150
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, RTK_MB_RTCM_RATE},
151
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, RTK_MB_RTCM_RATE},
152
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, RTK_MB_RTCM_RATE},
153
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
154
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
155
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
156
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
157
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
158
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
159
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
160
};
161

162
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart2[] {
163
 { ConfigKey::CFG_UART2_ENABLED, 1},
164
 { ConfigKey::CFG_UART2_BAUDRATE, 460800},
165
 { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 1},
166
 { ConfigKey::CFG_UART1OUTPROT_RTCM3X, 0},
167
 { ConfigKey::CFG_UART1INPROT_RTCM3X, 1},
168
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
169
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0},
170
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, RTK_MB_RTCM_RATE},
171
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, RTK_MB_RTCM_RATE},
172
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, RTK_MB_RTCM_RATE},
173
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, RTK_MB_RTCM_RATE},
174
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, RTK_MB_RTCM_RATE},
175
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, RTK_MB_RTCM_RATE},
176
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, RTK_MB_RTCM_RATE},
177
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
178
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
179
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
180
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
181
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
182
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
183
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
184
};
185

186

187
/*
188
  config for F9 GPS in moving baseline rover role
189
  See ZED-F9P integration manual section 3.1.5.6.1.
190
  Note that we list the RTCM msg types as 0 to prevent getting RTCM
191
  data from a GPS previously configured as a base
192
 */
193
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart1[] {
194
 { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
195
 { ConfigKey::CFG_UART1INPROT_RTCM3X, 1},
196
 { ConfigKey::CFG_UART2INPROT_RTCM3X, 0},
197
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1},
198
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
199
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
200
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
201
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
202
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
203
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
204
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
205
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
206
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
207
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
208
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
209
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
210
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
211
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
212
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
213
};
214

215
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
216
 { ConfigKey::CFG_UART2_ENABLED, 1},
217
 { ConfigKey::CFG_UART2_BAUDRATE, 460800},
218
 { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
219
 { ConfigKey::CFG_UART2INPROT_RTCM3X, 1},
220
 { ConfigKey::CFG_UART1INPROT_RTCM3X, 0},
221
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1},
222
 { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
223
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
224
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
225
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
226
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
227
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
228
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
229
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
230
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
231
 { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
232
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
233
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
234
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
235
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
236
 { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
237
};
238
#endif // GPS_MOVING_BASELINE
239

240
/*
241
  config changes for M10
242
  we need to use B1C not B1 signal for Beidou on M10 to allow solid 5Hz,
243
  and also disable Glonass and enable QZSS
244
 */
245
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_M10[] {
246
 { ConfigKey::CFG_SIGNAL_BDS_ENA, 1},
247
 { ConfigKey::CFG_SIGNAL_BDS_B1_ENA, 0},
248
 { ConfigKey::CFG_SIGNAL_BDS_B1C_ENA, 1},
249
 { ConfigKey::CFG_SIGNAL_GLO_ENA, 0},
250
 { ConfigKey::CFG_SIGNAL_QZSS_ENA, 1},
251
 { ConfigKey::CFG_SIGNAL_QZSS_L1CA_ENA, 1},
252
 { ConfigKey::CFG_SIGNAL_QZSS_L1S_ENA, 1},
253
 { ConfigKey::CFG_NAVSPG_DYNMODEL, 8}, // Air < 4g
254
};
255

256

257
/*
258
  config changes for L5 modules
259
*/
260
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_ena[] {
261
    {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 1},
262
    {ConfigKey::CFG_SIGNAL_GPS_L5_ENA, 1},
263
};
264

265
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] {
266
    {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0},
267
};
268

269
void
270
AP_GPS_UBLOX::_request_next_config(void)
271
{
272
    // don't request config if we shouldn't configure the GPS
273
    if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
274
        return;
275
    }
276

277
    // Ensure there is enough space for the largest possible outgoing message
278
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
279
        // not enough space - do it next time
280
        return;
281
    }
282

283
    if ((_unconfigured_messages & CONFIG_RATE_SOL) != 0 && havePvtMsg) {
284
        /*
285
          we don't need SOL if we have PVT and TIMEGPS. This is needed
286
          as F9P doesn't support the SOL message
287
         */
288
        _unconfigured_messages &= ~CONFIG_RATE_SOL;
289
    }
290

291
    Debug("Unconfigured messages: 0x%x Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message);
292

293
    // check AP_GPS_UBLOX.h for the enum that controls the order.
294
    // This switch statement isn't maintained against the enum in order to reduce code churn
295
    switch (_next_message++) {
296
    case STEP_PVT:
297
        if(!_request_message_rate(CLASS_NAV, MSG_PVT)) {
298
            _next_message--;
299
        }
300
        break;
301
    case STEP_TIMEGPS:
302
        if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) {
303
            _next_message--;
304
        }
305
        break;
306
    case STEP_PORT:
307
        _request_port();
308
        break;
309
    case STEP_POLL_SVINFO:
310
        // not required once we know what generation we are on
311
        if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) {
312
            if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) {
313
                _next_message--;
314
            }
315
        }
316
        break;
317
    case STEP_POLL_SBAS:
318
        if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
319
            _send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0);
320
        } else {
321
            _unconfigured_messages &= ~CONFIG_SBAS;
322
        }
323
        break;
324
    case STEP_POLL_NAV:
325
        if (!_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0)) {
326
            _next_message--;
327
        }
328
        break;
329
    case STEP_POLL_GNSS:
330
        if (supports_F9_config()) {
331
            if (last_configured_gnss != params.gnss_mode) {
332
                _unconfigured_messages |= CONFIG_F9;
333
            }
334
            break;
335
        }
336
        if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) {
337
            _next_message--;
338
        }
339
        break;
340
    case STEP_POLL_TP5:
341
#if CONFIGURE_PPS_PIN
342
        if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) {
343
            _next_message--;
344
        }
345
#endif
346
        break;
347
    case STEP_NAV_RATE:
348
        if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) {
349
            _next_message--;
350
        }
351
        break;
352
    case STEP_POSLLH:
353
        if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
354
            _next_message--;
355
        }
356
        break;
357
    case STEP_STATUS:
358
        if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
359
            _next_message--;
360
        }
361
        break;
362
    case STEP_SOL:
363
        if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
364
            _next_message--;
365
        }
366
        break;
367
    case STEP_VELNED:
368
        if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
369
            _next_message--;
370
        }
371
        break;
372
    case STEP_DOP:
373
       if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
374
            _next_message--;
375
        }
376
        break;
377
    case STEP_MON_HW:
378
        if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
379
            _next_message--;
380
        }
381
        break;
382
    case STEP_MON_HW2:
383
        if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
384
            _next_message--;
385
        }
386
        break;
387
    case STEP_RAW:
388
#if UBLOX_RXM_RAW_LOGGING
389
        if(gps._raw_data == 0) {
390
            _unconfigured_messages &= ~CONFIG_RATE_RAW;
391
        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
392
            _next_message--;
393
        }
394
#else
395
        _unconfigured_messages & = ~CONFIG_RATE_RAW;
396
#endif
397
        break;
398
    case STEP_RAWX:
399
#if UBLOX_RXM_RAW_LOGGING
400
        if(gps._raw_data == 0) {
401
            _unconfigured_messages &= ~CONFIG_RATE_RAW;
402
        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
403
            _next_message--;
404
        }
405
#else
406
        _unconfigured_messages & = ~CONFIG_RATE_RAW;
407
#endif
408
        break;
409
    case STEP_VERSION:
410
        if(!_have_version && !hal.util->get_soft_armed()) {
411
            _request_version();
412
        } else {
413
            _unconfigured_messages &= ~CONFIG_VERSION;
414
        }
415
        break;
416
    case STEP_TMODE:
417
        if (supports_F9_config()) {
418
            if (!_configure_valget(ConfigKey::TMODE_MODE)) {
419
                _next_message--;
420
            }
421
        }
422
        break;
423
    case STEP_RTK_MOVBASE:
424
#if GPS_MOVING_BASELINE
425
        if (supports_F9_config()) {
426
            static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1");
427
            static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2");
428
            static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart1), "done_mask too small, rover1");
429
            static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart2), "done_mask too small, rover2");
430
            if (role == AP_GPS::GPS_ROLE_MB_BASE) {
431
                const config_list *list = mb_use_uart2()?config_MB_Base_uart2:config_MB_Base_uart1;
432
                uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Base_uart2):ARRAY_SIZE(config_MB_Base_uart1);
433
                if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
434
                    _next_message--;
435
                }
436
            }
437
            if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
438
                const config_list *list = mb_use_uart2()?config_MB_Rover_uart2:config_MB_Rover_uart1;
439
                uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Rover_uart2):ARRAY_SIZE(config_MB_Rover_uart1);
440
                if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
441
                    _next_message--;
442
                }
443
            }
444
        }
445
#endif
446
        break;
447
    case STEP_TIM_TM2:
448
#if UBLOX_TIM_TM2_LOGGING
449
        if(!_request_message_rate(CLASS_TIM, MSG_TIM_TM2)) {
450
            _next_message--;
451
        }
452
#else
453
        _unconfigured_messages &= ~CONFIG_TIM_TM2;
454
#endif
455
        break;
456

457
    case STEP_F9: {
458
        if (_hardware_generation == UBLOX_F9 ||
459
            _hardware_generation == UBLOX_M10) {
460
            uint8_t cfg_count = populate_F9_gnss();
461
            // special handling of F9 config
462
            if (cfg_count > 0) {
463
                CFG_Debug("Sending F9 settings, GNSS=%u", unsigned(params.gnss_mode));
464

465
                if (!_configure_list_valset(config_GNSS, cfg_count, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
466
                    _next_message--;
467
                    break;
468
                }
469
                _f9_config_time = AP_HAL::millis();
470
            }
471
        }
472
        break;
473
    }
474

475
    case STEP_F9_VALIDATE: {
476
        if (_hardware_generation == UBLOX_F9 ||
477
            _hardware_generation == UBLOX_M10) {
478
            // GNSS takes 0.5s to reset
479
            if (AP_HAL::millis() - _f9_config_time < 500) {
480
                _next_message--;
481
                break;
482
            }
483
            _f9_config_time = 0;
484
            uint8_t cfg_count = populate_F9_gnss();
485
            // special handling of F9 config
486
            if (cfg_count > 0) {
487
                CFG_Debug("Validating F9 settings, GNSS=%u", unsigned(params.gnss_mode));
488
                // now validate all of the settings, this is a no-op if the first call succeeded
489
                if (!_configure_config_set(config_GNSS, cfg_count, CONFIG_F9, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
490
                    _next_message--;
491
                }
492
            }
493
        }
494
        break;
495
    }
496
    case STEP_M10: {
497
        if (_hardware_generation == UBLOX_M10) {
498
            // special handling of M10 config
499
            const config_list *list = config_M10;
500
            const uint8_t list_length = ARRAY_SIZE(config_M10);
501
            Debug("Sending M10 settings");
502
            if (!_configure_config_set(list, list_length, CONFIG_M10, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
503
                _next_message--;
504
            }
505
        }
506
        break;
507
    }
508

509
    case STEP_L5: {
510
        if (supports_l5 && option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
511
            const config_list *list = config_L5_ovrd_ena;
512
            const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_ena);
513
            if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
514
                _next_message--;
515
            }
516
        } else if (supports_l5 && !option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
517
            const config_list *list = config_L5_ovrd_dis;
518
            const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_dis);
519
            if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
520
                _next_message--;
521
            }
522
        }
523
        break;
524
    }
525

526
    default:
527
        // this case should never be reached, do a full reset if it is hit
528
        _next_message = STEP_PVT;
529
        break;
530
    }
531
}
532

533
void
534
AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
535
    uint8_t desired_rate;
536
    uint32_t config_msg_id;
537
    switch(msg_class) {
538
    case CLASS_NAV:
539
        switch(msg_id) {
540
        case MSG_POSLLH:
541
            desired_rate = havePvtMsg ? 0 : RATE_POSLLH;
542
            config_msg_id = CONFIG_RATE_POSLLH;
543
            break;
544
        case MSG_STATUS:
545
            desired_rate = havePvtMsg ? 0 : RATE_STATUS;
546
            config_msg_id = CONFIG_RATE_STATUS;
547
            break;
548
        case MSG_SOL:
549
            desired_rate = havePvtMsg ? 0 : RATE_SOL;
550
            config_msg_id = CONFIG_RATE_SOL;
551
            break;
552
        case MSG_PVT:
553
            desired_rate = RATE_PVT;
554
            config_msg_id = CONFIG_RATE_PVT;
555
            break;
556
        case MSG_TIMEGPS:
557
            desired_rate = RATE_TIMEGPS;
558
            config_msg_id = CONFIG_RATE_TIMEGPS;
559
            break;
560
        case MSG_VELNED:
561
            desired_rate = havePvtMsg ? 0 : RATE_VELNED;
562
            config_msg_id = CONFIG_RATE_VELNED;
563
            break;
564
        case MSG_DOP:
565
            desired_rate = RATE_DOP;
566
            config_msg_id = CONFIG_RATE_DOP;
567
            break;
568
        default:
569
            return;
570
        }
571
        break;
572
    case CLASS_MON:
573
        switch(msg_id) {
574
        case MSG_MON_HW:
575
            desired_rate = RATE_HW;
576
            config_msg_id = CONFIG_RATE_MON_HW;
577
            break;
578
        case MSG_MON_HW2:
579
            desired_rate = RATE_HW2;
580
            config_msg_id = CONFIG_RATE_MON_HW2;
581
            break;
582
        default:
583
            return;
584
        }
585
        break;
586
#if UBLOX_RXM_RAW_LOGGING
587
    case CLASS_RXM:
588
        switch(msg_id) {
589
        case MSG_RXM_RAW:
590
            desired_rate = gps._raw_data;
591
            config_msg_id = CONFIG_RATE_RAW;
592
            break;
593
        case MSG_RXM_RAWX:
594
            desired_rate = gps._raw_data;
595
            config_msg_id = CONFIG_RATE_RAW;
596
            break;
597
        default:
598
            return;
599
        }
600
        break;
601
#endif // UBLOX_RXM_RAW_LOGGING
602
#if UBLOX_TIM_TM2_LOGGING
603
    case CLASS_TIM:
604
        if (msg_id == MSG_TIM_TM2) {
605
            desired_rate = RATE_TIM_TM2;
606
            config_msg_id = CONFIG_TIM_TM2;
607
            break;
608
        }
609
        return;
610
#endif // UBLOX_TIM_TM2_LOGGING
611
    default:
612
        return;
613
    }
614

615
    if (rate == desired_rate) {
616
        // coming in at correct rate; mark as configured
617
        _unconfigured_messages &= ~config_msg_id;
618
        return;
619
    }
620

621
    // coming in at wrong rate; try to configure it
622
    _configure_message_rate(msg_class, msg_id, desired_rate);
623
    _unconfigured_messages |= config_msg_id;
624
    _cfg_needs_save = true;
625
}
626

627
// Requests the ublox driver to identify what port we are using to communicate
628
void
629
AP_GPS_UBLOX::_request_port(void)
630
{
631
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+2)) {
632
        // not enough space - do it next time
633
        return;
634
    }
635
    _send_message(CLASS_CFG, MSG_CFG_PRT, nullptr, 0);
636
}
637

638
// Ensure there is enough space for the largest possible outgoing message
639
// Process bytes available from the stream
640
//
641
// The stream is assumed to contain only messages we recognise.  If it
642
// contains other messages, and those messages contain the preamble
643
// bytes, it is possible for this code to fail to synchronise to the
644
// stream immediately.  Without buffering the entire message and
645
// re-processing it from the top, this is unavoidable. The parser
646
// attempts to avoid this when possible.
647
//
648
bool
649
AP_GPS_UBLOX::read(void)
650
{
651
    bool parsed = false;
652
    uint32_t millis_now = AP_HAL::millis();
653

654
    // walk through the gps configuration at 1 message per second
655
    if (millis_now - _last_config_time >= _delay_time) {
656
        _request_next_config();
657
        _last_config_time = millis_now;
658
        if (_unconfigured_messages) {
659
            // send the updates faster until fully configured
660
            _delay_time = 200;
661
        } else {
662
            _delay_time = 2000;
663
        }
664
    }
665

666
#if 0
667
    // this can be modified to force a particular config state for
668
    // state machine config debugging
669
    static bool done_force_config_error;
670
    if (!_unconfigured_messages && !done_force_config_error) {
671
        done_force_config_error = true;
672
        _unconfigured_messages = CONFIG_F9 | CONFIG_RATE_SOL;
673
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing config state 0x%04x", unsigned(_unconfigured_messages));
674
    }
675
#endif
676

677
    if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
678
       _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
679
       !hal.util->get_soft_armed()) {
680
        //save the configuration sent until now
681
        if (gps._save_config == 1 ||
682
            (gps._save_config == 2 && _cfg_needs_save)) {
683
            _save_cfg();
684
        }
685
    }
686

687
    const uint16_t numc = MIN(port->available(), 8192U);
688
    for (uint16_t i = 0; i < numc; i++) {        // Process bytes received
689

690
        // read the next byte
691
        uint8_t data;
692
        if (!port->read(data)) {
693
            break;
694
        }
695
#if AP_GPS_DEBUG_LOGGING_ENABLED
696
        log_data(&data, 1);
697
#endif
698

699
#if GPS_MOVING_BASELINE
700
        if (rtcm3_parser) {
701
            if (rtcm3_parser->read(data)) {
702
                // we've found a RTCMv3 packet. We stop parsing at
703
                // this point and reset u-blox parse state. We need to
704
                // stop parsing to give the higher level driver a
705
                // chance to send the RTCMv3 packet to another (rover)
706
                // GPS
707
                _step = 0;
708
                break;
709
            }
710
        }
711
#endif
712

713
	reset:
714
        switch(_step) {
715

716
        // Message preamble detection
717
        //
718
        // If we fail to match any of the expected bytes, we reset
719
        // the state machine and re-consider the failed byte as
720
        // the first byte of the preamble.  This improves our
721
        // chances of recovering from a mismatch and makes it less
722
        // likely that we will be fooled by the preamble appearing
723
        // as data in some other message.
724
        //
725
        case 1:
726
            if (PREAMBLE2 == data) {
727
                _step++;
728
                break;
729
            }
730
            _step = 0;
731
            Debug("reset %u", __LINE__);
732
            FALLTHROUGH;
733
        case 0:
734
            if(PREAMBLE1 == data)
735
                _step++;
736
            break;
737

738
        // Message header processing
739
        //
740
        // We sniff the class and message ID to decide whether we
741
        // are going to gather the message bytes or just discard
742
        // them.
743
        //
744
        // We always collect the length so that we can avoid being
745
        // fooled by preamble bytes in messages.
746
        //
747
        case 2:
748
            _step++;
749
            _class = data;
750
            _ck_b = _ck_a = data;                       // reset the checksum accumulators
751
            break;
752
        case 3:
753
            _step++;
754
            _ck_b += (_ck_a += data);                   // checksum byte
755
            _msg_id = data;
756
            break;
757
        case 4:
758
            _step++;
759
            _ck_b += (_ck_a += data);                   // checksum byte
760
            _payload_length = data;                     // payload length low byte
761
            break;
762
        case 5:
763
            _step++;
764
            _ck_b += (_ck_a += data);                   // checksum byte
765

766
            _payload_length += (uint16_t)(data<<8);
767
            if (_payload_length > sizeof(_buffer)) {
768
                Debug("large payload %u", (unsigned)_payload_length);
769
                // assume any payload bigger then what we know about is noise
770
                _payload_length = 0;
771
                _step = 0;
772
				goto reset;
773
            }
774
            _payload_counter = 0;                       // prepare to receive payload
775
            if (_payload_length == 0) {
776
                // bypass payload and go straight to checksum
777
                _step++;
778
            }
779
            break;
780

781
        // Receive message data
782
        //
783
        case 6:
784
            _ck_b += (_ck_a += data);                   // checksum byte
785
            if (_payload_counter < sizeof(_buffer)) {
786
                _buffer[_payload_counter] = data;
787
            }
788
            if (++_payload_counter == _payload_length)
789
                _step++;
790
            break;
791

792
        // Checksum and message processing
793
        //
794
        case 7:
795
            _step++;
796
            if (_ck_a != data) {
797
                Debug("bad cka %x should be %x", data, _ck_a);
798
                _step = 0;
799
				goto reset;
800
            }
801
            break;
802
        case 8:
803
            _step = 0;
804
            if (_ck_b != data) {
805
                Debug("bad ckb %x should be %x", data, _ck_b);
806
                break;                                                  // bad checksum
807
            }
808

809
#if GPS_MOVING_BASELINE
810
            if (rtcm3_parser) {
811
                // this is a uBlox packet, discard any partial RTCMv3 state
812
                rtcm3_parser->reset();
813
            }
814
#endif
815
            if (_parse_gps()) {
816
                parsed = true;
817
            }
818
            break;
819
        }
820
    }
821
    return parsed;
822
}
823

824
// Private Methods /////////////////////////////////////////////////////////////
825
void AP_GPS_UBLOX::log_mon_hw(void)
826
{
827
#if HAL_LOGGING_ENABLED
828
    if (!should_log()) {
829
        return;
830
    }
831
    struct log_Ubx1 pkt = {
832
        LOG_PACKET_HEADER_INIT(LOG_GPS_UBX1_MSG),
833
        time_us    : AP_HAL::micros64(),
834
        instance   : state.instance,
835
        noisePerMS : _buffer.mon_hw_60.noisePerMS,
836
        jamInd     : _buffer.mon_hw_60.jamInd,
837
        aPower     : _buffer.mon_hw_60.aPower,
838
        agcCnt     : _buffer.mon_hw_60.agcCnt,
839
        config     : _unconfigured_messages,
840
    };
841
    if (_payload_length == 68) {
842
        pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS;
843
        pkt.jamInd     = _buffer.mon_hw_68.jamInd;
844
        pkt.aPower     = _buffer.mon_hw_68.aPower;
845
        pkt.agcCnt     = _buffer.mon_hw_68.agcCnt;
846
    }
847
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
848
#endif
849
}
850

851
void AP_GPS_UBLOX::log_mon_hw2(void)
852
{
853
#if HAL_LOGGING_ENABLED
854
    if (!should_log()) {
855
        return;
856
    }
857

858
    struct log_Ubx2 pkt = {
859
        LOG_PACKET_HEADER_INIT(LOG_GPS_UBX2_MSG),
860
        time_us   : AP_HAL::micros64(),
861
        instance  : state.instance,
862
        ofsI      : _buffer.mon_hw2.ofsI,
863
        magI      : _buffer.mon_hw2.magI,
864
        ofsQ      : _buffer.mon_hw2.ofsQ,
865
        magQ      : _buffer.mon_hw2.magQ,
866
    };
867
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
868
#endif
869
}
870

871
#if UBLOX_TIM_TM2_LOGGING
872
void AP_GPS_UBLOX::log_tim_tm2(void)
873
{
874
#if HAL_LOGGING_ENABLED
875
    if (!should_log()) {
876
        return;
877
    }
878

879
// @LoggerMessage: UBXT
880
// @Description: uBlox specific UBX-TIM-TM2 logging, see uBlox interface description
881
// @Field: TimeUS: Time since system startup
882
// @Field: I: GPS instance number
883
// @Field: ch: Channel (i.e. EXTINT) upon which the pulse was measured
884
// @Field: flags: Bitmask
885
// @Field: count: Rising edge counter
886
// @Field: wnR: Week number of last rising edge
887
// @Field: MsR: Tow of rising edge
888
// @Field: SubMsR: Millisecond fraction of tow of rising edge in nanoseconds
889
// @Field: wnF: Week number of last falling edge
890
// @Field: MsF: Tow of falling edge
891
// @Field: SubMsF: Millisecond fraction of tow of falling edge in nanoseconds
892
// @Field: accEst: Accuracy estimate
893

894
    AP::logger().WriteStreaming("UBXT",
895
        "TimeUS,I,ch,flags,count,wnR,MsR,SubMsR,wnF,MsF,SubMsF,accEst",
896
        "s#----ss-sss",
897
        "F-----CI-CII",
898
        "QBBBHHIIHIII",
899
        AP_HAL::micros64(),
900
        state.instance,
901
        _buffer.tim_tm2.ch,
902
        _buffer.tim_tm2.flags,
903
        _buffer.tim_tm2.count,
904
        _buffer.tim_tm2.wnR,
905
        _buffer.tim_tm2.towMsR,
906
        _buffer.tim_tm2.towSubMsR,
907
        _buffer.tim_tm2.wnF,
908
        _buffer.tim_tm2.towMsF,
909
        _buffer.tim_tm2.towSubMsF,
910
        _buffer.tim_tm2.accEst);
911
#endif
912
}
913
#endif // UBLOX_TIM_TM2_LOGGING
914

915
#if UBLOX_RXM_RAW_LOGGING
916
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
917
{
918
#if HAL_LOGGING_ENABLED
919
    if (!should_log()) {
920
        return;
921
    }
922

923
    uint64_t now = AP_HAL::micros64();
924
    for (uint8_t i=0; i<raw.numSV; i++) {
925
        struct log_GPS_RAW pkt = {
926
            LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
927
            time_us    : now,
928
            iTOW       : raw.iTOW,
929
            week       : raw.week,
930
            numSV      : raw.numSV,
931
            sv         : raw.svinfo[i].sv,
932
            cpMes      : raw.svinfo[i].cpMes,
933
            prMes      : raw.svinfo[i].prMes,
934
            doMes      : raw.svinfo[i].doMes,
935
            mesQI      : raw.svinfo[i].mesQI,
936
            cno        : raw.svinfo[i].cno,
937
            lli        : raw.svinfo[i].lli
938
        };
939
        AP::logger().WriteBlock(&pkt, sizeof(pkt));
940
    }
941
#endif
942
}
943

944
void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
945
{
946
#if HAL_LOGGING_ENABLED
947
    if (!should_log()) {
948
        return;
949
    }
950

951
    uint64_t now = AP_HAL::micros64();
952

953
    struct log_GPS_RAWH header = {
954
        LOG_PACKET_HEADER_INIT(LOG_GPS_RAWH_MSG),
955
        time_us    : now,
956
        rcvTow     : raw.rcvTow,
957
        week       : raw.week,
958
        leapS      : raw.leapS,
959
        numMeas    : raw.numMeas,
960
        recStat    : raw.recStat
961
    };
962
    AP::logger().WriteBlock(&header, sizeof(header));
963

964
    for (uint8_t i=0; i<raw.numMeas; i++) {
965
        struct log_GPS_RAWS pkt = {
966
            LOG_PACKET_HEADER_INIT(LOG_GPS_RAWS_MSG),
967
            time_us    : now,
968
            prMes      : raw.svinfo[i].prMes,
969
            cpMes      : raw.svinfo[i].cpMes,
970
            doMes      : raw.svinfo[i].doMes,
971
            gnssId     : raw.svinfo[i].gnssId,
972
            svId       : raw.svinfo[i].svId,
973
            freqId     : raw.svinfo[i].freqId,
974
            locktime   : raw.svinfo[i].locktime,
975
            cno        : raw.svinfo[i].cno,
976
            prStdev    : raw.svinfo[i].prStdev,
977
            cpStdev    : raw.svinfo[i].cpStdev,
978
            doStdev    : raw.svinfo[i].doStdev,
979
            trkStat    : raw.svinfo[i].trkStat
980
        };
981
        AP::logger().WriteBlock(&pkt, sizeof(pkt));
982
    }
983
#endif
984
}
985
#endif // UBLOX_RXM_RAW_LOGGING
986

987
void AP_GPS_UBLOX::unexpected_message(void)
988
{
989
    Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
990
    if (++_disable_counter == 0) {
991
        // disable future sends of this message id, but
992
        // only do this every 256 messages, as some
993
        // message types can't be disabled and we don't
994
        // want to get into an ack war
995
        Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
996
        _configure_message_rate(_class, _msg_id, 0);
997
    }
998
}
999

1000
// return size of a config key, or 0 if unknown
1001
uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
1002
{
1003
    // step over the value
1004
    const uint8_t key_size = (uint32_t(key) >> 28) & 0x07; // mask off the storage size
1005
    switch (key_size) {
1006
    case 0x1: // bit
1007
    case 0x2: // byte
1008
        return 1;
1009
    case 0x3: // 2 bytes
1010
        return 2;
1011
    case 0x4: // 4 bytes
1012
        return 4;
1013
    case 0x5: // 8 bytes
1014
        return 8;
1015
    default:
1016
        break;
1017
    }
1018
    // invalid
1019
    return 0;
1020
}
1021

1022
/*
1023
  find index of a config key in the active_config list, or -1
1024
 */
1025
int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
1026
{
1027
    if (active_config.list == nullptr) {
1028
        return -1;
1029
    }
1030
    for (uint8_t i=0; i<active_config.count; i++) {
1031
        if (key == active_config.list[i].key) {
1032
            return (int8_t)i;
1033
        }
1034
    }
1035

1036
    return -1;
1037
}
1038

1039
bool
1040
AP_GPS_UBLOX::_parse_gps(void)
1041
{
1042
    if (_class == CLASS_ACK) {
1043
        Debug("ACK %u", (unsigned)_msg_id);
1044

1045
        if(_msg_id == MSG_ACK_ACK) {
1046
            switch(_buffer.ack.clsID) {
1047
            case CLASS_CFG:
1048
                switch(_buffer.ack.msgID) {
1049
                case MSG_CFG_CFG:
1050
                    _cfg_saved = true;
1051
                    _cfg_needs_save = false;
1052
                    break;
1053
                case MSG_CFG_GNSS:
1054
                    _unconfigured_messages &= ~CONFIG_GNSS;
1055
                    break;
1056
                case MSG_CFG_MSG:
1057
                    // There is no way to know what MSG config was ack'ed, assume it was the last
1058
                    // one requested. To verify it rerequest the last config we sent. If we miss
1059
                    // the actual ack we will catch it next time through the poll loop, but that
1060
                    // will be a good chunk of time later.
1061
                    break;
1062
                case MSG_CFG_NAV_SETTINGS:
1063
                    _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
1064
                    break;
1065
                case MSG_CFG_RATE:
1066
                    // The GPS will ACK a update rate that is invalid. in order to detect this
1067
                    // only accept the rate as configured by reading the settings back and
1068
                    // validating that they all match the target values
1069
                    break;
1070
                case MSG_CFG_SBAS:
1071
                    _unconfigured_messages &= ~CONFIG_SBAS;
1072
                    break;
1073
                case MSG_CFG_TP5:
1074
                    _unconfigured_messages &= ~CONFIG_TP5;
1075
                    break;
1076
                }
1077

1078
                break;
1079
            case CLASS_MON:
1080
                switch(_buffer.ack.msgID) {
1081
                case MSG_MON_HW:
1082
                    _unconfigured_messages &= ~CONFIG_RATE_MON_HW;
1083
                    break;
1084
                case MSG_MON_HW2:
1085
                    _unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
1086
                    break;
1087
                }
1088
            }
1089
        }
1090
        if(_msg_id == MSG_ACK_NACK) {
1091
            switch(_buffer.nack.clsID) {
1092
            case CLASS_CFG:
1093
                switch(_buffer.nack.msgID) {
1094
                case MSG_CFG_VALGET:
1095
                    CFG_Debug("NACK VALGET 0x%x", (unsigned)_buffer.nack.msgID);
1096
                    if (active_config.list != nullptr) {
1097
                        /*
1098
                          likely this device does not support fetching multiple keys at once, go one at a time
1099
                        */
1100
                        if (active_config.fetch_index == -1) {
1101
                            CFG_Debug("NACK starting %u", unsigned(active_config.count));
1102
                            active_config.fetch_index = 0;
1103
                            use_single_valget = true;
1104
                        } else {
1105
                            // the device does not support the config key we asked for,
1106
                            // consider the bit as done
1107
                            active_config.done_mask |= (1U<<active_config.fetch_index);
1108
                            CFG_Debug("NACK %d 0x%x done=0x%x",
1109
                                     int(active_config.fetch_index),
1110
                                     unsigned(active_config.list[active_config.fetch_index].key),
1111
                                     unsigned(active_config.done_mask));
1112
                            if (active_config.done_mask == (1U<<active_config.count)-1 ||
1113
                                active_config.fetch_index >= active_config.count) {
1114
                                // all done!
1115
                                _unconfigured_messages &= ~active_config.unconfig_bit;
1116
                            }
1117
                            active_config.fetch_index++;
1118
                        }
1119
                        if (active_config.fetch_index < active_config.count) {
1120
                            _configure_valget(active_config.list[active_config.fetch_index].key);
1121
                        }
1122
                    }
1123
                    break;
1124
                case MSG_CFG_VALSET:
1125
                    if (active_config.list != nullptr) {
1126
                        CFG_Debug("NACK VALSET 0x%x 0x%x", (unsigned)_buffer.nack.msgID,
1127
                                  unsigned(active_config.list[active_config.set_index].key));
1128
                        if (is_gnss_key(active_config.list[active_config.set_index].key)) {
1129
                            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: unable to configure band 0x%02x",
1130
                                          unsigned(state.instance + 1), unsigned(active_config.list[active_config.set_index].key));
1131

1132
                        }
1133
                    }
1134
                    break;
1135
                }
1136
            }
1137
        }
1138
        return false;
1139
    }
1140

1141
    if (_class == CLASS_CFG) {
1142
        switch(_msg_id) {
1143
        case  MSG_CFG_NAV_SETTINGS:
1144
	    Debug("Got settings %u min_elev %d drLimit %u\n", 
1145
                  (unsigned)_buffer.nav_settings.dynModel,
1146
                  (int)_buffer.nav_settings.minElev,
1147
                  (unsigned)_buffer.nav_settings.drLimit);
1148
            _buffer.nav_settings.mask = 0;
1149
            if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
1150
                _buffer.nav_settings.dynModel != gps._navfilter) {
1151
                // we've received the current nav settings, change the engine
1152
                // settings and send them back
1153
                Debug("Changing engine setting from %u to %u\n",
1154
                      (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
1155
                _buffer.nav_settings.dynModel = gps._navfilter;
1156
                _buffer.nav_settings.mask |= 1;
1157
            }
1158
            if (gps._min_elevation != -100 &&
1159
                _buffer.nav_settings.minElev != gps._min_elevation) {
1160
                Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
1161
                _buffer.nav_settings.minElev = gps._min_elevation;
1162
                _buffer.nav_settings.mask |= 2;
1163
            }
1164
            if (_buffer.nav_settings.mask != 0) {
1165
                _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
1166
                              &_buffer.nav_settings,
1167
                              sizeof(_buffer.nav_settings));
1168
                _unconfigured_messages |= CONFIG_NAV_SETTINGS;
1169
                _cfg_needs_save = true;
1170
            } else {
1171
                _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
1172
            }
1173
            return false;
1174

1175
#if UBLOX_GNSS_SETTINGS
1176
        case MSG_CFG_GNSS:
1177
            if (params.gnss_mode != 0 && !supports_F9_config()) {
1178
                struct ubx_cfg_gnss start_gnss = _buffer.gnss;
1179
                uint8_t gnssCount = 0;
1180
                Debug("Got GNSS Settings %u %u %u %u:\n",
1181
                    (unsigned)_buffer.gnss.msgVer,
1182
                    (unsigned)_buffer.gnss.numTrkChHw,
1183
                    (unsigned)_buffer.gnss.numTrkChUse,
1184
                    (unsigned)_buffer.gnss.numConfigBlocks);
1185
#if UBLOX_DEBUGGING
1186
                for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
1187
                    Debug("  %u %u %u 0x%08x\n",
1188
                    (unsigned)_buffer.gnss.configBlock[i].gnssId,
1189
                    (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
1190
                    (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
1191
                    (unsigned)_buffer.gnss.configBlock[i].flags);
1192
                }
1193
#endif
1194

1195
                for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
1196
                    if((params.gnss_mode & (1 << i)) && i != GNSS_SBAS) {
1197
                        gnssCount++;
1198
                    }
1199
                }
1200
                for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
1201
                    // Reserve an equal portion of channels for all enabled systems that supports it
1202
                    if(params.gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
1203
                        if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId && (_hardware_generation > UBLOX_M8 || GNSS_GALILEO !=_buffer.gnss.configBlock[i].gnssId)) {
1204
                            _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
1205
                            _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
1206
                        } else {
1207
                            if(GNSS_SBAS ==_buffer.gnss.configBlock[i].gnssId) {
1208
                                _buffer.gnss.configBlock[i].resTrkCh = 1;
1209
                                _buffer.gnss.configBlock[i].maxTrkCh = 3;
1210
                            }
1211
                            if(GNSS_GALILEO ==_buffer.gnss.configBlock[i].gnssId) {
1212
                                _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
1213
                                _buffer.gnss.configBlock[i].maxTrkCh = 8; //Per the M8 receiver description UBX-13003221 - R16, 4.1.1.3 it is not recommended to set the number of galileo channels higher then eight
1214
                            }
1215
                        }
1216
                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
1217
                    } else {
1218
                        _buffer.gnss.configBlock[i].resTrkCh = 0;
1219
                        _buffer.gnss.configBlock[i].maxTrkCh = 0;
1220
                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
1221
                    }
1222
                }
1223
                if (memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
1224
                    _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
1225
                    _unconfigured_messages |= CONFIG_GNSS;
1226
                    _cfg_needs_save = true;
1227
                } else {
1228
                    _unconfigured_messages &= ~CONFIG_GNSS;
1229
                }
1230
            } else {
1231
                _unconfigured_messages &= ~CONFIG_GNSS;
1232
            }
1233
            return false;
1234
#endif
1235

1236
        case MSG_CFG_SBAS:
1237
            if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
1238
	        Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
1239
                      (unsigned)_buffer.sbas.mode,
1240
                      (unsigned)_buffer.sbas.usage,
1241
                      (unsigned)_buffer.sbas.maxSBAS,
1242
                      (unsigned)_buffer.sbas.scanmode2,
1243
                      (unsigned)_buffer.sbas.scanmode1);
1244
                if (_buffer.sbas.mode != gps._sbas_mode) {
1245
                    _buffer.sbas.mode = gps._sbas_mode;
1246
                    _send_message(CLASS_CFG, MSG_CFG_SBAS,
1247
                                  &_buffer.sbas,
1248
                                  sizeof(_buffer.sbas));
1249
                    _unconfigured_messages |= CONFIG_SBAS;
1250
                    _cfg_needs_save = true;
1251
                } else {
1252
                    _unconfigured_messages &= ~CONFIG_SBAS;
1253
                }
1254
            } else {
1255
                    _unconfigured_messages &= ~CONFIG_SBAS;
1256
            }
1257
            return false;
1258
        case MSG_CFG_MSG:
1259
            if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
1260
                // can't verify the setting without knowing the port
1261
                // request the port again
1262
                if(_ublox_port >= UBLOX_MAX_PORTS) {
1263
                    _request_port();
1264
                    return false;
1265
                }
1266
                _verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
1267
                             _buffer.msg_rate_6.rates[_ublox_port]);
1268
            } else {
1269
                _verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
1270
                             _buffer.msg_rate.rate);
1271
            }
1272
            return false;
1273
        case MSG_CFG_PRT:
1274
           _ublox_port = _buffer.prt.portID;
1275
           return false;
1276
        case MSG_CFG_RATE:
1277
            if(_buffer.nav_rate.measure_rate_ms != params.rate_ms ||
1278
               _buffer.nav_rate.nav_rate != 1 ||
1279
               _buffer.nav_rate.timeref != 0) {
1280
               _configure_rate();
1281
                _unconfigured_messages |= CONFIG_RATE_NAV;
1282
                _cfg_needs_save = true;
1283
            } else {
1284
                _unconfigured_messages &= ~CONFIG_RATE_NAV;
1285
            }
1286
            return false;
1287
            
1288
#if CONFIGURE_PPS_PIN
1289
        case MSG_CFG_TP5: {
1290
            // configure the PPS pin for 1Hz, zero delay
1291
            Debug("Got TP5 ver=%u 0x%04x %u\n", 
1292
                  (unsigned)_buffer.nav_tp5.version,
1293
                  (unsigned)_buffer.nav_tp5.flags,
1294
                  (unsigned)_buffer.nav_tp5.freqPeriod);
1295
#ifdef HAL_GPIO_PPS
1296
            hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING);
1297
#endif
1298
            const uint16_t desired_flags = 0x003f;
1299
            const uint16_t desired_period_hz = _pps_freq;
1300

1301
            if (_buffer.nav_tp5.flags != desired_flags ||
1302
                _buffer.nav_tp5.freqPeriod != desired_period_hz) {
1303
                _buffer.nav_tp5.tpIdx = 0;
1304
                _buffer.nav_tp5.reserved1[0] = 0;
1305
                _buffer.nav_tp5.reserved1[1] = 0;
1306
                _buffer.nav_tp5.antCableDelay = 0;
1307
                _buffer.nav_tp5.rfGroupDelay = 0;
1308
                _buffer.nav_tp5.freqPeriod = desired_period_hz;
1309
                _buffer.nav_tp5.freqPeriodLock = desired_period_hz;
1310
                _buffer.nav_tp5.pulseLenRatio = 1;
1311
                _buffer.nav_tp5.pulseLenRatioLock = 2;
1312
                _buffer.nav_tp5.userConfigDelay = 0;
1313
                _buffer.nav_tp5.flags = desired_flags;
1314
                _send_message(CLASS_CFG, MSG_CFG_TP5,
1315
                              &_buffer.nav_tp5,
1316
                              sizeof(_buffer.nav_tp5));
1317
                _unconfigured_messages |= CONFIG_TP5;
1318
                _cfg_needs_save = true;
1319
            } else {
1320
                _unconfigured_messages &= ~CONFIG_TP5;
1321
            }
1322
            return false;
1323
        }
1324
#endif // CONFIGURE_PPS_PIN
1325
        case MSG_CFG_VALGET: {
1326
            uint8_t cfg_len = _payload_length - sizeof(ubx_cfg_valget);
1327
            const uint8_t *cfg_data = (const uint8_t *)(&_buffer) + sizeof(ubx_cfg_valget);
1328
            while (cfg_len >= 5) {
1329
                ConfigKey id;
1330
                memcpy(&id, cfg_data, sizeof(uint32_t));
1331
                cfg_len -= 4;
1332
                cfg_data += 4;
1333
                switch (id) {
1334
                    case ConfigKey::TMODE_MODE: {
1335
                        uint8_t mode = cfg_data[0];
1336
                        if (mode != 0) {
1337
                            // ask for mode 0, to disable TIME mode
1338
                            mode = 0;
1339
                            _configure_valset(ConfigKey::TMODE_MODE, &mode);
1340
                            _cfg_needs_save = true;
1341
                            _unconfigured_messages |= CONFIG_TMODE_MODE;
1342
                        } else {
1343
                            _unconfigured_messages &= ~CONFIG_TMODE_MODE;
1344
                        }
1345
                        break;
1346
                    }
1347
                    default:
1348
                        break;
1349
                }
1350
                // see if it is in active config list
1351
                int8_t cfg_idx = find_active_config_index(id);
1352
                if (cfg_idx >= 0) {
1353
                    CFG_Debug("valset(0x%lx): %u", (long unsigned)id, unsigned((*cfg_data) & 0x1));
1354
                    const uint8_t key_size = config_key_size(id);
1355
                    if (cfg_len < key_size
1356
                        // for keys of length 1 only the LSB is significant
1357
                        || (key_size == 1 && (active_config.list[cfg_idx].value & 0x1) != (*cfg_data & 0x1))
1358
                        || memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
1359
                        _configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers);
1360
                        _unconfigured_messages |= active_config.unconfig_bit;
1361
                        active_config.done_mask &= ~(1U << cfg_idx);
1362
                        active_config.set_index = cfg_idx;
1363
                        _cfg_needs_save = true;
1364
                    } else {
1365
                        active_config.done_mask |= (1U << cfg_idx);
1366
                        CFG_Debug("done %u mask=0x%x all_mask=0x%x",
1367
                                  unsigned(cfg_idx),
1368
                                  unsigned(active_config.done_mask),
1369
                                  (1U<<active_config.count)-1);
1370
                        if (active_config.done_mask == (1U<<active_config.count)-1) {
1371
                            // all done!
1372
                            _unconfigured_messages &= ~active_config.unconfig_bit;
1373
                        }
1374
                    }
1375
                    if (active_config.fetch_index >= 0 &&
1376
                        active_config.fetch_index < active_config.count &&
1377
                        id == active_config.list[active_config.fetch_index].key) {
1378
                        active_config.fetch_index++;
1379
                        if (active_config.fetch_index < active_config.count) {
1380
                            _configure_valget(active_config.list[active_config.fetch_index].key);
1381
                            CFG_Debug("valget %d 0x%x", int(active_config.fetch_index),
1382
                                  unsigned(active_config.list[active_config.fetch_index].key));
1383
                        }
1384
                    }
1385
                } else {
1386
                    CFG_Debug("valget no active config for 0x%lx", (long unsigned)id);
1387
                }
1388

1389
                // step over the value
1390
                uint8_t step_size = config_key_size(id);
1391
                if (step_size == 0) {
1392
                    return false;
1393
                }
1394
                cfg_len -= step_size;
1395
                cfg_data += step_size;
1396
            }
1397
        }
1398
        }
1399
    }
1400

1401
    if (_class == CLASS_MON) {
1402
        switch(_msg_id) {
1403
        case MSG_MON_HW:
1404
            if (_payload_length == 60 || _payload_length == 68) {
1405
                log_mon_hw();
1406
            }
1407
            break;
1408
        case MSG_MON_HW2:
1409
            if (_payload_length == 28) {
1410
                log_mon_hw2();  
1411
            }
1412
            break;
1413
        case MSG_MON_VER: {
1414
            bool check_L1L5 = false;
1415
            _have_version = true;
1416
            strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
1417
            strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
1418
            void* mod = memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "MOD=", 4);
1419
            if (mod != nullptr) {
1420
                strncpy(_module, (char*)mod+4, UBLOX_MODULE_LEN-1);
1421
            }
1422

1423
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, 
1424
                                             "u-blox %s%s%d HW: %s SW: %s",
1425
                                             _module, mod != nullptr ? " " : "",
1426
                                             state.instance + 1,
1427
                                             _version.hwVersion,
1428
                                             _version.swVersion);
1429
            // check for F9 and M9. The F9 does not respond to SVINFO,
1430
            // so we need to use MON_VER for hardware generation
1431
            if (strncmp(_version.hwVersion, "00190000", 8) == 0) {
1432
                if (strncmp(_version.swVersion, "EXT CORE 1", 10) == 0) {
1433
                    // a F9
1434
                    if (_hardware_generation != UBLOX_F9) {
1435
                        // need to ensure time mode is correctly setup on F9
1436
                        _unconfigured_messages |= CONFIG_TMODE_MODE;
1437
                    }
1438
                    _hardware_generation = UBLOX_F9;
1439
                    _unconfigured_messages |= CONFIG_F9;
1440
                    _unconfigured_messages &= ~CONFIG_GNSS;
1441
                    if (strncmp(_module, "ZED-F9P", UBLOX_MODULE_LEN) == 0) {
1442
                        _hardware_variant = UBLOX_F9_ZED;
1443
                    } else if (strncmp(_module, "NEO-F9P", UBLOX_MODULE_LEN) == 0) {
1444
                        _hardware_variant = UBLOX_F9_NEO;
1445
                    }
1446
                }
1447
                if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
1448
                    // a M9
1449
                    _hardware_generation = UBLOX_M9;
1450
                }
1451
                check_L1L5 = true;
1452
            }
1453
            // check for M10
1454
            if (strncmp(_version.hwVersion, "000A0000", 8) == 0) {
1455
                _hardware_generation = UBLOX_M10;
1456
                _unconfigured_messages |= CONFIG_M10;
1457
                // M10 does not support CONFIG_GNSS
1458
                _unconfigured_messages &= ~CONFIG_GNSS;
1459
                check_L1L5 = true;
1460

1461
                // M10 does not support multi-valued VALGET
1462
                use_single_valget = true;
1463
            }
1464
            if (check_L1L5) {
1465
                // check if L1L5 in extension
1466
                if (memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "L1L5", 4) != nullptr) {
1467
                    supports_l5 = true;
1468
                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox supports L5 Band");
1469
                    _unconfigured_messages |= CONFIG_L5;
1470
                }
1471
            }
1472
            break;
1473
        }
1474
        default:
1475
            unexpected_message();
1476
        }
1477
        return false;
1478
    }
1479

1480
#if UBLOX_RXM_RAW_LOGGING
1481
    if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
1482
        log_rxm_raw(_buffer.rxm_raw);
1483
        return false;
1484
    } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
1485
        log_rxm_rawx(_buffer.rxm_rawx);
1486
        return false;
1487
    }
1488
#endif // UBLOX_RXM_RAW_LOGGING
1489

1490
#if UBLOX_TIM_TM2_LOGGING
1491
    if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
1492
        log_tim_tm2();
1493
        return false;
1494
    }
1495
#endif // UBLOX_TIM_TM2_LOGGING
1496

1497
    if (_class != CLASS_NAV) {
1498
        unexpected_message();
1499
        return false;
1500
    }
1501

1502
    switch (_msg_id) {
1503
    case MSG_POSLLH:
1504
        Debug("MSG_POSLLH next_fix=%u", next_fix);
1505
        if (havePvtMsg) {
1506
            _unconfigured_messages |= CONFIG_RATE_POSLLH;
1507
            break;
1508
        }
1509
        _check_new_itow(_buffer.posllh.itow);
1510
        _last_pos_time        = _buffer.posllh.itow;
1511
        state.location.lng    = _buffer.posllh.longitude;
1512
        state.location.lat    = _buffer.posllh.latitude;
1513
        state.have_undulation = true;
1514
        state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
1515
        set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10);
1516

1517
        state.status          = next_fix;
1518
        _new_position = true;
1519
        state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
1520
        state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
1521
        state.have_horizontal_accuracy = true;
1522
        state.have_vertical_accuracy = true;
1523
#if UBLOX_FAKE_3DLOCK
1524
        state.location.lng = 1491652300L;
1525
        state.location.lat = -353632610L;
1526
        state.location.alt = 58400;
1527
        state.vertical_accuracy = 0;
1528
        state.horizontal_accuracy = 0;
1529
#endif
1530
        break;
1531
    case MSG_STATUS:
1532
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
1533
              _buffer.status.fix_status,
1534
              _buffer.status.fix_type);
1535
        _check_new_itow(_buffer.status.itow);
1536
        if (havePvtMsg) {
1537
            // when we have PVT we don't need status, just change the rate for STATUS to zero
1538
            _unconfigured_messages &= ~CONFIG_RATE_STATUS;
1539
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
1540
            break;
1541
        }
1542
        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
1543
            if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
1544
                (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
1545
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
1546
            }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
1547
                next_fix = AP_GPS::GPS_OK_FIX_3D;
1548
            }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
1549
                next_fix = AP_GPS::GPS_OK_FIX_2D;
1550
            }else{
1551
                next_fix = AP_GPS::NO_FIX;
1552
                state.status = AP_GPS::NO_FIX;
1553
            }
1554
        }else{
1555
            next_fix = AP_GPS::NO_FIX;
1556
            state.status = AP_GPS::NO_FIX;
1557
        }
1558
#if UBLOX_FAKE_3DLOCK
1559
        state.status = AP_GPS::GPS_OK_FIX_3D;
1560
        next_fix = state.status;
1561
#endif
1562
        break;
1563
    case MSG_DOP:
1564
        Debug("MSG_DOP");
1565
        noReceivedHdop = false;
1566
        _check_new_itow(_buffer.dop.itow);
1567
        state.hdop        = _buffer.dop.hDOP;
1568
        state.vdop        = _buffer.dop.vDOP;
1569
#if UBLOX_FAKE_3DLOCK
1570
        state.hdop = 130;
1571
        state.hdop = 170;
1572
#endif
1573
        break;
1574
    case MSG_SOL:
1575
        Debug("MSG_SOL fix_status=%u fix_type=%u",
1576
              _buffer.solution.fix_status,
1577
              _buffer.solution.fix_type);
1578
        _check_new_itow(_buffer.solution.itow);
1579
        if (havePvtMsg) {
1580
            state.time_week = _buffer.solution.week;
1581
            break;
1582
        }
1583
        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
1584
            if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
1585
                (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
1586
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
1587
            }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
1588
                next_fix = AP_GPS::GPS_OK_FIX_3D;
1589
            }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
1590
                next_fix = AP_GPS::GPS_OK_FIX_2D;
1591
            }else{
1592
                next_fix = AP_GPS::NO_FIX;
1593
                state.status = AP_GPS::NO_FIX;
1594
            }
1595
        }else{
1596
            next_fix = AP_GPS::NO_FIX;
1597
            state.status = AP_GPS::NO_FIX;
1598
        }
1599
        if(noReceivedHdop) {
1600
            state.hdop = _buffer.solution.position_DOP;
1601
        }
1602
        state.num_sats    = _buffer.solution.satellites;
1603
        if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
1604
            state.last_gps_time_ms = AP_HAL::millis();
1605
            state.time_week_ms    = _buffer.solution.itow;
1606
            state.time_week       = _buffer.solution.week;
1607
        }
1608
#if UBLOX_FAKE_3DLOCK
1609
        next_fix = state.status;
1610
        state.num_sats = 10;
1611
        state.time_week = 1721;
1612
        state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
1613
        state.last_gps_time_ms = AP_HAL::millis();
1614
        state.hdop = 130;
1615
#endif
1616
        break;
1617

1618
#if GPS_MOVING_BASELINE
1619
    case MSG_RELPOSNED:
1620
        {
1621
            if (role != AP_GPS::GPS_ROLE_MB_ROVER) {
1622
                // ignore RELPOSNED if not configured as a rover
1623
                break;
1624
            }
1625
            // note that we require the yaw to come from a fixed solution, not a float solution
1626
            // yaw from a float solution would only be acceptable with a very large separation between
1627
            // GPS modules
1628
            const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) |
1629
                                        static_cast<uint32_t>(RELPOSNED::relPosValid) |
1630
                                        static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
1631
                                        static_cast<uint32_t>(RELPOSNED::isMoving) |
1632
                                        static_cast<uint32_t>(RELPOSNED::carrSolnFixed);
1633
            const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
1634
                                          static_cast<uint32_t>(RELPOSNED::refObsMiss) |
1635
                                          static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
1636

1637
            _check_new_itow(_buffer.relposned.iTOW);
1638
            if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
1639
                // useful for looking at packet loss on links
1640
                MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
1641
            }
1642
            _last_relposned_itow = _buffer.relposned.iTOW;
1643
            MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask);
1644
            if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
1645
                ((_buffer.relposned.flags & invalid_mask) == 0)) {
1646
                if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
1647
                                          _buffer.relposned.relPosLength * 0.01,
1648
                                          _buffer.relposned.relPosD*0.01)) {
1649
                    state.have_gps_yaw_accuracy = true;
1650
                    state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
1651
                    _last_relposned_ms = AP_HAL::millis();
1652
                }
1653
                state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5;
1654
                state.relPosLength  = _buffer.relposned.relPosLength * 0.01;
1655
                state.relPosD       = _buffer.relposned.relPosD * 0.01;
1656
                state.accHeading    = _buffer.relposned.accHeading * 1e-5;
1657
                state.relposheading_ts = AP_HAL::millis();
1658
            } else {
1659
                state.have_gps_yaw_accuracy = false;
1660
            }
1661
        }
1662
        break;
1663
#endif // GPS_MOVING_BASELINE
1664

1665
    case MSG_PVT:
1666
        Debug("MSG_PVT");
1667

1668
        havePvtMsg = true;
1669

1670
        // if we have PVT we don't want MSG_STATUS
1671
        _unconfigured_messages &= ~CONFIG_RATE_STATUS;
1672

1673
        // position
1674
        _check_new_itow(_buffer.pvt.itow);
1675
        // Only adjust if:
1676
        // we already have a valid week,
1677
        // PVT iTOW wrapped,
1678
        // and our time_week_ms still looks like end-of-week
1679
        // (meaning we haven't already accepted the rollover via TIMEGPS/SOL)
1680
        if (state.time_week != 0 &&
1681
            _last_pvt_itow != 0 &&
1682
            _buffer.pvt.itow < _last_pvt_itow &&
1683
            state.time_week_ms > END_OF_WEEK_THRESHOLD_MS &&
1684
            _buffer.pvt.itow < START_OF_WEEK_THRESHOLD_MS) {
1685
            state.time_week++;
1686
        }
1687

1688
        _last_pvt_itow = _buffer.pvt.itow;
1689
        _last_pos_time        = _buffer.pvt.itow;
1690
        state.location.lng    = _buffer.pvt.lon;
1691
        state.location.lat    = _buffer.pvt.lat;
1692
        state.have_undulation = true;
1693
        state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
1694
        set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10);
1695
        switch (_buffer.pvt.fix_type)
1696
        {
1697
            case 0:
1698
                state.status = AP_GPS::NO_FIX;
1699
                break;
1700
            case 1:
1701
                state.status = AP_GPS::NO_FIX;
1702
                break;
1703
            case 2:
1704
                state.status = AP_GPS::GPS_OK_FIX_2D;
1705
                break;
1706
            case 3:
1707
                state.status = AP_GPS::GPS_OK_FIX_3D;
1708
                if (_buffer.pvt.flags & 0b00000010)  // diffsoln
1709
                    state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
1710
                if (_buffer.pvt.flags & 0b01000000)  // carrsoln - float
1711
                    state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
1712
                if (_buffer.pvt.flags & 0b10000000)  // carrsoln - fixed
1713
                    state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
1714
                break;
1715
            case 4:
1716
                GCS_SEND_TEXT(MAV_SEVERITY_INFO,
1717
                                "Unexpected state %d", _buffer.pvt.flags);
1718
                state.status = AP_GPS::GPS_OK_FIX_3D;
1719
                break;
1720
            case 5:
1721
                state.status = AP_GPS::NO_FIX;
1722
                break;
1723
            default:
1724
                state.status = AP_GPS::NO_FIX;
1725
                break;
1726
        }
1727
        next_fix = state.status;
1728
        _new_position = true;
1729
        state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f;
1730
        state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f;
1731
        state.have_horizontal_accuracy = true;
1732
        state.have_vertical_accuracy = true;
1733
        // SVs
1734
        state.num_sats    = _buffer.pvt.num_sv;
1735
        // velocity     
1736
        _last_vel_time         = _buffer.pvt.itow;
1737
        state.ground_speed     = _buffer.pvt.gspeed*0.001f;          // m/s
1738
        state.ground_course    = wrap_360(_buffer.pvt.head_mot * 1.0e-5f);       // Heading 2D deg * 100000
1739
        state.have_vertical_velocity = true;
1740
        state.velocity.x = _buffer.pvt.velN * 0.001f;
1741
        state.velocity.y = _buffer.pvt.velE * 0.001f;
1742
        state.velocity.z = _buffer.pvt.velD * 0.001f;
1743
        state.have_speed_accuracy = true;
1744
        state.speed_accuracy = _buffer.pvt.s_acc*0.001f;
1745
        _new_speed = true;
1746
        // dop
1747
        if(noReceivedHdop) {
1748
            state.hdop        = _buffer.pvt.p_dop;
1749
            state.vdop        = _buffer.pvt.p_dop;
1750
        }
1751

1752
        if (_buffer.pvt.fix_type >= 2) {
1753
            state.last_gps_time_ms = AP_HAL::millis();
1754
        }
1755
        
1756
        // time
1757
        state.time_week_ms    = _buffer.pvt.itow;
1758
#if UBLOX_FAKE_3DLOCK
1759
        state.location.lng = 1491652300L;
1760
        state.location.lat = -353632610L;
1761
        state.location.alt = 58400;
1762
        state.vertical_accuracy = 0;
1763
        state.horizontal_accuracy = 0;
1764
        state.status = AP_GPS::GPS_OK_FIX_3D;
1765
        state.num_sats = 10;
1766
        state.time_week = 1721;
1767
        state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
1768
        state.last_gps_time_ms = AP_HAL::millis();
1769
        state.hdop = 130;
1770
        state.speed_accuracy = 0;
1771
        next_fix = state.status;
1772
#endif
1773
        break;
1774
    case MSG_TIMEGPS:
1775
        Debug("MSG_TIMEGPS");
1776
        _check_new_itow(_buffer.timegps.itow);
1777
        if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) {
1778
            state.time_week_ms = _buffer.timegps.itow;
1779
            state.time_week = _buffer.timegps.week;
1780
            state.last_gps_time_ms = AP_HAL::millis();
1781
        }
1782
        break;
1783
    case MSG_VELNED:
1784
        Debug("MSG_VELNED");
1785
        if (havePvtMsg) {
1786
            _unconfigured_messages |= CONFIG_RATE_VELNED;
1787
            break;
1788
        }
1789
        _check_new_itow(_buffer.velned.itow);
1790
        _last_vel_time         = _buffer.velned.itow;
1791
        state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
1792
        state.ground_course    = wrap_360(_buffer.velned.heading_2d * 1.0e-5f);       // Heading 2D deg * 100000
1793
        state.have_vertical_velocity = true;
1794
        state.velocity.x = _buffer.velned.ned_north * 0.01f;
1795
        state.velocity.y = _buffer.velned.ned_east * 0.01f;
1796
        state.velocity.z = _buffer.velned.ned_down * 0.01f;
1797
        velocity_to_speed_course(state);
1798
        state.have_speed_accuracy = true;
1799
        state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
1800
#if UBLOX_FAKE_3DLOCK
1801
        state.speed_accuracy = 0;
1802
#endif
1803
        _new_speed = true;
1804
        break;
1805
    case MSG_NAV_SVINFO:
1806
        {
1807
        Debug("MSG_NAV_SVINFO\n");
1808
        static const uint8_t HardwareGenerationMask = 0x07;
1809
        _check_new_itow(_buffer.svinfo_header.itow);
1810
        _hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
1811
        switch (_hardware_generation) {
1812
            case UBLOX_5:
1813
            case UBLOX_6:
1814
                // only 7 and newer support CONFIG_GNSS
1815
                _unconfigured_messages &= ~CONFIG_GNSS;
1816
                break;
1817
            case UBLOX_7:
1818
            case UBLOX_M8:
1819
#if UBLOX_SPEED_CHANGE
1820
                port->begin(4000000U);
1821
                Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
1822
#endif
1823
                break;
1824
            default:
1825
                hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
1826
                break;
1827
        };
1828
        _unconfigured_messages &= ~CONFIG_VERSION;
1829
        /* We don't need that anymore */
1830
        _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
1831
        break;
1832
        }
1833
    default:
1834
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
1835
        if (++_disable_counter == 0) {
1836
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
1837
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
1838
        }
1839
        return false;
1840
    }
1841

1842
    if (state.have_gps_yaw) {
1843
        // when we are a rover we want to ensure we have both the new
1844
        // PVT and the new RELPOSNED message so that we give a
1845
        // consistent view
1846
        if (AP_HAL::millis() - _last_relposned_ms > 400) {
1847
            // we have stopped receiving valid RELPOSNED messages, disable yaw reporting
1848
            state.have_gps_yaw = false;
1849
        } else if (_last_relposned_itow != _last_pvt_itow) {
1850
            // wait until ITOW matches
1851
            return false;
1852
        }
1853
    }
1854

1855
    // we only return true when we get new position and speed data
1856
    // this ensures we don't use stale data
1857
    if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
1858
        _new_speed = _new_position = false;
1859
        return true;
1860
    }
1861
    return false;
1862
}
1863

1864
/*
1865
 *  handle pps interrupt
1866
 */
1867
#ifdef HAL_GPIO_PPS
1868
void
1869
AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
1870
{
1871
    _last_pps_time_us = AP_HAL::micros64();
1872
}
1873

1874
void
1875
AP_GPS_UBLOX::set_pps_desired_freq(uint8_t freq)
1876
{
1877
    _pps_freq = freq;
1878
    _unconfigured_messages |= CONFIG_TP5;
1879
}
1880
#endif
1881

1882

1883
// UBlox auto configuration
1884

1885
/*
1886
 *  update checksum for a set of bytes
1887
 */
1888
void
1889
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b)
1890
{
1891
    while (len--) {
1892
        ck_a += *data;
1893
        ck_b += ck_a;
1894
        data++;
1895
    }
1896
}
1897

1898

1899
/*
1900
 *  send a ublox message
1901
 */
1902
bool
1903
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, const void *msg, uint16_t size)
1904
{
1905
    if (port->txspace() < (sizeof(struct ubx_header) + 2 + size)) {
1906
        return false;
1907
    }
1908
    struct ubx_header header;
1909
    uint8_t ck_a=0, ck_b=0;
1910
    header.preamble1 = PREAMBLE1;
1911
    header.preamble2 = PREAMBLE2;
1912
    header.msg_class = msg_class;
1913
    header.msg_id    = msg_id;
1914
    header.length    = size;
1915

1916
    _update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
1917
    _update_checksum((uint8_t *)msg, size, ck_a, ck_b);
1918

1919
    port->write((const uint8_t *)&header, sizeof(header));
1920
    port->write((const uint8_t *)msg, size);
1921
    port->write((const uint8_t *)&ck_a, 1);
1922
    port->write((const uint8_t *)&ck_b, 1);
1923
    return true;
1924
}
1925

1926
/*
1927
 *  requests the given message rate for a specific message class
1928
 *  and msg_id
1929
 *  returns true if it sent the request, false if waiting on knowing the port
1930
 */
1931
bool
1932
AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id)
1933
{
1934
    // Without knowing what communication port is being used it isn't possible to verify
1935
    // always ensure we have a port before sending the request
1936
    if(_ublox_port >= UBLOX_MAX_PORTS) {
1937
        _request_port();
1938
        return false;
1939
    } else {
1940
        struct ubx_cfg_msg msg;
1941
        msg.msg_class = msg_class;
1942
        msg.msg_id    = msg_id;
1943
        return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
1944
    }
1945
}
1946

1947
/*
1948
 *  configure a UBlox GPS for the given message rate for a specific
1949
 *  message class and msg_id
1950
 */
1951
bool
1952
AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
1953
{
1954
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
1955
        return false;
1956
    }
1957

1958
    struct ubx_cfg_msg_rate msg;
1959
    msg.msg_class = msg_class;
1960
    msg.msg_id    = msg_id;
1961
    msg.rate      = rate;
1962
    return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
1963
}
1964

1965
/*
1966
 *  configure F9/M10 based key/value pair - VALSET
1967
 */
1968
bool
1969
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value, uint8_t layers)
1970
{
1971
    if (!supports_F9_config()) {
1972
        return false;
1973
    }
1974

1975
    const uint8_t len = config_key_size(key);
1976
    struct ubx_cfg_valset msg {};
1977
    uint8_t buf[sizeof(msg)+len];
1978
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
1979
        return false;
1980
    }
1981
    msg.version = 1;
1982
    msg.layers = layers;
1983
    msg.transaction = 0;
1984
    msg.key = uint32_t(key);
1985
    memcpy(buf, &msg, sizeof(msg));
1986
    memcpy(&buf[sizeof(msg)], value, len);
1987
    auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, sizeof(buf));
1988
    return ret;
1989
}
1990

1991
/*
1992
 *  configure F9 based key/value pair - VALGET
1993
 */
1994
bool
1995
AP_GPS_UBLOX::_configure_valget(ConfigKey key)
1996
{
1997
    if (!supports_F9_config()) {
1998
        return false;
1999
    }
2000
    struct {
2001
        struct ubx_cfg_valget msg;
2002
        ConfigKey key;
2003
    } msg {};
2004
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(msg)+2)) {
2005
        return false;
2006
    }
2007
    msg.msg.version = 0;
2008
    msg.msg.layers = 0; // ram
2009
    msg.key = key;
2010
    return _send_message(CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg));
2011
}
2012

2013
/*
2014
 *  configure F9 based key/value pair for a complete configuration set
2015
 *
2016
 *  this method requests each configuration variable from the GPS.
2017
 *  When we handle the reply in _parse_gps we may then choose to set a
2018
 *  MSG_CFG_VALSET back to the GPS if we don't like its response.
2019
 */
2020
bool
2021
AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers)
2022
{
2023
    active_config.list = list;
2024
    active_config.count = count;
2025
    active_config.done_mask = 0;
2026
    active_config.unconfig_bit = unconfig_bit;
2027
    active_config.layers = layers;
2028
    // we start by fetching multiple values at once (with fetch_index
2029
    // -1) then if we get a NACK for VALGET we switch to fetching one
2030
    // value at a time. This copes with the M10S which can only fetch
2031
    // one value at a time
2032
    active_config.fetch_index = use_single_valget? 0 :-1;
2033

2034
    uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
2035
    struct ubx_cfg_valget msg {};
2036
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
2037
        return false;
2038
    }
2039
    msg.version = 0;
2040
    msg.layers = 0; // ram
2041
    memcpy(buf, &msg, sizeof(msg));
2042
    for (uint8_t i=0; i<count; i++) {
2043
        memcpy(&buf[sizeof(msg)+i*sizeof(ConfigKey)], &list[i].key, sizeof(ConfigKey));
2044
    }
2045
    return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
2046
}
2047

2048
/*
2049
 *  configure F9 based key/value pair for a complete configuration set
2050
 *
2051
 *  this method sends all the key/value pairs in a block, but makes no attempt to check
2052
 *  the results. Sending in a block is necessary for updates such as GNSS where certain
2053
 *  combinations are invalid and setting one at a time will not produce the correct result
2054
 *  if the result needs to be validated then a subsequent _configure_config_set() can be
2055
 *  issued which will get all the values and reset those that are not properly updated.
2056
 */
2057
bool
2058
AP_GPS_UBLOX::_configure_list_valset(const config_list *list, uint8_t count, uint8_t layers)
2059
{
2060
    if (!supports_F9_config()) {
2061
        return false;
2062
    }
2063

2064
    struct ubx_cfg_valset msg {};
2065
    uint8_t buf[sizeof(msg)+sizeof(config_list)*count];
2066
    if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
2067
        return false;
2068
    }
2069
    msg.version = 1;
2070
    msg.layers = layers;
2071
    msg.transaction = 0;
2072
    uint32_t msg_len = sizeof(msg) - sizeof(msg.key);
2073
    memcpy(buf, &msg, msg_len);
2074

2075
    uint8_t* payload = &buf[msg_len];
2076
    for (uint8_t i=0; i<count; i++) {
2077
        const uint8_t len = config_key_size(list[i].key);
2078
        memcpy(payload, &list[i].key, sizeof(ConfigKey));
2079
        payload += sizeof(ConfigKey);
2080
        msg_len += sizeof(ConfigKey);
2081
        memcpy(payload, &list[i].value, len);
2082
        payload += len;
2083
        msg_len += len;
2084
    }
2085

2086
    auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, msg_len);
2087
    return ret;
2088
}
2089

2090
/*
2091
 * save gps configurations to non-volatile memory sent until the call of
2092
 * this message
2093
 */
2094
void
2095
AP_GPS_UBLOX::_save_cfg()
2096
{
2097
    static const ubx_cfg_cfg save_cfg {
2098
      clearMask: 0,
2099
      saveMask: SAVE_CFG_ALL,
2100
      loadMask: 0
2101
    };
2102
    _send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
2103
    _last_cfg_sent_time = AP_HAL::millis();
2104
    _num_cfg_save_tries++;
2105
    GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2106
                                     "GPS %d: u-blox saving config",
2107
                                     state.instance + 1);
2108
}
2109

2110
/*
2111
  detect a Ublox GPS. Adds one byte, and returns true if the stream
2112
  matches a UBlox
2113
 */
2114
bool
2115
AP_GPS_UBLOX::_detect(struct UBLOX_detect_state &state, uint8_t data)
2116
{
2117
reset:
2118
	switch (state.step) {
2119
        case 1:
2120
            if (PREAMBLE2 == data) {
2121
                state.step++;
2122
                break;
2123
            }
2124
            state.step = 0;
2125
            FALLTHROUGH;
2126
        case 0:
2127
            if (PREAMBLE1 == data)
2128
                state.step++;
2129
            break;
2130
        case 2:
2131
            state.step++;
2132
            state.ck_b = state.ck_a = data;
2133
            break;
2134
        case 3:
2135
            state.step++;
2136
            state.ck_b += (state.ck_a += data);
2137
            break;
2138
        case 4:
2139
            state.step++;
2140
            state.ck_b += (state.ck_a += data);
2141
            state.payload_length = data;
2142
            break;
2143
        case 5:
2144
            state.step++;
2145
            state.ck_b += (state.ck_a += data);
2146
            state.payload_counter = 0;
2147
            break;
2148
        case 6:
2149
            state.ck_b += (state.ck_a += data);
2150
            if (++state.payload_counter == state.payload_length)
2151
                state.step++;
2152
            break;
2153
        case 7:
2154
            state.step++;
2155
            if (state.ck_a != data) {
2156
                state.step = 0;
2157
				goto reset;
2158
            }
2159
            break;
2160
        case 8:
2161
            state.step = 0;
2162
			if (state.ck_b == data) {
2163
				// a valid UBlox packet
2164
				return true;
2165
			} else {
2166
				goto reset;
2167
			}
2168
    }
2169
    return false;
2170
}
2171

2172
void
2173
AP_GPS_UBLOX::_request_version(void)
2174
{
2175
    _send_message(CLASS_MON, MSG_MON_VER, nullptr, 0);
2176
}
2177

2178
void
2179
AP_GPS_UBLOX::_configure_rate(void)
2180
{
2181
    struct ubx_cfg_nav_rate msg;
2182
    // require a minimum measurement rate of 5Hz
2183
    msg.measure_rate_ms = gps.get_rate_ms(state.instance);
2184
    msg.nav_rate        = 1;
2185
    msg.timeref         = 0;     // UTC time
2186
    _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
2187
}
2188

2189
static const char *reasons[] = {"navigation rate",
2190
                                "posllh rate",
2191
                                "status rate",
2192
                                "solution rate",
2193
                                "velned rate",
2194
                                "dop rate",
2195
                                "hw monitor rate",
2196
                                "hw2 monitor rate",
2197
                                "raw rate",
2198
                                "version",
2199
                                "navigation settings",
2200
                                "GNSS settings",
2201
                                "SBAS settings",
2202
                                "PVT rate",
2203
                                "time pulse settings",
2204
                                "TIMEGPS rate",
2205
                                "Time mode settings",
2206
                                "RTK MB",
2207
                                "TIM TM2",
2208
                                "F9",
2209
                                "M10",
2210
                                "L5 Enable Disable"};
2211

2212
static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
2213

2214
void
2215
AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
2216
    for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) {
2217
        if (_unconfigured_messages & (1 << i)) {
2218
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x",
2219
                (unsigned int)(state.instance + 1), reasons[i], (unsigned int)_unconfigured_messages);
2220
            break;
2221
        }
2222
    }
2223
}
2224

2225
/*
2226
  return velocity lag in seconds
2227
 */
2228
bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
2229
{
2230
    switch (_hardware_generation) {
2231
    case UBLOX_UNKNOWN_HARDWARE_GENERATION:
2232
        lag_sec = 0.22f;
2233
        // always bail out in this case, it's used to indicate we have yet to receive a valid
2234
        // hardware generation, however the user may have inhibited us detecting the generation
2235
        // so if we aren't allowed to do configuration, we will accept this as the default delay
2236
        return gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE;
2237
    case UBLOX_5:
2238
    case UBLOX_6:
2239
    default:
2240
        lag_sec = 0.22f;
2241
        break;
2242
    case UBLOX_7:
2243
    case UBLOX_M8:
2244
        // based on flight logs the 7 and 8 series seem to produce about 120ms lag
2245
        lag_sec = 0.12f;
2246
        break;
2247
    case UBLOX_F9:
2248
    case UBLOX_M9:
2249
    case UBLOX_M10:
2250
        // F9 lag not verified yet from flight log, but likely to be at least
2251
        // as good as M8
2252
        lag_sec = 0.12f;
2253
#if GPS_MOVING_BASELINE
2254
        if (role == AP_GPS::GPS_ROLE_MB_BASE ||
2255
            role == AP_GPS::GPS_ROLE_MB_ROVER) {
2256
            // the moving baseline rover will lag about 40ms from the
2257
            // base. We need to provide the more conservative value to
2258
            // ensure that the EKF allocates a larger enough buffer
2259
            lag_sec += 0.04;
2260
        }
2261
#endif
2262
        break;
2263
    };
2264
    return true;
2265
}
2266

2267
#if HAL_LOGGING_ENABLED
2268
void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const
2269
{
2270
    AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages();
2271

2272
    if (_have_version) {
2273
        AP::logger().Write_MessageF("u-blox %d HW: %s SW: %s",
2274
                                           state.instance+1,
2275
                                           _version.hwVersion,
2276
                                           _version.swVersion);
2277
    }
2278
}
2279
#endif
2280

2281
// uBlox specific check_new_itow(), handling message length
2282
void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
2283
{
2284
    check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
2285
}
2286

2287
// support for retrieving RTCMv3 data from a moving baseline base
2288
bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
2289
{
2290
#if GPS_MOVING_BASELINE
2291
    if (rtcm3_parser) {
2292
        len = rtcm3_parser->get_len(bytes);
2293
        return len > 0;
2294
    }
2295
#endif
2296
    return false;
2297
}
2298

2299
// clear previous RTCM3 packet
2300
void AP_GPS_UBLOX::clear_RTCMV3(void)
2301
{
2302
#if GPS_MOVING_BASELINE
2303
    if (rtcm3_parser) {
2304
        rtcm3_parser->clear_packet();
2305
    }
2306
#endif
2307
}
2308

2309
// ublox specific healthy checks
2310
bool AP_GPS_UBLOX::is_healthy(void) const
2311
{
2312
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2313
    if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
2314
        // allow for fake ublox moving baseline
2315
        return true;
2316
    }
2317
#endif
2318
#if GPS_MOVING_BASELINE
2319
    if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
2320
        role == AP_GPS::GPS_ROLE_MB_ROVER) &&
2321
        !supports_F9_config()) {
2322
        // need F9 or above for moving baseline
2323
        return false;
2324
    }
2325
    if (role == AP_GPS::GPS_ROLE_MB_BASE && rtcm3_parser == nullptr && !mb_use_uart2()) {
2326
        // we haven't initialised RTCMv3 parser
2327
        return false;
2328
    }
2329
#endif
2330
    return true;
2331
}
2332

2333
// populate config_GNSS with F9 GNSS configuration
2334
uint8_t AP_GPS_UBLOX::populate_F9_gnss(void)
2335
{
2336
    uint8_t cfg_count = 0;
2337

2338
    if (params.gnss_mode == 0) {
2339
        _unconfigured_messages &= ~CONFIG_F9;
2340
        last_configured_gnss = params.gnss_mode;
2341
        return 0;
2342
    }
2343

2344
    if ((_unconfigured_messages & CONFIG_F9) != 0) {
2345
        // ZED-F9P defaults are
2346
        // GPS L1C/A+L2C(ZED)
2347
        // SBAS L1C/A
2348
        // GALILEO E1+E5B(ZED)+E5A(NEO)
2349
        // BEIDOU B1+B2(ZED)+B2A(NEO)
2350
        // QZSS L1C/A+L2C(ZED)+L5(NEO)
2351
        // GLONASS L1+L2(ZED)
2352
        // IMES not supported
2353
        // GPS and QZSS should be enabled/disabled together, but we will leave them alone
2354
        // QZSS and SBAS can only be enabled if GPS is enabled
2355

2356
        if (config_GNSS == nullptr) {
2357
            config_GNSS = (config_list*)calloc(UBLOX_MAX_GNSS_CONFIG_BLOCKS*3, sizeof(config_list));
2358
        }
2359

2360
        if (config_GNSS == nullptr) {
2361
            return 0;
2362
        }
2363

2364
        uint8_t gnss_mode = params.gnss_mode;
2365
        gnss_mode |= 1U<<GNSS_GPS;
2366
        gnss_mode |= 1U<<GNSS_QZSS;
2367
        gnss_mode &= ~(1U<<GNSS_IMES);
2368
        params.gnss_mode.set_and_save(gnss_mode);
2369

2370
        for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
2371
            bool ena = gnss_mode & (1U<<i);
2372
            switch (i) {
2373
            case GNSS_SBAS:
2374
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_ENA, ena };
2375
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_L1CA_ENA, ena };
2376
                break;
2377
            case GNSS_GALILEO:
2378
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_ENA, ena };
2379
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E1_ENA, ena };
2380
                if (_hardware_variant == UBLOX_F9_ZED) {
2381
                    config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5B_ENA, ena };
2382
                } else {
2383
                    config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5A_ENA, ena };
2384
                }
2385
                break;
2386
            case GNSS_BEIDOU:
2387
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_ENA, ena };
2388
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B1_ENA, ena };
2389
                if (_hardware_variant == UBLOX_F9_ZED) {
2390
                    config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2_ENA, ena };
2391
                } else {
2392
                    config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2A_ENA, ena };
2393
                }
2394
                break;
2395
            case GNSS_GLONASS:
2396
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_ENA, ena };
2397
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L1_ENA, ena };
2398
                if (_hardware_variant == UBLOX_F9_ZED) {
2399
                    config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L2_ENA, ena };
2400
                }
2401
                break;
2402
            case GNSS_NAVIC:
2403
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_NAVIC_ENA, ena };
2404
                config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_NAVIC_L5_ENA, ena };
2405
                break;
2406
            // not supported or leave alone
2407
            case GNSS_IMES:
2408
            case GNSS_QZSS:
2409
            case GNSS_GPS:
2410
                break;
2411
            }
2412
        }
2413
    }
2414

2415
    last_configured_gnss = params.gnss_mode;
2416

2417
    return cfg_count;
2418
}
2419

2420
// return true if GPS is capable of F9 config
2421
bool AP_GPS_UBLOX::supports_F9_config(void) const
2422
{
2423
    return _hardware_generation == UBLOX_F9 || _hardware_generation == UBLOX_M10;
2424
}
2425

2426
// return true if GPS is capable of F9 config
2427
bool AP_GPS_UBLOX::is_gnss_key(ConfigKey key) const
2428
{
2429
    return (unsigned(key) & 0xFFFF0000) == 0x10310000;
2430
}
2431

2432
#endif
2433

2434
Product

Resources

Company
