Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: large outliers in sub terrain test have low sq

This commit is contained in:
Clyde McQueen 2024-08-01 08:03:28 -07:00 committed by Peter Barker
parent 95fb9bd533
commit 8b37100772
2 changed files with 41 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Tools/autotest/ardusub.py
View File

@ -287,7 +287,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.disarm_vehicle()
self.context_pop()
def prepare_synthetic_seafloor_test(self, sea_floor_depth):
def prepare_synthetic_seafloor_test(self, sea_floor_depth, rf_target):
self.set_parameters({
"SCR_ENABLE": 1,
"RNGFND1_TYPE": 36,
@ -297,6 +297,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
"SCR_USER1": 2, # Configuration bundle
"SCR_USER2": sea_floor_depth, # Depth in meters
"SCR_USER3": 101, # Output log records
"SCR_USER4": rf_target, # Rangefinder target in meters
})
self.install_example_script_context("sub_test_synthetic_seafloor.lua")
@ -359,7 +360,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
validation_delta = 1.5 # Largest allowed distance between sub height and desired height
self.context_push()
self.prepare_synthetic_seafloor_test(sea_floor_depth)
self.prepare_synthetic_seafloor_test(sea_floor_depth, match_distance)
self.change_mode('MANUAL')
self.arm_vehicle()
@ -390,7 +391,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.set_rc(Joystick.Forward, 1650)
self.watch_true_distance_maintained(match_distance, delta=validation_delta, timeout=60)
# The mission ends at end_altitude. Do a check to insure that the sub is at this altitude
# The mission ends at end_altitude. Do a check to ensure that the sub is at this altitude
self.wait_altitude(altitude_min=end_altitude-validation_delta/2, altitude_max=end_altitude+validation_delta/2,
relative=False, timeout=1)
@ -410,7 +411,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
validation_delta = 1.5 # Largest allowed distance between sub height and desired height
self.context_push()
self.prepare_synthetic_seafloor_test(sea_floor_depth)
self.prepare_synthetic_seafloor_test(sea_floor_depth, match_distance)
# The synthetic seafloor has an east-west ridge south of the sub.
# The mission contained in terrain_mission.txt instructs the sub
@ -432,7 +433,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.change_mode('AUTO')
self.watch_true_distance_maintained(match_distance, delta=validation_delta, timeout=500.0, final_waypoint=4)
# The mission ends at end_altitude. Do a check to insure that the sub is at this altitude.
# The mission ends at end_altitude. Do a check to ensure that the sub is at this altitude.
self.wait_altitude(altitude_min=end_altitude-validation_delta/2, altitude_max=end_altitude+validation_delta/2,
relative=False, timeout=1)
@ -680,7 +681,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.assert_mode('SURFACE')
def MAV_CMD_MISSION_START(self):
'''test handling of MAV_CMD_NAV_LAND received via mavlink'''
'''test handling of MAV_CMD_MISSION_START received via mavlink'''
self.upload_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0),
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
@ -866,7 +867,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
# restart GPS driver
self.reboot_sitl()
def backup_origin(self):
def BackupOrigin(self):
"""Test ORIGIN_LAT and ORIGIN_LON parameters"""
self.context_push()
@ -910,7 +911,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
if found_sel != expect_sel:
raise NotAchievedException("Expected mag fusion selection %d, found %d" % (expect_sel, found_sel))
def fuse_mag(self):
def FuseMag(self):
"""Test EK3_MAG_CAL values"""
# WHEN_FLYING: switch to FUSE_MAG after sub is armed for 5 seconds; switch to FUSE_YAW on disarm
@ -975,8 +976,8 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.MAV_CMD_DO_REPOSITION,
self.TerrainMission,
self.SetGlobalOrigin,
self.backup_origin,
self.fuse_mag,
self.BackupOrigin,
self.FuseMag,
])
return ret

54
libraries/AP_Scripting/examples/sub_test_synthetic_seafloor.lua
View File

