diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 193d7812f2..2441738b8a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -26,6 +26,7 @@ #include #include #include +#include class NavEKF3_core; class AP_AHRS; @@ -523,4 +524,15 @@ private: // new_primary - index of the ekf instance that we are about to switch to as the primary // old_primary - index of the ekf instance that we are currently using as the primary void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary); + + // logging functions shared by cores: + void Log_Write_EKF1(uint8_t core, LogMessages msg_id, uint64_t time_us) const; + void Log_Write_NKF2a(uint8_t core, LogMessages msg_id, uint64_t time_us) const; + void Log_Write_NKF3(uint8_t core, LogMessages msg_id, uint64_t time_us) const; + void Log_Write_NKF4(uint8_t core, LogMessages msg_id, uint64_t time_us) const; + void Log_Write_NKF5(uint64_t time_us) const; + void Log_Write_Quaternion(uint8_t core, LogMessages msg_id, uint64_t time_us) const; + void Log_Write_Beacon(uint64_t time_us) const; + void Log_Write_BodyOdom(uint64_t time_us) const; + void Log_Write_State_Variances(uint64_t time_us) const; }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 8b914bf9e8..480187a43d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -3,16 +3,8 @@ #include #include -void NavEKF3::Log_Write() +void NavEKF3::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us) const { - // only log if enabled - if (activeCores() <= 0) { - return; - } - - AP_Logger &logger = AP::logger(); - - uint64_t time_us = AP_HAL::micros64(); // Write first EKF packet Vector3f euler; Vector2f posNE; @@ -21,17 +13,17 @@ void NavEKF3::Log_Write() Vector3f gyroBias; float posDownDeriv; Location originLLH; - getEulerAngles(0,euler); - getVelNED(0,velNED); - getPosNE(0,posNE); - getPosD(0,posD); - getGyroBias(0,gyroBias); - posDownDeriv = getPosDownDerivative(0); - if (!getOriginLLH(0,originLLH)) { + getEulerAngles(_core,euler); + getVelNED(_core,velNED); + getPosNE(_core,posNE); + getPosD(_core,posD); + getGyroBias(_core,gyroBias); + posDownDeriv = getPosDownDerivative(_core); + if (!getOriginLLH(_core,originLLH)) { originLLH.alt = 0; } - struct log_EKF1 pkt = { - LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG), + const struct log_EKF1 pkt{ + LOG_PACKET_HEADER_INIT(msg_id), time_us : time_us, 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) @@ -48,20 +40,23 @@ void NavEKF3::Log_Write() gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm }; - logger.WriteBlock(&pkt, sizeof(pkt)); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} +void NavEKF3::Log_Write_NKF2a(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +{ // Write second EKF packet Vector3f accelBias; Vector3f wind; Vector3f magNED; Vector3f magXYZ; - uint8_t magIndex = getActiveMag(0); - getAccelBias(0,accelBias); - getWind(0,wind); - getMagNED(0,magNED); - getMagXYZ(0,magXYZ); - struct log_NKF2a pkt2 = { - LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG), + uint8_t magIndex = getActiveMag(_core); + getAccelBias(_core,accelBias); + getWind(_core,wind); + getMagNED(_core,magNED); + getMagXYZ(_core,magXYZ); + const struct log_NKF2a pkt2{ + LOG_PACKET_HEADER_INIT(msg_id), time_us : time_us, accBiasX : (int16_t)(100*accelBias.x), accBiasY : (int16_t)(100*accelBias.y), @@ -76,17 +71,20 @@ void NavEKF3::Log_Write() magZ : (int16_t)(magXYZ.z), index : (uint8_t)(magIndex) }; - logger.WriteBlock(&pkt2, sizeof(pkt2)); + AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); +} +void NavEKF3::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +{ // Write third EKF packet Vector3f velInnov; Vector3f posInnov; Vector3f magInnov; float tasInnov = 0; float yawInnov = 0; - getInnovations(0,velInnov, posInnov, magInnov, tasInnov, yawInnov); - struct log_NKF3 pkt3 = { - LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG), + getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov); + const struct log_NKF3 pkt3{ + LOG_PACKET_HEADER_INIT(msg_id), time_us : time_us, innovVN : (int16_t)(100*velInnov.x), innovVE : (int16_t)(100*velInnov.y), @@ -100,8 +98,11 @@ void NavEKF3::Log_Write() innovYaw : (int16_t)(100*degrees(yawInnov)), innovVT : (int16_t)(100*tasInnov) }; - logger.WriteBlock(&pkt3, sizeof(pkt3)); + AP::logger().WriteBlock(&pkt3, sizeof(pkt3)); +} +void NavEKF3::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +{ // Write fourth EKF packet float velVar = 0; float posVar = 0; @@ -113,17 +114,17 @@ void NavEKF3::Log_Write() uint8_t timeoutStatus=0; nav_filter_status solutionStatus {}; nav_gps_status gpsStatus {}; - getVariances(0,velVar, posVar, hgtVar, magVar, tasVar, offset); + getVariances(_core,velVar, posVar, hgtVar, magVar, tasVar, offset); float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); - getFilterFaults(0,faultStatus); - getFilterTimeouts(0,timeoutStatus); - getFilterStatus(0,solutionStatus); - getFilterGpsStatus(0,gpsStatus); + getFilterFaults(_core,faultStatus); + getFilterTimeouts(_core,timeoutStatus); + getFilterStatus(_core,solutionStatus); + getFilterGpsStatus(_core,gpsStatus); float tiltError; - getTiltError(0,tiltError); + getTiltError(_core,tiltError); uint8_t primaryIndex = getPrimaryCoreIndex(); - struct log_NKF4 pkt4 = { - LOG_PACKET_HEADER_INIT(LOG_XKF4_MSG), + const struct log_NKF4 pkt4{ + LOG_PACKET_HEADER_INIT(msg_id), time_us : time_us, sqrtvarV : (int16_t)(100*velVar), sqrtvarP : (int16_t)(100*posVar), @@ -139,8 +140,12 @@ void NavEKF3::Log_Write() gps : (uint16_t)(gpsStatus.value), primary : (int8_t)primaryIndex }; - logger.WriteBlock(&pkt4, sizeof(pkt4)); + AP::logger().WriteBlock(&pkt4, sizeof(pkt4)); +} + +void NavEKF3::Log_Write_NKF5(uint64_t time_us) const +{ // Write fifth EKF packet - take data from the primary instance 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 @@ -153,7 +158,7 @@ void NavEKF3::Log_Write() Vector3f predictorErrors; // output predictor angle, velocity and position tracking error getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr); getOutputTrackingError(-1,predictorErrors); - struct log_NKF5 pkt5 = { + const struct log_NKF5 pkt5{ LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG), time_us : time_us, normInnov : (uint8_t)(MIN(100*normInnov,255)), @@ -169,135 +174,27 @@ void NavEKF3::Log_Write() velErr : (float)predictorErrors.y, posErr : (float)predictorErrors.z }; - logger.WriteBlock(&pkt5, sizeof(pkt5)); + AP::logger().WriteBlock(&pkt5, sizeof(pkt5)); +} +void NavEKF3::Log_Write_Quaternion(uint8_t _core, LogMessages msg_id, uint64_t time_us) const +{ // log quaternion Quaternion quat; - getQuaternion(0, quat); - struct log_Quaternion pktq1 = { - LOG_PACKET_HEADER_INIT(LOG_XKQ1_MSG), + getQuaternion(_core, quat); + const struct log_Quaternion pktq1{ + LOG_PACKET_HEADER_INIT(msg_id), time_us : time_us, q1 : quat.q1, q2 : quat.q2, q3 : quat.q3, q4 : quat.q4 }; - logger.WriteBlock(&pktq1, sizeof(pktq1)); - - // log innovations for the second IMU if enabled - if (activeCores() >= 2) { - // Write 6th EKF packet - getEulerAngles(1,euler); - getVelNED(1,velNED); - getPosNE(1,posNE); - getPosD(1,posD); - getGyroBias(1,gyroBias); - posDownDeriv = getPosDownDerivative(1); - if (!getOriginLLH(1,originLLH)) { - originLLH.alt = 0; - } - struct log_EKF1 pkt6 = { - LOG_PACKET_HEADER_INIT(LOG_XKF6_MSG), - time_us : time_us, - 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) - velN : (float)(velNED.x), // velocity North (m/s) - velE : (float)(velNED.y), // velocity East (m/s) - velD : (float)(velNED.z), // velocity Down (m/s) - posD_dot : (float)(posDownDeriv), // first derivative of down position - posN : (float)(posNE.x), // metres North - posE : (float)(posNE.y), // metres East - posD : (float)(posD), // metres Down - gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string - gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string - gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string - originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm - }; - logger.WriteBlock(&pkt6, sizeof(pkt6)); + AP::logger().WriteBlock(&pktq1, sizeof(pktq1)); +} - // Write 7th EKF packet - getAccelBias(1,accelBias); - getWind(1,wind); - getMagNED(1,magNED); - getMagXYZ(1,magXYZ); - magIndex = getActiveMag(1); - struct log_NKF2a pkt7 = { - LOG_PACKET_HEADER_INIT(LOG_XKF7_MSG), - time_us : time_us, - accBiasX : (int16_t)(100*accelBias.x), - accBiasY : (int16_t)(100*accelBias.y), - accBiasZ : (int16_t)(100*accelBias.z), - windN : (int16_t)(100*wind.x), - windE : (int16_t)(100*wind.y), - magN : (int16_t)(magNED.x), - magE : (int16_t)(magNED.y), - magD : (int16_t)(magNED.z), - magX : (int16_t)(magXYZ.x), - magY : (int16_t)(magXYZ.y), - magZ : (int16_t)(magXYZ.z), - index : (uint8_t)(magIndex) - }; - logger.WriteBlock(&pkt7, sizeof(pkt7)); - - // Write 8th EKF packet - getInnovations(1,velInnov, posInnov, magInnov, tasInnov, yawInnov); - struct log_NKF3 pkt8 = { - LOG_PACKET_HEADER_INIT(LOG_XKF8_MSG), - time_us : time_us, - innovVN : (int16_t)(100*velInnov.x), - innovVE : (int16_t)(100*velInnov.y), - innovVD : (int16_t)(100*velInnov.z), - innovPN : (int16_t)(100*posInnov.x), - innovPE : (int16_t)(100*posInnov.y), - innovPD : (int16_t)(100*posInnov.z), - innovMX : (int16_t)(magInnov.x), - innovMY : (int16_t)(magInnov.y), - innovMZ : (int16_t)(magInnov.z), - innovYaw : (int16_t)(100*degrees(yawInnov)), - innovVT : (int16_t)(100*tasInnov) - }; - logger.WriteBlock(&pkt8, sizeof(pkt8)); - - // Write 9th EKF packet - getVariances(1,velVar, posVar, hgtVar, magVar, tasVar, offset); - tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); - getFilterFaults(1,faultStatus); - getFilterTimeouts(1,timeoutStatus); - getFilterStatus(1,solutionStatus); - getFilterGpsStatus(1,gpsStatus); - getTiltError(1,tiltError); - struct log_NKF4 pkt9 = { - LOG_PACKET_HEADER_INIT(LOG_XKF9_MSG), - time_us : time_us, - 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, - offsetNorth : (int8_t)(offset.x), - offsetEast : (int8_t)(offset.y), - faults : (uint16_t)(faultStatus), - timeouts : (uint8_t)(timeoutStatus), - solution : (uint16_t)(solutionStatus.value), - gps : (uint16_t)(gpsStatus.value), - primary : (int8_t)primaryIndex - }; - logger.WriteBlock(&pkt9, sizeof(pkt9)); - - // log quaternion - getQuaternion(1, quat); - struct log_Quaternion pktq2 = { - LOG_PACKET_HEADER_INIT(LOG_XKQ2_MSG), - time_us : time_us, - q1 : quat.q1, - q2 : quat.q2, - q3 : quat.q3, - q4 : quat.q4 - }; - logger.WriteBlock(&pktq2, sizeof(pktq2)); - } +void NavEKF3::Log_Write_Beacon(uint64_t time_us) const +{ // write range beacon fusion debug packet if the range value is non-zero uint8_t ID; float rng; @@ -310,7 +207,7 @@ void NavEKF3::Log_Write() Vector3f posNED; if (getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow, posNED)) { if (rng > 0.0f) { - struct log_RngBcnDebug pkt10 = { + const struct log_RngBcnDebug pkt10{ LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG), time_us : time_us, ID : (uint8_t)ID, @@ -328,15 +225,18 @@ void NavEKF3::Log_Write() posD : (int16_t)(100*posNED.z) }; - logger.WriteBlock(&pkt10, sizeof(pkt10)); + AP::logger().WriteBlock(&pkt10, sizeof(pkt10)); } } - // write debug data for body frame odometry fusion +} + +void NavEKF3::Log_Write_BodyOdom(uint64_t time_us) const +{ Vector3f velBodyInnov,velBodyInnovVar; static uint32_t lastUpdateTime_ms = 0; uint32_t updateTime_ms = getBodyFrameOdomDebug(-1, velBodyInnov, velBodyInnovVar); if (updateTime_ms > lastUpdateTime_ms) { - struct log_ekfBodyOdomDebug pkt11 = { + const struct log_ekfBodyOdomDebug pkt11{ LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG), time_us : time_us, velInnovX : velBodyInnov.x, @@ -346,17 +246,19 @@ void NavEKF3::Log_Write() velInnovVarY : velBodyInnovVar.y, velInnovVarZ : velBodyInnovVar.z }; - logger.WriteBlock(&pkt11, sizeof(pkt11)); + AP::logger().WriteBlock(&pkt11, sizeof(pkt11)); lastUpdateTime_ms = updateTime_ms; } +} - // log state variances every 0.49s +void NavEKF3::Log_Write_State_Variances(uint64_t time_us) const +{ static uint32_t lastEkfStateVarLogTime_ms = 0; if (AP_HAL::millis() - lastEkfStateVarLogTime_ms > 490) { lastEkfStateVarLogTime_ms = AP_HAL::millis(); float stateVar[24]; getStateVariances(-1, stateVar); - struct log_ekfStateVar pktv1 = { + const struct log_ekfStateVar pktv1{ LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG), time_us : time_us, v00 : stateVar[0], @@ -372,8 +274,8 @@ void NavEKF3::Log_Write() v10 : stateVar[10], v11 : stateVar[11] }; - logger.WriteBlock(&pktv1, sizeof(pktv1)); - struct log_ekfStateVar pktv2 = { + AP::logger().WriteBlock(&pktv1, sizeof(pktv1)); + const struct log_ekfStateVar pktv2{ LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG), time_us : time_us, v00 : stateVar[12], @@ -389,9 +291,43 @@ void NavEKF3::Log_Write() v10 : stateVar[22], v11 : stateVar[23] }; - logger.WriteBlock(&pktv2, sizeof(pktv2)); + AP::logger().WriteBlock(&pktv2, sizeof(pktv2)); + } +} + +void NavEKF3::Log_Write() +{ + // only log if enabled + if (activeCores() <= 0) { + return; } + uint64_t time_us = AP_HAL::micros64(); + + Log_Write_EKF1(0, LOG_XKF1_MSG, time_us); + Log_Write_NKF2a(0, LOG_XKF2_MSG, time_us); + Log_Write_NKF3(0, LOG_XKF3_MSG, time_us); + Log_Write_NKF4(0, LOG_XKF4_MSG, time_us); + Log_Write_NKF5(time_us); + Log_Write_Quaternion(0, LOG_XKQ1_MSG, time_us); + + // log innovations for the second IMU if enabled + if (activeCores() >= 2) { + Log_Write_EKF1(1, LOG_XKF6_MSG, time_us); + Log_Write_NKF2a(1, LOG_XKF7_MSG, time_us); + Log_Write_NKF3(1, LOG_XKF8_MSG, time_us); + Log_Write_NKF4(1, LOG_XKF9_MSG, time_us); + Log_Write_Quaternion(1, LOG_XKQ2_MSG, time_us); + } + + // write range beacon fusion debug packet if the range value is non-zero + Log_Write_Beacon(time_us); + + // write debug data for body frame odometry fusion + Log_Write_BodyOdom(time_us); + + // log state variances every 0.49s + Log_Write_State_Variances(time_us); // log EKF timing statistics every 5s static uint32_t lastTimingLogTime_ms = 0; @@ -400,7 +336,7 @@ void NavEKF3::Log_Write() struct ekf_timing timing; for (uint8_t i=0; i