From 7e8f3fca53efa55f6bfe001219a2c80aad605ee4 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 20 Dec 2016 10:23:23 +1100 Subject: [PATCH] AP_NavEKF3: Address review comments Removes a leftover debug printf statement. Fixes documentation errors. Replace remaining fmaxf function calls with Ardupilot MAX function --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 9 ++++++--- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 3 +-- .../AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 17 +++++++++-------- 7 files changed, 21 insertions(+), 18 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 5f68ddb2a9..fb8456d85e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -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), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index a1441063f1..f1f7ba0022 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 5b77bf89ad..b1706dbf39 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -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); } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 7f77ff3f48..33c1c3d796 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 31f2a74ffa..5a2cd0bc7f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 59065ef767..6f9bb46664 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index eca166cc62..a44aeb0c7f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -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