zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_GPS: fixes from PR review

zr-v5.1
Andrew Tridgell 5 years ago
parent
41289fe945
commit
e660532f67
3 changed files with 50 additions and 26 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      libraries/AP_GPS/AP_GPS.cpp
  2. 60
      libraries/AP_GPS/AP_GPS_UBLOX.cpp
  3. 6
      libraries/AP_GPS/AP_GPS_UBLOX.h

10
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -73,7 +73,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { @@ -73,7 +73,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: TYPE
// @DisplayName: GPS type
// @Description: GPS type
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
@ -82,7 +82,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { @@ -82,7 +82,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
@ -852,8 +852,10 @@ void AP_GPS::update_primary(void) @@ -852,8 +852,10 @@ void AP_GPS::update_primary(void)
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER &&
state[i].status == GPS_OK_FIX_3D_RTK_FIXED &&
state[i].have_gps_yaw) {
primary_instance = i;
_last_instance_swap_ms = now;
if (primary_instance != i) {
_last_instance_swap_ms = now;
primary_instance = i;
}
return;
}
}

60
libraries/AP_GPS/AP_GPS_UBLOX.cpp
Unescape View File

@ -90,6 +90,13 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART @@ -90,6 +90,13 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
#if CONFIGURE_PPS_PIN
_unconfigured_messages |= CONFIG_TP5;
#endif
if (role == AP_GPS::GPS_ROLE_MB_BASE) {
rtcm3_parser = new RTCM3_Parser;
if (rtcm3_parser == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1);
}
}
}
AP_GPS_UBLOX::~AP_GPS_UBLOX()
@ -352,14 +359,16 @@ AP_GPS_UBLOX::_request_next_config(void) @@ -352,14 +359,16 @@ AP_GPS_UBLOX::_request_next_config(void)
}
break;
case STEP_TMODE:
if (_hardware_generation >= UBLOX_F9) {
if (supports_F9_config()) {
if (!_configure_valget(ConfigKey::TMODE_MODE)) {
_next_message--;
}
}
break;
case STEP_RTK_MOVBASE:
if (_hardware_generation >= UBLOX_F9) {
if (supports_F9_config()) {
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base), "done_mask too small, base");
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover), "done_mask too small, rover");
if (role == AP_GPS::GPS_ROLE_MB_BASE &&
!_configure_config_set(config_MB_Base, ARRAY_SIZE(config_MB_Base), CONFIG_RTK_MOVBASE)) {
_next_message--;
@ -824,7 +833,6 @@ uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const @@ -824,7 +833,6 @@ uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
case 0x1: // bit
case 0x2: // byte
return 1;
break;
case 0x3: // 2 bytes
return 2;
case 0x4: // 4 bytes
@ -1110,7 +1118,9 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -1110,7 +1118,9 @@ AP_GPS_UBLOX::_parse_gps(void)
// see if it is in active config list
int8_t cfg_idx = find_active_config_index(id);
if (cfg_idx >= 0) {
if (active_config.list[cfg_idx].value != cfg_data[0]) {
const uint8_t key_size = config_key_size(id);
if (cfg_len < key_size ||
memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
_configure_valset(id, &active_config.list[cfg_idx].value);
_unconfigured_messages |= active_config.unconfig_bit;
active_config.done_mask &= ~(1U << cfg_idx);
@ -1166,23 +1176,10 @@ AP_GPS_UBLOX::_parse_gps(void) @@ -1166,23 +1176,10 @@ AP_GPS_UBLOX::_parse_gps(void)
// see if RTK moving baseline base is wanted
if (role == AP_GPS::GPS_ROLE_MB_BASE) {
rtcm3_parser = new RTCM3_Parser;
if (rtcm3_parser == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,
"GPS: u-blox %d MB Base FAILED NO MEMORY",
state.instance + 1);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"GPS: u-blox %d config start MB Base",
state.instance + 1);
}
_unconfigured_messages |= CONFIG_RTK_MOVBASE;
}
// see if RTK moving baseline rover is wanted
if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"GPS: u-blox %d config start MB Rover",
state.instance + 1);
_unconfigured_messages |= CONFIG_RTK_MOVBASE;
}
}
@ -1632,7 +1629,7 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t @@ -1632,7 +1629,7 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t
bool
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value)
{
if (_hardware_generation < UBLOX_F9) {
if (!supports_F9_config()) {
return false;
}
const uint8_t len = config_key_size(key);
@ -1657,7 +1654,7 @@ AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value) @@ -1657,7 +1654,7 @@ AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value)
bool
AP_GPS_UBLOX::_configure_valget(ConfigKey key)
{
if (_hardware_generation < UBLOX_F9) {
if (!supports_F9_config()) {
return false;
}
struct {
@ -1679,9 +1676,6 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key) @@ -1679,9 +1676,6 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key)
bool
AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit)
{
if (_hardware_generation < UBLOX_F9) {
return false;
}
active_config.list = list;
active_config.count = count;
active_config.done_mask = 0;
@ -1906,3 +1900,25 @@ void AP_GPS_UBLOX::clear_RTCMV3(void) @@ -1906,3 +1900,25 @@ void AP_GPS_UBLOX::clear_RTCMV3(void)
rtcm3_parser->clear_packet();
}
}
// ublox specific healthy checks
bool AP_GPS_UBLOX::is_healthy(void) const
{
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) {
// we haven't initialised RTCMv3 parser
return false;
}
return true;
}
// 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_UNKNOWN_HARDWARE_GENERATION;
}

6
libraries/AP_GPS/AP_GPS_UBLOX.h
Unescape View File

@ -141,6 +141,9 @@ public: @@ -141,6 +141,9 @@ public:
// support for retrieving RTCMv3 data from a moving baseline base
bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) override;
void clear_RTCMV3(void) override;
// ublox specific healthy checks
bool is_healthy(void) const override;
private:
// u-blox UBX protocol essentials
@ -747,6 +750,9 @@ private: @@ -747,6 +750,9 @@ private:
// find index in active_config list
int8_t find_active_config_index(ConfigKey key) const;
// return true if GPS is capable of F9 config
bool supports_F9_config(void) const;
// config for moving baseline base
static const config_list config_MB_Base[];

Write Preview
Loading…
Cancel
Save