Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Address review comments 

Removes a leftover debug printf statement.
Fixes documentation errors.
Replace remaining fmaxf function calls with Ardupilot MAX function
This commit is contained in:
priseborough 2016-12-20 10:23:23 +11:00 committed by Randy Mackay
parent b723966b8d
commit 7e8f3fca53
7 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -214,9 +214,10 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: HGT_DELAY
// @DisplayName: Height measurement delay (msec)
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter.
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
// @Range: 0 250
// @Increment: 10
// @RebootRequired: True
// @User: Advanced
// @Units: msec
AP_GROUPINFO("HGT_DELAY", 12, NavEKF3, _hgtDelay_ms, 60),
@ -315,9 +316,10 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: FLOW_DELAY
// @DisplayName: Optical Flow measurement delay (msec)
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. The autpilot should be restarted after adjusting this parameter.
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
// @Range: 0 250
// @Increment: 10
// @RebootRequired: True
// @User: Advanced
// @Units: msec
AP_GROUPINFO("FLOW_DELAY", 23, NavEKF3, _flowDelay_ms, FLOW_MEAS_DELAY),
@ -496,9 +498,10 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: BCN_DELAY
// @DisplayName: Range beacon measurement delay (msec)
// @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter.
// @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements.
// @Range: 0 250
// @Increment: 10
// @RebootRequired: True
// @User: Advanced
// @Units: msec
AP_GROUPINFO("BCN_DELAY", 46, NavEKF3, _rngBcnDelay_ms, 50),

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -110,7 +110,7 @@ void NavEKF3_core::controlMagYawReset()
stateStruct.quat = newQuat;
// update the yaw angle variance using the variance of the measurement
angleErrVarVec.z = sq(fmaxf(frontend->_yawNoise, 1.0e-2f));
angleErrVarVec.z = sq(MAX(frontend->_yawNoise, 1.0e-2f));
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angleErrVarVec);

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -618,7 +618,7 @@ void NavEKF3_core::calcFiltGpsHgtOffset()
// correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
if (ratio < 5.0f) {
EKF_origin.alt -= (int)(100.0f * gain * innovation);
ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f);
ekfOriginHgtVar -= MAX(gain * ekfOriginHgtVar , 0.0f);
}
}

3
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -92,9 +92,8 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon receiver NED position (m)
offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate (m)
offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate (m)
posNED = receiverPos; // beacon system NED offset (m)
posNED = receiverPos; // beacon system NED offset (m)
rngBcnFuseDataReportIndex++;
printf("%6.2f\n",(double)receiverPos.x);
return true;
}

2
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -35,7 +35,7 @@ void NavEKF3_core::SelectRngBcnFusion()
FuseRngBcn();
} else {
// If we are using GPS, then GPS is the primary reference, but we continue to use the beacon data
// to calculate an indpendant position that is used to update the beacon position offset if we need to
// to calculate an independant position that is used to update the beacon position offset if we need to
// start using beacon data as the primary reference.
FuseRngBcnStatic();
// record that the beacon origin needs to be initialised

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -56,7 +56,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
// Calculate the expected EKF time step
if (_ahrs->get_ins().get_sample_rate() > 0) {
dtEkfAvg = 1.0f / _ahrs->get_ins().get_sample_rate();
dtEkfAvg = fmaxf(dtEkfAvg,EKF_TARGET_DT);
dtEkfAvg = MAX(dtEkfAvg,EKF_TARGET_DT);
} else {
return false;
}
@ -738,7 +738,7 @@ void NavEKF3_core::calcOutputStates()
// calculate a gain that provides tight tracking of the estimator states and
// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms);
timeDelay = fmaxf(timeDelay, dtIMUavg);
timeDelay = MAX(timeDelay, dtIMUavg);
float errorGain = 0.5f / timeDelay;
// calculate a corrrection to the delta angle

17
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -899,15 +899,16 @@ private:
uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report wass sent (msec)
// Specify preferred source of data to be used for a state reset
// Specify source of data to be used for a partial state reset
// Checking the availability and quality of the data source specified is the responsibility of the caller
enum resetDataSource {
DEFAULT=0, // Use best available data
GPS=1, // Use GPS if available
RNGBCN=2, // Use beacon range data if available
FLOW=3, // Use optical flow rates if available
BARO=4, // Use Baro height if available
MAG=5, // Use magnetometer data if available
RNGFND=6 // Use rangefinder data if available
DEFAULT=0, // Use data source selected by reset function internal rules
GPS=1, // Use GPS
RNGBCN=2, // Use beacon range data
FLOW=3, // Use optical flow rates
BARO=4, // Use Baro height
MAG=5, // Use magnetometer data
RNGFND=6 // Use rangefinder data
};
resetDataSource posResetSource; // preferred soure of data for position reset
resetDataSource velResetSource; // preferred source of data for a velocity reset