ArduRover FW: 4.5.7
Board Cubepilot Orange+
There is no GPS on this rover, there is only LidarOdometry LIVOX MID360 which is running on Raspberry Pi 5 and sending VISION_POSITION_ESTIMATE to Cube by SERIAL2 port.
The rover icon display on Mission Planner map normally once set EKF home. The rover position is showing correctly on the map when moving in manual. But after a while during moving, the warning message “Unhealthy AHRS” occurs, rover icon disappar from map and with couple seconds later “EKF failsafe cleared” shown, and rover icon back to map. This happened repeatedly every something like 10 seconds.
Major changed paramters:
VISO_TYPE = 2
VISO_DELAY_MS = 35
EK3_SRC1_POSXY = 6
EK3_SRC1_YAW = 6
FS_EKF_THRESH = 1
I checked on MAVLink inspector, the VISION_POSITION_ESTIMATE is continuously coming all the time even when Unhealthy AHRS showing.
Here is data flash log, telemetry log.
I check on the ERR, subsys is 16 and 17 which is EKF Check and EKF Failsafe, it keeps triggering “Bad Variance (position estimate bad)” and “Variance Cleared” .
Does anyone have any idea what is happening? and how can I get rid of this Unhealthy AHRS warning?
Update a bit. I also updated the FW to 4.6.0, unfortunately still this issue hasn’t gone away.
After tracking down on ekf_check.cpp code I found on this line, if both horiz_pos_abs and horiz_pos_rel are 0 then the EKF Failsafe will be trigger.
Then I try to check more when this horiz_pos_abs and horiz_pos_rel will become 0. So I found they are decided by these conditions.
void NavEKF3_core::updateFilterStatus(void)
{
// init return value
nav_filter_status status;
status.value = 0;
bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000);
bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid;
bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip()) || !dragTimeout;
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && (useGpsVertVel || useExtNavVel)) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout && dragTimeout) || doingFlowNav || doingBodyVelNav;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin;
// set individual flags
status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
status.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_rel; // EKF3 enters the required mode before flight
status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = ((imuSampleTime_ms - lastGpsPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy
status.flags.gps_quality_good = gpsGoodToAlign;
// for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data
status.flags.rejecting_airspeed = lastTasFailTime_ms != 0 &&
(imuSampleTime_ms - lastTasFailTime_ms) < 1000 &&
(imuSampleTime_ms - lastTasPassTime_ms) > 3000;
status.flags.initalized = status.flags.initalized || healthy();
status.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav);
filterStatus.value = status.value;
}
Looking closely on this line
status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
The doingNormalGpsNav must be 1 otherwise all the time otherwise both horiz_pos_abs and horiz_pos_rel cannot be 1. So the key to decide horiz_pos_abs and horiz_pos_rel will be 0 or 1 is filterHealthy
The filterHealthy will be 1 meaning,
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
tiltAlignComplete and yawAlignComplete seems to be one time setup (I guess), so I think the important is healthy() which is said from bool NavEKF3_core::healthy(). But I couldn’t find this functions in ardupilot project.
I am kind of stucking here…
If anyone could explain, I will appreciate a LOT.