@ -8,9 +8,10 @@
-- in the perpendicular direction to define a 2-D height function (h=f(x,y)).
--
-- The following Parameters can be set by the test script to control how this driver behaves
-- SCR_USER1 is an index into a table of configuration bundles.
-- SCR_USER1 is an index into a table of configuration bundles
-- SCR_USER2 is the average bottom depth in meters
-- SCR_USER3 is a bit field that controls driver logging.
-- SCR_USER3 is a bit field that controls driver logging
-- SRC_USER4 is the rangefinder target in meters
--
---@diagnostic disable: param-type-mismatch
@ -33,6 +34,8 @@ local RNGFND_STATUS_GOOD = 4
-- Copied from libraries/AP_RangeFinder/AP_RangeFinder.h
local SIGNAL_QUALITY_MIN = 0
local SIGNAL_QUALITY_MAX = 100
-- Copied from ArduSub/mode.h enum Mode::Number {}.
local MODE_SURFTRAK = 21
@ -536,7 +539,7 @@ local rngfnd_backend
local range_model
local measurement_noise_func
local signal_quality_noise_func
local signal_quality_func
@ -560,7 +563,7 @@ local function range_finder_driver(sub_loc)
-- Generate a simulated range measurement
local true_range_m = range_model:get_range(sub_loc)
local range_m = measurement_noise_func(true_range_m)
local signal_quality = signal_quality_noise_func(SIGNAL_QUALITY_MAX)
local signal_quality = signal_quality_func(range_m, true_range_m)
-- Return this measurement to the range finder backend
rf_state:status(RNGFND_STATUS_GOOD)
@ -627,16 +630,7 @@ local function initialize_model()
outlier_rate_ops = 0.0,
outlier_mean = 0.0,
outlier_std_dev = 0.0,
delay_s = 0.0,
callback_interval_ms = UPDATE_PERIOD_MS,
}
local config_signal_quality_noise = {
mean = 0.0,
std_dev = 0.0,
outlier_rate_ops = 0.0,
outlier_mean = 0.0,
outlier_std_dev = 0.0,
outlier_good_sq_limit = 0.0,
delay_s = 0.0,
callback_interval_ms = UPDATE_PERIOD_MS,
}
@ -649,6 +643,7 @@ local function initialize_model()
config_measurement_noise.outlier_rate_ops = .2
config_measurement_noise.outlier_mean = 5
config_measurement_noise.outlier_std_dev = 2
config_measurement_noise.outlier_good_sq_limit = 1
config_measurement_noise.delay_s = 0.00
end
@ -657,17 +652,15 @@ local function initialize_model()
measurement_noise_func = add_noise_funcfactory(config_measurement_noise)
-- Constrain signal quality values
local signal_quality_noise_pre = add_noise_funcfactory(config_signal_quality_noise)
signal_quality_noise_func = function(m)
m = signal_quality_noise_pre(m)
if m > SIGNAL_QUALITY_MAX then
-- A rapid series of large outliers can cause test failure
-- Mark large outliers with a poor signal quality to minimize flakiness
-- This also exercises signal_quality handling
signal_quality_func = function(m, true_m)
if math.abs(m - true_m) > config_measurement_noise.outlier_good_sq_limit then
return 50
else
return SIGNAL_QUALITY_MAX
end
if m < SIGNAL_QUALITY_MIN then
return SIGNAL_QUALITY_MIN
end
return m
end
end
@ -676,6 +669,13 @@ end
-- update functions
-- SCR_USER4 is the rangefinder target in meters
local rf_target_cm = param:get('SCR_USER4') * 100
if not rf_target_cm or rf_target_cm < 50 or rf_target_cm > 5000 then
rf_target_cm = 1500
end
local function update_run()
local loc_c = ahrs:get_location()
@ -693,6 +693,12 @@ local function update_run()
end
end
-- Check if we have to set the rangefinder target
if vehicle:get_mode() == MODE_SURFTRAK and sub:get_rangefinder_target_cm() ~= rf_target_cm then
gcs:send_text(6, string.format("Set rangefinder target to %g cm", rf_target_cm))
sub:set_rangefinder_target_cm(rf_target_cm)
end
-- Update with range finder driver
range_finder_driver(loc_c)
@ -716,7 +722,7 @@ local function update_init()
initialize_model()
if not range_model or not measurement_noise_func or not signal_quality_noise_func then
if not range_model or not measurement_noise_func or not signal_quality_func then
return fatal_error("Could not initialize model")
end