Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_NavEKF: add EKF initialized flag #13515

Merged
merged 5 commits into from
Feb 18, 2020
Merged

AP_NavEKF: add EKF initialized flag #13515

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1c36fcb
AP_NavEKF: add initalized flag and change to uint32_t
IamPete1 Feb 7, 2020
3973eca
AP_NavEKF2: add initalized flag and change to uint32_t
IamPete1 Feb 7, 2020
6e7f171
AP_NavEKF3: add initalized flag and change to uint32_t
IamPete1 Feb 7, 2020
e3085b7
AP_Logger: EKF4 change flags state from uint16_t to uint32_t
IamPete1 Feb 11, 2020
4815b2a
Mavlink: add initalized bit to EKF_STATUS_FLAGS
IamPete1 Feb 18, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions libraries/AP_Logger/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -570,7 +570,7 @@ struct PACKED log_NKF4 {
int8_t offsetEast;
uint16_t faults;
uint8_t timeouts;
uint16_t solution;
uint32_t solution;
uint16_t gps;
int8_t primary;
};
Expand Down Expand Up @@ -1409,7 +1409,7 @@ struct PACKED log_Arm_Disarm {
{ LOG_NKF3_MSG, sizeof(log_NKF3), \
"NKF3","QBcccccchhhcc","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "s#nnnmmmGGG??", "F-BBBBBBCCCBB" }, \
{ LOG_NKF4_MSG, sizeof(log_NKF4), \
"NKF4","QBcccccfbbHBHHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
"NKF4","QBcccccfbbHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
{ LOG_NKF5_MSG, sizeof(log_NKF5), \
"NKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s----m???mrnm", "F----BBBBB000" }, \
{ LOG_NKF10_MSG, sizeof(log_RngBcnDebug), \
Expand All @@ -1422,7 +1422,7 @@ struct PACKED log_Arm_Disarm {
{ LOG_XKF3_MSG, sizeof(log_NKF3), \
"XKF3","QBcccccchhhcc","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT", "s#nnnmmmGGG??", "F-BBBBBBCCCBB" }, \
{ LOG_XKF4_MSG, sizeof(log_NKF4), \
"XKF4","QBcccccfbbHBHHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
"XKF4","QBcccccfbbHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
{ LOG_XKF5_MSG, sizeof(log_NKF5), \
"XKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s----m???mrnm", "F----BBBBB000" }, \
{ LOG_XKF10_MSG, sizeof(log_RngBcnDebug), \
Expand Down
5 changes: 3 additions & 2 deletions libraries/AP_NavEKF/AP_Nav_Common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,12 @@ union nav_filter_status {
bool using_gps : 1; // 13 - true if we are using GPS position
bool gps_glitching : 1; // 14 - true if GPS glitching is affecting navigation accuracy
bool gps_quality_good : 1; // 15 - true if we can use GPS for navigation
bool initalized : 1; // 16 - true if the EKF has ever been healthy
} flags;
uint16_t value;
uint32_t value;
};

static_assert(sizeof(uint16_t) == sizeof(nav_filter_status), "nav_filter_status must be uint16_t");
static_assert(sizeof(uint32_t) == sizeof(nav_filter_status), "nav_filter_status must be uint32_t");
IamPete1 marked this conversation as resolved.
Show resolved Hide resolved

union nav_gps_status {
struct {
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -521,5 +521,6 @@ void NavEKF2_core::updateFilterStatus(void)
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign;
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why the || healthy()?

Copy link
Member Author

@IamPete1 IamPete1 Feb 8, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The thought was to make it initialized as soon as it goes healthy the first time

}

2 changes: 1 addition & 1 deletion libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ void NavEKF2::Log_Write_NKF4(uint8_t _core, uint64_t time_us) const
offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value),
solution : (uint32_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex
};
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -577,6 +577,9 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) const
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}

// get variances
float velVar, posVar, hgtVar, tasVar;
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -563,5 +563,6 @@ void NavEKF3_core::updateFilterStatus(void)
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3); // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign;
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
}

2 changes: 1 addition & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ void NavEKF3::Log_Write_XKF4(uint8_t _core, uint64_t time_us) const
offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value),
solution : (uint32_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex
};
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -585,6 +585,9 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
if (filterStatus.flags.gps_glitching) {
flags |= (1<<15);
}
Expand Down
2 changes: 1 addition & 1 deletion modules/mavlink
Open in desktop
Submodule mavlink updated 1 files
+3 −0 message_definitions/v1.0/ardupilotmega.xml