mirror of https://github.com/ArduPilot/ardupilot
2392 lines
85 KiB
C++
2392 lines
85 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
//
|
|
// u-blox GPS driver for ArduPilot
|
|
// Origin code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
|
|
// Substantially rewritten for new GPS driver structure by Andrew Tridgell
|
|
//
|
|
#include "AP_GPS_UBLOX.h"
|
|
|
|
#if AP_GPS_UBLOX_ENABLED
|
|
|
|
#include "AP_GPS.h"
|
|
#include <AP_HAL/Util.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include "RTCM3_Parser.h"
|
|
#include <stdio.h>
|
|
|
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
|
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
|
|
#define UBLOX_SPEED_CHANGE 1
|
|
#else
|
|
#define UBLOX_SPEED_CHANGE 0
|
|
#endif
|
|
|
|
|
|
#define UBLOX_DEBUGGING 0
|
|
#define UBLOX_FAKE_3DLOCK 0
|
|
#ifndef CONFIGURE_PPS_PIN
|
|
#define CONFIGURE_PPS_PIN 0
|
|
#endif
|
|
|
|
// this is number of epochs per output. A higher value will reduce
|
|
// the uart bandwidth needed and allow for higher latency
|
|
#define RTK_MB_RTCM_RATE 1
|
|
|
|
// use this to enable debugging of moving baseline configs
|
|
#define UBLOX_MB_DEBUGGING 0
|
|
|
|
// debug VALGET/VALSET configuration
|
|
#define UBLOX_CFG_DEBUGGING 0
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#if UBLOX_DEBUGGING
|
|
#if defined(HAL_BUILD_AP_PERIPH)
|
|
extern "C" {
|
|
void can_printf(const char *fmt, ...);
|
|
}
|
|
# define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
|
|
#else
|
|
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
|
#endif
|
|
#else
|
|
# define Debug(fmt, args ...)
|
|
#endif
|
|
|
|
#if UBLOX_MB_DEBUGGING
|
|
#if defined(HAL_BUILD_AP_PERIPH)
|
|
extern "C" {
|
|
void can_printf(const char *fmt, ...);
|
|
}
|
|
# define MB_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
|
|
#else
|
|
# define MB_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
|
#endif
|
|
#else
|
|
# define MB_Debug(fmt, args ...)
|
|
#endif
|
|
|
|
#if UBLOX_CFG_DEBUGGING
|
|
#if defined(HAL_BUILD_AP_PERIPH)
|
|
extern "C" {
|
|
void can_printf(const char *fmt, ...);
|
|
}
|
|
# define CFG_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
|
|
#else
|
|
# define CFG_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
|
#endif
|
|
#else
|
|
# define CFG_Debug(fmt, args ...)
|
|
#endif
|
|
|
|
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps,
|
|
AP_GPS::Params &_params,
|
|
AP_GPS::GPS_State &_state,
|
|
AP_HAL::UARTDriver *_port,
|
|
AP_GPS::GPS_Role _role) :
|
|
AP_GPS_Backend(_gps, _params, _state, _port),
|
|
role(_role)
|
|
{
|
|
// stop any config strings that are pending
|
|
gps.send_blob_start(state.instance, nullptr, 0);
|
|
|
|
// start the process of updating the GPS rates
|
|
_request_next_config();
|
|
|
|
#if CONFIGURE_PPS_PIN
|
|
_unconfigured_messages |= CONFIG_TP5;
|
|
#endif
|
|
|
|
#if GPS_MOVING_BASELINE
|
|
if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) {
|
|
rtcm3_parser = NEW_NOTHROW RTCM3_Parser;
|
|
if (rtcm3_parser == nullptr) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1);
|
|
}
|
|
_unconfigured_messages |= CONFIG_RTK_MOVBASE;
|
|
}
|
|
if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
|
|
_unconfigured_messages |= CONFIG_RTK_MOVBASE;
|
|
state.gps_yaw_configured = true;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
AP_GPS_UBLOX::~AP_GPS_UBLOX()
|
|
{
|
|
#if GPS_MOVING_BASELINE
|
|
delete rtcm3_parser;
|
|
#endif
|
|
}
|
|
|
|
#if GPS_MOVING_BASELINE
|
|
/*
|
|
config for F9 GPS in moving baseline base role
|
|
See ZED-F9P integration manual section 3.1.5.6.1
|
|
*/
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart1[] {
|
|
{ ConfigKey::CFG_UART1OUTPROT_RTCM3X, 1},
|
|
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
|
|
};
|
|
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart2[] {
|
|
{ ConfigKey::CFG_UART2_ENABLED, 1},
|
|
{ ConfigKey::CFG_UART2_BAUDRATE, 460800},
|
|
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 1},
|
|
{ ConfigKey::CFG_UART1OUTPROT_RTCM3X, 0},
|
|
{ ConfigKey::CFG_UART1INPROT_RTCM3X, 1},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, RTK_MB_RTCM_RATE},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
|
};
|
|
|
|
|
|
/*
|
|
config for F9 GPS in moving baseline rover role
|
|
See ZED-F9P integration manual section 3.1.5.6.1.
|
|
Note that we list the RTCM msg types as 0 to prevent getting RTCM
|
|
data from a GPS previously configured as a base
|
|
*/
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart1[] {
|
|
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
|
|
{ ConfigKey::CFG_UART1INPROT_RTCM3X, 1},
|
|
{ ConfigKey::CFG_UART2INPROT_RTCM3X, 0},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
|
|
};
|
|
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
|
|
{ ConfigKey::CFG_UART2_ENABLED, 1},
|
|
{ ConfigKey::CFG_UART2_BAUDRATE, 460800},
|
|
{ ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0},
|
|
{ ConfigKey::CFG_UART2INPROT_RTCM3X, 1},
|
|
{ ConfigKey::CFG_UART1INPROT_RTCM3X, 0},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1},
|
|
{ ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
|
|
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
|
|
};
|
|
#endif // GPS_MOVING_BASELINE
|
|
|
|
/*
|
|
config changes for M10
|
|
we need to use B1C not B1 signal for Beidou on M10 to allow solid 5Hz,
|
|
and also disable Glonass and enable QZSS
|
|
*/
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_M10[] {
|
|
{ ConfigKey::CFG_SIGNAL_BDS_ENA, 1},
|
|
{ ConfigKey::CFG_SIGNAL_BDS_B1_ENA, 0},
|
|
{ ConfigKey::CFG_SIGNAL_BDS_B1C_ENA, 1},
|
|
{ ConfigKey::CFG_SIGNAL_GLO_ENA, 0},
|
|
{ ConfigKey::CFG_SIGNAL_QZSS_ENA, 1},
|
|
{ ConfigKey::CFG_SIGNAL_QZSS_L1CA_ENA, 1},
|
|
{ ConfigKey::CFG_SIGNAL_QZSS_L1S_ENA, 1},
|
|
{ ConfigKey::CFG_NAVSPG_DYNMODEL, 8}, // Air < 4g
|
|
};
|
|
|
|
|
|
/*
|
|
config changes for L5 modules
|
|
*/
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_ena[] {
|
|
{ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 1},
|
|
{ConfigKey::CFG_SIGNAL_GPS_L5_ENA, 1},
|
|
};
|
|
|
|
const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] {
|
|
{ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0},
|
|
};
|
|
|
|
void
|
|
AP_GPS_UBLOX::_request_next_config(void)
|
|
{
|
|
// don't request config if we shouldn't configure the GPS
|
|
if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
|
|
return;
|
|
}
|
|
|
|
// Ensure there is enough space for the largest possible outgoing message
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
|
|
// not enough space - do it next time
|
|
return;
|
|
}
|
|
|
|
if (_unconfigured_messages == CONFIG_RATE_SOL && havePvtMsg) {
|
|
/*
|
|
we don't need SOL if we have PVT and TIMEGPS. This is needed
|
|
as F9P doesn't support the SOL message
|
|
*/
|
|
_unconfigured_messages &= ~CONFIG_RATE_SOL;
|
|
}
|
|
|
|
Debug("Unconfigured messages: 0x%x Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message);
|
|
|
|
// check AP_GPS_UBLOX.h for the enum that controls the order.
|
|
// This switch statement isn't maintained against the enum in order to reduce code churn
|
|
switch (_next_message++) {
|
|
case STEP_PVT:
|
|
if(!_request_message_rate(CLASS_NAV, MSG_PVT)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_TIMEGPS:
|
|
if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_PORT:
|
|
_request_port();
|
|
break;
|
|
case STEP_POLL_SVINFO:
|
|
// not required once we know what generation we are on
|
|
if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) {
|
|
if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
break;
|
|
case STEP_POLL_SBAS:
|
|
if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
|
|
_send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0);
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_SBAS;
|
|
}
|
|
break;
|
|
case STEP_POLL_NAV:
|
|
if (!_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_POLL_GNSS:
|
|
if (supports_F9_config()) {
|
|
if (last_configured_gnss != params.gnss_mode) {
|
|
_unconfigured_messages |= CONFIG_F9;
|
|
}
|
|
break;
|
|
}
|
|
if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_POLL_TP5:
|
|
#if CONFIGURE_PPS_PIN
|
|
if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) {
|
|
_next_message--;
|
|
}
|
|
#endif
|
|
break;
|
|
case STEP_NAV_RATE:
|
|
if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_POSLLH:
|
|
if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_STATUS:
|
|
if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_SOL:
|
|
if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_VELNED:
|
|
if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_DOP:
|
|
if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_MON_HW:
|
|
if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_MON_HW2:
|
|
if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
|
|
_next_message--;
|
|
}
|
|
break;
|
|
case STEP_RAW:
|
|
#if UBLOX_RXM_RAW_LOGGING
|
|
if(gps._raw_data == 0) {
|
|
_unconfigured_messages &= ~CONFIG_RATE_RAW;
|
|
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
|
|
_next_message--;
|
|
}
|
|
#else
|
|
_unconfigured_messages & = ~CONFIG_RATE_RAW;
|
|
#endif
|
|
break;
|
|
case STEP_RAWX:
|
|
#if UBLOX_RXM_RAW_LOGGING
|
|
if(gps._raw_data == 0) {
|
|
_unconfigured_messages &= ~CONFIG_RATE_RAW;
|
|
} else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
|
|
_next_message--;
|
|
}
|
|
#else
|
|
_unconfigured_messages & = ~CONFIG_RATE_RAW;
|
|
#endif
|
|
break;
|
|
case STEP_VERSION:
|
|
if(!_have_version && !hal.util->get_soft_armed()) {
|
|
_request_version();
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_VERSION;
|
|
}
|
|
break;
|
|
case STEP_TMODE:
|
|
if (supports_F9_config()) {
|
|
if (!_configure_valget(ConfigKey::TMODE_MODE)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
break;
|
|
case STEP_RTK_MOVBASE:
|
|
#if GPS_MOVING_BASELINE
|
|
if (supports_F9_config()) {
|
|
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1");
|
|
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2");
|
|
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart1), "done_mask too small, rover1");
|
|
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart2), "done_mask too small, rover2");
|
|
if (role == AP_GPS::GPS_ROLE_MB_BASE) {
|
|
const config_list *list = mb_use_uart2()?config_MB_Base_uart2:config_MB_Base_uart1;
|
|
uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Base_uart2):ARRAY_SIZE(config_MB_Base_uart1);
|
|
if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
|
|
const config_list *list = mb_use_uart2()?config_MB_Rover_uart2:config_MB_Rover_uart1;
|
|
uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Rover_uart2):ARRAY_SIZE(config_MB_Rover_uart1);
|
|
if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
break;
|
|
case STEP_TIM_TM2:
|
|
#if UBLOX_TIM_TM2_LOGGING
|
|
if(!_request_message_rate(CLASS_TIM, MSG_TIM_TM2)) {
|
|
_next_message--;
|
|
}
|
|
#else
|
|
_unconfigured_messages &= ~CONFIG_TIM_TM2;
|
|
#endif
|
|
break;
|
|
|
|
case STEP_F9: {
|
|
if (_hardware_generation == UBLOX_F9) {
|
|
uint8_t cfg_count = populate_F9_gnss();
|
|
// special handling of F9 config
|
|
if (cfg_count > 0) {
|
|
CFG_Debug("Sending F9 settings, GNSS=%u", params.gnss_mode);
|
|
|
|
if (!_configure_list_valset(config_GNSS, cfg_count, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
|
|
_next_message--;
|
|
break;
|
|
}
|
|
_f9_config_time = AP_HAL::millis();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case STEP_F9_VALIDATE: {
|
|
if (_hardware_generation == UBLOX_F9) {
|
|
// GNSS takes 0.5s to reset
|
|
if (AP_HAL::millis() - _f9_config_time < 500) {
|
|
_next_message--;
|
|
break;
|
|
}
|
|
_f9_config_time = 0;
|
|
uint8_t cfg_count = populate_F9_gnss();
|
|
// special handling of F9 config
|
|
if (cfg_count > 0) {
|
|
CFG_Debug("Validating F9 settings, GNSS=%u", params.gnss_mode);
|
|
// now validate all of the settings, this is a no-op if the first call succeeded
|
|
if (!_configure_config_set(config_GNSS, cfg_count, CONFIG_F9, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case STEP_M10: {
|
|
if (_hardware_generation == UBLOX_M10) {
|
|
// special handling of M10 config
|
|
const config_list *list = config_M10;
|
|
const uint8_t list_length = ARRAY_SIZE(config_M10);
|
|
Debug("Sending M10 settings");
|
|
if (!_configure_config_set(list, list_length, CONFIG_M10, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case STEP_L5: {
|
|
if (supports_l5 && option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
|
|
const config_list *list = config_L5_ovrd_ena;
|
|
const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_ena);
|
|
if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
|
|
_next_message--;
|
|
}
|
|
} else if (supports_l5 && !option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
|
|
const config_list *list = config_L5_ovrd_dis;
|
|
const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_dis);
|
|
if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
|
|
_next_message--;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
default:
|
|
// this case should never be reached, do a full reset if it is hit
|
|
_next_message = STEP_PVT;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void
|
|
AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
|
|
uint8_t desired_rate;
|
|
uint32_t config_msg_id;
|
|
switch(msg_class) {
|
|
case CLASS_NAV:
|
|
switch(msg_id) {
|
|
case MSG_POSLLH:
|
|
desired_rate = havePvtMsg ? 0 : RATE_POSLLH;
|
|
config_msg_id = CONFIG_RATE_POSLLH;
|
|
break;
|
|
case MSG_STATUS:
|
|
desired_rate = havePvtMsg ? 0 : RATE_STATUS;
|
|
config_msg_id = CONFIG_RATE_STATUS;
|
|
break;
|
|
case MSG_SOL:
|
|
desired_rate = havePvtMsg ? 0 : RATE_SOL;
|
|
config_msg_id = CONFIG_RATE_SOL;
|
|
break;
|
|
case MSG_PVT:
|
|
desired_rate = RATE_PVT;
|
|
config_msg_id = CONFIG_RATE_PVT;
|
|
break;
|
|
case MSG_TIMEGPS:
|
|
desired_rate = RATE_TIMEGPS;
|
|
config_msg_id = CONFIG_RATE_TIMEGPS;
|
|
break;
|
|
case MSG_VELNED:
|
|
desired_rate = havePvtMsg ? 0 : RATE_VELNED;
|
|
config_msg_id = CONFIG_RATE_VELNED;
|
|
break;
|
|
case MSG_DOP:
|
|
desired_rate = RATE_DOP;
|
|
config_msg_id = CONFIG_RATE_DOP;
|
|
break;
|
|
default:
|
|
return;
|
|
}
|
|
break;
|
|
case CLASS_MON:
|
|
switch(msg_id) {
|
|
case MSG_MON_HW:
|
|
desired_rate = RATE_HW;
|
|
config_msg_id = CONFIG_RATE_MON_HW;
|
|
break;
|
|
case MSG_MON_HW2:
|
|
desired_rate = RATE_HW2;
|
|
config_msg_id = CONFIG_RATE_MON_HW2;
|
|
break;
|
|
default:
|
|
return;
|
|
}
|
|
break;
|
|
#if UBLOX_RXM_RAW_LOGGING
|
|
case CLASS_RXM:
|
|
switch(msg_id) {
|
|
case MSG_RXM_RAW:
|
|
desired_rate = gps._raw_data;
|
|
config_msg_id = CONFIG_RATE_RAW;
|
|
break;
|
|
case MSG_RXM_RAWX:
|
|
desired_rate = gps._raw_data;
|
|
config_msg_id = CONFIG_RATE_RAW;
|
|
break;
|
|
default:
|
|
return;
|
|
}
|
|
break;
|
|
#endif // UBLOX_RXM_RAW_LOGGING
|
|
#if UBLOX_TIM_TM2_LOGGING
|
|
case CLASS_TIM:
|
|
if (msg_id == MSG_TIM_TM2) {
|
|
desired_rate = RATE_TIM_TM2;
|
|
config_msg_id = CONFIG_TIM_TM2;
|
|
break;
|
|
}
|
|
return;
|
|
#endif // UBLOX_TIM_TM2_LOGGING
|
|
default:
|
|
return;
|
|
}
|
|
|
|
if (rate == desired_rate) {
|
|
// coming in at correct rate; mark as configured
|
|
_unconfigured_messages &= ~config_msg_id;
|
|
return;
|
|
}
|
|
|
|
// coming in at wrong rate; try to configure it
|
|
_configure_message_rate(msg_class, msg_id, desired_rate);
|
|
_unconfigured_messages |= config_msg_id;
|
|
_cfg_needs_save = true;
|
|
}
|
|
|
|
// Requests the ublox driver to identify what port we are using to communicate
|
|
void
|
|
AP_GPS_UBLOX::_request_port(void)
|
|
{
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+2)) {
|
|
// not enough space - do it next time
|
|
return;
|
|
}
|
|
_send_message(CLASS_CFG, MSG_CFG_PRT, nullptr, 0);
|
|
}
|
|
|
|
// Ensure there is enough space for the largest possible outgoing message
|
|
// Process bytes available from the stream
|
|
//
|
|
// The stream is assumed to contain only messages we recognise. If it
|
|
// contains other messages, and those messages contain the preamble
|
|
// bytes, it is possible for this code to fail to synchronise to the
|
|
// stream immediately. Without buffering the entire message and
|
|
// re-processing it from the top, this is unavoidable. The parser
|
|
// attempts to avoid this when possible.
|
|
//
|
|
bool
|
|
AP_GPS_UBLOX::read(void)
|
|
{
|
|
bool parsed = false;
|
|
uint32_t millis_now = AP_HAL::millis();
|
|
|
|
// walk through the gps configuration at 1 message per second
|
|
if (millis_now - _last_config_time >= _delay_time) {
|
|
_request_next_config();
|
|
_last_config_time = millis_now;
|
|
if (_unconfigured_messages) { // send the updates faster until fully configured
|
|
if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) {
|
|
_delay_time = 300;
|
|
} else {
|
|
_delay_time = 750;
|
|
}
|
|
} else {
|
|
_delay_time = 2000;
|
|
}
|
|
}
|
|
|
|
if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
|
|
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
|
|
!hal.util->get_soft_armed()) {
|
|
//save the configuration sent until now
|
|
if (gps._save_config == 1 ||
|
|
(gps._save_config == 2 && _cfg_needs_save)) {
|
|
_save_cfg();
|
|
}
|
|
}
|
|
|
|
const uint16_t numc = MIN(port->available(), 8192U);
|
|
for (uint16_t i = 0; i < numc; i++) { // Process bytes received
|
|
|
|
// read the next byte
|
|
uint8_t data;
|
|
if (!port->read(data)) {
|
|
break;
|
|
}
|
|
#if AP_GPS_DEBUG_LOGGING_ENABLED
|
|
log_data(&data, 1);
|
|
#endif
|
|
|
|
#if GPS_MOVING_BASELINE
|
|
if (rtcm3_parser) {
|
|
if (rtcm3_parser->read(data)) {
|
|
// we've found a RTCMv3 packet. We stop parsing at
|
|
// this point and reset u-blox parse state. We need to
|
|
// stop parsing to give the higher level driver a
|
|
// chance to send the RTCMv3 packet to another (rover)
|
|
// GPS
|
|
_step = 0;
|
|
break;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
reset:
|
|
switch(_step) {
|
|
|
|
// Message preamble detection
|
|
//
|
|
// If we fail to match any of the expected bytes, we reset
|
|
// the state machine and re-consider the failed byte as
|
|
// the first byte of the preamble. This improves our
|
|
// chances of recovering from a mismatch and makes it less
|
|
// likely that we will be fooled by the preamble appearing
|
|
// as data in some other message.
|
|
//
|
|
case 1:
|
|
if (PREAMBLE2 == data) {
|
|
_step++;
|
|
break;
|
|
}
|
|
_step = 0;
|
|
Debug("reset %u", __LINE__);
|
|
FALLTHROUGH;
|
|
case 0:
|
|
if(PREAMBLE1 == data)
|
|
_step++;
|
|
break;
|
|
|
|
// Message header processing
|
|
//
|
|
// We sniff the class and message ID to decide whether we
|
|
// are going to gather the message bytes or just discard
|
|
// them.
|
|
//
|
|
// We always collect the length so that we can avoid being
|
|
// fooled by preamble bytes in messages.
|
|
//
|
|
case 2:
|
|
_step++;
|
|
_class = data;
|
|
_ck_b = _ck_a = data; // reset the checksum accumulators
|
|
break;
|
|
case 3:
|
|
_step++;
|
|
_ck_b += (_ck_a += data); // checksum byte
|
|
_msg_id = data;
|
|
break;
|
|
case 4:
|
|
_step++;
|
|
_ck_b += (_ck_a += data); // checksum byte
|
|
_payload_length = data; // payload length low byte
|
|
break;
|
|
case 5:
|
|
_step++;
|
|
_ck_b += (_ck_a += data); // checksum byte
|
|
|
|
_payload_length += (uint16_t)(data<<8);
|
|
if (_payload_length > sizeof(_buffer)) {
|
|
Debug("large payload %u", (unsigned)_payload_length);
|
|
// assume any payload bigger then what we know about is noise
|
|
_payload_length = 0;
|
|
_step = 0;
|
|
goto reset;
|
|
}
|
|
_payload_counter = 0; // prepare to receive payload
|
|
if (_payload_length == 0) {
|
|
// bypass payload and go straight to checksum
|
|
_step++;
|
|
}
|
|
break;
|
|
|
|
// Receive message data
|
|
//
|
|
case 6:
|
|
_ck_b += (_ck_a += data); // checksum byte
|
|
if (_payload_counter < sizeof(_buffer)) {
|
|
_buffer[_payload_counter] = data;
|
|
}
|
|
if (++_payload_counter == _payload_length)
|
|
_step++;
|
|
break;
|
|
|
|
// Checksum and message processing
|
|
//
|
|
case 7:
|
|
_step++;
|
|
if (_ck_a != data) {
|
|
Debug("bad cka %x should be %x", data, _ck_a);
|
|
_step = 0;
|
|
goto reset;
|
|
}
|
|
break;
|
|
case 8:
|
|
_step = 0;
|
|
if (_ck_b != data) {
|
|
Debug("bad ckb %x should be %x", data, _ck_b);
|
|
break; // bad checksum
|
|
}
|
|
|
|
#if GPS_MOVING_BASELINE
|
|
if (rtcm3_parser) {
|
|
// this is a uBlox packet, discard any partial RTCMv3 state
|
|
rtcm3_parser->reset();
|
|
}
|
|
#endif
|
|
if (_parse_gps()) {
|
|
parsed = true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
return parsed;
|
|
}
|
|
|
|
// Private Methods /////////////////////////////////////////////////////////////
|
|
void AP_GPS_UBLOX::log_mon_hw(void)
|
|
{
|
|
#if HAL_LOGGING_ENABLED
|
|
if (!should_log()) {
|
|
return;
|
|
}
|
|
struct log_Ubx1 pkt = {
|
|
LOG_PACKET_HEADER_INIT(LOG_GPS_UBX1_MSG),
|
|
time_us : AP_HAL::micros64(),
|
|
instance : state.instance,
|
|
noisePerMS : _buffer.mon_hw_60.noisePerMS,
|
|
jamInd : _buffer.mon_hw_60.jamInd,
|
|
aPower : _buffer.mon_hw_60.aPower,
|
|
agcCnt : _buffer.mon_hw_60.agcCnt,
|
|
config : _unconfigured_messages,
|
|
};
|
|
if (_payload_length == 68) {
|
|
pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS;
|
|
pkt.jamInd = _buffer.mon_hw_68.jamInd;
|
|
pkt.aPower = _buffer.mon_hw_68.aPower;
|
|
pkt.agcCnt = _buffer.mon_hw_68.agcCnt;
|
|
}
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
#endif
|
|
}
|
|
|
|
void AP_GPS_UBLOX::log_mon_hw2(void)
|
|
{
|
|
#if HAL_LOGGING_ENABLED
|
|
if (!should_log()) {
|
|
return;
|
|
}
|
|
|
|
struct log_Ubx2 pkt = {
|
|
LOG_PACKET_HEADER_INIT(LOG_GPS_UBX2_MSG),
|
|
time_us : AP_HAL::micros64(),
|
|
instance : state.instance,
|
|
ofsI : _buffer.mon_hw2.ofsI,
|
|
magI : _buffer.mon_hw2.magI,
|
|
ofsQ : _buffer.mon_hw2.ofsQ,
|
|
magQ : _buffer.mon_hw2.magQ,
|
|
};
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
#endif
|
|
}
|
|
|
|
#if UBLOX_TIM_TM2_LOGGING
|
|
void AP_GPS_UBLOX::log_tim_tm2(void)
|
|
{
|
|
#if HAL_LOGGING_ENABLED
|
|
if (!should_log()) {
|
|
return;
|
|
}
|
|
|
|
// @LoggerMessage: UBXT
|
|
// @Description: uBlox specific UBX-TIM-TM2 logging, see uBlox interface description
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: I: GPS instance number
|
|
// @Field: ch: Channel (i.e. EXTINT) upon which the pulse was measured
|
|
// @Field: flags: Bitmask
|
|
// @Field: count: Rising edge counter
|
|
// @Field: wnR: Week number of last rising edge
|
|
// @Field: MsR: Tow of rising edge
|
|
// @Field: SubMsR: Millisecond fraction of tow of rising edge in nanoseconds
|
|
// @Field: wnF: Week number of last falling edge
|
|
// @Field: MsF: Tow of falling edge
|
|
// @Field: SubMsF: Millisecond fraction of tow of falling edge in nanoseconds
|
|
// @Field: accEst: Accuracy estimate
|
|
|
|
AP::logger().WriteStreaming("UBXT",
|
|
"TimeUS,I,ch,flags,count,wnR,MsR,SubMsR,wnF,MsF,SubMsF,accEst",
|
|
"s#----ss-sss",
|
|
"F-----CI-CII",
|
|
"QBBBHHIIHIII",
|
|
AP_HAL::micros64(),
|
|
state.instance,
|
|
_buffer.tim_tm2.ch,
|
|
_buffer.tim_tm2.flags,
|
|
_buffer.tim_tm2.count,
|
|
_buffer.tim_tm2.wnR,
|
|
_buffer.tim_tm2.towMsR,
|
|
_buffer.tim_tm2.towSubMsR,
|
|
_buffer.tim_tm2.wnF,
|
|
_buffer.tim_tm2.towMsF,
|
|
_buffer.tim_tm2.towSubMsF,
|
|
_buffer.tim_tm2.accEst);
|
|
#endif
|
|
}
|
|
#endif // UBLOX_TIM_TM2_LOGGING
|
|
|
|
#if UBLOX_RXM_RAW_LOGGING
|
|
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
|
|
{
|
|
#if HAL_LOGGING_ENABLED
|
|
if (!should_log()) {
|
|
return;
|
|
}
|
|
|
|
uint64_t now = AP_HAL::micros64();
|
|
for (uint8_t i=0; i<raw.numSV; i++) {
|
|
struct log_GPS_RAW pkt = {
|
|
LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
|
|
time_us : now,
|
|
iTOW : raw.iTOW,
|
|
week : raw.week,
|
|
numSV : raw.numSV,
|
|
sv : raw.svinfo[i].sv,
|
|
cpMes : raw.svinfo[i].cpMes,
|
|
prMes : raw.svinfo[i].prMes,
|
|
doMes : raw.svinfo[i].doMes,
|
|
mesQI : raw.svinfo[i].mesQI,
|
|
cno : raw.svinfo[i].cno,
|
|
lli : raw.svinfo[i].lli
|
|
};
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
|
|
{
|
|
#if HAL_LOGGING_ENABLED
|
|
if (!should_log()) {
|
|
return;
|
|
}
|
|
|
|
uint64_t now = AP_HAL::micros64();
|
|
|
|
struct log_GPS_RAWH header = {
|
|
LOG_PACKET_HEADER_INIT(LOG_GPS_RAWH_MSG),
|
|
time_us : now,
|
|
rcvTow : raw.rcvTow,
|
|
week : raw.week,
|
|
leapS : raw.leapS,
|
|
numMeas : raw.numMeas,
|
|
recStat : raw.recStat
|
|
};
|
|
AP::logger().WriteBlock(&header, sizeof(header));
|
|
|
|
for (uint8_t i=0; i<raw.numMeas; i++) {
|
|
struct log_GPS_RAWS pkt = {
|
|
LOG_PACKET_HEADER_INIT(LOG_GPS_RAWS_MSG),
|
|
time_us : now,
|
|
prMes : raw.svinfo[i].prMes,
|
|
cpMes : raw.svinfo[i].cpMes,
|
|
doMes : raw.svinfo[i].doMes,
|
|
gnssId : raw.svinfo[i].gnssId,
|
|
svId : raw.svinfo[i].svId,
|
|
freqId : raw.svinfo[i].freqId,
|
|
locktime : raw.svinfo[i].locktime,
|
|
cno : raw.svinfo[i].cno,
|
|
prStdev : raw.svinfo[i].prStdev,
|
|
cpStdev : raw.svinfo[i].cpStdev,
|
|
doStdev : raw.svinfo[i].doStdev,
|
|
trkStat : raw.svinfo[i].trkStat
|
|
};
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
}
|
|
#endif
|
|
}
|
|
#endif // UBLOX_RXM_RAW_LOGGING
|
|
|
|
void AP_GPS_UBLOX::unexpected_message(void)
|
|
{
|
|
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
|
|
if (++_disable_counter == 0) {
|
|
// disable future sends of this message id, but
|
|
// only do this every 256 messages, as some
|
|
// message types can't be disabled and we don't
|
|
// want to get into an ack war
|
|
Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
|
|
_configure_message_rate(_class, _msg_id, 0);
|
|
}
|
|
}
|
|
|
|
// return size of a config key, or 0 if unknown
|
|
uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
|
|
{
|
|
// step over the value
|
|
const uint8_t key_size = (uint32_t(key) >> 28) & 0x07; // mask off the storage size
|
|
switch (key_size) {
|
|
case 0x1: // bit
|
|
case 0x2: // byte
|
|
return 1;
|
|
case 0x3: // 2 bytes
|
|
return 2;
|
|
case 0x4: // 4 bytes
|
|
return 4;
|
|
case 0x5: // 8 bytes
|
|
return 8;
|
|
default:
|
|
break;
|
|
}
|
|
// invalid
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
find index of a config key in the active_config list, or -1
|
|
*/
|
|
int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
|
|
{
|
|
if (active_config.list == nullptr) {
|
|
return -1;
|
|
}
|
|
for (uint8_t i=0; i<active_config.count; i++) {
|
|
if (key == active_config.list[i].key) {
|
|
return (int8_t)i;
|
|
}
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
bool
|
|
AP_GPS_UBLOX::_parse_gps(void)
|
|
{
|
|
if (_class == CLASS_ACK) {
|
|
Debug("ACK %u", (unsigned)_msg_id);
|
|
|
|
if(_msg_id == MSG_ACK_ACK) {
|
|
switch(_buffer.ack.clsID) {
|
|
case CLASS_CFG:
|
|
switch(_buffer.ack.msgID) {
|
|
case MSG_CFG_CFG:
|
|
_cfg_saved = true;
|
|
_cfg_needs_save = false;
|
|
break;
|
|
case MSG_CFG_GNSS:
|
|
_unconfigured_messages &= ~CONFIG_GNSS;
|
|
break;
|
|
case MSG_CFG_MSG:
|
|
// There is no way to know what MSG config was ack'ed, assume it was the last
|
|
// one requested. To verify it rerequest the last config we sent. If we miss
|
|
// the actual ack we will catch it next time through the poll loop, but that
|
|
// will be a good chunk of time later.
|
|
break;
|
|
case MSG_CFG_NAV_SETTINGS:
|
|
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
|
|
break;
|
|
case MSG_CFG_RATE:
|
|
// The GPS will ACK a update rate that is invalid. in order to detect this
|
|
// only accept the rate as configured by reading the settings back and
|
|
// validating that they all match the target values
|
|
break;
|
|
case MSG_CFG_SBAS:
|
|
_unconfigured_messages &= ~CONFIG_SBAS;
|
|
break;
|
|
case MSG_CFG_TP5:
|
|
_unconfigured_messages &= ~CONFIG_TP5;
|
|
break;
|
|
}
|
|
|
|
break;
|
|
case CLASS_MON:
|
|
switch(_buffer.ack.msgID) {
|
|
case MSG_MON_HW:
|
|
_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
|
|
break;
|
|
case MSG_MON_HW2:
|
|
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if(_msg_id == MSG_ACK_NACK) {
|
|
switch(_buffer.nack.clsID) {
|
|
case CLASS_CFG:
|
|
switch(_buffer.nack.msgID) {
|
|
case MSG_CFG_VALGET:
|
|
CFG_Debug("NACK VALGET 0x%x", (unsigned)_buffer.nack.msgID);
|
|
if (active_config.list != nullptr) {
|
|
/*
|
|
likely this device does not support fetching multiple keys at once, go one at a time
|
|
*/
|
|
if (active_config.fetch_index == -1) {
|
|
CFG_Debug("NACK starting %u", unsigned(active_config.count));
|
|
active_config.fetch_index = 0;
|
|
} else {
|
|
// the device does not support the config key we asked for,
|
|
// consider the bit as done
|
|
active_config.done_mask |= (1U<<active_config.fetch_index);
|
|
CFG_Debug("NACK %d 0x%x done=0x%x",
|
|
int(active_config.fetch_index),
|
|
unsigned(active_config.list[active_config.fetch_index].key),
|
|
unsigned(active_config.done_mask));
|
|
if (active_config.done_mask == (1U<<active_config.count)-1) {
|
|
// all done!
|
|
_unconfigured_messages &= ~active_config.unconfig_bit;
|
|
}
|
|
active_config.fetch_index++;
|
|
}
|
|
if (active_config.fetch_index < active_config.count) {
|
|
_configure_valget(active_config.list[active_config.fetch_index].key);
|
|
}
|
|
}
|
|
break;
|
|
case MSG_CFG_VALSET:
|
|
CFG_Debug("NACK VALSET 0x%x 0x%x", (unsigned)_buffer.nack.msgID,
|
|
unsigned(active_config.list[active_config.set_index].key));
|
|
if (is_gnss_key(active_config.list[active_config.set_index].key)) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: unable to configure band 0x%02x",
|
|
unsigned(state.instance + 1), unsigned(active_config.list[active_config.set_index].key));
|
|
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
if (_class == CLASS_CFG) {
|
|
switch(_msg_id) {
|
|
case MSG_CFG_NAV_SETTINGS:
|
|
Debug("Got settings %u min_elev %d drLimit %u\n",
|
|
(unsigned)_buffer.nav_settings.dynModel,
|
|
(int)_buffer.nav_settings.minElev,
|
|
(unsigned)_buffer.nav_settings.drLimit);
|
|
_buffer.nav_settings.mask = 0;
|
|
if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
|
|
_buffer.nav_settings.dynModel != gps._navfilter) {
|
|
// we've received the current nav settings, change the engine
|
|
// settings and send them back
|
|
Debug("Changing engine setting from %u to %u\n",
|
|
(unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
|
|
_buffer.nav_settings.dynModel = gps._navfilter;
|
|
_buffer.nav_settings.mask |= 1;
|
|
}
|
|
if (gps._min_elevation != -100 &&
|
|
_buffer.nav_settings.minElev != gps._min_elevation) {
|
|
Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
|
|
_buffer.nav_settings.minElev = gps._min_elevation;
|
|
_buffer.nav_settings.mask |= 2;
|
|
}
|
|
if (_buffer.nav_settings.mask != 0) {
|
|
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
|
|
&_buffer.nav_settings,
|
|
sizeof(_buffer.nav_settings));
|
|
_unconfigured_messages |= CONFIG_NAV_SETTINGS;
|
|
_cfg_needs_save = true;
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
|
|
}
|
|
return false;
|
|
|
|
#if UBLOX_GNSS_SETTINGS
|
|
case MSG_CFG_GNSS:
|
|
if (params.gnss_mode != 0 && !supports_F9_config()) {
|
|
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
|
|
uint8_t gnssCount = 0;
|
|
Debug("Got GNSS Settings %u %u %u %u:\n",
|
|
(unsigned)_buffer.gnss.msgVer,
|
|
(unsigned)_buffer.gnss.numTrkChHw,
|
|
(unsigned)_buffer.gnss.numTrkChUse,
|
|
(unsigned)_buffer.gnss.numConfigBlocks);
|
|
#if UBLOX_DEBUGGING
|
|
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
|
|
Debug(" %u %u %u 0x%08x\n",
|
|
(unsigned)_buffer.gnss.configBlock[i].gnssId,
|
|
(unsigned)_buffer.gnss.configBlock[i].resTrkCh,
|
|
(unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
|
|
(unsigned)_buffer.gnss.configBlock[i].flags);
|
|
}
|
|
#endif
|
|
|
|
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
|
|
if((params.gnss_mode & (1 << i)) && i != GNSS_SBAS) {
|
|
gnssCount++;
|
|
}
|
|
}
|
|
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
|
|
// Reserve an equal portion of channels for all enabled systems that supports it
|
|
if(params.gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
|
|
if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId && (_hardware_generation > UBLOX_M8 || GNSS_GALILEO !=_buffer.gnss.configBlock[i].gnssId)) {
|
|
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
|
|
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
|
|
} else {
|
|
if(GNSS_SBAS ==_buffer.gnss.configBlock[i].gnssId) {
|
|
_buffer.gnss.configBlock[i].resTrkCh = 1;
|
|
_buffer.gnss.configBlock[i].maxTrkCh = 3;
|
|
}
|
|
if(GNSS_GALILEO ==_buffer.gnss.configBlock[i].gnssId) {
|
|
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
|
|
_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
|
|
}
|
|
}
|
|
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
|
|
} else {
|
|
_buffer.gnss.configBlock[i].resTrkCh = 0;
|
|
_buffer.gnss.configBlock[i].maxTrkCh = 0;
|
|
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
|
|
}
|
|
}
|
|
if (memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
|
|
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
|
|
_unconfigured_messages |= CONFIG_GNSS;
|
|
_cfg_needs_save = true;
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_GNSS;
|
|
}
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_GNSS;
|
|
}
|
|
return false;
|
|
#endif
|
|
|
|
case MSG_CFG_SBAS:
|
|
if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
|
|
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
|
|
(unsigned)_buffer.sbas.mode,
|
|
(unsigned)_buffer.sbas.usage,
|
|
(unsigned)_buffer.sbas.maxSBAS,
|
|
(unsigned)_buffer.sbas.scanmode2,
|
|
(unsigned)_buffer.sbas.scanmode1);
|
|
if (_buffer.sbas.mode != gps._sbas_mode) {
|
|
_buffer.sbas.mode = gps._sbas_mode;
|
|
_send_message(CLASS_CFG, MSG_CFG_SBAS,
|
|
&_buffer.sbas,
|
|
sizeof(_buffer.sbas));
|
|
_unconfigured_messages |= CONFIG_SBAS;
|
|
_cfg_needs_save = true;
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_SBAS;
|
|
}
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_SBAS;
|
|
}
|
|
return false;
|
|
case MSG_CFG_MSG:
|
|
if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
|
|
// can't verify the setting without knowing the port
|
|
// request the port again
|
|
if(_ublox_port >= UBLOX_MAX_PORTS) {
|
|
_request_port();
|
|
return false;
|
|
}
|
|
_verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
|
|
_buffer.msg_rate_6.rates[_ublox_port]);
|
|
} else {
|
|
_verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
|
|
_buffer.msg_rate.rate);
|
|
}
|
|
return false;
|
|
case MSG_CFG_PRT:
|
|
_ublox_port = _buffer.prt.portID;
|
|
return false;
|
|
case MSG_CFG_RATE:
|
|
if(_buffer.nav_rate.measure_rate_ms != params.rate_ms ||
|
|
_buffer.nav_rate.nav_rate != 1 ||
|
|
_buffer.nav_rate.timeref != 0) {
|
|
_configure_rate();
|
|
_unconfigured_messages |= CONFIG_RATE_NAV;
|
|
_cfg_needs_save = true;
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_RATE_NAV;
|
|
}
|
|
return false;
|
|
|
|
#if CONFIGURE_PPS_PIN
|
|
case MSG_CFG_TP5: {
|
|
// configure the PPS pin for 1Hz, zero delay
|
|
Debug("Got TP5 ver=%u 0x%04x %u\n",
|
|
(unsigned)_buffer.nav_tp5.version,
|
|
(unsigned)_buffer.nav_tp5.flags,
|
|
(unsigned)_buffer.nav_tp5.freqPeriod);
|
|
#ifdef HAL_GPIO_PPS
|
|
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);
|
|
#endif
|
|
const uint16_t desired_flags = 0x003f;
|
|
const uint16_t desired_period_hz = _pps_freq;
|
|
|
|
if (_buffer.nav_tp5.flags != desired_flags ||
|
|
_buffer.nav_tp5.freqPeriod != desired_period_hz) {
|
|
_buffer.nav_tp5.tpIdx = 0;
|
|
_buffer.nav_tp5.reserved1[0] = 0;
|
|
_buffer.nav_tp5.reserved1[1] = 0;
|
|
_buffer.nav_tp5.antCableDelay = 0;
|
|
_buffer.nav_tp5.rfGroupDelay = 0;
|
|
_buffer.nav_tp5.freqPeriod = desired_period_hz;
|
|
_buffer.nav_tp5.freqPeriodLock = desired_period_hz;
|
|
_buffer.nav_tp5.pulseLenRatio = 1;
|
|
_buffer.nav_tp5.pulseLenRatioLock = 2;
|
|
_buffer.nav_tp5.userConfigDelay = 0;
|
|
_buffer.nav_tp5.flags = desired_flags;
|
|
_send_message(CLASS_CFG, MSG_CFG_TP5,
|
|
&_buffer.nav_tp5,
|
|
sizeof(_buffer.nav_tp5));
|
|
_unconfigured_messages |= CONFIG_TP5;
|
|
_cfg_needs_save = true;
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_TP5;
|
|
}
|
|
return false;
|
|
}
|
|
#endif // CONFIGURE_PPS_PIN
|
|
case MSG_CFG_VALGET: {
|
|
uint8_t cfg_len = _payload_length - sizeof(ubx_cfg_valget);
|
|
const uint8_t *cfg_data = (const uint8_t *)(&_buffer) + sizeof(ubx_cfg_valget);
|
|
while (cfg_len >= 5) {
|
|
ConfigKey id;
|
|
memcpy(&id, cfg_data, sizeof(uint32_t));
|
|
cfg_len -= 4;
|
|
cfg_data += 4;
|
|
switch (id) {
|
|
case ConfigKey::TMODE_MODE: {
|
|
uint8_t mode = cfg_data[0];
|
|
if (mode != 0) {
|
|
// ask for mode 0, to disable TIME mode
|
|
mode = 0;
|
|
_configure_valset(ConfigKey::TMODE_MODE, &mode);
|
|
_cfg_needs_save = true;
|
|
_unconfigured_messages |= CONFIG_TMODE_MODE;
|
|
} else {
|
|
_unconfigured_messages &= ~CONFIG_TMODE_MODE;
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
// see if it is in active config list
|
|
int8_t cfg_idx = find_active_config_index(id);
|
|
if (cfg_idx >= 0) {
|
|
CFG_Debug("valset(0x%lx): %u", uint32_t(id), (*cfg_data) & 0x1);
|
|
const uint8_t key_size = config_key_size(id);
|
|
if (cfg_len < key_size
|
|
// for keys of length 1 only the LSB is significant
|
|
|| (key_size == 1 && (active_config.list[cfg_idx].value & 0x1) != (*cfg_data & 0x1))
|
|
|| memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
|
|
_configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers);
|
|
_unconfigured_messages |= active_config.unconfig_bit;
|
|
active_config.done_mask &= ~(1U << cfg_idx);
|
|
active_config.set_index = cfg_idx;
|
|
_cfg_needs_save = true;
|
|
} else {
|
|
active_config.done_mask |= (1U << cfg_idx);
|
|
CFG_Debug("done %u mask=0x%x all_mask=0x%x",
|
|
unsigned(cfg_idx),
|
|
unsigned(active_config.done_mask),
|
|
(1U<<active_config.count)-1);
|
|
if (active_config.done_mask == (1U<<active_config.count)-1) {
|
|
// all done!
|
|
_unconfigured_messages &= ~active_config.unconfig_bit;
|
|
}
|
|
}
|
|
if (active_config.fetch_index >= 0 &&
|
|
active_config.fetch_index < active_config.count &&
|
|
id == active_config.list[active_config.fetch_index].key) {
|
|
active_config.fetch_index++;
|
|
if (active_config.fetch_index < active_config.count) {
|
|
_configure_valget(active_config.list[active_config.fetch_index].key);
|
|
CFG_Debug("valget %d 0x%x", int(active_config.fetch_index),
|
|
unsigned(active_config.list[active_config.fetch_index].key));
|
|
}
|
|
}
|
|
} else {
|
|
CFG_Debug("valget no active config for 0x%lx", (uint32_t)id);
|
|
}
|
|
|
|
// step over the value
|
|
uint8_t step_size = config_key_size(id);
|
|
if (step_size == 0) {
|
|
return false;
|
|
}
|
|
cfg_len -= step_size;
|
|
cfg_data += step_size;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (_class == CLASS_MON) {
|
|
switch(_msg_id) {
|
|
case MSG_MON_HW:
|
|
if (_payload_length == 60 || _payload_length == 68) {
|
|
log_mon_hw();
|
|
}
|
|
break;
|
|
case MSG_MON_HW2:
|
|
if (_payload_length == 28) {
|
|
log_mon_hw2();
|
|
}
|
|
break;
|
|
case MSG_MON_VER: {
|
|
bool check_L1L5 = false;
|
|
_have_version = true;
|
|
strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
|
|
strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
|
|
void* mod = memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "MOD=", 4);
|
|
if (mod != nullptr) {
|
|
strncpy(_module, (char*)mod+4, UBLOX_MODULE_LEN-1);
|
|
}
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
|
"u-blox %s%s%d HW: %s SW: %s",
|
|
_module, mod != nullptr ? " " : "",
|
|
state.instance + 1,
|
|
_version.hwVersion,
|
|
_version.swVersion);
|
|
// check for F9 and M9. The F9 does not respond to SVINFO,
|
|
// so we need to use MON_VER for hardware generation
|
|
if (strncmp(_version.hwVersion, "00190000", 8) == 0) {
|
|
if (strncmp(_version.swVersion, "EXT CORE 1", 10) == 0) {
|
|
// a F9
|
|
if (_hardware_generation != UBLOX_F9) {
|
|
// need to ensure time mode is correctly setup on F9
|
|
_unconfigured_messages |= CONFIG_TMODE_MODE;
|
|
}
|
|
_hardware_generation = UBLOX_F9;
|
|
_unconfigured_messages |= CONFIG_F9;
|
|
_unconfigured_messages &= ~CONFIG_GNSS;
|
|
if (strncmp(_module, "ZED-F9P", UBLOX_MODULE_LEN) == 0) {
|
|
_hardware_variant = UBLOX_F9_ZED;
|
|
} else if (strncmp(_module, "NEO-F9P", UBLOX_MODULE_LEN) == 0) {
|
|
_hardware_variant = UBLOX_F9_NEO;
|
|
}
|
|
}
|
|
if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
|
|
// a M9
|
|
_hardware_generation = UBLOX_M9;
|
|
}
|
|
check_L1L5 = true;
|
|
}
|
|
// check for M10
|
|
if (strncmp(_version.hwVersion, "000A0000", 8) == 0) {
|
|
_hardware_generation = UBLOX_M10;
|
|
_unconfigured_messages |= CONFIG_M10;
|
|
// M10 does not support CONFIG_GNSS
|
|
_unconfigured_messages &= ~CONFIG_GNSS;
|
|
check_L1L5 = true;
|
|
}
|
|
if (check_L1L5) {
|
|
// check if L1L5 in extension
|
|
if (memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "L1L5", 4) != nullptr) {
|
|
supports_l5 = true;
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox supports L5 Band");
|
|
_unconfigured_messages |= CONFIG_L5;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
unexpected_message();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
#if UBLOX_RXM_RAW_LOGGING
|
|
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
|
|
log_rxm_raw(_buffer.rxm_raw);
|
|
return false;
|
|
} else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
|
|
log_rxm_rawx(_buffer.rxm_rawx);
|
|
return false;
|
|
}
|
|
#endif // UBLOX_RXM_RAW_LOGGING
|
|
|
|
#if UBLOX_TIM_TM2_LOGGING
|
|
if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
|
|
log_tim_tm2();
|
|
return false;
|
|
}
|
|
#endif // UBLOX_TIM_TM2_LOGGING
|
|
|
|
if (_class != CLASS_NAV) {
|
|
unexpected_message();
|
|
return false;
|
|
}
|
|
|
|
switch (_msg_id) {
|
|
case MSG_POSLLH:
|
|
Debug("MSG_POSLLH next_fix=%u", next_fix);
|
|
if (havePvtMsg) {
|
|
_unconfigured_messages |= CONFIG_RATE_POSLLH;
|
|
break;
|
|
}
|
|
_check_new_itow(_buffer.posllh.itow);
|
|
_last_pos_time = _buffer.posllh.itow;
|
|
state.location.lng = _buffer.posllh.longitude;
|
|
state.location.lat = _buffer.posllh.latitude;
|
|
state.have_undulation = true;
|
|
state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
|
|
set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10);
|
|
|
|
state.status = next_fix;
|
|
_new_position = true;
|
|
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
|
|
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
|
|
state.have_horizontal_accuracy = true;
|
|
state.have_vertical_accuracy = true;
|
|
#if UBLOX_FAKE_3DLOCK
|
|
state.location.lng = 1491652300L;
|
|
state.location.lat = -353632610L;
|
|
state.location.alt = 58400;
|
|
state.vertical_accuracy = 0;
|
|
state.horizontal_accuracy = 0;
|
|
#endif
|
|
break;
|
|
case MSG_STATUS:
|
|
Debug("MSG_STATUS fix_status=%u fix_type=%u",
|
|
_buffer.status.fix_status,
|
|
_buffer.status.fix_type);
|
|
_check_new_itow(_buffer.status.itow);
|
|
if (havePvtMsg) {
|
|
_unconfigured_messages |= CONFIG_RATE_STATUS;
|
|
break;
|
|
}
|
|
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
|
|
if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
|
|
(_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
|
|
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
|
|
}else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
|
|
next_fix = AP_GPS::GPS_OK_FIX_3D;
|
|
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
|
|
next_fix = AP_GPS::GPS_OK_FIX_2D;
|
|
}else{
|
|
next_fix = AP_GPS::NO_FIX;
|
|
state.status = AP_GPS::NO_FIX;
|
|
}
|
|
}else{
|
|
next_fix = AP_GPS::NO_FIX;
|
|
state.status = AP_GPS::NO_FIX;
|
|
}
|
|
#if UBLOX_FAKE_3DLOCK
|
|
state.status = AP_GPS::GPS_OK_FIX_3D;
|
|
next_fix = state.status;
|
|
#endif
|
|
break;
|
|
case MSG_DOP:
|
|
Debug("MSG_DOP");
|
|
noReceivedHdop = false;
|
|
_check_new_itow(_buffer.dop.itow);
|
|
state.hdop = _buffer.dop.hDOP;
|
|
state.vdop = _buffer.dop.vDOP;
|
|
#if UBLOX_FAKE_3DLOCK
|
|
state.hdop = 130;
|
|
state.hdop = 170;
|
|
#endif
|
|
break;
|
|
case MSG_SOL:
|
|
Debug("MSG_SOL fix_status=%u fix_type=%u",
|
|
_buffer.solution.fix_status,
|
|
_buffer.solution.fix_type);
|
|
_check_new_itow(_buffer.solution.itow);
|
|
if (havePvtMsg) {
|
|
state.time_week = _buffer.solution.week;
|
|
break;
|
|
}
|
|
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
|
|
if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
|
|
(_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
|
|
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
|
|
}else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
|
|
next_fix = AP_GPS::GPS_OK_FIX_3D;
|
|
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
|
|
next_fix = AP_GPS::GPS_OK_FIX_2D;
|
|
}else{
|
|
next_fix = AP_GPS::NO_FIX;
|
|
state.status = AP_GPS::NO_FIX;
|
|
}
|
|
}else{
|
|
next_fix = AP_GPS::NO_FIX;
|
|
state.status = AP_GPS::NO_FIX;
|
|
}
|
|
if(noReceivedHdop) {
|
|
state.hdop = _buffer.solution.position_DOP;
|
|
}
|
|
state.num_sats = _buffer.solution.satellites;
|
|
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
|
|
state.last_gps_time_ms = AP_HAL::millis();
|
|
state.time_week_ms = _buffer.solution.itow;
|
|
state.time_week = _buffer.solution.week;
|
|
}
|
|
#if UBLOX_FAKE_3DLOCK
|
|
next_fix = state.status;
|
|
state.num_sats = 10;
|
|
state.time_week = 1721;
|
|
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
|
|
state.last_gps_time_ms = AP_HAL::millis();
|
|
state.hdop = 130;
|
|
#endif
|
|
break;
|
|
|
|
#if GPS_MOVING_BASELINE
|
|
case MSG_RELPOSNED:
|
|
{
|
|
if (role != AP_GPS::GPS_ROLE_MB_ROVER) {
|
|
// ignore RELPOSNED if not configured as a rover
|
|
break;
|
|
}
|
|
// note that we require the yaw to come from a fixed solution, not a float solution
|
|
// yaw from a float solution would only be acceptable with a very large separation between
|
|
// GPS modules
|
|
const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) |
|
|
static_cast<uint32_t>(RELPOSNED::relPosValid) |
|
|
static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
|
|
static_cast<uint32_t>(RELPOSNED::isMoving) |
|
|
static_cast<uint32_t>(RELPOSNED::carrSolnFixed);
|
|
const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
|
|
static_cast<uint32_t>(RELPOSNED::refObsMiss) |
|
|
static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
|
|
|
|
_check_new_itow(_buffer.relposned.iTOW);
|
|
if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
|
|
// useful for looking at packet loss on links
|
|
MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
|
|
}
|
|
_last_relposned_itow = _buffer.relposned.iTOW;
|
|
MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask);
|
|
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
|
|
((_buffer.relposned.flags & invalid_mask) == 0)) {
|
|
if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
|
|
_buffer.relposned.relPosLength * 0.01,
|
|
_buffer.relposned.relPosD*0.01)) {
|
|
state.have_gps_yaw_accuracy = true;
|
|
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
|
|
_last_relposned_ms = AP_HAL::millis();
|
|
}
|
|
state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5;
|
|
state.relPosLength = _buffer.relposned.relPosLength * 0.01;
|
|
state.relPosD = _buffer.relposned.relPosD * 0.01;
|
|
state.accHeading = _buffer.relposned.accHeading * 1e-5;
|
|
state.relposheading_ts = AP_HAL::millis();
|
|
} else {
|
|
state.have_gps_yaw_accuracy = false;
|
|
}
|
|
}
|
|
break;
|
|
#endif // GPS_MOVING_BASELINE
|
|
|
|
case MSG_PVT:
|
|
Debug("MSG_PVT");
|
|
|
|
havePvtMsg = true;
|
|
// position
|
|
_check_new_itow(_buffer.pvt.itow);
|
|
_last_pvt_itow = _buffer.pvt.itow;
|
|
_last_pos_time = _buffer.pvt.itow;
|
|
state.location.lng = _buffer.pvt.lon;
|
|
state.location.lat = _buffer.pvt.lat;
|
|
state.have_undulation = true;
|
|
state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
|
|
set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10);
|
|
switch (_buffer.pvt.fix_type)
|
|
{
|
|
case 0:
|
|
state.status = AP_GPS::NO_FIX;
|
|
break;
|
|
case 1:
|
|
state.status = AP_GPS::NO_FIX;
|
|
break;
|
|
case 2:
|
|
state.status = AP_GPS::GPS_OK_FIX_2D;
|
|
break;
|
|
case 3:
|
|
state.status = AP_GPS::GPS_OK_FIX_3D;
|
|
if (_buffer.pvt.flags & 0b00000010) // diffsoln
|
|
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
|
|
if (_buffer.pvt.flags & 0b01000000) // carrsoln - float
|
|
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
|
|
if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed
|
|
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
|
|
break;
|
|
case 4:
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
|
"Unexpected state %d", _buffer.pvt.flags);
|
|
state.status = AP_GPS::GPS_OK_FIX_3D;
|
|
break;
|
|
case 5:
|
|
state.status = AP_GPS::NO_FIX;
|
|
break;
|
|
default:
|
|
state.status = AP_GPS::NO_FIX;
|
|
break;
|
|
}
|
|
next_fix = state.status;
|
|
_new_position = true;
|
|
state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f;
|
|
state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f;
|
|
state.have_horizontal_accuracy = true;
|
|
state.have_vertical_accuracy = true;
|
|
// SVs
|
|
state.num_sats = _buffer.pvt.num_sv;
|
|
// velocity
|
|
_last_vel_time = _buffer.pvt.itow;
|
|
state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s
|
|
state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000
|
|
state.have_vertical_velocity = true;
|
|
state.velocity.x = _buffer.pvt.velN * 0.001f;
|
|
state.velocity.y = _buffer.pvt.velE * 0.001f;
|
|
state.velocity.z = _buffer.pvt.velD * 0.001f;
|
|
state.have_speed_accuracy = true;
|
|
state.speed_accuracy = _buffer.pvt.s_acc*0.001f;
|
|
_new_speed = true;
|
|
// dop
|
|
if(noReceivedHdop) {
|
|
state.hdop = _buffer.pvt.p_dop;
|
|
state.vdop = _buffer.pvt.p_dop;
|
|
}
|
|
|
|
if (_buffer.pvt.fix_type >= 2) {
|
|
state.last_gps_time_ms = AP_HAL::millis();
|
|
}
|
|
|
|
// time
|
|
state.time_week_ms = _buffer.pvt.itow;
|
|
#if UBLOX_FAKE_3DLOCK
|
|
state.location.lng = 1491652300L;
|
|
state.location.lat = -353632610L;
|
|
state.location.alt = 58400;
|
|
state.vertical_accuracy = 0;
|
|
state.horizontal_accuracy = 0;
|
|
state.status = AP_GPS::GPS_OK_FIX_3D;
|
|
state.num_sats = 10;
|
|
state.time_week = 1721;
|
|
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
|
|
state.last_gps_time_ms = AP_HAL::millis();
|
|
state.hdop = 130;
|
|
state.speed_accuracy = 0;
|
|
next_fix = state.status;
|
|
#endif
|
|
break;
|
|
case MSG_TIMEGPS:
|
|
Debug("MSG_TIMEGPS");
|
|
_check_new_itow(_buffer.timegps.itow);
|
|
if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) {
|
|
state.time_week = _buffer.timegps.week;
|
|
}
|
|
break;
|
|
case MSG_VELNED:
|
|
Debug("MSG_VELNED");
|
|
if (havePvtMsg) {
|
|
_unconfigured_messages |= CONFIG_RATE_VELNED;
|
|
break;
|
|
}
|
|
_check_new_itow(_buffer.velned.itow);
|
|
_last_vel_time = _buffer.velned.itow;
|
|
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
|
|
state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000
|
|
state.have_vertical_velocity = true;
|
|
state.velocity.x = _buffer.velned.ned_north * 0.01f;
|
|
state.velocity.y = _buffer.velned.ned_east * 0.01f;
|
|
state.velocity.z = _buffer.velned.ned_down * 0.01f;
|
|
velocity_to_speed_course(state);
|
|
state.have_speed_accuracy = true;
|
|
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
|
|
#if UBLOX_FAKE_3DLOCK
|
|
state.speed_accuracy = 0;
|
|
#endif
|
|
_new_speed = true;
|
|
break;
|
|
case MSG_NAV_SVINFO:
|
|
{
|
|
Debug("MSG_NAV_SVINFO\n");
|
|
static const uint8_t HardwareGenerationMask = 0x07;
|
|
_check_new_itow(_buffer.svinfo_header.itow);
|
|
_hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
|
|
switch (_hardware_generation) {
|
|
case UBLOX_5:
|
|
case UBLOX_6:
|
|
// only 7 and newer support CONFIG_GNSS
|
|
_unconfigured_messages &= ~CONFIG_GNSS;
|
|
break;
|
|
case UBLOX_7:
|
|
case UBLOX_M8:
|
|
#if UBLOX_SPEED_CHANGE
|
|
port->begin(4000000U);
|
|
Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
|
|
#endif
|
|
break;
|
|
default:
|
|
hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
|
|
break;
|
|
};
|
|
_unconfigured_messages &= ~CONFIG_VERSION;
|
|
/* We don't need that anymore */
|
|
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
|
|
break;
|
|
}
|
|
default:
|
|
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
|
|
if (++_disable_counter == 0) {
|
|
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
|
|
_configure_message_rate(CLASS_NAV, _msg_id, 0);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
if (state.have_gps_yaw) {
|
|
// when we are a rover we want to ensure we have both the new
|
|
// PVT and the new RELPOSNED message so that we give a
|
|
// consistent view
|
|
if (AP_HAL::millis() - _last_relposned_ms > 400) {
|
|
// we have stopped receiving valid RELPOSNED messages, disable yaw reporting
|
|
state.have_gps_yaw = false;
|
|
} else if (_last_relposned_itow != _last_pvt_itow) {
|
|
// wait until ITOW matches
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// we only return true when we get new position and speed data
|
|
// this ensures we don't use stale data
|
|
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
|
|
_new_speed = _new_position = false;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
* handle pps interrupt
|
|
*/
|
|
#ifdef HAL_GPIO_PPS
|
|
void
|
|
AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
|
|
{
|
|
_last_pps_time_us = AP_HAL::micros64();
|
|
}
|
|
|
|
void
|
|
AP_GPS_UBLOX::set_pps_desired_freq(uint8_t freq)
|
|
{
|
|
_pps_freq = freq;
|
|
_unconfigured_messages |= CONFIG_TP5;
|
|
}
|
|
#endif
|
|
|
|
|
|
// UBlox auto configuration
|
|
|
|
/*
|
|
* update checksum for a set of bytes
|
|
*/
|
|
void
|
|
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b)
|
|
{
|
|
while (len--) {
|
|
ck_a += *data;
|
|
ck_b += ck_a;
|
|
data++;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* send a ublox message
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, const void *msg, uint16_t size)
|
|
{
|
|
if (port->txspace() < (sizeof(struct ubx_header) + 2 + size)) {
|
|
return false;
|
|
}
|
|
struct ubx_header header;
|
|
uint8_t ck_a=0, ck_b=0;
|
|
header.preamble1 = PREAMBLE1;
|
|
header.preamble2 = PREAMBLE2;
|
|
header.msg_class = msg_class;
|
|
header.msg_id = msg_id;
|
|
header.length = size;
|
|
|
|
_update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
|
|
_update_checksum((uint8_t *)msg, size, ck_a, ck_b);
|
|
|
|
port->write((const uint8_t *)&header, sizeof(header));
|
|
port->write((const uint8_t *)msg, size);
|
|
port->write((const uint8_t *)&ck_a, 1);
|
|
port->write((const uint8_t *)&ck_b, 1);
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
* requests the given message rate for a specific message class
|
|
* and msg_id
|
|
* returns true if it sent the request, false if waiting on knowing the port
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id)
|
|
{
|
|
// Without knowing what communication port is being used it isn't possible to verify
|
|
// always ensure we have a port before sending the request
|
|
if(_ublox_port >= UBLOX_MAX_PORTS) {
|
|
_request_port();
|
|
return false;
|
|
} else {
|
|
struct ubx_cfg_msg msg;
|
|
msg.msg_class = msg_class;
|
|
msg.msg_id = msg_id;
|
|
return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
|
|
}
|
|
}
|
|
|
|
/*
|
|
* configure a UBlox GPS for the given message rate for a specific
|
|
* message class and msg_id
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
|
|
{
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
|
|
return false;
|
|
}
|
|
|
|
struct ubx_cfg_msg_rate msg;
|
|
msg.msg_class = msg_class;
|
|
msg.msg_id = msg_id;
|
|
msg.rate = rate;
|
|
return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
|
|
}
|
|
|
|
/*
|
|
* configure F9/M10 based key/value pair - VALSET
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value, uint8_t layers)
|
|
{
|
|
if (!supports_F9_config()) {
|
|
return false;
|
|
}
|
|
|
|
const uint8_t len = config_key_size(key);
|
|
struct ubx_cfg_valset msg {};
|
|
uint8_t buf[sizeof(msg)+len];
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
|
|
return false;
|
|
}
|
|
msg.version = 1;
|
|
msg.layers = layers;
|
|
msg.transaction = 0;
|
|
msg.key = uint32_t(key);
|
|
memcpy(buf, &msg, sizeof(msg));
|
|
memcpy(&buf[sizeof(msg)], value, len);
|
|
auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, sizeof(buf));
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
* configure F9 based key/value pair - VALGET
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_configure_valget(ConfigKey key)
|
|
{
|
|
if (!supports_F9_config()) {
|
|
return false;
|
|
}
|
|
struct {
|
|
struct ubx_cfg_valget msg;
|
|
ConfigKey key;
|
|
} msg {};
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(msg)+2)) {
|
|
return false;
|
|
}
|
|
msg.msg.version = 0;
|
|
msg.msg.layers = 0; // ram
|
|
msg.key = key;
|
|
return _send_message(CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg));
|
|
}
|
|
|
|
/*
|
|
* configure F9 based key/value pair for a complete configuration set
|
|
*
|
|
* this method requests each configuration variable from the GPS.
|
|
* When we handle the reply in _parse_gps we may then choose to set a
|
|
* MSG_CFG_VALSET back to the GPS if we don't like its response.
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers)
|
|
{
|
|
active_config.list = list;
|
|
active_config.count = count;
|
|
active_config.done_mask = 0;
|
|
active_config.unconfig_bit = unconfig_bit;
|
|
active_config.layers = layers;
|
|
// we start by fetching multiple values at once (with fetch_index
|
|
// -1) then if we get a NACK for VALGET we switch to fetching one
|
|
// value at a time. This copes with the M10S which can only fetch
|
|
// one value at a time
|
|
active_config.fetch_index = -1;
|
|
|
|
uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
|
|
struct ubx_cfg_valget msg {};
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
|
|
return false;
|
|
}
|
|
msg.version = 0;
|
|
msg.layers = 0; // ram
|
|
memcpy(buf, &msg, sizeof(msg));
|
|
for (uint8_t i=0; i<count; i++) {
|
|
memcpy(&buf[sizeof(msg)+i*sizeof(ConfigKey)], &list[i].key, sizeof(ConfigKey));
|
|
}
|
|
return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
|
|
}
|
|
|
|
/*
|
|
* configure F9 based key/value pair for a complete configuration set
|
|
*
|
|
* this method sends all the key/value pairs in a block, but makes no attempt to check
|
|
* the results. Sending in a block is necessary for updates such as GNSS where certain
|
|
* combinations are invalid and setting one at a time will not produce the correct result
|
|
* if the result needs to be validated then a subsequent _configure_config_set() can be
|
|
* issued which will get all the values and reset those that are not properly updated.
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_configure_list_valset(const config_list *list, uint8_t count, uint8_t layers)
|
|
{
|
|
if (!supports_F9_config()) {
|
|
return false;
|
|
}
|
|
|
|
struct ubx_cfg_valset msg {};
|
|
uint8_t buf[sizeof(msg)+sizeof(config_list)*count];
|
|
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
|
|
return false;
|
|
}
|
|
msg.version = 1;
|
|
msg.layers = layers;
|
|
msg.transaction = 0;
|
|
uint32_t msg_len = sizeof(msg) - sizeof(msg.key);
|
|
memcpy(buf, &msg, msg_len);
|
|
|
|
uint8_t* payload = &buf[msg_len];
|
|
for (uint8_t i=0; i<count; i++) {
|
|
const uint8_t len = config_key_size(list[i].key);
|
|
memcpy(payload, &list[i].key, sizeof(ConfigKey));
|
|
payload += sizeof(ConfigKey);
|
|
msg_len += sizeof(ConfigKey);
|
|
memcpy(payload, &list[i].value, len);
|
|
payload += len;
|
|
msg_len += len;
|
|
}
|
|
|
|
auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, msg_len);
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
* save gps configurations to non-volatile memory sent until the call of
|
|
* this message
|
|
*/
|
|
void
|
|
AP_GPS_UBLOX::_save_cfg()
|
|
{
|
|
static const ubx_cfg_cfg save_cfg {
|
|
clearMask: 0,
|
|
saveMask: SAVE_CFG_ALL,
|
|
loadMask: 0
|
|
};
|
|
_send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
|
|
_last_cfg_sent_time = AP_HAL::millis();
|
|
_num_cfg_save_tries++;
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
|
"GPS %d: u-blox saving config",
|
|
state.instance + 1);
|
|
}
|
|
|
|
/*
|
|
detect a Ublox GPS. Adds one byte, and returns true if the stream
|
|
matches a UBlox
|
|
*/
|
|
bool
|
|
AP_GPS_UBLOX::_detect(struct UBLOX_detect_state &state, uint8_t data)
|
|
{
|
|
reset:
|
|
switch (state.step) {
|
|
case 1:
|
|
if (PREAMBLE2 == data) {
|
|
state.step++;
|
|
break;
|
|
}
|
|
state.step = 0;
|
|
FALLTHROUGH;
|
|
case 0:
|
|
if (PREAMBLE1 == data)
|
|
state.step++;
|
|
break;
|
|
case 2:
|
|
state.step++;
|
|
state.ck_b = state.ck_a = data;
|
|
break;
|
|
case 3:
|
|
state.step++;
|
|
state.ck_b += (state.ck_a += data);
|
|
break;
|
|
case 4:
|
|
state.step++;
|
|
state.ck_b += (state.ck_a += data);
|
|
state.payload_length = data;
|
|
break;
|
|
case 5:
|
|
state.step++;
|
|
state.ck_b += (state.ck_a += data);
|
|
state.payload_counter = 0;
|
|
break;
|
|
case 6:
|
|
state.ck_b += (state.ck_a += data);
|
|
if (++state.payload_counter == state.payload_length)
|
|
state.step++;
|
|
break;
|
|
case 7:
|
|
state.step++;
|
|
if (state.ck_a != data) {
|
|
state.step = 0;
|
|
goto reset;
|
|
}
|
|
break;
|
|
case 8:
|
|
state.step = 0;
|
|
if (state.ck_b == data) {
|
|
// a valid UBlox packet
|
|
return true;
|
|
} else {
|
|
goto reset;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void
|
|
AP_GPS_UBLOX::_request_version(void)
|
|
{
|
|
_send_message(CLASS_MON, MSG_MON_VER, nullptr, 0);
|
|
}
|
|
|
|
void
|
|
AP_GPS_UBLOX::_configure_rate(void)
|
|
{
|
|
struct ubx_cfg_nav_rate msg;
|
|
// require a minimum measurement rate of 5Hz
|
|
msg.measure_rate_ms = gps.get_rate_ms(state.instance);
|
|
msg.nav_rate = 1;
|
|
msg.timeref = 0; // UTC time
|
|
_send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
|
|
}
|
|
|
|
static const char *reasons[] = {"navigation rate",
|
|
"posllh rate",
|
|
"status rate",
|
|
"solution rate",
|
|
"velned rate",
|
|
"dop rate",
|
|
"hw monitor rate",
|
|
"hw2 monitor rate",
|
|
"raw rate",
|
|
"version",
|
|
"navigation settings",
|
|
"GNSS settings",
|
|
"SBAS settings",
|
|
"PVT rate",
|
|
"time pulse settings",
|
|
"TIMEGPS rate",
|
|
"Time mode settings",
|
|
"RTK MB",
|
|
"TIM TM2",
|
|
"F9",
|
|
"M10",
|
|
"L5 Enable Disable"};
|
|
|
|
static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
|
|
|
|
void
|
|
AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
|
|
for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) {
|
|
if (_unconfigured_messages & (1 << i)) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x",
|
|
(unsigned int)(state.instance + 1), reasons[i], (unsigned int)_unconfigured_messages);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
return velocity lag in seconds
|
|
*/
|
|
bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
|
|
{
|
|
switch (_hardware_generation) {
|
|
case UBLOX_UNKNOWN_HARDWARE_GENERATION:
|
|
lag_sec = 0.22f;
|
|
// always bail out in this case, it's used to indicate we have yet to receive a valid
|
|
// hardware generation, however the user may have inhibited us detecting the generation
|
|
// so if we aren't allowed to do configuration, we will accept this as the default delay
|
|
return gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE;
|
|
case UBLOX_5:
|
|
case UBLOX_6:
|
|
default:
|
|
lag_sec = 0.22f;
|
|
break;
|
|
case UBLOX_7:
|
|
case UBLOX_M8:
|
|
// based on flight logs the 7 and 8 series seem to produce about 120ms lag
|
|
lag_sec = 0.12f;
|
|
break;
|
|
case UBLOX_F9:
|
|
case UBLOX_M9:
|
|
case UBLOX_M10:
|
|
// F9 lag not verified yet from flight log, but likely to be at least
|
|
// as good as M8
|
|
lag_sec = 0.12f;
|
|
#if GPS_MOVING_BASELINE
|
|
if (role == AP_GPS::GPS_ROLE_MB_BASE ||
|
|
role == AP_GPS::GPS_ROLE_MB_ROVER) {
|
|
// the moving baseline rover will lag about 40ms from the
|
|
// base. We need to provide the more conservative value to
|
|
// ensure that the EKF allocates a larger enough buffer
|
|
lag_sec += 0.04;
|
|
}
|
|
#endif
|
|
break;
|
|
};
|
|
return true;
|
|
}
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const
|
|
{
|
|
AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages();
|
|
|
|
if (_have_version) {
|
|
AP::logger().Write_MessageF("u-blox %d HW: %s SW: %s",
|
|
state.instance+1,
|
|
_version.hwVersion,
|
|
_version.swVersion);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
// uBlox specific check_new_itow(), handling message length
|
|
void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
|
|
{
|
|
check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
|
|
}
|
|
|
|
// support for retrieving RTCMv3 data from a moving baseline base
|
|
bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
|
|
{
|
|
#if GPS_MOVING_BASELINE
|
|
if (rtcm3_parser) {
|
|
len = rtcm3_parser->get_len(bytes);
|
|
return len > 0;
|
|
}
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
// clear previous RTCM3 packet
|
|
void AP_GPS_UBLOX::clear_RTCMV3(void)
|
|
{
|
|
#if GPS_MOVING_BASELINE
|
|
if (rtcm3_parser) {
|
|
rtcm3_parser->clear_packet();
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// ublox specific healthy checks
|
|
bool AP_GPS_UBLOX::is_healthy(void) const
|
|
{
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
|
|
// allow for fake ublox moving baseline
|
|
return true;
|
|
}
|
|
#endif
|
|
#if GPS_MOVING_BASELINE
|
|
if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
|
|
role == AP_GPS::GPS_ROLE_MB_ROVER) &&
|
|
!supports_F9_config()) {
|
|
// need F9 or above for moving baseline
|
|
return false;
|
|
}
|
|
if (role == AP_GPS::GPS_ROLE_MB_BASE && rtcm3_parser == nullptr && !mb_use_uart2()) {
|
|
// we haven't initialised RTCMv3 parser
|
|
return false;
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
// populate config_GNSS with F9 GNSS configuration
|
|
uint8_t AP_GPS_UBLOX::populate_F9_gnss(void)
|
|
{
|
|
uint8_t cfg_count = 0;
|
|
|
|
if (params.gnss_mode == 0) {
|
|
_unconfigured_messages &= ~CONFIG_F9;
|
|
last_configured_gnss = params.gnss_mode;
|
|
return 0;
|
|
}
|
|
|
|
if ((_unconfigured_messages & CONFIG_F9) != 0) {
|
|
// ZED-F9P defaults are
|
|
// GPS L1C/A+L2C(ZED)
|
|
// SBAS L1C/A
|
|
// GALILEO E1+E5B(ZED)+E5A(NEO)
|
|
// BEIDOU B1+B2(ZED)+B2A(NEO)
|
|
// QZSS L1C/A+L2C(ZED)+L5(NEO)
|
|
// GLONASS L1+L2(ZED)
|
|
// IMES not supported
|
|
// GPS and QZSS should be enabled/disabled together, but we will leave them alone
|
|
// QZSS and SBAS can only be enabled if GPS is enabled
|
|
|
|
if (config_GNSS == nullptr) {
|
|
config_GNSS = (config_list*)calloc(UBLOX_MAX_GNSS_CONFIG_BLOCKS*3, sizeof(config_list));
|
|
}
|
|
|
|
if (config_GNSS == nullptr) {
|
|
return 0;
|
|
}
|
|
|
|
uint8_t gnss_mode = params.gnss_mode;
|
|
gnss_mode |= 1U<<GNSS_GPS;
|
|
gnss_mode |= 1U<<GNSS_QZSS;
|
|
gnss_mode &= ~(1U<<GNSS_IMES);
|
|
params.gnss_mode.set_and_save(gnss_mode);
|
|
|
|
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
|
|
bool ena = gnss_mode & (1U<<i);
|
|
switch (i) {
|
|
case GNSS_SBAS:
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_ENA, ena };
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_L1CA_ENA, ena };
|
|
break;
|
|
case GNSS_GALILEO:
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_ENA, ena };
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E1_ENA, ena };
|
|
if (_hardware_variant == UBLOX_F9_ZED) {
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5B_ENA, ena };
|
|
} else {
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5A_ENA, ena };
|
|
}
|
|
break;
|
|
case GNSS_BEIDOU:
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_ENA, ena };
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B1_ENA, ena };
|
|
if (_hardware_variant == UBLOX_F9_ZED) {
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2_ENA, ena };
|
|
} else {
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2A_ENA, ena };
|
|
}
|
|
break;
|
|
case GNSS_GLONASS:
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_ENA, ena };
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L1_ENA, ena };
|
|
if (_hardware_variant == UBLOX_F9_ZED) {
|
|
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L2_ENA, ena };
|
|
}
|
|
break;
|
|
// not supported or leave alone
|
|
case GNSS_IMES:
|
|
case GNSS_QZSS:
|
|
case GNSS_GPS:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
last_configured_gnss = params.gnss_mode;
|
|
|
|
return cfg_count;
|
|
}
|
|
|
|
// return true if GPS is capable of F9 config
|
|
bool AP_GPS_UBLOX::supports_F9_config(void) const
|
|
{
|
|
return _hardware_generation == UBLOX_F9 || _hardware_generation == UBLOX_M10;
|
|
}
|
|
|
|
// return true if GPS is capable of F9 config
|
|
bool AP_GPS_UBLOX::is_gnss_key(ConfigKey key) const
|
|
{
|
|
return (unsigned(key) & 0xFFFF0000) == 0x10310000;
|
|
}
|
|
|
|
#endif
|