AP_AHRS: rotate type=10 wind to correct direction

Tools/autotest/arduplane.py

@@ -2800,6 +2800,81 @@ def fly_external_AHRS(self, sim, eahrs_type, mission):
         self.arm_vehicle()
         self.fly_mission(mission)
 
+    def wait_and_maintain_wind_estimate(self, want_speed, want_dir, timeout=5, **kwargs):
+        '''wait for wind estimate to reach speed and direction'''
+        speed_tolerance = 0.5
+        dir_tolerance = 5
+
+        def validator(last, _min, _max):
+            '''returns false of spd or direction is too-far wrong'''
+            (spd, di) = last
+            _min_spd, _min_dir = _min
+            _max_spd, _max_dir = _max
+            if spd < _min_spd or spd > _max_spd:
+                return False
+            # my apologies to whoever is staring at this and wondering
+            # why we're not wrapping angles here...
+            if di < _min_dir or di > _max_dir:
+                return False
+            return True
+
+        def value_getter():
+            '''returns a tuple of (wind_speed, wind_dir), where wind_dir is 45 if
+            wind is coming from NE'''
+            m = self.assert_receive_message("WIND")
+            return (m.speed, m.direction)
+
+        class ValueAverager(object):
+            def __init__(self):
+                self.speed_average = -1
+                self.dir_average = -1
+                self.count = 0.0
+
+            def add_value(self, value):
+                (spd, di) = value
+                if self.speed_average == -1:
+                    self.speed_average = spd
+                    self.dir_average = di
+                else:
+                    self.speed_average += spd
+                    self.di_average += spd
+                self.count += 1
+                return (self.speed_average/self.count, self.dir_average/self.count)
+
+            def reset(self):
+                self.count = 0
+                self.speed_average = -1
+                self.dir_average = -1
+
+        self.wait_and_maintain_range(
+            value_name="WindEstimates",
+            minimum=(want_speed-speed_tolerance, want_dir-dir_tolerance),
+            maximum=(want_speed+speed_tolerance, want_dir+dir_tolerance),
+            current_value_getter=value_getter,
+            value_averager=ValueAverager(),
+            validator=lambda last, _min, _max: validator(last, _min, _max),
+            timeout=timeout,
+            **kwargs
+        )
+
+    def WindEstimates(self):
+        '''fly non-external AHRS, ensure wind estimate correct'''
+        self.set_parameters({
+            "SIM_WIND_SPD": 5,
+            "SIM_WIND_DIR": 45,
+        })
+        self.wait_ready_to_arm()
+        self.takeoff(70)  # default wind sim wind is a sqrt function up to 60m
+        self.change_mode('LOITER')
+        # use default estimator to determine when to check others:
+        self.wait_and_maintain_wind_estimate(5, 45, timeout=120)
+
+        for ahrs_type in 0, 2, 3, 10:
+            self.start_subtest("Checking AHRS_EKF_TYPE=%u" % ahrs_type)
+            self.set_parameter("AHRS_EKF_TYPE", ahrs_type)
+            self.wait_and_maintain_wind_estimate(5, 45)
+        self.fly_home_land_and_disarm()
+
     def VectorNavEAHRS(self):
         '''Test VectorNav EAHRS support'''
         self.fly_external_AHRS("VectorNav", 1, "ap1.txt")
@@ -4156,6 +4231,7 @@ def tests(self):
             self.EmbeddedParamParser,
             self.AerobaticsScripting,
             self.MANUAL_CONTROL,
+            self.WindEstimates,
         ])
         return ret
 

Tools/autotest/common.py

@@ -6107,6 +6107,7 @@ def wait_and_maintain_range(self,
                                 maximum,
                                 current_value_getter,
                                 validator=None,
+                                value_averager=None,
                                 timeout=30,
                                 print_diagnostics_as_target_not_range=False,
                                 **kwargs):
@@ -6165,28 +6166,45 @@ def wait_and_maintain_range(self,
                          achieved_duration_bit)
                     )
                 else:
-                    self.progress(
-                        "%s=%0.2f (%s between %s and %s)%s" %
-                        (value_name,
-                         last_value,
-                         want_or_got,
-                         str(minimum),
-                         str(maximum),
-                         achieved_duration_bit)
-                    )
+                    if type(last_value) is float:
+                        self.progress(
+                            "%s=%0.2f (%s between %s and %s)%s" %
+                            (value_name,
+                             last_value,
+                             want_or_got,
+                             str(minimum),
+                             str(maximum),
+                             achieved_duration_bit)
+                        )
+                    else:
+                        self.progress(
+                            "%s=%s (%s between %s and %s)%s" %
+                            (value_name,
+                             last_value,
+                             want_or_got,
+                             str(minimum),
+                             str(maximum),
+                             achieved_duration_bit)
+                        )
                 last_print_time = self.get_sim_time_cached()
             if is_value_valid:
-                sum_of_achieved_values += last_value
-                count_of_achieved_values += 1.0
+                if value_averager is not None:
+                    average = value_averager.add_value(last_value)
+                else:
+                    sum_of_achieved_values += last_value
+                    count_of_achieved_values += 1.0
+                    average = sum_of_achieved_values / count_of_achieved_values
                 if achieving_duration_start is None:
                     achieving_duration_start = self.get_sim_time_cached()
                 if self.get_sim_time_cached() - achieving_duration_start >= minimum_duration:
-                    self.progress("Attained %s=%f" % (value_name, sum_of_achieved_values / count_of_achieved_values))
+                    self.progress("Attained %s=%s" % (value_name, average))
                     return True
             else:
                 achieving_duration_start = None
                 sum_of_achieved_values = 0.0
                 count_of_achieved_values = 0
+                if value_averager is not None:
+                    value_averager.reset()
         if print_diagnostics_as_target_not_range:
             raise AutoTestTimeoutException(
                 "Failed to attain %s want %s, reached %s" %

libraries/AP_AHRS/AP_AHRS.cpp

@@ -779,6 +779,7 @@ Vector3f AP_AHRS::wind_estimate(void) const
         if (_sitl) {
             const auto &fdm = _sitl->state;
             wind = fdm.wind_ef;
+            wind.rotate_xy(radians(180));
         }
         break;
 #endif

libraries/AP_HAL/SIMState.cpp

@@ -43,9 +43,6 @@ void SIMState::_sitl_setup(const char *home_str)
     _home_str = home_str;
 
     printf("Starting SITL input\n");
-
-    // find the barometer object if it exists
-    _barometer = AP_Baro::get_singleton();
 }
 
 
@@ -235,6 +232,9 @@ void SIMState::_simulator_servos(struct sitl_input &input)
     uint32_t now = AP_HAL::micros();
     // last_update_usec = now;
 
+    // find the barometer object if it exists
+    const auto *_barometer = AP_Baro::get_singleton();
+
     float altitude = _barometer?_barometer->get_altitude():0;
     float wind_speed = 0;
     float wind_direction = 0;

libraries/AP_HAL/SIMState.h

@@ -87,8 +87,6 @@ class AP_HAL::SIMState {
     pid_t _parent_pid;
     uint32_t _update_count;
 
-    class AP_Baro *_barometer;
-
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SocketAPM _sitl_rc_in{true};
 #endif