123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630 |
- #include <AP_HAL/AP_HAL.h>
- #include "AP_NavEKF3.h"
- #include "AP_NavEKF3_core.h"
- #include <AP_AHRS/AP_AHRS.h>
- #include <AP_Vehicle/AP_Vehicle.h>
- #include <AP_GPS/AP_GPS.h>
- extern const AP_HAL::HAL& hal;
- bool NavEKF3_core::healthy(void) const
- {
- uint16_t faultInt;
- getFilterFaults(faultInt);
- if (faultInt > 0) {
- return false;
- }
- if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
-
-
- return false;
- }
-
- if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
- return false;
- }
-
- float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
- if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
- return false;
- }
-
- return true;
- }
- float NavEKF3_core::errorScore() const
- {
- float score = 0.0f;
- if (tiltAlignComplete && yawAlignComplete) {
-
- score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
-
- score = MAX(score, hgtTestRatio);
- }
- return score;
- }
- void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
- {
- varFlow = MAX(flowTestRatio[0],flowTestRatio[1]);
- gndOffset = terrainState;
- flowInnovX = innovOptFlow[0];
- flowInnovY = innovOptFlow[1];
- auxInnov = norm(auxFlowObsInnov.x,auxFlowObsInnov.y);
- HAGL = terrainState - stateStruct.position.z;
- rngInnov = innovRng;
- range = rangeDataDelayed.rng;
- gndOffsetErr = sqrtf(Popt);
- }
- uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
- {
- velInnov.x = innovBodyVel[0];
- velInnov.y = innovBodyVel[1];
- velInnov.z = innovBodyVel[2];
- velInnovVar.x = varInnovBodyVel[0];
- velInnovVar.y = varInnovBodyVel[1];
- velInnovVar.z = varInnovBodyVel[2];
- return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
- }
- bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
- float &offsetHigh, float &offsetLow, Vector3f &posNED)
- {
-
- if (!statesInitialised || N_beacons == 0) {
- return false;
- }
-
- if (rngBcnFuseDataReportIndex >= N_beacons) {
- rngBcnFuseDataReportIndex = 0;
- }
-
- ID = rngBcnFuseDataReportIndex;
- rng = rngBcnFusionReport[rngBcnFuseDataReportIndex].rng;
- innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov;
- innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar;
- testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio;
- beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED;
- offsetHigh = bcnPosDownOffsetMax;
- offsetLow = bcnPosDownOffsetMin;
- posNED = receiverPos;
- rngBcnFuseDataReportIndex++;
- return true;
- }
- bool NavEKF3_core::getHeightControlLimit(float &height) const
- {
-
- if (frontend->_fusionModeGPS == 3) {
-
- height = MAX(float(frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f);
-
- if (frontend->_altSource != 1) {
- height -= terrainState;
- }
- return true;
- } else {
- return false;
- }
- }
- void NavEKF3_core::getEulerAngles(Vector3f &euler) const
- {
- outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
- euler = euler - _ahrs->get_trim();
- }
- void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
- {
- if (dtEkfAvg < 1e-6f) {
- gyroBias.zero();
- return;
- }
- gyroBias = stateStruct.gyro_bias / dtEkfAvg;
- }
- void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
- {
- if (!statesInitialised) {
- accelBias.zero();
- return;
- }
- accelBias = stateStruct.accel_bias / dtEkfAvg;
- }
- void NavEKF3_core::getTiltError(float &ang) const
- {
- ang = stateStruct.quat.length();
- }
- void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
- {
- outputDataNew.quat.rotation_matrix(mat);
- mat = mat * _ahrs->get_rotation_vehicle_body_to_autopilot_body();
- }
- void NavEKF3_core::getQuaternion(Quaternion& ret) const
- {
- ret = outputDataNew.quat;
- }
- uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const
- {
- yawAng = yawResetAngle;
- return lastYawReset_ms;
- }
- uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const
- {
- pos = posResetNE;
- return lastPosReset_ms;
- }
- uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const
- {
- posD = posResetD;
- return lastPosResetD_ms;
- }
- uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
- {
- vel = velResetNE;
- return lastVelReset_ms;
- }
- void NavEKF3_core::getWind(Vector3f &wind) const
- {
- wind.x = stateStruct.wind_vel.x;
- wind.y = stateStruct.wind_vel.y;
- wind.z = 0.0f;
- }
- void NavEKF3_core::getVelNED(Vector3f &vel) const
- {
-
- vel = outputDataNew.velocity + velOffsetNED;
- }
- float NavEKF3_core::getPosDownDerivative(void) const
- {
-
-
- return posDownDerivative + velOffsetNED.z;
- }
- void NavEKF3_core::getAccelNED(Vector3f &accelNED) const {
- accelNED = velDotNED;
- accelNED.z -= GRAVITY_MSS;
- }
- bool NavEKF3_core::getPosNE(Vector2f &posNE) const
- {
-
- if (PV_AidingMode != AID_NONE) {
-
-
- posNE.x = outputDataNew.position.x + posOffsetNED.x;
- posNE.y = outputDataNew.position.y + posOffsetNED.y;
- return true;
- } else {
-
- if(validOrigin) {
- if ((AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
-
- const struct Location &gpsloc = AP::gps().location();
- const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
- posNE.x = tempPosNE.x;
- posNE.y = tempPosNE.y;
- return false;
- } else if (rngBcnAlignmentStarted) {
-
- posNE.x = receiverPos.x;
- posNE.y = receiverPos.y;
- return false;
- } else {
-
- posNE.x = outputDataNew.position.x;
- posNE.y = outputDataNew.position.y;
- return false;
- }
- } else {
-
- posNE.x = 0.0f;
- posNE.y = 0.0f;
- return false;
- }
- }
- return false;
- }
- bool NavEKF3_core::getPosD(float &posD) const
- {
-
-
-
- if ((frontend->_originHgtMode & (1<<2)) == 0) {
-
- posD = outputDataNew.position.z + posOffsetNED.z;
- } else {
-
-
- posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
- }
-
- return filterStatus.flags.vert_pos;
- }
- bool NavEKF3_core::getHAGL(float &HAGL) const
- {
- HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
-
- return !hgtTimeout && gndOffsetValid && healthy();
- }
- bool NavEKF3_core::getLLH(struct Location &loc) const
- {
- const AP_GPS &gps = AP::gps();
- Location origin;
- float posD;
- if(getPosD(posD) && getOriginLLH(origin)) {
-
- loc.alt = origin.alt - posD*100;
- loc.relative_alt = 0;
- loc.terrain_alt = 0;
-
- if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
- loc.lat = EKF_origin.lat;
- loc.lng = EKF_origin.lng;
- loc.offset(outputDataNew.position.x, outputDataNew.position.y);
- return true;
- } else {
-
-
- if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) {
-
- const struct Location &gpsloc = gps.location();
- loc.lat = gpsloc.lat;
- loc.lng = gpsloc.lng;
- return true;
- } else {
-
- loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y);
- return false;
- }
- }
- } else {
-
-
- if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) {
- const struct Location &gpsloc = gps.location();
- loc = gpsloc;
- loc.relative_alt = 0;
- loc.terrain_alt = 0;
- }
- return false;
- }
- }
- void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
- {
-
- bool relyingOnFlowData = (imuSampleTime_ms - prevBodyVelFuseTime_ms > 1000)
- && (imuSampleTime_ms - flowValidMeaTime_ms <= 10000);
-
-
- if (PV_AidingMode == AID_RELATIVE && relyingOnFlowData) {
-
- ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd);
-
- ekfNavVelGainScaler = 4.0f / MAX((terrainState - stateStruct.position[2]),4.0f);
- } else {
- ekfGndSpdLimit = 400.0f;
- ekfNavVelGainScaler = 1.0f;
- }
- }
- bool NavEKF3_core::getOriginLLH(struct Location &loc) const
- {
- if (validOrigin) {
- loc = EKF_origin;
-
- if ((frontend->_originHgtMode & (1<<2)) == 0) {
- loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);
- }
- }
- return validOrigin;
- }
- void NavEKF3_core::getMagNED(Vector3f &magNED) const
- {
- magNED = stateStruct.earth_magfield * 1000.0f;
- }
- void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
- {
- magXYZ = stateStruct.body_magfield*1000.0f;
- }
- bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
- {
- if (!_ahrs->get_compass()) {
- return false;
- }
-
-
- const float maxMagVar = 5E-6f;
- bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
- if ((mag_idx == magSelectIndex) &&
- finalInflightMagInit &&
- !inhibitMagStates &&
- _ahrs->get_compass()->healthy(magSelectIndex) &&
- variancesConverged) {
- magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
- return true;
- } else {
- magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
- return false;
- }
- }
- uint8_t NavEKF3_core::getActiveMag() const
- {
- return (uint8_t)magSelectIndex;
- }
- void NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
- {
- velInnov.x = innovVelPos[0];
- velInnov.y = innovVelPos[1];
- velInnov.z = innovVelPos[2];
- posInnov.x = innovVelPos[3];
- posInnov.y = innovVelPos[4];
- posInnov.z = innovVelPos[5];
- magInnov.x = 1e3f*innovMag[0];
- magInnov.y = 1e3f*innovMag[1];
- magInnov.z = 1e3f*innovMag[2];
- tasInnov = innovVtas;
- yawInnov = innovYaw;
- }
- 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);
-
- 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;
- }
- void NavEKF3_core::getStateVariances(float stateVar[24])
- {
- for (uint8_t i=0; i<24; i++) {
- stateVar[i] = P[i][i];
- }
- }
- void NavEKF3_core::getFilterFaults(uint16_t &faults) const
- {
- faults = (stateStruct.quat.is_nan()<<0 |
- stateStruct.velocity.is_nan()<<1 |
- faultStatus.bad_xmag<<2 |
- faultStatus.bad_ymag<<3 |
- faultStatus.bad_zmag<<4 |
- faultStatus.bad_airspeed<<5 |
- faultStatus.bad_sideslip<<6 |
- !statesInitialised<<7);
- }
- void NavEKF3_core::getFilterTimeouts(uint8_t &timeouts) const
- {
- timeouts = (posTimeout<<0 |
- velTimeout<<1 |
- hgtTimeout<<2 |
- magTimeout<<3 |
- tasTimeout<<4);
- }
- void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
- {
- status = filterStatus;
- }
- void NavEKF3_core::getFilterGpsStatus(nav_gps_status &faults) const
- {
-
- faults.value = 0;
-
- faults.flags.bad_sAcc = gpsCheckStatus.bad_sAcc;
- faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc;
- faults.flags.bad_vAcc = gpsCheckStatus.bad_vAcc;
- faults.flags.bad_yaw = gpsCheckStatus.bad_yaw;
- faults.flags.bad_sats = gpsCheckStatus.bad_sats;
- faults.flags.bad_horiz_drift = gpsCheckStatus.bad_horiz_drift;
- faults.flags.bad_hdop = gpsCheckStatus.bad_hdop;
- faults.flags.bad_vert_vel = gpsCheckStatus.bad_vert_vel;
- faults.flags.bad_fix = gpsCheckStatus.bad_fix;
- faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel;
- }
- void NavEKF3_core::send_status_report(mavlink_channel_t chan)
- {
-
- uint16_t flags = 0;
- if (filterStatus.flags.attitude) {
- flags |= EKF_ATTITUDE;
- }
- if (filterStatus.flags.horiz_vel) {
- flags |= EKF_VELOCITY_HORIZ;
- }
- if (filterStatus.flags.vert_vel) {
- flags |= EKF_VELOCITY_VERT;
- }
- if (filterStatus.flags.horiz_pos_rel) {
- flags |= EKF_POS_HORIZ_REL;
- }
- if (filterStatus.flags.horiz_pos_abs) {
- flags |= EKF_POS_HORIZ_ABS;
- }
- if (filterStatus.flags.vert_pos) {
- flags |= EKF_POS_VERT_ABS;
- }
- if (filterStatus.flags.terrain_alt) {
- flags |= EKF_POS_VERT_AGL;
- }
- if (filterStatus.flags.const_pos_mode) {
- flags |= EKF_CONST_POS_MODE;
- }
- if (filterStatus.flags.pred_horiz_pos_rel) {
- flags |= EKF_PRED_POS_HORIZ_REL;
- }
- if (filterStatus.flags.pred_horiz_pos_abs) {
- flags |= EKF_PRED_POS_HORIZ_ABS;
- }
- if (filterStatus.flags.gps_glitching) {
- flags |= (1<<15);
- }
-
- float velVar, posVar, hgtVar, tasVar;
- Vector3f magVar;
- Vector2f offset;
- getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
-
-
-
- float temp;
- if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
- temp = sqrtf(auxRngTestRatio);
- } else {
- temp = 0.0f;
- }
-
- mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp, tasVar);
- }
- const char *NavEKF3_core::prearm_failure_reason(void) const
- {
- if (gpsGoodToAlign) {
-
- return nullptr;
- }
- return prearm_fail_string;
- }
- uint8_t NavEKF3_core::getFramesSincePredict(void) const
- {
- return framesSincePredict;
- }
- void NavEKF3_core::getOutputTrackingError(Vector3f &error) const
- {
- error = outputTrackError;
- }
|