Add test for EK3_ORGN_HGT_MASK

Tools/autotest/arducopter.py

@@ -11024,6 +11024,46 @@ def AutoRTL(self):
         # Done
         self.land_and_disarm()
 
+    def EK3_OGN_HGT_MASK(self):
+        '''test baraometer-alt-compensation based on long-term GPS readings'''
+        self.context_push()
+        self.set_parameters({
+            'EK3_OGN_HGT_MASK': 1,  # compensate baro drift using GPS
+        })
+        self.reboot_sitl()
+
+        expected_alt = 10
+
+        self.change_mode('GUIDED')
+        self.wait_ready_to_arm()
+        current_alt = self.get_altitude()
+
+        expected_alt_abs = current_alt + expected_alt
+
+        self.takeoff(expected_alt, mode='GUIDED')
+        self.delay_sim_time(5)
+
+        self.set_parameter("SIM_BARO_DRIFT", 0.01)  # 1cm/second
+
+        def check_altitude(mav, m):
+            m_type = m.get_type()
+            epsilon = 10  # in metres
+            if m_type == 'GPS_RAW_INT':
+                got_gps_alt = m.alt * 0.001
+                if abs(expected_alt_abs - got_gps_alt) > epsilon:
+                    raise NotAchievedException(f"Bad GPS altitude (got={got_gps_alt} want={expected_alt_abs})")
+            elif m_type == 'GLOBAL_POSITION_INT':
+                got_canonical_alt = m.relative_alt * 0.001
+                if abs(expected_alt - got_canonical_alt) > epsilon:
+                    raise NotAchievedException(f"Bad canonical altitude (got={got_canonical_alt} want={expected_alt})")
+
+        self.install_message_hook_context(check_altitude)
+
+        self.delay_sim_time(1500)
+
+        self.context_pop()
+        self.reboot_sitl(force=True)
+
     def tests2b(self):  # this block currently around 9.5mins here
         '''return list of all tests'''
         ret = ([
@@ -11106,6 +11146,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.GuidedModeThrust,
             self.CompassMot,
             self.AutoRTL,
+            self.EK3_OGN_HGT_MASK,
         ])
         return ret
 

Tools/autotest/vehicle_test_suite.py

@@ -2331,7 +2331,6 @@ def get_sim_parameter_documentation_get_whitelist(self):
             "SIM_BARO_COUNT",
             "SIM_BARO_DELAY",
             "SIM_BARO_DISABLE",
-            "SIM_BARO_DRIFT",
             "SIM_BARO_FREEZE",
             "SIM_BARO_WCF_BAK",
             "SIM_BARO_WCF_DN",
@@ -6045,13 +6044,14 @@ def context_stop_collecting(self, msg_type):
         del context.collections[msg_type]
         return ret
 
-    def context_pop(self, process_interaction_allowed=True):
+    def context_pop(self, process_interaction_allowed=True, hooks_already_removed=False):
         """Set parameters to origin values in reverse order."""
         dead = self.contexts.pop()
         # remove hooks first; these hooks can raise exceptions which
         # we really don't want...
-        for hook in dead.message_hooks:
-            self.remove_message_hook(hook)
+        if not hooks_already_removed:
+            for hook in dead.message_hooks:
+                self.remove_message_hook(hook)
         for script in dead.installed_scripts:
             self.remove_installed_script(script)
         for (message_id, interval_us) in dead.overridden_message_rates.items():
@@ -8409,7 +8409,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout=
 
         tee = TeeBoth(test_output_filename, 'w', self.mavproxy_logfile, suppress_stdout=suppress_stdout)
 
-        start_message_hooks = self.mav.message_hooks
+        start_message_hooks = copy.copy(self.mav.message_hooks)
 
         prettyname = "%s (%s)" % (name, desc)
         self.start_test(prettyname)
@@ -8421,6 +8421,8 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout=
 
         orig_speedup = None
 
+        hooks_removed = False
+
         ex = None
         try:
             self.check_rc_defaults()
@@ -8444,6 +8446,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout=
             for h in self.mav.message_hooks:
                 if h not in start_message_hooks:
                     self.mav.message_hooks.remove(h)
+            hooks_removed = True
         self.test_timings[desc] = time.time() - start_time
         reset_needed = self.contexts[-1].sitl_commandline_customised
 
@@ -8470,7 +8473,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout=
             reset_needed = True
 
         try:
-            self.context_pop(process_interaction_allowed=ardupilot_alive)
+            self.context_pop(process_interaction_allowed=ardupilot_alive, hooks_already_removed=hooks_removed)
         except Exception as e:
             self.print_exception_caught(e, send_statustext=False)
             passed = False
@@ -8522,7 +8525,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout=
             # pop off old contexts to clean up message hooks etc
             while len(self.contexts) > old_contexts_length:
                 try:
-                    self.context_pop(process_interaction_allowed=ardupilot_alive)
+                    self.context_pop(process_interaction_allowed=ardupilot_alive, hooks_already_removed=hooks_removed)
                 except Exception as e:
                     self.print_exception_caught(e, send_statustext=False)
             self.progress("Done popping extra contexts")

libraries/AP_Baro/AP_Baro_SITL.cpp

@@ -66,7 +66,11 @@ void AP_Baro_SITL::_timer()
         return;
     }
 
-    sim_alt += _sitl->baro[_instance].drift * now * 0.001f;
+    const auto drift_delta_t_ms = now - last_drift_delta_t_ms;
+    last_drift_delta_t_ms = now;
+    total_alt_drift += _sitl->baro[_instance].drift * drift_delta_t_ms * 0.001f;
+
+    sim_alt += total_alt_drift;
     sim_alt += _sitl->baro[_instance].noise * rand_float();
 
     // add baro glitch

libraries/AP_Baro/AP_Baro_SITL.h

@@ -48,5 +48,7 @@ class AP_Baro_SITL : public AP_Baro_Backend {
     float _recent_press;
     float _last_altitude;
 
+    uint32_t last_drift_delta_t_ms;  // allows for integration of drift over time
+    float total_alt_drift;  // integrated altitude drift in metres
 };
 #endif  // AP_SIM_BARO_ENABLED

libraries/SITL/SITL.cpp

@@ -548,6 +548,11 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
 // user settable parameters for the barometers
 const AP_Param::GroupInfo SIM::BaroParm::var_info[] = {
     AP_GROUPINFO("RND",      1, SIM::BaroParm,  noise, 0.2f),
+    // @Param: BARO_DRIFT
+    // @DisplayName: Baro altitude drift
+    // @Description: Barometer altitude drifts at this rate
+    // @Units: m/s
+    // @User: Advanced
     AP_GROUPINFO("DRIFT",    2, SIM::BaroParm,  drift, 0),
     AP_GROUPINFO("DISABLE",  3, SIM::BaroParm,  disable, 0),
     AP_GROUPINFO("GLITCH",   4, SIM::BaroParm,  glitch, 0),