zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

DataFlash: Add EKF3 and remove EKF1

master
Andrew Tridgell 9 years ago
parent
75688c8342
commit
b00ec26957
3 changed files with 349 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/DataFlash/DataFlash.h
  2. 300
      libraries/DataFlash/LogFile.cpp
  3. 47
      libraries/DataFlash/LogStructure.h

2
libraries/DataFlash/DataFlash.h
Unescape View File

@ -123,7 +123,9 @@ public: @@ -123,7 +123,9 @@ public:
void Log_Write_AHRS2(AP_AHRS &ahrs);
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);
void Log_Write_EKF3(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);

300
libraries/DataFlash/LogFile.cpp
Unescape View File

@ -1122,6 +1122,18 @@ void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs) @@ -1122,6 +1122,18 @@ void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs)
}
#if AP_AHRS_NAVEKF_AVAILABLE
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{
// only log EKF2 if enabled
if (ahrs.get_NavEKF2().activeCores() > 0) {
Log_Write_EKF2(ahrs, optFlowEnabled);
}
// only log EKF3 if enabled
if (ahrs.get_NavEKF3().activeCores() > 0) {
Log_Write_EKF3(ahrs, optFlowEnabled);
}
}
void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{
uint64_t time_us = AP_HAL::micros64();
@ -1415,6 +1427,294 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) @@ -1415,6 +1427,294 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
}
}
}
void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{
uint64_t time_us = AP_HAL::micros64();
// Write first EKF packet
Vector3f euler;
Vector2f posNE;
float posD;
Vector3f velNED;
Vector3f dAngBias;
Vector3f dVelBias;
Vector3f gyroBias;
float posDownDeriv;
ahrs.get_NavEKF3().getEulerAngles(0,euler);
ahrs.get_NavEKF3().getVelNED(0,velNED);
ahrs.get_NavEKF3().getPosNE(0,posNE);
ahrs.get_NavEKF3().getPosD(0,posD);
ahrs.get_NavEKF3().getGyroBias(0,gyroBias);
posDownDeriv = ahrs.get_NavEKF3().getPosDownDerivative(0);
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_XKF1_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
};
WriteBlock(&pkt, sizeof(pkt));
// Write second EKF packet
Vector3f accelBias;
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
uint8_t magIndex = ahrs.get_NavEKF3().getActiveMag(0);
ahrs.get_NavEKF3().getAccelBias(0,accelBias);
ahrs.get_NavEKF3().getWind(0,wind);
ahrs.get_NavEKF3().getMagNED(0,magNED);
ahrs.get_NavEKF3().getMagXYZ(0,magXYZ);
struct log_NKF2a pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_XKF2_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)
};
WriteBlock(&pkt2, sizeof(pkt2));
// Write third EKF packet
Vector3f velInnov;
Vector3f posInnov;
Vector3f magInnov;
float tasInnov = 0;
float yawInnov = 0;
ahrs.get_NavEKF3().getInnovations(0,velInnov, posInnov, magInnov, tasInnov, yawInnov);
struct log_NKF3 pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_XKF3_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)
};
WriteBlock(&pkt3, sizeof(pkt3));
// Write fourth EKF packet
float velVar = 0;
float posVar = 0;
float hgtVar = 0;
Vector3f magVar;
float tasVar = 0;
Vector2f offset;
uint16_t faultStatus=0;
uint8_t timeoutStatus=0;
nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {};
ahrs.get_NavEKF3().getVariances(0,velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
ahrs.get_NavEKF3().getFilterFaults(0,faultStatus);
ahrs.get_NavEKF3().getFilterTimeouts(0,timeoutStatus);
ahrs.get_NavEKF3().getFilterStatus(0,solutionStatus);
ahrs.get_NavEKF3().getFilterGpsStatus(0,gpsStatus);
float tiltError;
ahrs.get_NavEKF3().getTiltError(0,tiltError);
uint8_t primaryIndex = ahrs.get_NavEKF3().getPrimaryCoreIndex();
struct log_NKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_XKF4_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
};
WriteBlock(&pkt4, sizeof(pkt4));
// 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
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
ahrs.get_NavEKF3().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
ahrs.get_NavEKF3().getOutputTrackingError(-1,predictorErrors);
struct log_NKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG),
time_us : time_us,
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
};
WriteBlock(&pkt5, sizeof(pkt5));
// log innovations for the second IMU if enabled
if (ahrs.get_NavEKF3().activeCores() >= 2) {
// Write 6th EKF packet
ahrs.get_NavEKF3().getEulerAngles(1,euler);
ahrs.get_NavEKF3().getVelNED(1,velNED);
ahrs.get_NavEKF3().getPosNE(1,posNE);
ahrs.get_NavEKF3().getPosD(1,posD);
ahrs.get_NavEKF3().getGyroBias(1,gyroBias);
posDownDeriv = ahrs.get_NavEKF3().getPosDownDerivative(1);
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
};
WriteBlock(&pkt6, sizeof(pkt6));
// Write 7th EKF packet
ahrs.get_NavEKF3().getAccelBias(1,accelBias);
ahrs.get_NavEKF3().getWind(1,wind);
ahrs.get_NavEKF3().getMagNED(1,magNED);
ahrs.get_NavEKF3().getMagXYZ(1,magXYZ);
magIndex = ahrs.get_NavEKF3().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)
};
WriteBlock(&pkt7, sizeof(pkt7));
// Write 8th EKF packet
ahrs.get_NavEKF3().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)
};
WriteBlock(&pkt8, sizeof(pkt8));
// Write 9th EKF packet
ahrs.get_NavEKF3().getVariances(1,velVar, posVar, hgtVar, magVar, tasVar, offset);
tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
ahrs.get_NavEKF3().getFilterFaults(1,faultStatus);
ahrs.get_NavEKF3().getFilterTimeouts(1,timeoutStatus);
ahrs.get_NavEKF3().getFilterStatus(1,solutionStatus);
ahrs.get_NavEKF3().getFilterGpsStatus(1,gpsStatus);
ahrs.get_NavEKF3().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
};
WriteBlock(&pkt9, sizeof(pkt9));
}
// 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;
if (ahrs.get_NavEKF3().getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow)) {
if (rng > 0.0f) {
struct log_RngBcnDebug pkt10 = {
LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG),
time_us : time_us,
ID : (uint8_t)ID,
rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)),
testRatio : (uint16_t)(100*testRatio),
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)
};
WriteBlock(&pkt10, sizeof(pkt10));
}
}
}
#endif
// Write a command processing packet

