aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: add test for fixed yaw calibration

Browse Source
This commit is contained in:
Peter Barker 2020-02-24 13:12:12 +11:00 committed by Peter Barker
parent 10e367cacb
commit cb8d623624
2 changed files with 120 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/autotest/arducopter.py
View File

@ -4855,6 +4855,10 @@ class AutoTestCopter(AutoTest):
"Fly Gyro FFT",
self.fly_gyro_fft),
("FixedYawCalibration",
"Test Fixed Yaw Calibration",
self.test_fixed_yaw_calibration),
("LogDownLoad",
"Log download",
lambda: self.log_download(

116
Tools/autotest/common.py
View File

@ -2031,6 +2031,21 @@ class AutoTest(ABC):
m.result))
break
def verify_parameter_values(self, parameter_stuff):
bad = ""
for param in parameter_stuff:
fetched_value = self.get_parameter(param)
wanted_value = parameter_stuff[param]
max_delta = 0.0
if type(wanted_value) == tuple:
max_delta = wanted_value[1]
wanted_value = wanted_value[0]
if abs(fetched_value - wanted_value) > max_delta:
bad += "%s=%f (want=%f) " % (param, fetched_value, wanted_value)
if len(bad):
raise NotAchievedException("Bad parameter values: %s" %
(bad,))
#################################################
# UTILITIES
#################################################
@ -3282,6 +3297,107 @@ class AutoTest(ABC):
self.disarm_vehicle(force=True)
self.reboot_sitl()
def test_fixed_yaw_calibration(self):
self.context_push()
ex = None
try:
wanted = {
"COMPASS_OFS_X": (100, 3.0),
"COMPASS_OFS_Y": (200, 3.0),
"COMPASS_OFS_Z": (300, 3.0),
"COMPASS_DIA_X": 1,
"COMPASS_DIA_Y": 1,
"COMPASS_DIA_Z": 1,
"COMPASS_ODI_X": 0,
"COMPASS_ODI_Y": 0,
"COMPASS_ODI_Z": 0,
"COMPASS_OFS2_X": (100, 3.0),
"COMPASS_OFS2_Y": (200, 3.0),
"COMPASS_OFS2_Z": (300, 3.0),
"COMPASS_DIA2_X": 1,
"COMPASS_DIA2_Y": 1,
"COMPASS_DIA2_Z": 1,
"COMPASS_ODI2_X": 0,
"COMPASS_ODI2_Y": 0,
"COMPASS_ODI2_Z": 0,
"COMPASS_OFS3_X": (100, 3.0),
"COMPASS_OFS3_Y": (200, 3.0),
"COMPASS_OFS3_Z": (300, 3.0),
"COMPASS_DIA3_X": 1,
"COMPASS_DIA3_Y": 1,
"COMPASS_DIA3_Z": 1,
"COMPASS_ODI2_X": 0,
"COMPASS_ODI2_Y": 0,
"COMPASS_ODI2_Z": 0,
}
self.set_parameter("SIM_MAG_OFS_X", 100)
self.set_parameter("SIM_MAG_OFS_Y", 200)
self.set_parameter("SIM_MAG_OFS_Z", 300)
# 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:
value = 0.0
if "DIA" in param:
value = 1.001
elif "ODI" in param:
value = 0.001
self.set_parameter(param, value)
# zero the parameters:
self.progress("zeroing parameters")
self.drain_mav() # these two lines are odd....
self.get_sim_time()
self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS,
2, # param1 (compass0)
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
0 # param7
)
self.progress("zeroed parameters")
# ensure these are all zero; note that we don't set the DIA or
# ODI in SET_SENSOR_OFFSETS...
for axis in "X", "Y", "Z":
name = "COMPASS_OFS_%s" % axis
value = self.get_parameter(name)
if value != 0.0:
raise NotAchievedException("Failed to zero %s; got %f" %
(name, value))
self.change_mode('LOITER')
self.wait_ready_to_arm() # so we definitely have position
ss = self.mav.recv_match(type='SIMSTATE', blocking=True, timeout=1)
if ss is None:
raise NotAchievedException("Did not get SIMSTATE")
self.progress("Got SIMSTATE (%s)" % str(ss))
self.run_cmd(mavutil.mavlink.MAV_CMD_FIXED_MAG_CAL_YAW,
math.degrees(ss.yaw), # param1
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
0 # param7
)
self.verify_parameter_values(wanted)
self.progress("Rebooting and making sure we could arm with these values")
self.reboot_sitl()
self.wait_ready_to_arm(timeout=60)
except Exception as e:
ex = e
self.context_pop()
if ex is not None:
raise ex
def test_dataflash_over_mavlink(self):
self.context_push()
ex = None