Tridge EKF

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:

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),

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));
 }

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;

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

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 }, \