diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index ba9bf7d1d4..0d207f1605 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1239,39 +1239,6 @@ void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const } } -// return the magnetometer in use for the specified instance -uint8_t NavEKF3::getActiveMag(int8_t instance) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - return core[instance].getActiveMag(); - } else { - return 255; - } -} - -// return the baro in use for the specified instance -uint8_t NavEKF3::getActiveBaro(int8_t instance) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - return core[instance].getActiveBaro(); - } else { - return 255; - } -} - -// return the GPS in use for the specified instance -uint8_t NavEKF3::getActiveGPS(int8_t instance) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - return core[instance].getActiveGPS(); - } else { - return 255; - } -} - // return the airspeed sensor in use for the specified instance uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const { @@ -1408,15 +1375,6 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI } } -// publish output observer angular, velocity and position tracking error -void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - core[instance].getOutputTrackingError(error); - } -} - // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { @@ -1426,15 +1384,6 @@ void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float } } -// return the diagonals from the covariance matrix for the specified instance -void NavEKF3::getStateVariances(int8_t instance, float stateVar[24]) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - core[instance].getStateVariances(stateVar); - } -} - // get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance // returns true on success and results are placed in innovations and variances arguments bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const @@ -1538,15 +1487,6 @@ void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt } // return data for debugging optical flow fusion -void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, - float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - core[instance].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr); - } -} - /* * Write body frame linear and angular displacement measurements from a visual odometry sensor * @@ -1603,19 +1543,6 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec return ret; } -// return data for debugging EKF-GSF yaw estimator -// return false if data not available -bool NavEKF3::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - if (core[instance].getDataEKFGSF(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight)) { - return true; - } - } - return false; -} - // parameter conversion of EKF3 parameters void NavEKF3::convert_parameters() { @@ -1717,18 +1644,6 @@ void NavEKF3::convert_parameters() } } -// return data for debugging range beacon fusion -bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, - float &offsetHigh, float &offsetLow, Vector3f &posNED) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow, posNED); - } else { - return false; - } -} - // called by vehicle code to specify that a takeoff is happening // causes the EKF to compensate for expected barometer errors due to rotor wash ground interaction // causes the EKF to start the EKF-GSF yaw estimator @@ -2073,21 +1988,6 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_ } -/* - get timing statistics structure -*/ -void NavEKF3::getTimingStatistics(int8_t instance, struct ekf_timing &timing) const -{ - if (instance < 0 || instance >= num_cores) { - instance = primary; - } - if (core) { - core[instance].getTimingStatistics(timing); - } else { - memset(&timing, 0, sizeof(timing)); - } -} - // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. void NavEKF3::writeDefaultAirSpeed(float airspeed) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index b0a7216a81..36dca597c3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -141,9 +141,6 @@ public: // return the sensor in use for the specified instance // An out of range instance (eg -1) returns data for the primary instance - uint8_t getActiveMag(int8_t instance) const; - uint8_t getActiveBaro(int8_t instance) const; - uint8_t getActiveGPS(int8_t instance) const; uint8_t getActiveAirspeed(int8_t instance) const; // Return estimated magnetometer offsets @@ -189,16 +186,10 @@ public: // An out of range instance (eg -1) returns data for the primary instance void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; - // publish output observer angular, velocity and position tracking error - void getOutputTrackingError(int8_t instance, Vector3f &error) const; - // return the innovation consistency test ratios for the specified instance // An out of range instance (eg -1) returns data for the primary instance void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; - // return the diagonals from the covariance matrix for the specified instance - void getStateVariances(int8_t instance, float stateVar[24]) const; - // get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance // returns true on success and results are placed in innovations and variances arguments bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED; @@ -251,27 +242,6 @@ public: */ uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const; - // return data for debugging optical flow fusion for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; - - /* - Returns the following data for debugging range beacon fusion - ID : beacon identifier - rng : measured range to beacon (m) - innov : range innovation (m) - innovVar : innovation variance (m^2) - testRatio : innovation consistency test ratio - beaconPosNED : beacon NED position (m) - offsetHigh : high hypothesis for range beacons system vertical offset (m) - offsetLow : low hypothesis for range beacons system vertical offset (m) - posNED : North,East,Down position estimate of receiver from 3-state filter - - returns true if data could be found, false if it could not - */ - bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, - float &offsetHigh, float &offsetLow, Vector3f &posNED) const; - /* * Writes the measurement from a yaw angle sensor * @@ -395,9 +365,6 @@ public: // allow the enable flag to be set by Replay void set_enable(bool enable) { _enable.set_enable(enable); } - // get timing statistics structure - void getTimingStatistics(int8_t instance, struct ekf_timing &timing) const; - /* check if switching lanes will reduce the normalised innovations. This is called when the vehicle code is about to @@ -424,10 +391,6 @@ public: // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed); - // log debug data for yaw estimator - // return false if data not available - bool getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; - // parameter conversion void convert_parameters(); @@ -611,20 +574,6 @@ private: // checks for alignment bool coreBetterScore(uint8_t new_core, uint8_t current_core) const; - // logging functions shared by cores: - void Log_Write_XKF1(uint8_t core, uint64_t time_us) const; - void Log_Write_XKF2(uint8_t core, uint64_t time_us) const; - void Log_Write_XKF3(uint8_t core, uint64_t time_us) const; - void Log_Write_XKF4(uint8_t core, uint64_t time_us) const; - void Log_Write_XKF5(uint8_t core, uint64_t time_us) const; - void Log_Write_XKFS(uint8_t core, uint64_t time_us) const; - void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const; - void Log_Write_Beacon(uint8_t core, uint64_t time_us) const; - void Log_Write_BodyOdom(uint8_t core, uint64_t time_us) const; - void Log_Write_State_Variances(uint8_t core, uint64_t time_us) const; - void Log_Write_Timing(uint8_t core, uint64_t time_us) const; - void Log_Write_GSF(uint8_t core, uint64_t time_us) const; - // position, velocity and yaw source control AP_NavEKF_Source sources; }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index d14f1802d4..82945d2e76 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -1,11 +1,12 @@ #include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" #include #include #include -void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const { // Write first EKF packet Vector3f euler; @@ -15,19 +16,19 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const Vector3f gyroBias; float posDownDeriv; Location originLLH; - getEulerAngles(_core,euler); - getVelNED(_core,velNED); - getPosNE(_core,posNE); - getPosD(_core,posD); - getGyroBias(_core,gyroBias); - posDownDeriv = getPosDownDerivative(_core); - if (!getOriginLLH(_core,originLLH)) { + getEulerAngles(euler); + getVelNED(velNED); + getPosNE(posNE); + getPosD(posD); + getGyroBias(gyroBias); + posDownDeriv = getPosDownDerivative(); + if (!getOriginLLH(originLLH)) { originLLH.alt = 0; } const struct log_EKF1 pkt{ LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG), time_us : time_us, - core : DAL_CORE(_core), + core : DAL_CORE(core_index), roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string) yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string) @@ -46,21 +47,21 @@ void NavEKF3::Log_Write_XKF1(uint8_t _core, uint64_t time_us) const AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_XKF2(uint64_t time_us) const { // Write second EKF packet Vector3f accelBias; Vector3f wind; Vector3f magNED; Vector3f magXYZ; - getAccelBias(_core,accelBias); - getWind(_core,wind); - getMagNED(_core,magNED); - getMagXYZ(_core,magXYZ); + getAccelBias(accelBias); + getWind(wind); + getMagNED(magNED); + getMagXYZ(magXYZ); const struct log_XKF2 pkt2{ LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG), time_us : time_us, - core : DAL_CORE(_core), + core : DAL_CORE(core_index), accBiasX : (int16_t)(100*accelBias.x), accBiasY : (int16_t)(100*accelBias.y), accBiasZ : (int16_t)(100*accelBias.z), @@ -76,26 +77,22 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } -void NavEKF3::Log_Write_XKFS(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const { // Write sensor selection EKF packet - uint8_t magIndex = getActiveMag(_core); - uint8_t baroIndex = getActiveBaro(_core); - uint8_t GPSIndex = getActiveGPS(_core); - uint8_t airspeedIndex = getActiveAirspeed(_core); const struct log_EKFS pkt { LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG), time_us : time_us, - core : DAL_CORE(_core), - mag_index : (uint8_t)(magIndex), - baro_index : (uint8_t)(baroIndex), - gps_index : (uint8_t)(GPSIndex), - airspeed_index : (uint8_t)(airspeedIndex), + core : DAL_CORE(core_index), + mag_index : magSelectIndex, + baro_index : selected_baro, + gps_index : selected_gps, + airspeed_index : getActiveAirspeed() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_XKF3(uint64_t time_us) const { // Write third EKF packet Vector3f velInnov; @@ -103,11 +100,11 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const Vector3f magInnov; float tasInnov = 0; float yawInnov = 0; - getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov); + getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); const struct log_NKF3 pkt3{ LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG), time_us : time_us, - core : DAL_CORE(_core), + core : DAL_CORE(core_index), innovVN : (int16_t)(100*velInnov.x), innovVE : (int16_t)(100*velInnov.y), innovVD : (int16_t)(100*velInnov.z), @@ -119,13 +116,13 @@ void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const innovMZ : (int16_t)(magInnov.z), innovYaw : (int16_t)(100*degrees(yawInnov)), innovVT : (int16_t)(100*tasInnov), - rerr : coreRelativeErrors[_core], - errorScore : coreErrorScores[_core] + rerr : frontend->coreRelativeErrors[core_index], + errorScore : frontend->coreErrorScores[core_index] }; AP::logger().WriteBlock(&pkt3, sizeof(pkt3)); } -void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const { // Write fourth EKF packet float velVar = 0; @@ -133,90 +130,77 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const float hgtVar = 0; Vector3f magVar; float tasVar = 0; + uint16_t _faultStatus=0; Vector2f offset; - uint16_t faultStatus=0; uint8_t timeoutStatus=0; nav_filter_status solutionStatus {}; nav_gps_status gpsStatus {}; - getVariances(_core,velVar, posVar, hgtVar, magVar, tasVar, offset); + getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); - getFilterFaults(_core,faultStatus); - getFilterTimeouts(_core,timeoutStatus); - getFilterStatus(_core,solutionStatus); - getFilterGpsStatus(_core,gpsStatus); + getFilterFaults(_faultStatus); + getFilterTimeouts(timeoutStatus); + getFilterStatus(solutionStatus); + getFilterGpsStatus(gpsStatus); float tiltError; - getTiltError(_core,tiltError); - uint8_t primaryIndex = getPrimaryCoreIndex(); + getTiltError(tiltError); const struct log_NKF4 pkt4{ LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG), time_us : time_us, - core : DAL_CORE(_core), + core : DAL_CORE(core_index), sqrtvarV : (int16_t)(100*velVar), sqrtvarP : (int16_t)(100*posVar), sqrtvarH : (int16_t)(100*hgtVar), sqrtvarM : (int16_t)(100*tempVar), sqrtvarVT : (int16_t)(100*tasVar), - tiltErr : (float)tiltError, + tiltErr : tiltError, offsetNorth : (int8_t)(offset.x), offsetEast : (int8_t)(offset.y), - faults : (uint16_t)(faultStatus), - timeouts : (uint8_t)(timeoutStatus), - solution : (uint32_t)(solutionStatus.value), - gps : (uint16_t)(gpsStatus.value), - primary : (int8_t)primaryIndex + faults : _faultStatus, + timeouts : timeoutStatus, + solution : solutionStatus.value, + gps : gpsStatus.value, + primary : frontend->getPrimaryCoreIndex() }; AP::logger().WriteBlock(&pkt4, sizeof(pkt4)); } -void NavEKF3::Log_Write_XKF5(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const { - if (_core != primary) { + if (core_index != frontend->primary) { // log only primary instance for now return; } - // Write fifth EKF packet - float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter - float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum - float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter - float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator - float HAGL=0; // height above ground level - float rngInnov=0; // range finder innovations - float range=0; // measured range - float gndOffsetErr=0; // filter ground offset state error - Vector3f predictorErrors; // output predictor angle, velocity and position tracking error - getFlowDebug(_core, normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr); - getOutputTrackingError(_core, predictorErrors); const struct log_NKF5 pkt5{ LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG), time_us : time_us, - core : DAL_CORE(_core), - normInnov : (uint8_t)(MIN(100*normInnov,255)), - FIX : (int16_t)(1000*flowInnovX), - FIY : (int16_t)(1000*flowInnovY), - AFI : (int16_t)(1000*auxFlowInnov), - HAGL : (int16_t)(100*HAGL), - offset : (int16_t)(100*gndOffset), - RI : (int16_t)(100*rngInnov), - meaRng : (uint16_t)(100*range), - errHAGL : (uint16_t)(100*gndOffsetErr), - angErr : (float)predictorErrors.x, - velErr : (float)predictorErrors.y, - posErr : (float)predictorErrors.z + core : DAL_CORE(core_index), + normInnov : (uint8_t)(MIN(100*MAX(flowTestRatio[0],flowTestRatio[1]),255)), // normalised innovation variance ratio for optical flow observations fused by the main nav filter + FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter + FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter + AFI : (int16_t)(1000*norm(auxFlowObsInnov.x,auxFlowObsInnov.y)), // optical flow LOS rate innovation from terrain offset estimator + HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)), // height above ground level + 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() + 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 }; AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); } -void NavEKF3::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_Quaternion(uint64_t time_us) const { // log quaternion Quaternion quat; - getQuaternion(_core, quat); + getQuaternion( quat); const struct log_Quaternion pktq1{ LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG), time_us : time_us, - core : DAL_CORE(_core), + core : DAL_CORE(core_index), q1 : quat.q1, q2 : quat.q2, q3 : quat.q3, @@ -225,64 +209,68 @@ void NavEKF3::Log_Write_Quaternion(uint8_t _core, uint64_t time_us) const AP::logger().WriteBlock(&pktq1, sizeof(pktq1)); } -void NavEKF3::Log_Write_Beacon(uint8_t _core, uint64_t time_us) const +// logs beacon information, one beacon per call +void NavEKF3_core::Log_Write_Beacon(uint64_t time_us) { - if (_core != primary) { + if (core_index != frontend->primary) { // log only primary instance for now return; } + if (!statesInitialised || N_beacons == 0) { + return; + } + + // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates + if (rngBcnFuseDataReportIndex >= N_beacons) { + rngBcnFuseDataReportIndex = 0; + } + + const rngBcnFusionReport_t &report = rngBcnFusionReport[rngBcnFuseDataReportIndex]; + // write range beacon fusion debug packet if the range value is non-zero - uint8_t ID; - float rng; - float innovVar; - float innov; - float testRatio; - Vector3f beaconPosNED; - float bcnPosOffsetHigh; - float bcnPosOffsetLow; - Vector3f posNED; - if (getRangeBeaconDebug(_core, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow, posNED)) { - if (rng > 0.0f) { - const struct log_RngBcnDebug pkt10{ - LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG), - time_us : time_us, - core : DAL_CORE(_core), - ID : (uint8_t)ID, - rng : (int16_t)(100*rng), - innov : (int16_t)(100*innov), - sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)), - testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)), - beaconPosN : (int16_t)(100*beaconPosNED.x), - beaconPosE : (int16_t)(100*beaconPosNED.y), - beaconPosD : (int16_t)(100*beaconPosNED.z), - offsetHigh : (int16_t)(100*bcnPosOffsetHigh), - offsetLow : (int16_t)(100*bcnPosOffsetLow), - posN : (int16_t)(100*posNED.x), - posE : (int16_t)(100*posNED.y), - posD : (int16_t)(100*posNED.z) - - }; - AP::logger().WriteBlock(&pkt10, sizeof(pkt10)); - } + if (report.rng <= 0.0f) { + rngBcnFuseDataReportIndex++; + return; } + + const struct log_RngBcnDebug pkt10{ + LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG), + time_us : time_us, + core : DAL_CORE(core_index), + 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)), + beaconPosN : (int16_t)(100*report.beaconPosNED.x), + beaconPosE : (int16_t)(100*report.beaconPosNED.y), + beaconPosD : (int16_t)(100*report.beaconPosNED.z), + offsetHigh : (int16_t)(100*bcnPosDownOffsetMax), + offsetLow : (int16_t)(100*bcnPosDownOffsetMin), + posN : (int16_t)(100*receiverPos.x), + posE : (int16_t)(100*receiverPos.y), + posD : (int16_t)(100*receiverPos.z) + }; + AP::logger().WriteBlock(&pkt10, sizeof(pkt10)); + rngBcnFuseDataReportIndex++; } -void NavEKF3::Log_Write_BodyOdom(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us) { - if (_core != primary) { + if (core_index != frontend->primary) { // log only primary instance for now return; } Vector3f velBodyInnov,velBodyInnovVar; static uint32_t lastUpdateTime_ms = 0; - uint32_t updateTime_ms = getBodyFrameOdomDebug(_core, velBodyInnov, velBodyInnovVar); + uint32_t updateTime_ms = getBodyFrameOdomDebug( velBodyInnov, velBodyInnovVar); if (updateTime_ms > lastUpdateTime_ms) { const struct log_ekfBodyOdomDebug pkt11{ LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG), time_us : time_us, - core : DAL_CORE(_core), + core : DAL_CORE(core_index), velInnovX : velBodyInnov.x, velInnovY : velBodyInnov.y, velInnovZ : velBodyInnov.z, @@ -295,9 +283,9 @@ void NavEKF3::Log_Write_BodyOdom(uint8_t _core, uint64_t time_us) const } } -void NavEKF3::Log_Write_State_Variances(uint8_t _core, uint64_t time_us) const +void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const { - if (_core != primary) { + if (core_index != frontend->primary) { // log only primary instance for now return; } @@ -305,42 +293,40 @@ void NavEKF3::Log_Write_State_Variances(uint8_t _core, uint64_t time_us) const static uint32_t lastEkfStateVarLogTime_ms = 0; if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) { lastEkfStateVarLogTime_ms = AP::dal().millis(); - float stateVar[24]; - getStateVariances(_core, stateVar); const struct log_ekfStateVar pktv1{ LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG), time_us : time_us, - core : DAL_CORE(_core), - v00 : stateVar[0], - v01 : stateVar[1], - v02 : stateVar[2], - v03 : stateVar[3], - v04 : stateVar[4], - v05 : stateVar[5], - v06 : stateVar[6], - v07 : stateVar[7], - v08 : stateVar[8], - v09 : stateVar[9], - v10 : stateVar[10], - v11 : stateVar[11] + core : DAL_CORE(core_index), + v00 : P[0][0], + v01 : P[1][1], + v02 : P[2][2], + v03 : P[3][3], + v04 : P[4][4], + v05 : P[5][5], + v06 : P[6][6], + v07 : P[7][7], + v08 : P[8][8], + v09 : P[9][9], + v10 : P[10][10], + v11 : P[11][11] }; AP::logger().WriteBlock(&pktv1, sizeof(pktv1)); const struct log_ekfStateVar pktv2{ LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG), time_us : time_us, - core : DAL_CORE(_core), - v00 : stateVar[12], - v01 : stateVar[13], - v02 : stateVar[14], - v03 : stateVar[15], - v04 : stateVar[16], - v05 : stateVar[17], - v06 : stateVar[18], - v07 : stateVar[19], - v08 : stateVar[20], - v09 : stateVar[21], - v10 : stateVar[22], - v11 : stateVar[23] + core : DAL_CORE(core_index), + v00 : P[12][12], + v01 : P[13][13], + v02 : P[14][14], + v03 : P[15][15], + v04 : P[16][16], + v05 : P[17][17], + v06 : P[18][18], + v07 : P[19][19], + v08 : P[20][20], + v09 : P[21][21], + v10 : P[22][22], + v11 : P[23][23] }; AP::logger().WriteBlock(&pktv2, sizeof(pktv2)); } @@ -360,35 +346,40 @@ void NavEKF3::Log_Write() uint64_t time_us = AP::dal().micros64(); - // note that several of these functions exit-early if they're not - // attempting to log the primary core. for (uint8_t i=0; igetLogData(yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) { + return; + } // @LoggerMessage: XKY0 // @Description: EKF3 Yaw Estimator States @@ -497,5 +493,4 @@ void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const ive[2], ive[3], ive[4]); - } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 31a0839014..2e609b96a5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1175,13 +1175,6 @@ void NavEKF3_core::updateTimingStatistics(void) timing.count++; } -// get timing statistics structure -void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing) -{ - _timing = timing; - memset(&timing, 0, sizeof(timing)); -} - /* update estimates of inactive bias states. This keeps inactive IMUs as hot-spares so we can switch to them without causing a jump in the diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 439e8656a3..27a3218121 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -59,20 +59,6 @@ float NavEKF3_core::errorScore() const return score; } -// return data for debugging optical flow fusion -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); // note Popt is constrained to be non-negative in EstimateTerrainOffset() -} - // return data for debugging body frame odometry fusion uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar) { @@ -85,34 +71,6 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms); } -// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call -bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, - float &offsetHigh, float &offsetLow, Vector3f &posNED) -{ - // if the states have not been initialised or we have not received any beacon updates then return zeros - if (!statesInitialised || N_beacons == 0) { - return false; - } - - // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates - if (rngBcnFuseDataReportIndex >= N_beacons) { - rngBcnFuseDataReportIndex = 0; - } - - // Output the fusion status data for the specified beacon - ID = rngBcnFuseDataReportIndex; // beacon identifier - rng = rngBcnFusionReport[rngBcnFuseDataReportIndex].rng; // measured range to beacon (m) - innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m) - innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2) - testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio - beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon receiver NED position (m) - offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate (m) - offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate (m) - posNED = receiverPos; // beacon system NED offset (m) - rngBcnFuseDataReportIndex++; - return true; -} - // provides the height limit to be observed by the control loops // returns false if no height limiting is required // this is needed to ensure the vehicle does not fly too high when using optical flow navigation @@ -450,23 +408,6 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const } // return the index for the active magnetometer -uint8_t NavEKF3_core::getActiveMag() const -{ - return (uint8_t)magSelectIndex; -} - -// return the index for the active barometer -uint8_t NavEKF3_core::getActiveBaro() const -{ - return (uint8_t)selected_baro; -} - -// return the index for the active GPS -uint8_t NavEKF3_core::getActiveGPS() const -{ - return (uint8_t)selected_gps; -} - // return the index for the active airspeed uint8_t NavEKF3_core::getActiveAirspeed() const { @@ -505,14 +446,6 @@ void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve offset = posResetNE; } -// return the diagonals from the covariance matrix -void NavEKF3_core::getStateVariances(float stateVar[24]) -{ - for (uint8_t i=0; i<24; i++) { - stateVar[i] = P[i][i]; - } -} - // get a particular source's velocity innovations // returns true on success and results are placed in innovations and variances arguments bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const @@ -693,17 +626,3 @@ uint8_t NavEKF3_core::getFramesSincePredict(void) const { return framesSincePredict; } - -// publish output observer angular, velocity and position tracking error -void NavEKF3_core::getOutputTrackingError(Vector3f &error) const -{ - error = outputTrackError; -} - -bool NavEKF3_core::getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) -{ - if (yawEstimator != nullptr) { - return yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight); - } - return false; -} diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index ab1327cd7d..7f4e7f1074 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -163,9 +163,6 @@ public: void getMagXYZ(Vector3f &magXYZ) const; // return the index for the active sensors - uint8_t getActiveMag() const; - uint8_t getActiveBaro() const; - uint8_t getActiveGPS() const; uint8_t getActiveAirspeed() const; // Return estimated magnetometer offsets @@ -208,9 +205,6 @@ public: // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; - // return the diagonals from the covariance matrix - void getStateVariances(float stateVar[24]); - // get a particular source's velocity innovations // returns true on success and results are placed in innovations and variances arguments bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED; @@ -228,9 +222,6 @@ public: // posOffset is the XYZ flow sensor position in the body frame in m void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); - // return data for debugging optical flow fusion - void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; - /* * Write body frame linear and angular displacement measurements from a visual odometry sensor * @@ -265,23 +256,6 @@ public: */ uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar); - /* - Returns the following data for debugging range beacon fusion - ID : beacon identifier - rng : measured range to beacon (m) - innov : range innovation (m) - innovVar : innovation variance (m^2) - testRatio : innovation consistency test ratio - beaconPosNED : beacon NED position (m) - offsetHigh : high hypothesis for range beacons system vertical offset (m) - offsetLow : low hypothesis for range beacons system vertical offset (m) - posNED : North,East,Down position estimate of receiver from 3-state filter - - returns true if data could be found, false if it could not - */ - bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, - float &offsetHigh, float &offsetLow, Vector3f &posNED); - /* * Writes the measurement from a yaw angle sensor * @@ -406,16 +380,10 @@ public: // this is used by other instances to level load uint8_t getFramesSincePredict(void) const; - // publish output observer angular, velocity and position tracking error - void getOutputTrackingError(Vector3f &error) const; - // get the IMU index. For now we return the gyro index, as that is most // critical for use by other subsystems. uint8_t getIMUIndex(void) const { return gyro_index_active; } - // get timing statistics structure - void getTimingStatistics(struct ekf_timing &timing); - // values for EK3_MAG_CAL enum class MagCal { WHEN_FLYING = 0, @@ -429,10 +397,6 @@ public: // are we using an external yaw source? This is needed by AHRS attitudes_consistent check bool using_external_yaw(void) const; - - // get solution data for the EKF-GSF emergency yaw estimator - // return false if data not available - bool getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed); @@ -449,7 +413,9 @@ public: bool have_aligned_yaw(void) const { return yawAlignComplete; } - + + void Log_Write(uint64_t time_us); + private: EKFGSF_yaw *yawEstimator; AP_DAL &dal; @@ -1291,7 +1257,7 @@ private: // Range Beacon Fusion Debug Reporting uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported - struct { + struct rngBcnFusionReport_t { float rng; // measured range to beacon (m) float innov; // range innovation (m) float innovVar; // innovation variance (m^2) @@ -1483,4 +1449,18 @@ private: bool posxy_source_reset; // true when the horizontal position source has changed but the position has not yet been reset AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change) bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset + + // logging functions shared by cores: + void Log_Write_XKF1(uint64_t time_us) const; + void Log_Write_XKF2(uint64_t time_us) const; + void Log_Write_XKF3(uint64_t time_us) const; + void Log_Write_XKF4(uint64_t time_us) const; + void Log_Write_XKF5(uint64_t time_us) const; + void Log_Write_XKFS(uint64_t time_us) const; + void Log_Write_Quaternion(uint64_t time_us) const; + void Log_Write_Beacon(uint64_t time_us); + void Log_Write_BodyOdom(uint64_t time_us); + void Log_Write_State_Variances(uint64_t time_us) const; + void Log_Write_Timing(uint64_t time_us); + void Log_Write_GSF(uint64_t time_us); };