diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 0e258f3da5..309c22b8c4 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -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 diff --git a/libraries/AP_Scripting/examples/sub_test_synthetic_seafloor.lua b/libraries/AP_Scripting/examples/sub_test_synthetic_seafloor.lua index ceb12ba36b..f05b4625b5 100644 --- a/libraries/AP_Scripting/examples/sub_test_synthetic_seafloor.lua +++ b/libraries/AP_Scripting/examples/sub_test_synthetic_seafloor.lua @@ -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