diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e2d20ec844..b83f3d9951 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -157,6 +157,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed // @Units: milliseconds // @Values: 100:10Hz,125:8Hz,200:5Hz + // @Range: 100 200 // @User: Advanced AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200), @@ -165,6 +166,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed // @Units: milliseconds // @Values: 100:10Hz,125:8Hz,200:5Hz + // @Range: 100 200 // @User: Advanced AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200), @@ -254,6 +256,12 @@ void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_man // Initialise class variables used to do GPS blending _omega_lpf = 1.0f / constrain_float(_blend_tc, 5.0f, 30.0f); + // sanity check update rate + for (uint8_t i=0; i GPS_MAX_RATE_MS) { + _rate_ms[i] = GPS_MAX_RATE_MS; + } + } } // baudrates to try to detect GPSes with @@ -978,13 +986,25 @@ float AP_GPS::get_lag(uint8_t instance) const } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { // no GPS was detected in this instance // so return a default delay of 1 measurement interval - return 0.001f * (float)_rate_ms[instance]; + return 0.001f * (float)get_rate_ms(instance); } else { // the user has not specified a delay so we determine it from the GPS type return drivers[instance]->get_lag(); } } +/* + return gps update rate in milliseconds +*/ +uint16_t AP_GPS::get_rate_ms(uint8_t instance) const +{ + // sanity check + if (instance >= num_instances || _rate_ms[instance] <= 0) { + return GPS_MAX_RATE_MS; + } + return MIN(_rate_ms[instance], GPS_MAX_RATE_MS); +} + /* calculate the weightings used to blend GPs location and velocity data */ @@ -1010,8 +1030,8 @@ bool AP_GPS::calc_blend_weights(void) if ((state[i].last_gps_time_ms < min_ms) && (state[i].last_gps_time_ms > 0)) { min_ms = state[i].last_gps_time_ms; } - if (_rate_ms[i] > max_rate_ms) { - max_rate_ms = _rate_ms[i]; + if (get_rate_ms(i) > max_rate_ms) { + max_rate_ms = get_rate_ms(i); } } if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index b21532ae2f..35dca3df50 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -31,6 +31,7 @@ #define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data #define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximumum number of GPs instances including the 'virtual' GPS created by blending receiver data #define GPS_RTK_INJECT_TO_ALL 127 +#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms // the number of GPS leap seconds #define GPS_LEAPSECONDS_MILLIS 18000ULL @@ -142,7 +143,7 @@ public: // return number of active GPS sensors. Note that if the first GPS // is not present but the 2nd is then we return 2. Note that a blended - // GPS solution is treated as an addditional sensor. + // GPS solution is treated as an additional sensor. uint8_t num_sensors(void) const { if (!_output_is_blended) { return num_instances; @@ -152,7 +153,7 @@ public: } // Return the index of the primary sensor which is the index of the sensor contributing to - // the output. A blended solution is avalable as an additional instance + // the output. A blended solution is available as an additional instance uint8_t primary_sensor(void) const { return primary_instance; } @@ -327,6 +328,9 @@ public: float get_lag(uint8_t instance) const; float get_lag(void) const { return get_lag(primary_instance); } + // return gps update rate in milliseconds + uint16_t get_rate_ms(uint8_t instance) const; + // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin const Vector3f &get_antenna_offset(uint8_t instance) const { if (instance == GPS_MAX_RECEIVERS) { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 2374590381..1e08eeae26 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1269,7 +1269,7 @@ AP_GPS_UBLOX::_configure_rate(void) { struct ubx_cfg_nav_rate msg; // require a minimum measurement rate of 5Hz - msg.measure_rate_ms = MIN(gps._rate_ms[state.instance], MINIMUM_MEASURE_RATE_MS); + msg.measure_rate_ms = MIN(gps._rate_ms[state.instance], GPS_MAX_RATE_MS); msg.nav_rate = 1; msg.timeref = 0; // UTC time _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg)); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 23ae041bde..aa8d0a9fed 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -48,7 +48,6 @@ #define UBX_MSG_TYPES 2 #define UBLOX_MAX_PORTS 6 -#define MINIMUM_MEASURE_RATE_MS 200 #define RATE_POSLLH 1 #define RATE_STATUS 1