Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: allow for double EKF build

Browse Source
This commit is contained in:
Andrew Tridgell
2021-07-10 07:20:41 +10:00
parent 6aca0bb08a
commit 885e518741
14 changed files with 1758 additions and 1736 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF3/AP_NavEKF3.cpp
+1 -1
View File
@@ -1057,7 +1057,7 @@ void NavEKF3::updateCoreRelativeErrors()
// reduce error for a core only if its better than the primary lane by at least the Relative Error Threshold, this should prevent unnecessary lane changes
if (error > 0 || error < -MAX(_err_thresh, 0.05)) {
coreRelativeErrors[i] += error;
coreRelativeErrors[i] = constrain_float(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
coreRelativeErrors[i] = constrain_ftype(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
}
}
}
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
+144 -144
View File
File diff suppressed because one or more lines are too long
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+17 -17
View File
@@ -71,15 +71,15 @@ void NavEKF3_core::setWindMagStateLearningMode()
if (yawAlignComplete && useAirspeed()) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
// which assumes the vehicle has launched into the wind
Vector3f tempEuler;
Vector3F tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z);
// set the wind state variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(dal.get_EAS2TAS(), 0.9f, 10.0f));
P[index][index] = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(dal.get_EAS2TAS(), 0.9f, 10.0f));
}
} else {
// set the variances using a typical wind speed
@@ -232,7 +232,7 @@ void NavEKF3_core::setAidingMode()
}
}
// keep the IMU bias state variances, but zero the covariances
float oldBiasVariance[6];
ftype oldBiasVariance[6];
for (uint8_t row=0; row<6; row++) {
oldBiasVariance[row] = P[row+10][row+10];
}
@@ -407,7 +407,7 @@ void NavEKF3_core::setAidingMode()
}
// handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f));
ResetHeight();
#endif // EK3_FEATURE_EXTERNAL_NAV
}
@@ -438,7 +438,7 @@ void NavEKF3_core::checkAttitudeAlignmentStatus()
// Once the tilt variances have reduced, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if (!tiltAlignComplete) {
if (tiltErrorVariance < sq(radians(5.0f))) {
if (tiltErrorVariance < sq(radians(5.0))) {
tiltAlignComplete = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index);
}
@@ -631,16 +631,16 @@ void NavEKF3_core::recordYawReset()
void NavEKF3_core::checkGyroCalStatus(void)
{
// check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg));
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) &&
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
// which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);
Vector3f temp = prevTnb * delAngBiasVarVec;
delAngBiasLearned = (fabsf(temp.x) < delAngBiasVarMax) &&
(fabsf(temp.y) < delAngBiasVarMax);
Vector3F delAngBiasVarVec = Vector3F(P[10][10],P[11][11],P[12][12]);
Vector3F temp = prevTnb * delAngBiasVarVec;
delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) &&
(fabsF(temp.y) < delAngBiasVarMax);
} else {
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
(P[11][11] <= delAngBiasVarMax) &&
@@ -697,7 +697,7 @@ void NavEKF3_core::runYawEstimatorPrediction()
return;
}
float trueAirspeed;
ftype trueAirspeed;
if (assume_zero_sideslip()) {
trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
} else {
@@ -720,13 +720,13 @@ void NavEKF3_core::runYawEstimatorCorrection()
if (EKFGSF_run_filterbank) {
if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
// after velocity data has been fused the yaw variance estimate will have been refreshed and
// is used maintain a history of validity
float gsfYaw, gsfYawVariance;
ftype gsfYaw, gsfYawVariance;
if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
EKFGSF_yaw_valid_count++;
libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp
+1 -1
View File
@@ -17,7 +17,7 @@ void NavEKF3_core::resetGyroBias(void)
/*
vehicle specific initial gyro bias uncertainty in deg/sec
*/
float NavEKF3_core::InitialGyroBiasUncertainty(void) const
ftype NavEKF3_core::InitialGyroBiasUncertainty(void) const
{
return 2.5f;
}
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+7 -5
View File
@@ -6,6 +6,8 @@
#include <AP_DAL/AP_DAL.h>
#pragma GCC diagnostic ignored "-Wnarrowing"
void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
{
// Write first EKF packet
@@ -147,7 +149,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
nav_filter_status solutionStatus {};
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
float tempVar = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
getFilterFaults(_faultStatus);
getFilterStatus(solutionStatus);
const struct log_NKF4 pkt4{
@@ -159,7 +161,7 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : sqrtf(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
tiltErr : sqrtF(MAX(tiltErrorVariance,0.0f)), // estimated 1-sigma tilt error in radians
offsetNorth : offset.x,
offsetEast : offset.y,
faults : _faultStatus,
@@ -191,7 +193,7 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
offset : (int16_t)(100*terrainState), // filter ground offset state error
RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
errHAGL : (uint16_t)(100*sqrtf(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
errHAGL : (uint16_t)(100*sqrtF(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
angErr : (float)outputTrackError.x, // output predictor angle error
velErr : (float)outputTrackError.y, // output predictor velocity error
posErr : (float)outputTrackError.z // output predictor position tracking error
@@ -248,8 +250,8 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
ID : rngBcnFuseDataReportIndex,
rng : (int16_t)(100*report.rng),
innov : (int16_t)(100*report.innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(report.innovVar)),
testRatio : (uint16_t)(100*constrain_float(report.testRatio,0.0f,650.0f)),
sqrtInnovVar : (uint16_t)(100*sqrtF(report.innovVar)),
testRatio : (uint16_t)(100*constrain_ftype(report.testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*report.beaconPosNED.x),
beaconPosE : (int16_t)(100*report.beaconPosNED.y),
beaconPosD : (int16_t)(100*report.beaconPosNED.z),
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+166 -166
View File
File diff suppressed because it is too large Load Diff
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+90 -78
View File
File diff suppressed because it is too large Load Diff
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
+251 -251
View File
File diff suppressed because it is too large Load Diff
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
+26 -26
View File
@@ -23,7 +23,7 @@ bool NavEKF3_core::healthy(void) const
}
// position and height innovations must be within limits when on-ground and in a static mode of operation
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
return false;
}
@@ -98,7 +98,7 @@ void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
gyroBias.zero();
return;
}
gyroBias = stateStruct.gyro_bias / dtEkfAvg;
gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat();
}
// return accelerometer bias in m/s/s
@@ -108,7 +108,7 @@ void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
accelBias.zero();
return;
}
accelBias = stateStruct.accel_bias / dtEkfAvg;
accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat();
}
// return the transformation matrix from XYZ (body) to NED axes
@@ -121,7 +121,7 @@ void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF3_core::getQuaternion(Quaternion& ret) const
{
ret = outputDataNew.quat;
ret = outputDataNew.quat.tofloat();
}
// return the amount of yaw angle change due to the last yaw angle reset in radians
@@ -136,7 +136,7 @@ uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const
{
pos = posResetNE;
pos = posResetNE.tofloat();
return lastPosReset_ms;
}
@@ -152,7 +152,7 @@ uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
{
vel = velResetNE;
vel = velResetNE.tofloat();
return lastVelReset_ms;
}
@@ -171,7 +171,7 @@ bool NavEKF3_core::getWind(Vector3f &wind) const
void NavEKF3_core::getVelNED(Vector3f &vel) const
{
// correct for the IMU position offset (EKF calculations are at the IMU)
vel = outputDataNew.velocity + velOffsetNED;
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
}
// return estimate of true airspeed vector in body frame in m/s
@@ -181,7 +181,7 @@ bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const
if (inhibitWindStates || PV_AidingMode == AID_NONE) {
return false;
}
vel = outputDataNew.velocity + velOffsetNED;
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y;
Matrix3f Tnb; // rotation from nav to body frame
@@ -218,7 +218,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const struct Location &gpsloc = gps.location(selected_gps);
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y;
return false;
@@ -369,13 +369,13 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
// return earth magnetic field estimates in measurement units / 1000
void NavEKF3_core::getMagNED(Vector3f &magNED) const
{
magNED = stateStruct.earth_magfield * 1000.0f;
magNED = (stateStruct.earth_magfield * 1000.0f).tofloat();
}
// return body magnetic field estimates in measurement units / 1000
void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
{
magXYZ = stateStruct.body_magfield*1000.0f;
magXYZ = (stateStruct.body_magfield*1000.0f).tofloat();
}
// return magnetometer offsets
@@ -394,7 +394,7 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
!inhibitMagStates &&
dal.get_compass()->healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
magOffsets = dal.get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
return true;
} else {
magOffsets = dal.get_compass()->get_offsets(magSelectIndex);
@@ -440,15 +440,15 @@ void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaIn
// also return the delta in position due to the last position reset
void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
velVar = sqrtf(velTestRatio);
posVar = sqrtf(posTestRatio);
hgtVar = sqrtf(hgtTestRatio);
velVar = sqrtF(velTestRatio);
posVar = sqrtF(posTestRatio);
hgtVar = sqrtF(hgtTestRatio);
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio);
offset = posResetNE;
magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtF(tasTestRatio);
offset = posResetNE.tofloat();
}
// get a particular source's velocity innovations
@@ -461,8 +461,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
return false;
}
innovations = gpsVelInnov;
variances = gpsVelVarInnov;
innovations = gpsVelInnov.tofloat();
variances = gpsVelVarInnov.tofloat();
return true;
#if EK3_FEATURE_EXTERNAL_NAV
case AP_NavEKF_Source::SourceXY::EXTNAV:
@@ -470,8 +470,8 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
return false;
}
innovations = extNavVelInnov;
variances = extNavVelVarInnov;
innovations = extNavVelInnov.tofloat();
variances = extNavVelVarInnov.tofloat();
return true;
#endif // EK3_FEATURE_EXTERNAL_NAV
default:
@@ -565,11 +565,11 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
// range finder is fitted for other applications
float temp;
if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
temp = sqrtf(auxRngTestRatio);
temp = sqrtF(auxRngTestRatio);
} else {
temp = 0.0f;
}
const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z);
// send message
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar);
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+389 -387
View File
File diff suppressed because it is too large Load Diff
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+114 -114
View File
File diff suppressed because it is too large Load Diff
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
+16 -16
View File
@@ -22,7 +22,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
}
// User defined multiplier to be applied to check thresholds
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
ftype checkScaler = 0.01f*(ftype)frontend->_gpsCheckScaler;
if (gpsGoodToAlign) {
/*
@@ -51,9 +51,9 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
const auto &gps = dal.gps();
const struct Location &gpsloc = gps.location(preferred_gps); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
const ftype posFiltTimeConst = 10.0; // time constant used to decay position drift
// calculate time lapsed since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
ftype deltaTime = constrain_ftype(ftype(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
@@ -81,8 +81,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
if (gpsDataNew.have_vz && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else {
gpsVertVelFail = false;
}
@@ -91,7 +91,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
if (gpsVertVelFail) {
dal.snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsF(gpsVertVelFilt), (double)(0.3f*checkScaler));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
@@ -102,8 +102,8 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
@@ -240,14 +240,14 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
// use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
// set up varaibles and constants used by filter that is applied to GPS speed accuracy
const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const float tau = 10.0f; // time constant (sec) of peak hold decay
const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
const ftype tau = 10.0f; // time constant (sec) of peak hold decay
if (lastGpsCheckTime_ms == 0) {
lastGpsCheckTime_ms = imuSampleTime_ms;
}
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
lastGpsCheckTime_ms = imuSampleTime_ms;
float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f);
ftype alpha2 = constrain_ftype(dtLPF/tau,0.0f,1.0f);
// get the receivers reported speed accuracy
float gpsSpdAccRaw;
@@ -256,7 +256,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
}
// filter the raw speed accuracy using a LPF
sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
sAccFilterState1 = constrain_ftype((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
// apply a peak hold filter to the LPF output
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
@@ -305,7 +305,7 @@ void NavEKF3_core::detectFlight()
if (assume_zero_sideslip()) {
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
float gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
ftype gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
@@ -319,13 +319,13 @@ void NavEKF3_core::detectFlight()
}
// trigger on ground speed
const float gndSpdThresholdSq = sq(5.0f);
const ftype gndSpdThresholdSq = sq(5.0f);
if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) {
highGndSpd = true;
}
// trigger if more than 10m away from initial height
if (fabsf(hgtMea) > 10.0f) {
if (fabsF(hgtMea) > 10.0f) {
largeHgtChange = true;
}
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+349 -349
View File
File diff suppressed because it is too large Load Diff
libraries/AP_NavEKF3/AP_NavEKF3_core.h
+187 -181
View File
File diff suppressed because it is too large Load Diff