diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index c5c4ed95e8..a7785a9fc4 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -52,6 +52,12 @@ const AP_Param::GroupInfo AP_AHRS_DCM::var_info[] PROGMEM = { // @Increment: .01 AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS_DCM, gps_gain, 1.0), + // @Param: GPS_USE + // @DisplayName: enable/disable use of GPS for position estimation + // @Description: This controls how how much to use the GPS to correct the attitude. This is for testing the dead-reckoning code + // @User: Advanced + AP_GROUPINFO("GPS_USE", 3, AP_AHRS_DCM, _gps_use, 1), + AP_GROUPEND }; @@ -299,7 +305,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate) // return true if we have and should use GPS bool AP_AHRS_DCM::have_gps(void) { - if (!_gps || _gps->status() != GPS::GPS_OK) { + if (!_gps || _gps->status() != GPS::GPS_OK || !_gps_use) { return false; } return true; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 4282aaf2b1..95fae8c0cc 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -50,6 +50,9 @@ public: AP_Float _kp; AP_Float gps_gain; + // allow for runtime disabling of GPS usage for position + AP_Int8 _gps_use; + // for holding parameters static const struct AP_Param::GroupInfo var_info[]; @@ -127,7 +130,7 @@ private: Vector3f _last_fuse; Vector3f _last_vel; uint32_t _last_wind_time; - float _last_airspeed; + float _last_airspeed; }; #endif // AP_AHRS_DCM_H