Browse Source

AP_NavEKF3: rename GSF_RUN_MASK, GSF_USE_MASK, GSF_RST_MAX

c415-sdk
Randy Mackay 5 years ago committed by Andrew Tridgell
parent
commit
ed3d072318
  1. 12
      libraries/AP_NavEKF3/AP_NavEKF3.cpp

12
libraries/AP_NavEKF3/AP_NavEKF3.cpp

@ -604,21 +604,21 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: mGauss // @Units: mGauss
AP_GROUPINFO("MAG_EF_LIM", 56, NavEKF3, _mag_ef_limit, 50), AP_GROUPINFO("MAG_EF_LIM", 56, NavEKF3, _mag_ef_limit, 50),
// @Param: GSF_RUN // @Param: GSF_RUN_MASK
// @DisplayName: Bitmask of which EKF-GSF yaw estimators run // @DisplayName: Bitmask of which EKF-GSF yaw estimators run
// @Description: 1 byte bitmap of which EKF3 instances run an independant EKF-GSF yaw estimator to provide a backup yaw estimate that doesn't rely on magnetometer data. This estimator uses IMU, GPS and, if available, airspeed data. EKF-GSF yaw estimator data for the primary EKF3 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK3_GSF_USE, EK3_GSF_DELAY and EK3_GSF_MAXCOUNT parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK3_GSF_USE to 0. // @Description: 1 byte bitmap of which EKF3 instances run an independant EKF-GSF yaw estimator to provide a backup yaw estimate that doesn't rely on magnetometer data. This estimator uses IMU, GPS and, if available, airspeed data. EKF-GSF yaw estimator data for the primary EKF3 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK3_GSF_USE, EK3_GSF_DELAY and EK3_GSF_MAXCOUNT parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK3_GSF_USE to 0.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced // @User: Advanced
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("GSF_RUN", 57, NavEKF3, _gsfRunMask, 3), AP_GROUPINFO("GSF_RUN_MASK", 57, NavEKF3, _gsfRunMask, 3),
// @Param: GSF_USE // @Param: GSF_USE_MASK
// @DisplayName: Bitmask of which EKF-GSF yaw estimators are used // @DisplayName: Bitmask of which EKF-GSF yaw estimators are used
// @Description: 1 byte bitmap of which EKF3 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK3_GSF_RUN parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF3 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF3 instance. Additionally the EKF3 will initiate a reset internally if navigation is lost for more than EK3_GSF_DELAY milli seconds. // @Description: 1 byte bitmap of which EKF3 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK3_GSF_RUN parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF3 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF3 instance. Additionally the EKF3 will initiate a reset internally if navigation is lost for more than EK3_GSF_DELAY milli seconds.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced // @User: Advanced
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("GSF_USE", 58, NavEKF3, _gsfUseMask, 3), AP_GROUPINFO("GSF_USE_MASK", 58, NavEKF3, _gsfUseMask, 3),
// @Param: GSF_DELAY // @Param: GSF_DELAY
// @DisplayName: Delay from loss of navigation to yaw reset // @DisplayName: Delay from loss of navigation to yaw reset
@ -630,14 +630,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("GSF_DELAY", 59, NavEKF3, _gsfResetDelay, 1000), AP_GROUPINFO("GSF_DELAY", 59, NavEKF3, _gsfResetDelay, 1000),
// @Param: GSF_MAXCOUNT // @Param: GSF_RST_MAX
// @DisplayName: Maximum number of resets to the EKF-GSF yaw estimate allowed // @DisplayName: Maximum number of resets to the EKF-GSF yaw estimate allowed
// @Description: Sets the maximum number of times the EKF3 will be allowed to reset it's yaw to the estimate from the EKF-GSF yaw estimator. No resets will be allowed unless the use of the EKF-GSF yaw estimate is enabled via the EK3_GSF_USE parameter. // @Description: Sets the maximum number of times the EKF3 will be allowed to reset it's yaw to the estimate from the EKF-GSF yaw estimator. No resets will be allowed unless the use of the EKF-GSF yaw estimate is enabled via the EK3_GSF_USE parameter.
// @Range: 1 10 // @Range: 1 10
// @Increment: 1 // @Increment: 1
// @User: Advanced // @User: Advanced
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("GSF_MAXCOUNT", 60, NavEKF3, _gsfResetMaxCount, 2), AP_GROUPINFO("GSF_RST_MAX", 60, NavEKF3, _gsfResetMaxCount, 2),
AP_GROUPEND AP_GROUPEND
}; };

Loading…
Cancel
Save