diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 6b0b48c2ca..704745829e 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -102,6 +102,7 @@ public: void Log_Write_POS(AP_AHRS &ahrs); #if AP_AHRS_NAVEKF_AVAILABLE void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled); + void Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled); #endif bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd); void Log_Write_Radio(const mavlink_radio_t &packet); @@ -781,6 +782,18 @@ Format characters in the format string for binary log messages "EKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \ { LOG_EKF4_MSG, sizeof(log_EKF4), \ "EKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \ + { LOG_EKF5_MSG, sizeof(log_EKF5), \ + "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ + { LOG_NKF1_MSG, sizeof(log_EKF1), \ + "NKF1","QccCffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \ + { LOG_NKF2_MSG, sizeof(log_EKF2), \ + "NKF2","Qbbbcchhhhhh","TimeUS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \ + { LOG_NKF3_MSG, sizeof(log_EKF3), \ + "NKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \ + { LOG_NKF4_MSG, sizeof(log_EKF4), \ + "NKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \ + { LOG_NKF5_MSG, sizeof(log_EKF5), \ + "NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \ { LOG_UBX1_MSG, sizeof(log_Ubx1), \ @@ -813,8 +826,6 @@ Format characters in the format string for binary log messages "ESC7", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC8_MSG, sizeof(log_Esc), \ "ESC8", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ - { LOG_EKF5_MSG, sizeof(log_EKF5), \ - "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ { LOG_COMPASS2_MSG, sizeof(log_Compass), \ "MAG2","QhhhhhhhhhB", "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ { LOG_COMPASS3_MSG, sizeof(log_Compass), \ @@ -938,6 +949,11 @@ enum LogMessages { LOG_GPA2_MSG, LOG_RFND_MSG, LOG_BAR3_MSG, + LOG_NKF1_MSG, + LOG_NKF2_MSG, + LOG_NKF3_MSG, + LOG_NKF4_MSG, + LOG_NKF5_MSG, }; enum LogOriginType { diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 65d65f49ad..14a25b49b8 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1274,6 +1274,154 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) }; WriteBlock(&pkt5, sizeof(pkt5)); } + + // do EKF2 as well if enabled + if (ahrs.get_NavEKF2().enabled()) { + Log_Write_EKF2(ahrs, optFlowEnabled); + } +} + + +void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) +{ + // Write first EKF packet + Vector3f euler; + Vector3f posNED; + Vector3f velNED; + Vector3f dAngBias; + Vector3f dVelBias; + Vector3f gyroBias; + ahrs.get_NavEKF2().getEulerAngles(euler); + ahrs.get_NavEKF2().getVelNED(velNED); + ahrs.get_NavEKF2().getPosNED(posNED); + ahrs.get_NavEKF2().getGyroBias(gyroBias); + struct log_EKF1 pkt = { + LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG), + time_us : hal.scheduler->micros64(), + 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) + posN : (float)(posNED.x), // metres North + posE : (float)(posNED.y), // metres East + posD : (float)(posNED.z), // 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 + }; + WriteBlock(&pkt, sizeof(pkt)); + + // Write second EKF packet + float ratio; + float az1bias, az2bias; + Vector3f wind; + Vector3f magNED; + Vector3f magXYZ; + ahrs.get_NavEKF2().getIMU1Weighting(ratio); + ahrs.get_NavEKF2().getAccelZBias(az1bias, az2bias); + ahrs.get_NavEKF2().getWind(wind); + ahrs.get_NavEKF2().getMagNED(magNED); + ahrs.get_NavEKF2().getMagXYZ(magXYZ); + struct log_EKF2 pkt2 = { + LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG), + time_us : hal.scheduler->micros64(), + Ratio : (int8_t)(100*ratio), + AZ1bias : (int8_t)(100*az1bias), + AZ2bias : (int8_t)(100*az2bias), + 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) + }; + WriteBlock(&pkt2, sizeof(pkt2)); + + // Write third EKF packet + Vector3f velInnov; + Vector3f posInnov; + Vector3f magInnov; + float tasInnov; + float yawInnov; + ahrs.get_NavEKF2().getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); + struct log_EKF3 pkt3 = { + LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG), + time_us : hal.scheduler->micros64(), + 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), + innovVT : (int16_t)(100*tasInnov) + }; + WriteBlock(&pkt3, sizeof(pkt3)); + + // Write fourth EKF packet + float velVar; + float posVar; + float hgtVar; + Vector3f magVar; + float tasVar; + Vector2f offset; + uint8_t faultStatus, timeoutStatus; + nav_filter_status solutionStatus; + ahrs.get_NavEKF2().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + ahrs.get_NavEKF2().getFilterFaults(faultStatus); + ahrs.get_NavEKF2().getFilterTimeouts(timeoutStatus); + ahrs.get_NavEKF2().getFilterStatus(solutionStatus); + struct log_EKF4 pkt4 = { + LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG), + time_us : hal.scheduler->micros64(), + sqrtvarV : (int16_t)(100*velVar), + sqrtvarP : (int16_t)(100*posVar), + sqrtvarH : (int16_t)(100*hgtVar), + sqrtvarMX : (int16_t)(100*magVar.x), + sqrtvarMY : (int16_t)(100*magVar.y), + sqrtvarMZ : (int16_t)(100*magVar.z), + sqrtvarVT : (int16_t)(100*tasVar), + offsetNorth : (int8_t)(offset.x), + offsetEast : (int8_t)(offset.y), + faults : (uint8_t)(faultStatus), + timeouts : (uint8_t)(timeoutStatus), + solution : (uint16_t)(solutionStatus.value) + }; + WriteBlock(&pkt4, sizeof(pkt4)); + + + // Write fifth EKF packet + if (optFlowEnabled) { + float normInnov; // normalised innovation variance ratio for optical flow observations fused by the main nav filter + float gndOffset; // estimated vertical position of the terrain relative to the nav filter zero datum + float flowInnovX, flowInnovY; // optical flow LOS rate vector innovations from the main nav filter + float auxFlowInnov; // optical flow LOS rate innovation from terrain offset estimator + float HAGL; // height above ground level + float rngInnov; // range finder innovations + float range; // measured range + float gndOffsetErr; // filter ground offset state error + ahrs.get_NavEKF2().getFlowDebug(normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr); + struct log_EKF5 pkt5 = { + LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG), + time_us : hal.scheduler->micros64(), + 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) + }; + WriteBlock(&pkt5, sizeof(pkt5)); + } } #endif