diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 0d49d84d0cee3..c41454c82158a 100644 --- a/Tools/autotest/arduplane.py +++ b/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 diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index a800e98379574..56bebdda4d212 100644 --- a/Tools/autotest/common.py +++ b/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" % diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index c7f5f41498d41..f40b7b05d8d21 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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 diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 09dcadbd42655..ed2f7ae84d8c3 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/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; diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 5275b8b4d3bc5..b16f1144a2938 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/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