AP_NavEKF2: added EK2_IMU_MASK for multiple IMUs
no voting between multiple IMUs yet
This commit is contained in:
Andrew Tridgell
parent
56909ce973
commit
2ab2afc86a
@ -392,6 +392,13 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
|
||||
// @Bitmask: 0:NSats,1:HDoP,2:speed error,3:horiz pos error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_CHECK", 32, NavEKF2, _gpsCheck, 31),
|
||||
|
||||
// @Param: IMU_MASK
|
||||
// @DisplayName: Bitmask of active IMUs
|
||||
// @Description: 1 byte bitmap of IMUs to use in EKF2
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("IMU_MASK", 33, NavEKF2, _imuMask, 1),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -437,21 +444,47 @@ bool NavEKF2::InitialiseFilter(void)
|
||||
return false;
|
||||
}
|
||||
if (core == nullptr) {
|
||||
core = new NavEKF2_core(*this, _ahrs, _baro, _rng);
|
||||
|
||||
// count IMUs from mask
|
||||
num_cores = 0;
|
||||
for (uint8_t i=0; i<7; i++) {
|
||||
if (_imuMask & (1U<<i)) {
|
||||
num_cores++;
|
||||
}
|
||||
}
|
||||
|
||||
core = new NavEKF2_core[num_cores];
|
||||
if (core == nullptr) {
|
||||
_enable.set(0);
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
// set the IMU index for the cores
|
||||
num_cores = 0;
|
||||
for (uint8_t i=0; i<7; i++) {
|
||||
if (_imuMask & (1U<<i)) {
|
||||
core[num_cores++].setup_core(this, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
return core->InitialiseFilterBootstrap();
|
||||
|
||||
// initialse the cores. We return success only if all cores
|
||||
// initialise successfully
|
||||
bool ret = true;
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
ret &= core[i].InitialiseFilterBootstrap();
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void NavEKF2::UpdateFilter(void)
|
||||
{
|
||||
if (core) {
|
||||
core->UpdateFilter();
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
core[i].UpdateFilter();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -461,7 +494,7 @@ bool NavEKF2::healthy(void) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->healthy();
|
||||
return core[primary].healthy();
|
||||
}
|
||||
|
||||
// Return the last calculated NED position relative to the reference point (m).
|
||||
@ -472,14 +505,14 @@ bool NavEKF2::getPosNED(Vector3f &pos) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->getPosNED(pos);
|
||||
return core[primary].getPosNED(pos);
|
||||
}
|
||||
|
||||
// return NED velocity in m/s
|
||||
void NavEKF2::getVelNED(Vector3f &vel) const
|
||||
{
|
||||
if (core) {
|
||||
core->getVelNED(vel);
|
||||
core[primary].getVelNED(vel);
|
||||
}
|
||||
}
|
||||
|
||||
@ -488,7 +521,7 @@ float NavEKF2::getPosDownDerivative(void) const
|
||||
{
|
||||
// return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration
|
||||
if (core) {
|
||||
return core->getPosDownDerivative();
|
||||
return core[primary].getPosDownDerivative();
|
||||
}
|
||||
return 0.0f;
|
||||
}
|
||||
@ -497,7 +530,7 @@ float NavEKF2::getPosDownDerivative(void) const
|
||||
void NavEKF2::getAccelNED(Vector3f &accelNED) const
|
||||
{
|
||||
if (core) {
|
||||
core->getAccelNED(accelNED);
|
||||
core[primary].getAccelNED(accelNED);
|
||||
}
|
||||
}
|
||||
|
||||
@ -505,7 +538,7 @@ void NavEKF2::getAccelNED(Vector3f &accelNED) const
|
||||
void NavEKF2::getGyroBias(Vector3f &gyroBias) const
|
||||
{
|
||||
if (core) {
|
||||
core->getGyroBias(gyroBias);
|
||||
core[primary].getGyroBias(gyroBias);
|
||||
}
|
||||
}
|
||||
|
||||
@ -513,7 +546,7 @@ void NavEKF2::getGyroBias(Vector3f &gyroBias) const
|
||||
void NavEKF2::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
|
||||
{
|
||||
if (core) {
|
||||
core->getGyroScaleErrorPercentage(gyroScale);
|
||||
core[primary].getGyroScaleErrorPercentage(gyroScale);
|
||||
}
|
||||
}
|
||||
|
||||
@ -521,7 +554,7 @@ void NavEKF2::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
|
||||
void NavEKF2::getTiltError(float &ang) const
|
||||
{
|
||||
if (core) {
|
||||
core->getTiltError(ang);
|
||||
core[primary].getTiltError(ang);
|
||||
}
|
||||
}
|
||||
|
||||
@ -529,7 +562,7 @@ void NavEKF2::getTiltError(float &ang) const
|
||||
void NavEKF2::resetGyroBias(void)
|
||||
{
|
||||
if (core) {
|
||||
core->resetGyroBias();
|
||||
core[primary].resetGyroBias();
|
||||
}
|
||||
}
|
||||
|
||||
@ -543,7 +576,7 @@ bool NavEKF2::resetHeightDatum(void)
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->resetHeightDatum();
|
||||
return core[primary].resetHeightDatum();
|
||||
}
|
||||
|
||||
// Commands the EKF to not use GPS.
|
||||
@ -557,7 +590,7 @@ uint8_t NavEKF2::setInhibitGPS(void)
|
||||
if (!core) {
|
||||
return 0;
|
||||
}
|
||||
return core->setInhibitGPS();
|
||||
return core[primary].setInhibitGPS();
|
||||
}
|
||||
|
||||
// return the horizontal speed limit in m/s set by optical flow sensor limits
|
||||
@ -565,7 +598,7 @@ uint8_t NavEKF2::setInhibitGPS(void)
|
||||
void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
|
||||
{
|
||||
if (core) {
|
||||
core->getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
core[primary].getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
}
|
||||
}
|
||||
|
||||
@ -573,7 +606,7 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
|
||||
void NavEKF2::getAccelZBias(float &zbias) const
|
||||
{
|
||||
if (core) {
|
||||
core->getAccelZBias(zbias);
|
||||
core[primary].getAccelZBias(zbias);
|
||||
}
|
||||
}
|
||||
|
||||
@ -581,7 +614,7 @@ void NavEKF2::getAccelZBias(float &zbias) const
|
||||
void NavEKF2::getWind(Vector3f &wind) const
|
||||
{
|
||||
if (core) {
|
||||
core->getWind(wind);
|
||||
core[primary].getWind(wind);
|
||||
}
|
||||
}
|
||||
|
||||
@ -589,7 +622,7 @@ void NavEKF2::getWind(Vector3f &wind) const
|
||||
void NavEKF2::getMagNED(Vector3f &magNED) const
|
||||
{
|
||||
if (core) {
|
||||
core->getMagNED(magNED);
|
||||
core[primary].getMagNED(magNED);
|
||||
}
|
||||
}
|
||||
|
||||
@ -597,7 +630,7 @@ void NavEKF2::getMagNED(Vector3f &magNED) const
|
||||
void NavEKF2::getMagXYZ(Vector3f &magXYZ) const
|
||||
{
|
||||
if (core) {
|
||||
core->getMagXYZ(magXYZ);
|
||||
core[primary].getMagXYZ(magXYZ);
|
||||
}
|
||||
}
|
||||
|
||||
@ -608,7 +641,7 @@ bool NavEKF2::getMagOffsets(Vector3f &magOffsets) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->getMagOffsets(magOffsets);
|
||||
return core[primary].getMagOffsets(magOffsets);
|
||||
}
|
||||
|
||||
// Return the last calculated latitude, longitude and height in WGS-84
|
||||
@ -620,7 +653,7 @@ bool NavEKF2::getLLH(struct Location &loc) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->getLLH(loc);
|
||||
return core[primary].getLLH(loc);
|
||||
}
|
||||
|
||||
// return the latitude and longitude and height used to set the NED origin
|
||||
@ -631,7 +664,7 @@ bool NavEKF2::getOriginLLH(struct Location &loc) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->getOriginLLH(loc);
|
||||
return core[primary].getOriginLLH(loc);
|
||||
}
|
||||
|
||||
// set the latitude and longitude and height used to set the NED origin
|
||||
@ -643,7 +676,7 @@ bool NavEKF2::setOriginLLH(struct Location &loc)
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->setOriginLLH(loc);
|
||||
return core[primary].setOriginLLH(loc);
|
||||
}
|
||||
|
||||
// return estimated height above ground level
|
||||
@ -653,14 +686,14 @@ bool NavEKF2::getHAGL(float &HAGL) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->getHAGL(HAGL);
|
||||
return core[primary].getHAGL(HAGL);
|
||||
}
|
||||
|
||||
// return the Euler roll, pitch and yaw angle in radians
|
||||
void NavEKF2::getEulerAngles(Vector3f &eulers) const
|
||||
{
|
||||
if (core) {
|
||||
core->getEulerAngles(eulers);
|
||||
core[primary].getEulerAngles(eulers);
|
||||
}
|
||||
}
|
||||
|
||||
@ -668,7 +701,7 @@ void NavEKF2::getEulerAngles(Vector3f &eulers) const
|
||||
void NavEKF2::getRotationBodyToNED(Matrix3f &mat) const
|
||||
{
|
||||
if (core) {
|
||||
core->getRotationBodyToNED(mat);
|
||||
core[primary].getRotationBodyToNED(mat);
|
||||
}
|
||||
}
|
||||
|
||||
@ -676,7 +709,7 @@ void NavEKF2::getRotationBodyToNED(Matrix3f &mat) const
|
||||
void NavEKF2::getQuaternion(Quaternion &quat) const
|
||||
{
|
||||
if (core) {
|
||||
core->getQuaternion(quat);
|
||||
core[primary].getQuaternion(quat);
|
||||
}
|
||||
}
|
||||
|
||||
@ -684,7 +717,7 @@ void NavEKF2::getQuaternion(Quaternion &quat) const
|
||||
void NavEKF2::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
|
||||
{
|
||||
if (core) {
|
||||
core->getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
||||
core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
||||
}
|
||||
}
|
||||
|
||||
@ -692,7 +725,7 @@ void NavEKF2::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &m
|
||||
void NavEKF2::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
|
||||
{
|
||||
if (core) {
|
||||
core->getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
core[primary].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
}
|
||||
}
|
||||
|
||||
@ -703,7 +736,7 @@ bool NavEKF2::use_compass(void) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->use_compass();
|
||||
return core[primary].use_compass();
|
||||
}
|
||||
|
||||
// write the raw optical flow measurements
|
||||
@ -715,7 +748,7 @@ bool NavEKF2::use_compass(void) const
|
||||
void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
|
||||
{
|
||||
if (core) {
|
||||
core->writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas);
|
||||
core[primary].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas);
|
||||
}
|
||||
}
|
||||
|
||||
@ -724,7 +757,7 @@ void NavEKF2::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX,
|
||||
float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
|
||||
{
|
||||
if (core) {
|
||||
core->getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr);
|
||||
core[primary].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr);
|
||||
}
|
||||
}
|
||||
|
||||
@ -733,7 +766,7 @@ void NavEKF2::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX,
|
||||
void NavEKF2::setTakeoffExpected(bool val)
|
||||
{
|
||||
if (core) {
|
||||
core->setTakeoffExpected(val);
|
||||
core[primary].setTakeoffExpected(val);
|
||||
}
|
||||
}
|
||||
|
||||
@ -742,7 +775,7 @@ void NavEKF2::setTakeoffExpected(bool val)
|
||||
void NavEKF2::setTouchdownExpected(bool val)
|
||||
{
|
||||
if (core) {
|
||||
core->setTouchdownExpected(val);
|
||||
core[primary].setTouchdownExpected(val);
|
||||
}
|
||||
}
|
||||
|
||||
@ -760,7 +793,7 @@ void NavEKF2::setTouchdownExpected(bool val)
|
||||
void NavEKF2::getFilterFaults(uint8_t &faults) const
|
||||
{
|
||||
if (core) {
|
||||
core->getFilterFaults(faults);
|
||||
core[primary].getFilterFaults(faults);
|
||||
} else {
|
||||
faults = 0;
|
||||
}
|
||||
@ -780,7 +813,7 @@ void NavEKF2::getFilterFaults(uint8_t &faults) const
|
||||
void NavEKF2::getFilterTimeouts(uint8_t &timeouts) const
|
||||
{
|
||||
if (core) {
|
||||
core->getFilterTimeouts(timeouts);
|
||||
core[primary].getFilterTimeouts(timeouts);
|
||||
} else {
|
||||
timeouts = 0;
|
||||
}
|
||||
@ -792,7 +825,7 @@ void NavEKF2::getFilterTimeouts(uint8_t &timeouts) const
|
||||
void NavEKF2::getFilterStatus(nav_filter_status &status) const
|
||||
{
|
||||
if (core) {
|
||||
core->getFilterStatus(status);
|
||||
core[primary].getFilterStatus(status);
|
||||
} else {
|
||||
memset(&status, 0, sizeof(status));
|
||||
}
|
||||
@ -804,7 +837,7 @@ return filter gps quality check status
|
||||
void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const
|
||||
{
|
||||
if (core) {
|
||||
core->getFilterGpsStatus(status);
|
||||
core[primary].getFilterGpsStatus(status);
|
||||
} else {
|
||||
memset(&status, 0, sizeof(status));
|
||||
}
|
||||
@ -814,7 +847,7 @@ void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const
|
||||
void NavEKF2::send_status_report(mavlink_channel_t chan)
|
||||
{
|
||||
if (core) {
|
||||
core->send_status_report(chan);
|
||||
core[primary].send_status_report(chan);
|
||||
}
|
||||
}
|
||||
|
||||
@ -826,7 +859,7 @@ bool NavEKF2::getHeightControlLimit(float &height) const
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core->getHeightControlLimit(height);
|
||||
return core[primary].getHeightControlLimit(height);
|
||||
}
|
||||
|
||||
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
||||
@ -836,7 +869,7 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAng) const
|
||||
if (!core) {
|
||||
return 0;
|
||||
}
|
||||
return core->getLastYawResetAngle(yawAng);
|
||||
return core[primary].getLastYawResetAngle(yawAng);
|
||||
}
|
||||
|
||||
// return the amount of NE position change due to the last position reset in metres
|
||||
@ -846,7 +879,7 @@ uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &pos) const
|
||||
if (!core) {
|
||||
return 0;
|
||||
}
|
||||
return core->getLastPosNorthEastReset(pos);
|
||||
return core[primary].getLastPosNorthEastReset(pos);
|
||||
}
|
||||
|
||||
// return the amount of NE velocity change due to the last velocity reset in metres/sec
|
||||
@ -856,7 +889,7 @@ uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) const
|
||||
if (!core) {
|
||||
return 0;
|
||||
}
|
||||
return core->getLastVelNorthEastReset(vel);
|
||||
return core[primary].getLastVelNorthEastReset(vel);
|
||||
}
|
||||
|
||||
// report the reason for why the backend is refusing to initialise
|
||||
@ -865,7 +898,7 @@ const char *NavEKF2::prearm_failure_reason(void) const
|
||||
if (!core) {
|
||||
return nullptr;
|
||||
}
|
||||
return core->prearm_failure_reason();
|
||||
return core[primary].prearm_failure_reason();
|
||||
}
|
||||
|
||||
#endif //HAL_CPU_CLASS
|
||||
|
||||
@ -242,6 +242,8 @@ public:
|
||||
void set_enable(bool enable) { _enable.set(enable); }
|
||||
|
||||
private:
|
||||
uint8_t num_cores; // number of allocated cores
|
||||
uint8_t primary; // current primary core
|
||||
NavEKF2_core *core = nullptr;
|
||||
const AP_AHRS *_ahrs;
|
||||
AP_Baro &_baro;
|
||||
@ -281,6 +283,7 @@ private:
|
||||
AP_Float _gyroScaleProcessNoise;// gyro scale factor state process noise : 1/s
|
||||
AP_Float _rngNoise; // Range finder noise : m
|
||||
AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
|
||||
AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF2 for
|
||||
|
||||
// Tuning parameters
|
||||
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
|
||||
|
||||
@ -38,7 +38,7 @@ void NavEKF2_core::FuseAirspeed()
|
||||
float vwn;
|
||||
float vwe;
|
||||
float EAS2TAS = _ahrs->get_EAS2TAS();
|
||||
const float R_TAS = sq(constrain_float(frontend._easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
|
||||
const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
|
||||
Vector3 SH_TAS;
|
||||
float SK_TAS;
|
||||
Vector24 H_TAS;
|
||||
@ -121,11 +121,11 @@ void NavEKF2_core::FuseAirspeed()
|
||||
innovVtas = VtasPred - tasDataDelayed.tas;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
tasTestRatio = sq(innovVtas) / (sq(frontend._tasInnovGate) * varInnovVtas);
|
||||
tasTestRatio = sq(innovVtas) / (sq(frontend->_tasInnovGate) * varInnovVtas);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
|
||||
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend.tasRetryTime_ms;
|
||||
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
|
||||
|
||||
// test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
|
||||
if (tasHealth || (tasTimeout && posTimeout)) {
|
||||
@ -207,7 +207,7 @@ void NavEKF2_core::SelectTasFusion()
|
||||
readAirSpdData();
|
||||
|
||||
// If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
|
||||
if (imuSampleTime_ms - tasDataNew.time_ms > frontend.tasRetryTime_ms) {
|
||||
if (imuSampleTime_ms - tasDataNew.time_ms > frontend->tasRetryTime_ms) {
|
||||
tasTimeout = true;
|
||||
}
|
||||
|
||||
@ -278,9 +278,9 @@ void NavEKF2_core::SelectBetaFusion()
|
||||
}
|
||||
|
||||
// set true when the fusion time interval has triggered
|
||||
bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend.betaAvg_ms);
|
||||
bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend->betaAvg_ms);
|
||||
// set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
|
||||
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend.gpsRetryTimeNoTAS_ms));
|
||||
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->gpsRetryTimeNoTAS_ms));
|
||||
// set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
|
||||
bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
|
||||
// use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
|
||||
|
||||
@ -57,15 +57,15 @@ void NavEKF2_core::setWindMagStateLearningMode()
|
||||
|
||||
// Determine if learning of magnetic field states has been requested by the user
|
||||
bool magCalRequested =
|
||||
((frontend._magCal == 0) && inFlight) || // when flying
|
||||
((frontend._magCal == 1) && manoeuvring) || // when manoeuvring
|
||||
((frontend._magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
|
||||
(frontend._magCal == 4); // all the time
|
||||
((frontend->_magCal == 0) && inFlight) || // when flying
|
||||
((frontend->_magCal == 1) && manoeuvring) || // when manoeuvring
|
||||
((frontend->_magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
|
||||
(frontend->_magCal == 4); // all the time
|
||||
|
||||
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
|
||||
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
|
||||
// If we do nto have absolute position (eg GPS) then the earth field states cannot be learned
|
||||
bool magCalDenied = !use_compass() || (frontend._magCal == 2) || (PV_AidingMode == AID_NONE) || (onGround && frontend._magCal != 4);
|
||||
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) || (PV_AidingMode == AID_NONE) || (onGround && frontend->_magCal != 4);
|
||||
|
||||
// Inhibit the magnetic field calibration if not requested or denied
|
||||
inhibitMagStates = (!magCalRequested || magCalDenied);
|
||||
@ -96,7 +96,7 @@ void NavEKF2_core::setAidingMode()
|
||||
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
||||
bool filterIsStable = tiltAlignComplete && yawAlignComplete;
|
||||
// If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
|
||||
bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
|
||||
bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
|
||||
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
|
||||
// Latch to on. Aiding can be turned off by setting both
|
||||
isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
|
||||
@ -121,7 +121,7 @@ void NavEKF2_core::setAidingMode()
|
||||
meaHgtAtTakeOff = baroDataDelayed.hgt;
|
||||
// reset the vertical position state to faster recover from baro errors experienced during touchdown
|
||||
stateStruct.position.z = -meaHgtAtTakeOff;
|
||||
} else if (frontend._fusionModeGPS == 3) {
|
||||
} else if (frontend->_fusionModeGPS == 3) {
|
||||
// We have commenced aiding, but GPS useage has been prohibited so use optical flow only
|
||||
hal.console->printf("EKF2 is using optical flow\n");
|
||||
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
|
||||
@ -259,7 +259,7 @@ uint8_t NavEKF2_core::setInhibitGPS(void)
|
||||
return 0;
|
||||
}
|
||||
if (optFlowDataPresent()) {
|
||||
frontend._fusionModeGPS = 3;
|
||||
frontend->_fusionModeGPS = 3;
|
||||
//#error writing to a tuning parameter
|
||||
return 2;
|
||||
} else {
|
||||
|
||||
@ -136,7 +136,7 @@ void NavEKF2_core::SelectMagFusion()
|
||||
if (magHealth) {
|
||||
magTimeout = false;
|
||||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||||
} else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend.magFailTimeLimit_ms && use_compass()) {
|
||||
} else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend->magFailTimeLimit_ms && use_compass()) {
|
||||
magTimeout = true;
|
||||
}
|
||||
|
||||
@ -250,7 +250,7 @@ void NavEKF2_core::FuseMagnetometer()
|
||||
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
|
||||
|
||||
// scale magnetometer observation error with total angular rate to allow for timing errors
|
||||
R_MAG = sq(constrain_float(frontend._magNoise, 0.01f, 0.5f)) + sq(frontend.magVarRateScale*imuDataDelayed.delAng.length() / dtIMUavg);
|
||||
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / dtIMUavg);
|
||||
|
||||
// calculate observation jacobians
|
||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
@ -487,7 +487,7 @@ void NavEKF2_core::FuseMagnetometer()
|
||||
// calculate the measurement innovation
|
||||
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
|
||||
// calculate the innovation test ratio
|
||||
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(max(frontend._magInnovGate,1)) * varInnovMag[obsIndex]);
|
||||
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(max(frontend->_magInnovGate,1)) * varInnovMag[obsIndex]);
|
||||
// check the last values from all components and set magnetometer health accordingly
|
||||
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
|
||||
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
|
||||
@ -679,7 +679,7 @@ void NavEKF2_core::fuseCompass()
|
||||
}
|
||||
|
||||
// calculate the innovation test ratio
|
||||
yawTestRatio = sq(innovation) / (sq(max(frontend._magInnovGate,1)) * varInnov);
|
||||
yawTestRatio = sq(innovation) / (sq(max(frontend->_magInnovGate,1)) * varInnov);
|
||||
|
||||
// Declare the magnetometer unhealthy if the innovation test fails
|
||||
if (yawTestRatio > 1.0f) {
|
||||
|
||||
@ -25,15 +25,15 @@ void NavEKF2_core::readRangeFinder(void)
|
||||
uint8_t maxIndex;
|
||||
uint8_t minIndex;
|
||||
// get theoretical correct range when the vehicle is on the ground
|
||||
rngOnGnd = _rng.ground_clearance_cm() * 0.01f;
|
||||
if (_rng.status() == RangeFinder::RangeFinder_Good && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||||
rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f;
|
||||
if (frontend->_rng.status() == RangeFinder::RangeFinder_Good && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||||
// store samples and sample time into a ring buffer
|
||||
rngMeasIndex ++;
|
||||
if (rngMeasIndex > 2) {
|
||||
rngMeasIndex = 0;
|
||||
}
|
||||
storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms;
|
||||
storedRngMeas[rngMeasIndex] = _rng.distance_cm() * 0.01f;
|
||||
storedRngMeas[rngMeasIndex] = frontend->_rng.distance_cm() * 0.01f;
|
||||
// check for three fresh samples and take median
|
||||
bool sampleFresh[3];
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
@ -91,7 +91,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
|
||||
}
|
||||
// check for takeoff if relying on optical flow and zero measurements until takeoff detected
|
||||
// if we haven't taken off - constrain position and velocity states
|
||||
if (frontend._fusionModeGPS == 3) {
|
||||
if (frontend->_fusionModeGPS == 3) {
|
||||
detectOptFlowTakeoff();
|
||||
}
|
||||
// calculate rotation matrices at mid sample time for flow observations
|
||||
@ -111,10 +111,10 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
|
||||
// record time last observation was received so we can detect loss of data elsewhere
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
// estimate sample time of the measurement
|
||||
ofDataNew.time_ms = imuSampleTime_ms - frontend._flowDelay_ms - frontend.flowTimeDeltaAvg_ms/2;
|
||||
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||||
ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
// Save data to buffer
|
||||
@ -175,7 +175,7 @@ bool NavEKF2_core::RecallOF()
|
||||
bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
|
||||
{
|
||||
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
|
||||
if (firstMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
|
||||
if (firstMagYawInit && (frontend->_magCal != 2) && _ahrs->get_compass()->healthy(0)) {
|
||||
magOffsets = _ahrs->get_compass()->get_offsets(0) - stateStruct.body_magfield*1000.0f;
|
||||
return true;
|
||||
} else {
|
||||
@ -194,11 +194,11 @@ void NavEKF2_core::readMagData()
|
||||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec();
|
||||
|
||||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||||
magDataNew.time_ms = imuSampleTime_ms - frontend.magDelay_ms;
|
||||
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
||||
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||||
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
|
||||
// read compass data and scale to improve numerical conditioning
|
||||
magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
|
||||
@ -465,11 +465,11 @@ void NavEKF2_core::readGpsData()
|
||||
|
||||
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
|
||||
// ideally we should be using a timing signal from the GPS receiver to set this time
|
||||
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
|
||||
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
|
||||
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||||
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
@ -498,7 +498,7 @@ void NavEKF2_core::readGpsData()
|
||||
}
|
||||
|
||||
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
|
||||
if (_ahrs->get_gps().have_vertical_velocity() && frontend._fusionModeGPS == 0) {
|
||||
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
|
||||
useGpsVertVel = true;
|
||||
} else {
|
||||
useGpsVertVel = false;
|
||||
@ -528,7 +528,7 @@ void NavEKF2_core::readGpsData()
|
||||
// We are by definition at the origin at the instant of alignment so set NE position to zero
|
||||
gpsDataNew.pos.zero();
|
||||
// If GPS useage isn't explicitly prohibited, we switch to absolute position mode
|
||||
if (isAiding && frontend._fusionModeGPS != 3) {
|
||||
if (isAiding && frontend->_fusionModeGPS != 3) {
|
||||
PV_AidingMode = AID_ABSOLUTE;
|
||||
// Initialise EKF position and velocity states
|
||||
ResetPosition();
|
||||
@ -555,10 +555,10 @@ void NavEKF2_core::readGpsData()
|
||||
bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
|
||||
|
||||
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
|
||||
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend.gpsRetryTimeUseTAS_ms : frontend.gpsRetryTimeNoTAS_ms;
|
||||
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;
|
||||
|
||||
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
|
||||
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend.gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
|
||||
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
|
||||
|
||||
// If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
|
||||
if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
|
||||
@ -574,7 +574,7 @@ void NavEKF2_core::readGpsData()
|
||||
if (PV_AidingMode == AID_ABSOLUTE && !useAirspeed() && !assume_zero_sideslip()) {
|
||||
if (optFlowBackupAvailable) {
|
||||
// we can do optical flow only nav
|
||||
frontend._fusionModeGPS = 3;
|
||||
frontend->_fusionModeGPS = 3;
|
||||
PV_AidingMode = AID_RELATIVE;
|
||||
} else {
|
||||
// store the current position
|
||||
@ -659,36 +659,36 @@ void NavEKF2_core::readHgtData()
|
||||
{
|
||||
// check to see if baro measurement has changed so we know if a new measurement has arrived
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
if (_baro.get_last_update() - lastHgtReceived_ms > 70) {
|
||||
if (frontend->_baro.get_last_update() - lastHgtReceived_ms > 70) {
|
||||
// Don't use Baro height if operating in optical flow mode as we use range finder instead
|
||||
if (frontend._fusionModeGPS == 3 && frontend._altSource == 1) {
|
||||
if (frontend->_fusionModeGPS == 3 && frontend->_altSource == 1) {
|
||||
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
|
||||
// adjust range finder measurement to allow for effect of vehicle tilt and height of sensor
|
||||
baroDataNew.hgt = max(rngMea * Tnb_flow.c.z, rngOnGnd);
|
||||
// calculate offset to baro data that enables baro to be used as a backup
|
||||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||||
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||||
baroHgtOffset = 0.1f * (frontend->_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||||
} else if (isAiding && takeOffDetected) {
|
||||
// we have lost range finder measurements and are in optical flow flight
|
||||
// use baro measurement and correct for baro offset - failsafe use only as baro will drift
|
||||
baroDataNew.hgt = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd);
|
||||
baroDataNew.hgt = max(frontend->_baro.get_altitude() - baroHgtOffset, rngOnGnd);
|
||||
} else {
|
||||
// If we are on ground and have no range finder reading, assume the nominal on-ground height
|
||||
baroDataNew.hgt = rngOnGnd;
|
||||
// calculate offset to baro data that enables baro to be used as a backup
|
||||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||||
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||||
baroHgtOffset = 0.1f * (frontend->_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||||
}
|
||||
} else {
|
||||
// Normal operation is to use baro measurement
|
||||
baroDataNew.hgt = _baro.get_altitude();
|
||||
baroDataNew.hgt = frontend->_baro.get_altitude();
|
||||
}
|
||||
|
||||
// filtered baro data used to provide a reference for takeoff
|
||||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||||
if (!getTakeoffExpected()) {
|
||||
const float gndHgtFiltTC = 0.5f;
|
||||
const float dtBaro = frontend.hgtAvg_ms*1.0e-3f;
|
||||
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
|
||||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||||
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
||||
} else if (isAiding && getTakeoffExpected()) {
|
||||
@ -698,14 +698,14 @@ void NavEKF2_core::readHgtData()
|
||||
}
|
||||
|
||||
// time stamp used to check for new measurement
|
||||
lastHgtReceived_ms = _baro.get_last_update();
|
||||
lastHgtReceived_ms = frontend->_baro.get_last_update();
|
||||
|
||||
// estimate of time height measurement was taken, allowing for delays
|
||||
baroDataNew.time_ms = lastHgtReceived_ms - frontend._hgtDelay_ms;
|
||||
baroDataNew.time_ms = lastHgtReceived_ms - frontend->_hgtDelay_ms;
|
||||
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
baroDataNew.time_ms = roundToNearest(baroDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||||
baroDataNew.time_ms = roundToNearest(baroDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
@ -773,10 +773,10 @@ void NavEKF2_core::readAirSpdData()
|
||||
aspeed->last_update_ms() != timeTasReceived_ms) {
|
||||
tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
|
||||
timeTasReceived_ms = aspeed->last_update_ms();
|
||||
tasDataNew.time_ms = timeTasReceived_ms - frontend.tasDelay_ms;
|
||||
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
tasDataNew.time_ms = roundToNearest(tasDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||||
tasDataNew.time_ms = roundToNearest(tasDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
newDataTas = true;
|
||||
StoreTAS();
|
||||
RecallTAS();
|
||||
|
||||
@ -46,9 +46,9 @@ void NavEKF2_core::SelectFlowFusion()
|
||||
// check is the terrain offset estimate is still valid
|
||||
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
|
||||
// Perform tilt check
|
||||
bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
|
||||
bool tiltOK = (Tnb_flow.c.z > frontend->DCM33FlowMin);
|
||||
// Constrain measurements to zero if we are using optical flow and are on the ground
|
||||
if (frontend._fusionModeGPS == 3 && !takeOffDetected && isAiding) {
|
||||
if (frontend->_fusionModeGPS == 3 && !takeOffDetected && isAiding) {
|
||||
ofDataDelayed.flowRadXYcomp.zero();
|
||||
ofDataDelayed.flowRadXY.zero();
|
||||
flowDataValid = true;
|
||||
@ -83,7 +83,7 @@ void NavEKF2_core::SelectFlowFusion()
|
||||
if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE)
|
||||
{
|
||||
// Set the flow noise used by the fusion processes
|
||||
R_LOS = sq(max(frontend._flowNoise, 0.05f));
|
||||
R_LOS = sq(max(frontend->_flowNoise, 0.05f));
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
// Fuse the optical flow X and Y axis data into the main filter sequentially
|
||||
@ -129,7 +129,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
|
||||
// in addition to a terrain gradient error model, we also have a time based error growth that is scaled using the gradient parameter
|
||||
float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
||||
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend.gndGradientSigma))) + sq(float(frontend.gndGradientSigma) * timeLapsed);
|
||||
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(float(frontend->gndGradientSigma) * timeLapsed);
|
||||
Popt += Pincrement;
|
||||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||||
|
||||
@ -145,7 +145,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
|
||||
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
float R_RNG = frontend._rngNoise;
|
||||
float R_RNG = frontend->_rngNoise;
|
||||
|
||||
// calculate Kalman gain
|
||||
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
@ -161,7 +161,7 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
innovRng = predRngMeas - rngMea;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(frontend._rngInnovGate) * varInnovRng);
|
||||
auxRngTestRatio = sq(innovRng) / (sq(frontend->_rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((sq(innovRng)*SK_RNG) < 25.0f)
|
||||
@ -249,10 +249,10 @@ void NavEKF2_core::EstimateTerrainOffset()
|
||||
K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend._flowInnovGate) * auxFlowObsInnovVar);
|
||||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend->_flowInnovGate) * auxFlowObsInnovVar);
|
||||
|
||||
// don't fuse if optical flow data is outside valid range
|
||||
if (max(flowRadXY[0],flowRadXY[1]) < frontend._maxFlowRate) {
|
||||
if (max(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) {
|
||||
|
||||
// correct the state
|
||||
terrainState -= K_OPT * auxFlowObsInnov;
|
||||
@ -633,10 +633,10 @@ void NavEKF2_core::FuseOptFlow()
|
||||
}
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(frontend._flowInnovGate) * varInnovOptFlow[obsIndex]);
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(frontend->_flowInnovGate) * varInnovOptFlow[obsIndex]);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
|
||||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend._maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend._maxFlowRate)) {
|
||||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
|
||||
// record the last time observations were accepted for fusion
|
||||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||||
|
||||
|
||||
@ -61,9 +61,9 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
|
||||
bool NavEKF2_core::getHeightControlLimit(float &height) const
|
||||
{
|
||||
// only ask for limiting if we are doing optical flow navigation
|
||||
if (frontend._fusionModeGPS == 3) {
|
||||
if (frontend->_fusionModeGPS == 3) {
|
||||
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
|
||||
height = max(float(_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f);
|
||||
height = max(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f);
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
@ -287,7 +287,7 @@ void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa
|
||||
{
|
||||
if (PV_AidingMode == AID_RELATIVE) {
|
||||
// allow 1.0 rad/sec margin for angular motion
|
||||
ekfGndSpdLimit = max((frontend._maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd);
|
||||
ekfGndSpdLimit = max((frontend->_maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd);
|
||||
// use standard gains up to 5.0 metres height and reduce above that
|
||||
ekfNavVelGainScaler = 4.0f / max((terrainState - stateStruct.position[2]),4.0f);
|
||||
} else {
|
||||
@ -412,7 +412,7 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
|
||||
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
|
||||
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
|
||||
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
|
||||
bool optFlowNavPossible = flowDataValid && (frontend._fusionModeGPS == 3);
|
||||
bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3);
|
||||
bool gpsNavPossible = !gpsNotAvailable && (PV_AidingMode == AID_ABSOLUTE) && gpsGoodToAlign;
|
||||
bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete;
|
||||
|
||||
|
||||
@ -105,13 +105,13 @@ void NavEKF2_core::ResetHeight(void)
|
||||
bool NavEKF2_core::resetHeightDatum(void)
|
||||
{
|
||||
// if we are using a range finder for height, return false
|
||||
if (frontend._altSource == 1) {
|
||||
if (frontend->_altSource == 1) {
|
||||
return false;
|
||||
}
|
||||
// record the old height estimate
|
||||
float oldHgt = -stateStruct.position.z;
|
||||
// reset the barometer so that it reads zero at the current height
|
||||
_baro.update_calibration();
|
||||
frontend->_baro.update_calibration();
|
||||
// reset the height state
|
||||
stateStruct.position.z = 0.0f;
|
||||
// adjust the height of the EKF origin so that the origin plus baro height before and afer the reset is the same
|
||||
@ -143,7 +143,7 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
// Determine if we need to fuse position and velocity data on this time step
|
||||
if (RecallGPS() && PV_AidingMode == AID_ABSOLUTE) {
|
||||
// Don't fuse velocity data if GPS doesn't support it
|
||||
if (frontend._fusionModeGPS <= 1) {
|
||||
if (frontend->_fusionModeGPS <= 1) {
|
||||
fuseVelData = true;
|
||||
} else {
|
||||
fuseVelData = false;
|
||||
@ -159,7 +159,7 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
|
||||
// If we haven't received height data for a while, then declare the height data as being timed out
|
||||
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
||||
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend.hgtRetryTimeMode0_ms : frontend.hgtRetryTimeMode12_ms;
|
||||
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
||||
if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime_ms) {
|
||||
hgtTimeout = true;
|
||||
}
|
||||
@ -236,8 +236,8 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
|
||||
// set the GPS data timeout depending on whether airspeed data is present
|
||||
uint32_t gpsRetryTime;
|
||||
if (useAirspeed()) gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
|
||||
else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
|
||||
if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms;
|
||||
else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms;
|
||||
|
||||
// form the observation vector
|
||||
observation[0] = gpsDataDelayed.vel.x;
|
||||
@ -248,7 +248,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
observation[5] = -baroDataDelayed.hgt;
|
||||
|
||||
// calculate additional error in GPS position caused by manoeuvring
|
||||
posErr = frontend.gpsPosVarAccScale * accNavMag;
|
||||
posErr = frontend->gpsPosVarAccScale * accNavMag;
|
||||
|
||||
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
|
||||
@ -266,35 +266,35 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
} else {
|
||||
if (gpsSpdAccuracy > 0.0f) {
|
||||
// use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
|
||||
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsHorizVelNoise, 50.0f));
|
||||
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsVertVelNoise, 50.0f));
|
||||
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
||||
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
||||
} else {
|
||||
// calculate additional error in GPS velocity caused by manoeuvring
|
||||
R_OBS[0] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
|
||||
R_OBS[2] = sq(constrain_float(frontend._gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsDVelVarAccScale * accNavMag);
|
||||
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||
R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
|
||||
}
|
||||
R_OBS[1] = R_OBS[0];
|
||||
R_OBS[3] = sq(constrain_float(frontend._gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||||
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||||
R_OBS[4] = R_OBS[3];
|
||||
}
|
||||
R_OBS[5] = sq(constrain_float(frontend._baroAltNoise, 0.1f, 10.0f));
|
||||
R_OBS[5] = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
|
||||
|
||||
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
||||
if (getTakeoffExpected() || getTouchdownExpected()) {
|
||||
R_OBS[5] *= frontend.gndEffectBaroScaler;
|
||||
R_OBS[5] *= frontend->gndEffectBaroScaler;
|
||||
}
|
||||
|
||||
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
|
||||
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
|
||||
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
||||
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
|
||||
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
||||
|
||||
|
||||
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
|
||||
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
|
||||
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
|
||||
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend.hgtAvg_ms)) {
|
||||
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend->hgtAvg_ms)) {
|
||||
// calculate innovations for height and vertical GPS vel measurements
|
||||
float hgtErr = stateStruct.position.z - observation[5];
|
||||
float velDErr = stateStruct.velocity.z - observation[2];
|
||||
@ -315,7 +315,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
|
||||
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
float maxPosInnov2 = sq(frontend._gpsPosInnovGate)*(varInnovVelPos[3] + varInnovVelPos[4]);
|
||||
float maxPosInnov2 = sq(frontend->_gpsPosInnovGate)*(varInnovVelPos[3] + varInnovVelPos[4]);
|
||||
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
||||
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
||||
// declare a timeout condition if we have been too long without data or not aiding
|
||||
@ -327,7 +327,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
// if timed out or outside the specified uncertainty radius, reset to the GPS
|
||||
if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
|
||||
if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
|
||||
// reset the position to the current GPS position
|
||||
ResetPosition();
|
||||
// reset the velocity to the GPS velocity
|
||||
@ -338,7 +338,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
// Reset the position variances and corresponding covariances to a value that will pass the checks
|
||||
zeroRows(P,6,7);
|
||||
zeroCols(P,6,7);
|
||||
P[6][6] = sq(float(0.5f*frontend._gpsGlitchRadiusMax));
|
||||
P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
|
||||
P[7][7] = P[6][6];
|
||||
// Reset the normalised innovation to avoid failing the bad fusion tests
|
||||
posTestRatio = 0.0f;
|
||||
@ -355,7 +355,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
// test velocity measurements
|
||||
uint8_t imax = 2;
|
||||
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
|
||||
if (frontend._fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
|
||||
if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
|
||||
imax = 1;
|
||||
}
|
||||
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
||||
@ -373,7 +373,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
}
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
// calculate the test ratio
|
||||
velTestRatio = innovVelSumSq / (varVelSum * sq(frontend._gpsVelInnovGate));
|
||||
velTestRatio = innovVelSumSq / (varVelSum * sq(frontend->_gpsVelInnovGate));
|
||||
// fail if the ratio is greater than 1
|
||||
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
||||
// declare a timeout if we have not fused velocity data for too long or not aiding
|
||||
@ -404,7 +404,7 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
|
||||
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
|
||||
// calculate the innovation consistency test ratio
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend._hgtInnovGate) * varInnovVelPos[5]);
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]);
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
||||
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms;
|
||||
|
||||
@ -53,7 +53,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
gpsDriftNE = min(gpsDriftNE,10.0f);
|
||||
// Fail if more than 3 metres drift after filtering whilst on-ground
|
||||
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
|
||||
bool gpsDriftFail = (gpsDriftNE > 3.0f) && onGround && (frontend._gpsCheck & MASK_GPS_POS_DRIFT);
|
||||
bool gpsDriftFail = (gpsDriftNE > 3.0f) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsDriftFail) {
|
||||
@ -71,8 +71,8 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
// 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) && (frontend._gpsCheck & MASK_GPS_VERT_SPD);
|
||||
} else if ((frontend._fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
|
||||
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
|
||||
} else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
|
||||
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
|
||||
gpsVertVelFail = true;
|
||||
} else {
|
||||
@ -95,7 +95,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
if (onGround) {
|
||||
gpsHorizVelFilt = 0.1f * pythagorous2(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
|
||||
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
|
||||
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_HORIZ_SPD);
|
||||
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
|
||||
} else {
|
||||
gpsHorizVelFail = false;
|
||||
}
|
||||
@ -114,7 +114,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
float hAcc = 0.0f;
|
||||
bool hAccFail;
|
||||
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
|
||||
hAccFail = (hAcc > 5.0f) && (frontend._gpsCheck & MASK_GPS_POS_ERR);
|
||||
hAccFail = (hAcc > 5.0f) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
|
||||
} else {
|
||||
hAccFail = false;
|
||||
}
|
||||
@ -130,7 +130,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
}
|
||||
|
||||
// fail if reported speed accuracy greater than threshold
|
||||
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend._gpsCheck & MASK_GPS_SPD_ERR);
|
||||
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsSpdAccFail) {
|
||||
@ -143,7 +143,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
}
|
||||
|
||||
// fail if satellite geometry is poor
|
||||
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend._gpsCheck & MASK_GPS_HDOP);
|
||||
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (hdopFail) {
|
||||
@ -155,7 +155,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
}
|
||||
|
||||
// fail if not enough sats
|
||||
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend._gpsCheck & MASK_GPS_NSATS);
|
||||
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (numSatsFail) {
|
||||
@ -169,7 +169,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||||
// fail if magnetometer innovations are outside limits indicating bad yaw
|
||||
// with bad yaw we are unable to use GPS
|
||||
bool yawFail;
|
||||
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend._gpsCheck & MASK_GPS_YAW_ERR)) {
|
||||
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
|
||||
yawFail = true;
|
||||
} else {
|
||||
yawFail = false;
|
||||
@ -358,7 +358,7 @@ void NavEKF2_core::detectFlight()
|
||||
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
|
||||
bool NavEKF2_core::getTakeoffExpected()
|
||||
{
|
||||
if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend.gndEffectTimeout_ms) {
|
||||
if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {
|
||||
expectGndEffectTakeoff = false;
|
||||
}
|
||||
|
||||
@ -377,7 +377,7 @@ void NavEKF2_core::setTakeoffExpected(bool val)
|
||||
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
|
||||
bool NavEKF2_core::getTouchdownExpected()
|
||||
{
|
||||
if (expectGndEffectTouchdown && imuSampleTime_ms - touchdownExpectedSet_ms > frontend.gndEffectTimeout_ms) {
|
||||
if (expectGndEffectTouchdown && imuSampleTime_ms - touchdownExpectedSet_ms > frontend->gndEffectTimeout_ms) {
|
||||
expectGndEffectTouchdown = false;
|
||||
}
|
||||
|
||||
|
||||
@ -26,11 +26,7 @@ extern const AP_HAL::HAL& hal;
|
||||
#define GYRO_BIAS_LIMIT 0.349066f
|
||||
|
||||
// constructor
|
||||
NavEKF2_core::NavEKF2_core(NavEKF2 &_frontend, const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
|
||||
frontend(_frontend),
|
||||
_ahrs(ahrs),
|
||||
_baro(baro),
|
||||
_rng(rng),
|
||||
NavEKF2_core::NavEKF2_core(void) :
|
||||
stateStruct(*reinterpret_cast<struct state_elements *>(&statesArray)),
|
||||
|
||||
//variables
|
||||
@ -58,6 +54,15 @@ NavEKF2_core::NavEKF2_core(NavEKF2 &_frontend, const AP_AHRS *ahrs, AP_Baro &bar
|
||||
_perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9");
|
||||
}
|
||||
|
||||
// setup this core backend
|
||||
void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index)
|
||||
{
|
||||
frontend = _frontend;
|
||||
imu_index = _imu_index;
|
||||
_ahrs = frontend->_ahrs;
|
||||
}
|
||||
|
||||
|
||||
/********************************************************
|
||||
* INIT FUNCTIONS *
|
||||
********************************************************/
|
||||
@ -220,7 +225,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||||
Vector3f initAccVec;
|
||||
|
||||
// TODO we should average accel readings over several cycles
|
||||
initAccVec = _ahrs->get_ins().get_accel();
|
||||
initAccVec = _ahrs->get_ins().get_accel(imu_index);
|
||||
|
||||
// read the magnetometer data
|
||||
readMagData();
|
||||
@ -276,7 +281,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||||
// set to true now that states have be initialised
|
||||
statesInitialised = true;
|
||||
|
||||
hal.console->printf("EKF2 initialised\n");
|
||||
hal.console->printf("EKF2 initialised on IMU %u\n", (unsigned)imu_index);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -303,7 +308,7 @@ void NavEKF2_core::CovarianceInit()
|
||||
// positions
|
||||
P[6][6] = sq(15.0f);
|
||||
P[7][7] = P[6][6];
|
||||
P[8][8] = sq(frontend._baroAltNoise);
|
||||
P[8][8] = sq(frontend->_baroAltNoise);
|
||||
// gyro delta angle biases
|
||||
P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtIMUavg));
|
||||
P[10][10] = P[9][9];
|
||||
@ -375,7 +380,7 @@ void NavEKF2_core::UpdateFilter()
|
||||
|
||||
// perform a covariance prediction if the total delta angle has exceeded the limit
|
||||
// or the time limit will be exceeded at the next IMU update
|
||||
if (((dt >= (frontend.covTimeStepMax - dtIMUavg)) || (summedDelAng.length() > frontend.covDelAngMax))) {
|
||||
if (((dt >= (frontend->covTimeStepMax - dtIMUavg)) || (summedDelAng.length() > frontend->covDelAngMax))) {
|
||||
CovariancePrediction();
|
||||
} else {
|
||||
covPredStep = false;
|
||||
@ -695,12 +700,12 @@ void NavEKF2_core::CovariancePrediction()
|
||||
|
||||
// use filtered height rate to increase wind process noise when climbing or descending
|
||||
// this allows for wind gradient effects.
|
||||
windVelSigma = dt * constrain_float(frontend._windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(frontend._wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
|
||||
dAngBiasSigma = dt * constrain_float(frontend._gyroBiasProcessNoise, 0.0f, 1e-4f);
|
||||
dVelBiasSigma = dt * constrain_float(frontend._accelBiasProcessNoise, 1e-6f, 1e-2f);
|
||||
dAngScaleSigma = dt * constrain_float(frontend._gyroScaleProcessNoise,1e-6f,1e-2f);
|
||||
magEarthSigma = dt * constrain_float(frontend._magProcessNoise, 1e-4f, 1e-1f);
|
||||
magBodySigma = dt * constrain_float(frontend._magProcessNoise, 1e-4f, 1e-1f);
|
||||
windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
|
||||
dAngBiasSigma = dt * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1e-4f);
|
||||
dVelBiasSigma = dt * constrain_float(frontend->_accelBiasProcessNoise, 1e-6f, 1e-2f);
|
||||
dAngScaleSigma = dt * constrain_float(frontend->_gyroScaleProcessNoise,1e-6f,1e-2f);
|
||||
magEarthSigma = dt * constrain_float(frontend->_magProcessNoise, 1e-4f, 1e-1f);
|
||||
magBodySigma = dt * constrain_float(frontend->_magProcessNoise, 1e-4f, 1e-1f);
|
||||
for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f;
|
||||
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
|
||||
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
|
||||
@ -734,9 +739,9 @@ void NavEKF2_core::CovariancePrediction()
|
||||
day_s = stateStruct.gyro_scale.y;
|
||||
daz_s = stateStruct.gyro_scale.z;
|
||||
dvz_b = stateStruct.accel_zbias;
|
||||
float _gyrNoise = constrain_float(frontend._gyrNoise, 1e-3f, 5e-2f);
|
||||
float _gyrNoise = constrain_float(frontend->_gyrNoise, 1e-3f, 5e-2f);
|
||||
daxNoise = dayNoise = dazNoise = dt*_gyrNoise;
|
||||
float _accNoise = constrain_float(frontend._accNoise, 5e-2f, 1.0f);
|
||||
float _accNoise = constrain_float(frontend->_accNoise, 5e-2f, 1.0f);
|
||||
dvxNoise = dvyNoise = dvzNoise = dt*_accNoise;
|
||||
|
||||
// calculate the predicted covariance due to inertial sensor error propagation
|
||||
|
||||
@ -62,8 +62,11 @@ class NavEKF2_core
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
NavEKF2_core(NavEKF2 &frontend, const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng);
|
||||
NavEKF2_core(void);
|
||||
|
||||
// setup this core backend
|
||||
void setup_core(NavEKF2 *_frontend, uint8_t _imu_index);
|
||||
|
||||
// Initialise the states from accelerometer and magnetometer data (if present)
|
||||
// This method can only be used when the vehicle is static
|
||||
bool InitialiseFilterBootstrap(void);
|
||||
@ -261,7 +264,8 @@ public:
|
||||
|
||||
private:
|
||||
// Reference to the global EKF frontend for parameters
|
||||
NavEKF2 &frontend;
|
||||
NavEKF2 *frontend;
|
||||
uint8_t imu_index;
|
||||
|
||||
typedef float ftype;
|
||||
#if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
|
||||
@ -314,8 +318,6 @@ private:
|
||||
#endif
|
||||
|
||||
const AP_AHRS *_ahrs;
|
||||
AP_Baro &_baro;
|
||||
const RangeFinder &_rng;
|
||||
|
||||
// the states are available in two forms, either as a Vector31, or
|
||||
// broken down as individual elements. Both are equivalent (same
|
||||
|
||||
Loading…
Reference in New Issue
Block a user