47
libraries/DataFlash/LogStructure.h
Unescape View File

@ -273,6 +273,23 @@ struct PACKED log_NKF2 { @@ -273,6 +273,23 @@ struct PACKED log_NKF2 {
uint8_t index;
};
struct PACKED log_NKF2a {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t accBiasX;
int16_t accBiasY;
int16_t accBiasZ;
int16_t windN;
int16_t windE;
int16_t magN;
int16_t magE;
int16_t magD;
int16_t magX;
int16_t magY;
int16_t magZ;
uint8_t index;
};
struct PACKED log_EKF3 {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -838,6 +855,26 @@ Format characters in the format string for binary log messages @@ -838,6 +855,26 @@ Format characters in the format string for binary log messages
"NKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
{ LOG_NKF10_MSG, sizeof(log_RngBcnDebug), \
"NKF0","QBccCCccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL" }, \
{ LOG_XKF1_MSG, sizeof(log_EKF1), \
"XKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_XKF2_MSG, sizeof(log_NKF2a), \
"XKF2","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_XKF3_MSG, sizeof(log_NKF3), \
"XKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ LOG_XKF4_MSG, sizeof(log_NKF4), \
"XKF4","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
{ LOG_XKF5_MSG, sizeof(log_NKF5), \
"XKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos" }, \
{ LOG_XKF6_MSG, sizeof(log_EKF1), \
"XKF6","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_XKF7_MSG, sizeof(log_NKF2a), \
"XKF7","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_XKF8_MSG, sizeof(log_NKF3), \
"XKF8","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ LOG_XKF9_MSG, sizeof(log_NKF4), \
"XKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
{ LOG_XKF10_MSG, sizeof(log_RngBcnDebug), \
"XKF0","QBccCCccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
@ -1018,6 +1055,16 @@ enum LogMessages { @@ -1018,6 +1055,16 @@ enum LogMessages {
LOG_NKF8_MSG,
LOG_NKF9_MSG,
LOG_NKF10_MSG,
LOG_XKF1_MSG,
LOG_XKF2_MSG,
LOG_XKF3_MSG,
LOG_XKF4_MSG,
LOG_XKF5_MSG,
LOG_XKF6_MSG,
LOG_XKF7_MSG,
LOG_XKF8_MSG,
LOG_XKF9_MSG,
LOG_XKF10_MSG,
LOG_DF_MAV_STATS,
LOG_MSG_SBPHEALTH,

Write Preview
Loading…
Cancel
Save