AP_GPS: improve support for uBlox-M10

this sets up the M10 to use the BaiDou B1C signal instead of B1, and
disables glonass. This is needed to get a consistent 5Hz lock
This commit is contained in:
Andrew Tridgell 2022-11-21 14:33:46 +11:00 committed by Randy Mackay
parent 5780fac5fe
commit c4425d51e1
3 changed files with 112 additions and 26 deletions

View File

@ -2054,6 +2054,13 @@ bool AP_GPS::is_healthy(uint8_t instance) const
return false; return false;
} }
#ifdef HAL_BUILD_AP_PERIPH
/*
on AP_Periph handling of timing is done by the flight controller
receiving the DroneCAN messages
*/
return drivers[instance] != nullptr && drivers[instance]->is_healthy();
#else
/* /*
allow two lost frames before declaring the GPS unhealthy, but allow two lost frames before declaring the GPS unhealthy, but
require the average frame rate to be close to 5Hz. We allow for require the average frame rate to be close to 5Hz. We allow for
@ -2075,6 +2082,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const
return delay_ok && drivers[instance] != nullptr && return delay_ok && drivers[instance] != nullptr &&
drivers[instance]->is_healthy(); drivers[instance]->is_healthy();
#endif // HAL_BUILD_AP_PERIPH
} }
bool AP_GPS::prepare_for_arming(void) { bool AP_GPS::prepare_for_arming(void) {

View File

@ -224,6 +224,22 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
}; };
#endif // GPS_MOVING_BASELINE #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
};
void void
AP_GPS_UBLOX::_request_next_config(void) AP_GPS_UBLOX::_request_next_config(void)
@ -406,6 +422,19 @@ AP_GPS_UBLOX::_request_next_config(void)
_unconfigured_messages &= ~CONFIG_TIM_TM2; _unconfigured_messages &= ~CONFIG_TIM_TM2;
#endif #endif
break; break;
case STEP_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;
}
default: default:
// this case should never be reached, do a full reset if it is hit // this case should never be reached, do a full reset if it is hit
_next_message = STEP_PVT; _next_message = STEP_PVT;
@ -1175,26 +1204,27 @@ AP_GPS_UBLOX::_parse_gps(void)
default: default:
break; break;
} }
#if GPS_MOVING_BASELINE
// see if it is in active config list // see if it is in active config list
int8_t cfg_idx = find_active_config_index(id); int8_t cfg_idx = find_active_config_index(id);
if (cfg_idx >= 0) { if (cfg_idx >= 0) {
const uint8_t key_size = config_key_size(id); const uint8_t key_size = config_key_size(id);
if (cfg_len < key_size || if (cfg_len < key_size ||
memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) { memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
_configure_valset(id, &active_config.list[cfg_idx].value); _configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers);
_unconfigured_messages |= active_config.unconfig_bit; _unconfigured_messages |= active_config.unconfig_bit;
active_config.done_mask &= ~(1U << cfg_idx); active_config.done_mask &= ~(1U << cfg_idx);
_cfg_needs_save = true; _cfg_needs_save = true;
} else { } else {
active_config.done_mask |= (1U << cfg_idx); active_config.done_mask |= (1U << cfg_idx);
Debug("done %u mask=0x%x",
unsigned(cfg_idx),
unsigned(active_config.done_mask));
if (active_config.done_mask == (1U<<active_config.count)-1) { if (active_config.done_mask == (1U<<active_config.count)-1) {
// all done! // all done!
_unconfigured_messages &= ~active_config.unconfig_bit; _unconfigured_messages &= ~active_config.unconfig_bit;
} }
} }
} }
#endif // GPS_MOVING_BASELINE
// step over the value // step over the value
uint8_t step_size = config_key_size(id); uint8_t step_size = config_key_size(id);
@ -1245,8 +1275,13 @@ AP_GPS_UBLOX::_parse_gps(void)
_hardware_generation = UBLOX_M9; _hardware_generation = UBLOX_M9;
} }
} }
// none of the 9 series support the SOL message // check for M10
_unconfigured_messages &= ~CONFIG_RATE_SOL; if (strncmp(_version.hwVersion, "000A0000", 8) == 0) {
if (strncmp(_version.swVersion, "ROM SPG 5", 9) == 0) {
_hardware_generation = UBLOX_M10;
_unconfigured_messages |= CONFIG_M10;
}
}
break; break;
default: default:
unexpected_message(); unexpected_message();
@ -1722,10 +1757,10 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t
} }
/* /*
* configure F9 based key/value pair - VALSET * configure F9/M10 based key/value pair - VALSET
*/ */
bool bool
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value) AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value, uint8_t layers)
{ {
if (!supports_F9_config()) { if (!supports_F9_config()) {
return false; return false;
@ -1737,7 +1772,7 @@ AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value)
return false; return false;
} }
msg.version = 1; msg.version = 1;
msg.layers = 7; // all layers msg.layers = layers;
msg.transaction = 0; msg.transaction = 0;
msg.key = uint32_t(key); msg.key = uint32_t(key);
memcpy(buf, &msg, sizeof(msg)); memcpy(buf, &msg, sizeof(msg));
@ -1772,13 +1807,13 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key)
* configure F9 based key/value pair for a complete config list * configure F9 based key/value pair for a complete config list
*/ */
bool bool
AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit) AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers)
{ {
#if GPS_MOVING_BASELINE
active_config.list = list; active_config.list = list;
active_config.count = count; active_config.count = count;
active_config.done_mask = 0; active_config.done_mask = 0;
active_config.unconfig_bit = unconfig_bit; active_config.unconfig_bit = unconfig_bit;
active_config.layers = layers;
uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)]; uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
struct ubx_cfg_valget msg {}; struct ubx_cfg_valget msg {};
@ -1792,9 +1827,6 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint
memcpy(&buf[sizeof(msg)+i*sizeof(ConfigKey)], &list[i].key, sizeof(ConfigKey)); memcpy(&buf[sizeof(msg)+i*sizeof(ConfigKey)], &list[i].key, sizeof(ConfigKey));
} }
return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf)); return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
#else
return false;
#endif
} }
/* /*
@ -1914,7 +1946,8 @@ static const char *reasons[] = {"navigation rate",
"TIMEGPS rate", "TIMEGPS rate",
"Time mode settings", "Time mode settings",
"RTK MB", "RTK MB",
"TIM TM2"}; "TIM TM2",
"M10"};
static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description"); static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
@ -1953,6 +1986,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
break; break;
case UBLOX_F9: case UBLOX_F9:
case UBLOX_M9: case UBLOX_M9:
case UBLOX_M10:
// F9 lag not verified yet from flight log, but likely to be at least // F9 lag not verified yet from flight log, but likely to be at least
// as good as M8 // as good as M8
lag_sec = 0.12f; lag_sec = 0.12f;
@ -2039,7 +2073,7 @@ bool AP_GPS_UBLOX::is_healthy(void) const
// return true if GPS is capable of F9 config // return true if GPS is capable of F9 config
bool AP_GPS_UBLOX::supports_F9_config(void) const bool AP_GPS_UBLOX::supports_F9_config(void) const
{ {
return _hardware_generation == UBLOX_F9 && _hardware_generation != UBLOX_UNKNOWN_HARDWARE_GENERATION; return _hardware_generation == UBLOX_F9 || _hardware_generation == UBLOX_M10;
} }
#endif #endif

View File

@ -97,7 +97,8 @@
#define CONFIG_TMODE_MODE (1<<16) #define CONFIG_TMODE_MODE (1<<16)
#define CONFIG_RTK_MOVBASE (1<<17) #define CONFIG_RTK_MOVBASE (1<<17)
#define CONFIG_TIM_TM2 (1<<18) #define CONFIG_TIM_TM2 (1<<18)
#define CONFIG_LAST (1<<19) // this must always be the last bit #define CONFIG_M10 (1<<19)
#define CONFIG_LAST (1<<20) // this must always be the last bit
#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
@ -281,7 +282,45 @@ private:
MSGOUT_RTCM_3X_TYPE1127_UART2 = 0x209102d8, MSGOUT_RTCM_3X_TYPE1127_UART2 = 0x209102d8,
MSGOUT_RTCM_3X_TYPE1230_UART2 = 0x20910305, MSGOUT_RTCM_3X_TYPE1230_UART2 = 0x20910305,
MSGOUT_UBX_NAV_RELPOSNED_UART2 = 0x2091008f, MSGOUT_UBX_NAV_RELPOSNED_UART2 = 0x2091008f,
// enable specific signals and constellations
CFG_SIGNAL_GPS_ENA = 0x1031001f,
CFG_SIGNAL_GPS_L1CA_ENA = 0x10310001,
CFG_SIGNAL_GPS_L2C_ENA = 0x10310003,
CFG_SIGNAL_GPS_L5_ENA = 0x10310004,
CFG_SIGNAL_SBAS_ENA = 0x10310020,
CFG_SIGNAL_SBAS_L1CA_ENA = 0x10310005,
CFG_SIGNAL_GAL_ENA = 0x10310021,
CFG_SIGNAL_GAL_E1_ENA = 0x10310007,
CFG_SIGNAL_GAL_E5A_ENA = 0x10310009,
CFG_SIGNAL_GAL_E5B_ENA = 0x1031000a,
CFG_SIGNAL_BDS_ENA = 0x10310022,
CFG_SIGNAL_BDS_B1_ENA = 0x1031000d,
CFG_SIGNAL_BDS_B1C_ENA = 0x1031000f,
CFG_SIGNAL_BDS_B2_ENA = 0x1031000e,
CFG_SIGNAL_BDS_B2A_ENA = 0x10310028,
CFG_SIGNAL_QZSS_ENA = 0x10310024,
CFG_SIGNAL_QZSS_L1CA_ENA = 0x10310012,
CFG_SIGNAL_QZSS_L1S_ENA = 0x10310014,
CFG_SIGNAL_QZSS_L2C_ENA = 0x10310015,
CFG_SIGNAL_QZSS_L5_ENA = 0x10310017,
CFG_SIGNAL_GLO_ENA = 0x10310025,
CFG_SIGNAL_GLO_L1_ENA = 0x10310018,
CFG_SIGNAL_GLO_L2_ENA = 0x1031001a,
CFG_SIGNAL_NAVIC_ENA = 0x10310026,
CFG_SIGNAL_NAVIC_L5_ENA = 0x1031001d,
// other keys
CFG_NAVSPG_DYNMODEL = 0x20110021,
}; };
// layers for VALSET
#define UBX_VALSET_LAYER_RAM 0x1U
#define UBX_VALSET_LAYER_BBR 0x2U
#define UBX_VALSET_LAYER_FLASH 0x4U
#define UBX_VALSET_LAYER_ALL 0x7U
struct PACKED ubx_cfg_valset { struct PACKED ubx_cfg_valset {
uint8_t version; uint8_t version;
uint8_t layers; uint8_t layers;
@ -658,6 +697,7 @@ private:
UBLOX_M8, UBLOX_M8,
UBLOX_F9 = 0x80, // comes from MON_VER hwVersion/swVersion strings UBLOX_F9 = 0x80, // comes from MON_VER hwVersion/swVersion strings
UBLOX_M9 = 0x81, // comes from MON_VER hwVersion/swVersion strings UBLOX_M9 = 0x81, // comes from MON_VER hwVersion/swVersion strings
UBLOX_M10 = 0x82,
UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for
// flagging state in the driver // flagging state in the driver
}; };
@ -685,6 +725,7 @@ private:
STEP_VERSION, STEP_VERSION,
STEP_RTK_MOVBASE, // setup moving baseline STEP_RTK_MOVBASE, // setup moving baseline
STEP_TIM_TM2, STEP_TIM_TM2,
STEP_M10,
STEP_LAST STEP_LAST
}; };
@ -740,7 +781,7 @@ private:
bool havePvtMsg; bool havePvtMsg;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
bool _configure_valset(ConfigKey key, const void *value); bool _configure_valset(ConfigKey key, const void *value, uint8_t layers=UBX_VALSET_LAYER_ALL);
bool _configure_valget(ConfigKey key); bool _configure_valget(ConfigKey key);
void _configure_rate(void); void _configure_rate(void);
void _configure_sbas(bool enable); void _configure_sbas(bool enable);
@ -782,7 +823,7 @@ private:
// configure a set of config key/value pairs. The unconfig_bit corresponds to // configure a set of config key/value pairs. The unconfig_bit corresponds to
// a bit in _unconfigured_messages // a bit in _unconfigured_messages
bool _configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit); bool _configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers=UBX_VALSET_LAYER_ALL);
// find index in active_config list // find index in active_config list
int8_t find_active_config_index(ConfigKey key) const; int8_t find_active_config_index(ConfigKey key) const;
@ -796,6 +837,15 @@ private:
void set_pps_desired_freq(uint8_t freq) override; void set_pps_desired_freq(uint8_t freq) override;
#endif #endif
// status of active configuration for a role
struct {
const config_list *list;
uint8_t count;
uint32_t done_mask;
uint32_t unconfig_bit;
uint8_t layers;
} active_config;
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
// config for moving baseline base // config for moving baseline base
static const config_list config_MB_Base_uart1[]; static const config_list config_MB_Base_uart1[];
@ -805,17 +855,11 @@ private:
static const config_list config_MB_Rover_uart1[]; static const config_list config_MB_Rover_uart1[];
static const config_list config_MB_Rover_uart2[]; static const config_list config_MB_Rover_uart2[];
// status of active configuration for a role
struct {
const config_list *list;
uint8_t count;
uint32_t done_mask;
uint32_t unconfig_bit;
} active_config;
// RTCM3 parser for when in moving baseline base mode // RTCM3 parser for when in moving baseline base mode
RTCM3_Parser *rtcm3_parser; RTCM3_Parser *rtcm3_parser;
#endif // GPS_MOVING_BASELINE #endif // GPS_MOVING_BASELINE
static const config_list config_M10[];
}; };
#endif #endif