diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3ca7780e5ff45..b491c833ca6f0 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2005,18 +2005,50 @@ def AIRSPEED_AUTOCAL(self): self.fly_home_land_and_disarm() def deadreckoning_main(self, disable_airspeed_sensor=False): +<<<<<<< HEAD self.context_push() +======= +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df self.set_parameter("EK3_OPTIONS", 1) self.set_parameter("AHRS_OPTIONS", 3) self.set_parameter("LOG_REPLAY", 1) self.reboot_sitl() self.wait_ready_to_arm() +<<<<<<< HEAD if disable_airspeed_sensor: max_allowed_divergence = 300 else: max_allowed_divergence = 150 self.install_message_hook_context(vehicle_test_suite.TestSuite.ValidateGlobalPositionIntAgainstSimState(self, max_allowed_divergence=max_allowed_divergence)) # noqa +======= + def validate_global_position_int_against_simstate(mav, m): + if m.get_type() == 'GLOBAL_POSITION_INT': + self.gpi = m + elif m.get_type() == 'SIMSTATE': + self.simstate = m + if self.gpi is None: + return + if self.simstate is None: + return + divergence = self.get_distance_int(self.gpi, self.simstate) + if disable_airspeed_sensor: + max_allowed_divergence = 300 + else: + max_allowed_divergence = 150 + if (time.time() - self.last_print > 1 or + divergence > self.max_divergence): + self.progress("position-estimate-divergence=%fm" % (divergence,)) + self.last_print = time.time() + if divergence > self.max_divergence: + self.max_divergence = divergence + if divergence > max_allowed_divergence: + raise NotAchievedException( + "global-position-int diverged from simstate by %fm (max=%fm" % + (divergence, max_allowed_divergence,)) + + self.install_message_hook(validate_global_position_int_against_simstate) +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df try: # wind is from the West: @@ -2069,12 +2101,18 @@ def deadreckoning_main(self, disable_airspeed_sensor=False): t_enabled = self.get_sim_time() # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning # to prevent bad GPS being used when coming back after loss of lock due to interence. +<<<<<<< HEAD # The EKF_STATUS_REPORT does not tell us when the good to align check passes, so the minimum time # value of 3.0 seconds is an arbitrary value set on inspection of dataflash logs from this test self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) time_since_jamming_stopped = self.get_sim_time() - t_enabled if time_since_jamming_stopped < 3: raise NotAchievedException("GPS use re-started %f sec after jamming stopped" % time_since_jamming_stopped) +======= + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + if self.get_sim_time() < (t_enabled+9): + raise NotAchievedException("GPS use re-started too quickly after jamming") +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df self.set_rc(3, 1000) self.fly_home_land_and_disarm() finally: diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 2021b69ba2195..51cf15ba503b2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -735,8 +735,13 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f), // @Param: OPTIONS +<<<<<<< HEAD // @DisplayName: Optional EKF behaviour // @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad +======= + // @DisplayName: EKF optional behaviour + // @Description: EKF optional behaviour. Setting JammingExpected will change the EKF behaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df // @Bitmask: 0:JammingExpected // @User: Advanced AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0), diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 8a351d75b102e..4bb19debaca80 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -102,8 +102,12 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const airspeed_index : getActiveAirspeed(), source_set : frontend->sources.getPosVelYawSourceSet(), gps_good_to_align : gpsGoodToAlign, +<<<<<<< HEAD wait_for_gps_checks : waitingForGpsChecks, mag_fusion: (uint8_t) magFusionSel +======= + wait_for_gps_checks : waitingForGpsChecks +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 67ac2a57943a4..7b7034ff9fae3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -200,9 +200,15 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) } } } else if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { +<<<<<<< HEAD // There is no need to do a yaw reset yawAlignGpsValidCount = 0; recordYawResetsCompleted(); +======= + // There is no need to do a yaw reset + yawAlignGpsValidCount = 0; + recordYawResetsCompleted(); +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df } } else { yawAlignGpsValidCount = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 29b7310119db8..7d3c144261f0d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1110,7 +1110,7 @@ class NavEKF3_core : public NavEKF_core_common uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec) uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec) uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec) - uint32_t lastTimeGpsReceived_ms;// last time we received GPS data + uint32_t lastTimeGpsReceived_ms;// last time we received GPS data with 3D fix (or better) uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 256cf0468bbe3..2ebf5c204326a 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -346,7 +346,10 @@ struct PACKED log_XKQ { // @Field: SS: Source Set (primary=0/secondary=1/tertiary=2) // @Field: GPS_GTA: GPS good to align // @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass +<<<<<<< HEAD // @Field: MAG_FUSION: Magnetometer fusion (0=not fusing/1=fuse yaw/2=fuse mag) +======= +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df struct PACKED log_XKFS { LOG_PACKET_HEADER; uint64_t time_us; @@ -358,7 +361,10 @@ struct PACKED log_XKFS { uint8_t source_set; uint8_t gps_good_to_align; uint8_t wait_for_gps_checks; +<<<<<<< HEAD uint8_t mag_fusion; +======= +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df }; // @LoggerMessage: XKTV @@ -445,7 +451,11 @@ struct PACKED log_XKV { { LOG_XKFM_MSG, sizeof(log_XKFM), \ "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \ { LOG_XKFS_MSG, sizeof(log_XKFS), \ +<<<<<<< HEAD "XKFS","QBBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT,MAG_FUSION", "s#--------", "F---------" , true }, \ +======= + "XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \ +>>>>>>> a4f78a0bbf1a5678d4321c53c2eccd31d48387df { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \ { LOG_XKT_MSG, sizeof(log_XKT), \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \