diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 3549910bcb..8820b5f64c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -880,13 +880,15 @@ class AutoTestCopter(AutoTest): def test_battery_failsafe(self, timeout=300): self.progress("Configure battery failsafe parameters") - self.set_parameter('SIM_SPEEDUP', 4) - self.set_parameter('BATT_LOW_VOLT', 11.5) - self.set_parameter('BATT_CRT_VOLT', 10.1) - self.set_parameter('BATT_FS_LOW_ACT', 0) - self.set_parameter('BATT_FS_CRT_ACT', 0) - self.set_parameter('FS_OPTIONS', 0) - self.set_parameter('SIM_BATT_VOLTAGE', 12.5) + self.set_parameters({ + 'SIM_SPEEDUP': 4, + 'BATT_LOW_VOLT': 11.5, + 'BATT_CRT_VOLT': 10.1, + 'BATT_FS_LOW_ACT': 0, + 'BATT_FS_CRT_ACT': 0, + 'FS_OPTIONS': 0, + 'SIM_BATT_VOLTAGE': 12.5, + }) # Trigger low battery condition with failsafe disabled. Verify no action taken. self.start_subtest("Batt failsafe disabled test") @@ -2092,17 +2094,19 @@ class AutoTestCopter(AutoTest): try: self.set_rc_default() # magic tridge EKF type that dramatically speeds up the test - self.set_parameter("AHRS_EKF_TYPE", 10) - self.set_parameter("INS_LOG_BAT_MASK", 3) - self.set_parameter("INS_LOG_BAT_OPT", 0) - self.set_parameter("LOG_BITMASK", 958) - self.set_parameter("LOG_DISARMED", 0) - self.set_parameter("SIM_VIB_MOT_MAX", 350) - # these are real values taken from a 180mm Quad - self.set_parameter("SIM_GYR_RND", 20) - self.set_parameter("SIM_ACC_RND", 5) - self.set_parameter("SIM_ACC2_RND", 5) - self.set_parameter("SIM_INS_THR_MIN", 0.1) + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "LOG_BITMASK": 958, + "LOG_DISARMED": 0, + "SIM_VIB_MOT_MAX": 350, + # these are real values taken from a 180mm Quad: + "SIM_GYR_RND": 20, + "SIM_ACC_RND": 5, + "SIM_ACC2_RND": 5, + "SIM_INS_THR_MIN": 0.1, + }) self.reboot_sitl() self.takeoff(15, mode="ALT_HOLD") @@ -2125,11 +2129,13 @@ class AutoTestCopter(AutoTest): self.progress("Detected motor peak at %fHz" % freq) # now add a notch and check that post-filter the peak is squashed below 40dB - self.set_parameter("INS_LOG_BAT_OPT", 2) - self.set_parameter("INS_NOTCH_ENABLE", 1) - self.set_parameter("INS_NOTCH_FREQ", freq) - self.set_parameter("INS_NOTCH_ATT", 50) - self.set_parameter("INS_NOTCH_BW", freq/2) + self.set_parameters({ + "INS_LOG_BAT_OPT": 2, + "INS_NOTCH_ENABLE": 1, + "INS_NOTCH_FREQ": freq, + "INS_NOTCH_ATT": 50, + "INS_NOTCH_BW": freq/2, + }) self.reboot_sitl() self.takeoff(15, mode="ALT_HOLD") @@ -2254,18 +2260,20 @@ class AutoTestCopter(AutoTest): self.context_push() ex = None try: - self.set_parameter("VISO_TYPE", 2) # enable vicon - self.set_parameter("SERIAL5_PROTOCOL", 2) - self.set_parameter("EK3_ENABLE", 1) - self.set_parameter("EK3_SRC2_POSXY", 6) # External Nav - self.set_parameter("EK3_SRC2_POSZ", 6) # External Nav - self.set_parameter("EK3_SRC2_VELXY", 6) # External Nav - self.set_parameter("EK3_SRC2_VELZ", 6) # External Nav - self.set_parameter("EK3_SRC2_YAW", 6) # External Nav - self.set_parameter("RC7_OPTION", 80) # RC aux switch 7 set to Viso Align - self.set_parameter("RC8_OPTION", 90) # RC aux switch 8 set to EKF source selector - self.set_parameter("EK2_ENABLE", 0) - self.set_parameter("AHRS_EKF_TYPE", 3) + self.set_parameters({ + "VISO_TYPE": 2, # enable vicon + "SERIAL5_PROTOCOL": 2, + "EK3_ENABLE": 1, + "EK3_SRC2_POSXY": 6, # External Nav + "EK3_SRC2_POSZ": 6, # External Nav + "EK3_SRC2_VELXY": 6, # External Nav + "EK3_SRC2_VELZ": 6, # External Nav + "EK3_SRC2_YAW": 6, # External Nav + "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align + "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector + "EK2_ENABLE": 0, + "AHRS_EKF_TYPE": 3, + }) self.reboot_sitl() # switch to use GPS @@ -2548,10 +2556,12 @@ class AutoTestCopter(AutoTest): try: self.set_analog_rangefinder_parameters() - self.set_parameter("RNGFND1_MAX_CM", 1500) - self.set_parameter("EK2_RNG_USE_HGT", 70) - self.set_parameter("EK2_ENABLE", 1) - self.set_parameter("AHRS_EKF_TYPE", 2) + self.set_parameters({ + "RNGFND1_MAX_CM": 1500, + "EK2_RNG_USE_HGT": 70, + "EK2_ENABLE": 1, + "AHRS_EKF_TYPE": 2, + }) self.reboot_sitl() # needed for both rangefinder and initial position self.assert_vehicle_location_is_at_startup_location() @@ -4052,9 +4062,11 @@ class AutoTestCopter(AutoTest): def fly_throw_mode(self): # test boomerang mode: self.progress("Throwing vehicle away") - self.set_parameter("THROW_NEXTMODE", 6) - self.set_parameter("SIM_SHOVE_Z", -30) - self.set_parameter("SIM_SHOVE_X", -20) + self.set_parameters({ + "THROW_NEXTMODE": 6, + "SIM_SHOVE_Z": -30, + "SIM_SHOVE_X": -20, + }) self.change_mode('THROW') self.wait_ready_to_arm() self.arm_vehicle() @@ -5683,33 +5695,35 @@ class AutoTestCopter(AutoTest): raise ex def test_replay_gps_bit(self): - self.set_parameter("LOG_REPLAY", 1) - self.set_parameter("LOG_DISARMED", 1) - self.set_parameter("EK3_ENABLE", 1) - self.set_parameter("EK2_ENABLE", 1) - self.set_parameter("AHRS_TRIM_X", 0.01) - self.set_parameter("AHRS_TRIM_Y", -0.03) - self.set_parameter("GPS_TYPE2", 1) - self.set_parameter("GPS_POS1_X", 0.1) - self.set_parameter("GPS_POS1_Y", 0.2) - self.set_parameter("GPS_POS1_Z", 0.3) - self.set_parameter("GPS_POS2_X", -0.1) - self.set_parameter("GPS_POS2_Y", -0.02) - self.set_parameter("GPS_POS2_Z", -0.31) - self.set_parameter("INS_POS1_X", 0.12) - self.set_parameter("INS_POS1_Y", 0.14) - self.set_parameter("INS_POS1_Z", -0.02) - self.set_parameter("INS_POS2_X", 0.07) - self.set_parameter("INS_POS2_Y", 0.012) - self.set_parameter("INS_POS2_Z", -0.06) - self.set_parameter("RNGFND1_TYPE", 1) - self.set_parameter("RNGFND1_PIN", 0) - self.set_parameter("RNGFND1_SCALING", 30) - self.set_parameter("RNGFND1_POS_X", 0.17) - self.set_parameter("RNGFND1_POS_Y", -0.07) - self.set_parameter("RNGFND1_POS_Z", -0.005) - self.set_parameter("SIM_SONAR_SCALE", 30) - self.set_parameter("SIM_GPS2_DISABLE", 0) + self.set_parameters({ + "LOG_REPLAY": 1, + "LOG_DISARMED": 1, + "EK3_ENABLE": 1, + "EK2_ENABLE": 1, + "AHRS_TRIM_X": 0.01, + "AHRS_TRIM_Y": -0.03, + "GPS_TYPE2": 1, + "GPS_POS1_X": 0.1, + "GPS_POS1_Y": 0.2, + "GPS_POS1_Z": 0.3, + "GPS_POS2_X": -0.1, + "GPS_POS2_Y": -0.02, + "GPS_POS2_Z": -0.31, + "INS_POS1_X": 0.12, + "INS_POS1_Y": 0.14, + "INS_POS1_Z": -0.02, + "INS_POS2_X": 0.07, + "INS_POS2_Y": 0.012, + "INS_POS2_Z": -0.06, + "RNGFND1_TYPE": 1, + "RNGFND1_PIN": 0, + "RNGFND1_SCALING": 30, + "RNGFND1_POS_X": 0.17, + "RNGFND1_POS_Y": -0.07, + "RNGFND1_POS_Z": -0.005, + "SIM_SONAR_SCALE": 30, + "SIM_GPS2_DISABLE": 0, + }) self.reboot_sitl() current_log_filepath = self.current_onboard_log_filepath() diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 946260581c..30f55325ab 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1256,33 +1256,10 @@ class AutoTest(ABC): parameters = mavparm.MAVParmDict() correct_parameters = set() parameters.load(filepath) - failfetch = None - for i in range(10): - self.progress("Apply parameter file (%s) pass %u" % (filepath, i+1,)) - success = True - for p in parameters.keys(): - if p in correct_parameters: - continue - try: - current = self.get_parameter(p, verbose=False) - except Exception as e: - # may still be hidden - self.progress("get_parameter(%s) failed" % p) - failfetch = p - success = False - continue - delta = current - parameters[p] - self.progress("%s: want=%f got=%f delta=%f" % - (p, parameters[p], current, delta)) - if abs(delta) > 0.00001: - success = False - self.set_parameter(p, parameters[p], verbose=False) - continue - correct_parameters.add(p) - if success: - self.progress("Applied parameter file (%s)" % (filepath)) - return - raise NotAchievedException("Failed to load parameter file; last failfetch was %s" % failfetch) + param_dict = {} + for p in parameters.keys(): + param_dict[p] = parameters[p] + self.set_parameters(param_dict) def repeatedly_apply_parameter_file_mavproxy(self, filepath): '''keep applying a parameter file until no parameters changed''' @@ -5471,10 +5448,12 @@ Also, ignores heartbeats not from our target system''' ("SIM_MAG%d_ODI_Z" % count, "COMPASS_ODI%d_Z" % count, 42.0), ], ] self.wait_heartbeat() + to_set = {} for param_set in params: for param in param_set: (_, _out, value) = param - self.set_parameter(_out, value) + to_set[_out] = value + self.set_parameters(to_set) self.check_zero_mag_parameters(params) def check_mag_parameters(self, parameter_stuff, compass_number): @@ -5574,11 +5553,13 @@ Also, ignores heartbeats not from our target system''' # self.set_parameter("SIM_MAG%d_ORIENT" % count, MAG_ORIENT * (count % 41)) # # set compass external to check that orientation is found and auto set # self.set_parameter("COMPASS_EXTERN%d" % count, 1) + to_set = {} for param_set in params: for param in param_set: (_in, _out, value) = param - self.set_parameter(_in, value) - self.set_parameter(_out, value) + to_set[_in] = value + to_set[_out] = value + self.set_parameters(to_set) self.start_subtest("Zeroing Mag OFS parameters with Mavlink") self.zero_mag_offset_parameters() self.progress("=========================================") @@ -5936,8 +5917,7 @@ Also, ignores heartbeats not from our target system''' raise NotAchievedException("Values are not all unique!") self.progress("Setting parameters") - for param in originals.keys(): - self.set_parameter(param, originals[param]) + self.set_parameters(originals) self.reboot_sitl() @@ -6016,17 +5996,20 @@ Also, ignores heartbeats not from our target system''' "COMPASS_ODI3_Y": 0, "COMPASS_ODI3_Z": 0, } - self.set_parameter("SIM_MAG_OFS_X", MAG_OFS_X) - self.set_parameter("SIM_MAG_OFS_Y", MAG_OFS_Y) - self.set_parameter("SIM_MAG_OFS_Z", MAG_OFS_Z) + self.set_parameters({ + "SIM_MAG_OFS_X": MAG_OFS_X, + "SIM_MAG_OFS_Y": MAG_OFS_Y, + "SIM_MAG_OFS_Z": MAG_OFS_Z, - self.set_parameter("SIM_MAG2_OFS_X", MAG_OFS_X) - self.set_parameter("SIM_MAG2_OFS_Y", MAG_OFS_Y) - self.set_parameter("SIM_MAG2_OFS_Z", MAG_OFS_Z) + "SIM_MAG2_OFS_X": MAG_OFS_X, + "SIM_MAG2_OFS_Y": MAG_OFS_Y, + "SIM_MAG2_OFS_Z": MAG_OFS_Z, + + "SIM_MAG3_OFS_X": MAG_OFS_X, + "SIM_MAG3_OFS_Y": MAG_OFS_Y, + "SIM_MAG3_OFS_Z": MAG_OFS_Z, + }) - self.set_parameter("SIM_MAG3_OFS_X", MAG_OFS_X) - self.set_parameter("SIM_MAG3_OFS_Y", MAG_OFS_Y) - self.set_parameter("SIM_MAG3_OFS_Z", MAG_OFS_Z) # set to some sensible-ish initial values. If your initial # offsets are way, way off you can get some very odd effects. for param in wanted: @@ -6845,18 +6828,20 @@ Also, ignores heartbeats not from our target system''' def test_gripper(self): self.context_push() - self.set_parameter("GRIP_ENABLE", 1) - self.set_parameter("GRIP_GRAB", 2000) - self.set_parameter("GRIP_RELEASE", 1000) - self.set_parameter("GRIP_TYPE", 1) - self.set_parameter("SIM_GRPS_ENABLE", 1) - self.set_parameter("SIM_GRPS_PIN", 8) - self.set_parameter("SERVO8_FUNCTION", 28) - self.set_parameter("SERVO8_MIN", 1000) - self.set_parameter("SERVO8_MAX", 2000) - self.set_parameter("SERVO9_MIN", 1000) - self.set_parameter("SERVO9_MAX", 2000) - self.set_parameter("RC9_OPTION", 19) + self.set_parameters({ + "GRIP_ENABLE": 1, + "GRIP_GRAB": 2000, + "GRIP_RELEASE": 1000, + "GRIP_TYPE": 1, + "SIM_GRPS_ENABLE": 1, + "SIM_GRPS_PIN": 8, + "SERVO8_FUNCTION": 28, + "SERVO8_MIN": 1000, + "SERVO8_MAX": 2000, + "SERVO9_MIN": 1000, + "SERVO9_MAX": 2000, + "RC9_OPTION": 19, + }) self.set_rc(9, 1500) self.reboot_sitl() self.progress("Waiting for ready to arm") diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 82a748a650..cd943849bd 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -449,54 +449,59 @@ class AutoTestQuadPlane(AutoTest): self.set_rc_default() # magic tridge EKF type that dramatically speeds up the test - self.set_parameter("AHRS_EKF_TYPE", 10) - - self.set_parameter("INS_LOG_BAT_MASK", 3) - self.set_parameter("INS_LOG_BAT_OPT", 0) - self.set_parameter("INS_GYRO_FILTER", 100) - self.set_parameter("LOG_BITMASK", 45054) - self.set_parameter("LOG_DISARMED", 0) - self.set_parameter("SIM_DRIFT_SPEED", 0) - self.set_parameter("SIM_DRIFT_TIME", 0) - # enable a noisy motor peak - self.set_parameter("SIM_GYR_RND", 20) - # enabling FFT will also enable the arming check, self-testing the functionality - self.set_parameter("FFT_ENABLE", 1) - self.set_parameter("FFT_MINHZ", 80) - self.set_parameter("FFT_MAXHZ", 350) - self.set_parameter("FFT_SNR_REF", 10) - self.set_parameter("FFT_WINDOW_SIZE", 128) - self.set_parameter("FFT_WINDOW_OLAP", 0.75) + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, + "LOG_BITMASK": 45054, + "LOG_DISARMED": 0, + "SIM_DRIFT_SPEED": 0, + "SIM_DRIFT_TIME": 0, + # enable a noisy motor peak + "SIM_GYR_RND": 20, + # enabling FFT will also enable the arming check: self-testing the functionality + "FFT_ENABLE": 1, + "FFT_MINHZ": 80, + "FFT_MAXHZ": 350, + "FFT_SNR_REF": 10, + "FFT_WINDOW_SIZE": 128, + "FFT_WINDOW_OLAP": 0.75, + }); # Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft # can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so # a 250Hz peak should be detectable within 5% - self.set_parameter("SIM_VIB_FREQ_X", 250) - self.set_parameter("SIM_VIB_FREQ_Y", 250) - self.set_parameter("SIM_VIB_FREQ_Z", 250) - + self.set_parameters({ + "SIM_VIB_FREQ_X": 250, + "SIM_VIB_FREQ_Y": 250, + "SIM_VIB_FREQ_Z": 250, + }) self.reboot_sitl() # find a motor peak self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250) # Step 2: inject actual motor noise and use the standard length FFT to track it - self.set_parameter("SIM_VIB_MOT_MAX", 350) - self.set_parameter("FFT_WINDOW_SIZE", 32) - self.set_parameter("FFT_WINDOW_OLAP", 0.5) - + self.set_parameters({ + "SIM_VIB_MOT_MAX": 350, + "FFT_WINDOW_SIZE": 32, + "FFT_WINDOW_OLAP": 0.5, + }) self.reboot_sitl() # find a motor peak freq = self.hover_and_check_matched_frequency(-15, 200, 300, 32) # Step 3: add a FFT dynamic notch and check that the peak is squashed - self.set_parameter("INS_LOG_BAT_OPT", 2) - self.set_parameter("INS_HNTCH_ENABLE", 1) - self.set_parameter("INS_HNTCH_FREQ", freq) - self.set_parameter("INS_HNTCH_REF", 1.0) - self.set_parameter("INS_HNTCH_ATT", 50) - self.set_parameter("INS_HNTCH_BW", freq/2) - self.set_parameter("INS_HNTCH_MODE", 4) + self.set_parameters({ + "INS_LOG_BAT_OPT": 2, + "INS_HNTCH_ENABLE": 1, + "INS_HNTCH_FREQ": freq, + "INS_HNTCH_REF": 1.0, + "INS_HNTCH_ATT": 50, + "INS_HNTCH_BW": freq/2, + "INS_HNTCH_MODE": 4, + }) self.reboot_sitl() self.takeoff(10, mode="QHOVER") diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 6b82ffda69..c5773ffe62 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -242,24 +242,26 @@ class AutoTestRover(AutoTest): spinner_ch_trim = 1510 spinner_ch_max = 1975 - self.set_parameter("SPRAY_ENABLE", 1) + self.set_parameters({ + "SPRAY_ENABLE": 1, - self.set_parameter("SERVO%u_FUNCTION" % pump_ch, 22) - self.set_parameter("SERVO%u_MIN" % pump_ch, pump_ch_min) - self.set_parameter("SERVO%u_TRIM" % pump_ch, pump_ch_trim) - self.set_parameter("SERVO%u_MAX" % pump_ch, pump_ch_max) + "SERVO%u_FUNCTION" % pump_ch: 22, + "SERVO%u_MIN" % pump_ch: pump_ch_min, + "SERVO%u_TRIM" % pump_ch: pump_ch_trim, + "SERVO%u_MAX" % pump_ch: pump_ch_max, - self.set_parameter("SERVO%u_FUNCTION" % spinner_ch, 23) - self.set_parameter("SERVO%u_MIN" % spinner_ch, spinner_ch_min) - self.set_parameter("SERVO%u_TRIM" % spinner_ch, spinner_ch_trim) - self.set_parameter("SERVO%u_MAX" % spinner_ch, spinner_ch_max) + "SERVO%u_FUNCTION" % spinner_ch: 23, + "SERVO%u_MIN" % spinner_ch: spinner_ch_min, + "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, + "SERVO%u_MAX" % spinner_ch: spinner_ch_max, - self.set_parameter("SIM_SPR_ENABLE", 1) - self.set_parameter("SIM_SPR_PUMP", pump_ch) - self.set_parameter("SIM_SPR_SPIN", spinner_ch) + "SIM_SPR_ENABLE": 1, + "SIM_SPR_PUMP": pump_ch, + "SIM_SPR_SPIN": spinner_ch, - self.set_parameter("RC%u_OPTION" % rc_ch, 15) - self.set_parameter("LOG_DISARMED", 1) + "RC%u_OPTION" % rc_ch: 15, + "LOG_DISARMED": 1, + }) self.reboot_sitl()