zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

autotest: bug fixes for ekf lane switch test

zr-v5.1
Harshit Kumar Sankhla 4 years ago committed by Peter Barker
parent
c109df3dd5
commit
f0c237d88e
1 changed files with 59 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 109
      Tools/autotest/arduplane.py

109
Tools/autotest/arduplane.py
Unescape View File

@ -1879,110 +1879,119 @@ class AutoTestPlane(AutoTest): @@ -1879,110 +1879,119 @@ class AutoTestPlane(AutoTest):
self.change_mode('CIRCLE')
try:
###################################################################################################
#####################################################################################################################################################
self.progress("Checking EKF3 Lane Switching trigger from all sensors")
###################################################################################################
#####################################################################################################################################################
self.start_subtest("ACCELEROMETER: Change z-axis offset")
# create an accelerometer error by changing the Z-axis offset
self.context_collect("STATUSTEXT")
old_parameter = self.get_parameter("INS_ACCOFFS_Z")
self.set_parameter("INS_ACCOFFS_Z", old_parameter + 5)
self.wait_statustext(text="EKF3 lane switch", timeout=30)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=self.set_parameter("INS_ACCOFFS_Z", old_parameter + 5), check_context=True)
if self.lane_switches != [1]:
raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.set_parameter("INS_ACCOFFS_Z", old_parameter)
self.context_clear_collection("STATUSTEXT")
self.wait_heading(0, accuracy=10, timeout=60)
self.wait_heading(180, accuracy=10, timeout=60)
###################################################################################################
#####################################################################################################################################################
self.start_subtest("BAROMETER: Freeze to last measured value")
old_parameter = self.get_parameter("SIM_BARO2_FREEZE")
self.context_collect("STATUSTEXT")
# create a barometer error by inhibiting any pressure change while changing altitude
old_parameter = self.get_parameter("SIM_BARO2_FREEZE")
self.set_parameter("SIM_BARO2_FREEZE", 1)
self.set_rc(2, 2000)
self.delay_sim_time(2)
self.wait_statustext(text="EKF3 lane switch", timeout=30)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=lambda: self.set_rc(2, 2000), check_context=True)
if self.lane_switches != [1, 0]:
raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.set_rc(2, 1500)
self.set_parameter("SIM_BARO2_FREEZE", old_parameter)
self.context_clear_collection("STATUSTEXT")
self.wait_heading(0, accuracy=10, timeout=60)
self.wait_heading(180, accuracy=10, timeout=60)
###################################################################################################
#####################################################################################################################################################
self.start_subtest("GPS: Apply GPS Velocity Error in NED")
self.context_push()
# create a GPS velocity error by adding a 2m/s noise on each axis
self.set_parameter("SIM_GPS_VERR_X", self.get_parameter("SIM_GPS_VERR_X") + 2)
self.set_parameter("SIM_GPS_VERR_Y", self.get_parameter("SIM_GPS_VERR_Y") + 2)
self.set_parameter("SIM_GPS_VERR_Z", self.get_parameter("SIM_GPS_VERR_Z") + 2)
self.wait_statustext(text="EKF3 lane switch", timeout=30)
self.context_collect("STATUSTEXT")
# create a GPS velocity error by adding a random 2m/s noise on each axis
def sim_gps_verr():
self.set_parameter("SIM_GPS_VERR_X", self.get_parameter("SIM_GPS_VERR_X") + 2)
self.set_parameter("SIM_GPS_VERR_Y", self.get_parameter("SIM_GPS_VERR_Y") + 2)
self.set_parameter("SIM_GPS_VERR_Z", self.get_parameter("SIM_GPS_VERR_Z") + 2)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr(), check_context=True)
if self.lane_switches != [1, 0, 1]:
raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.context_pop()
self.context_clear_collection("STATUSTEXT")
self.wait_heading(0, accuracy=10, timeout=60)
self.wait_heading(180, accuracy=10, timeout=60)
###################################################################################################
#####################################################################################################################################################
self.start_subtest("MAGNETOMETER: Change X-Axis Offset")
old_parameter = self.get_parameter("SIM_MAG_OFS_X")
self.context_collect("STATUSTEXT")
# create a magnetometer error by changing the X-axis offset
self.set_parameter("SIM_MAG2_OFS_X",old_parameter + 250)
self.wait_statustext(text="EKF3 lane switch", timeout=30)
old_parameter = self.get_parameter("SIM_MAG2_OFS_X")
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=self.set_parameter("SIM_MAG2_OFS_X", old_parameter + 150), check_context=True)
if self.lane_switches != [1, 0, 1, 0]:
raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.set_parameter("SIM_MAG2_OFS_X", old_parameter)
self.context_clear_collection("STATUSTEXT")
self.wait_heading(0, accuracy=10, timeout=60)
self.wait_heading(180, accuracy=10, timeout=60)
###################################################################################################
#####################################################################################################################################################
self.start_subtest("AIRSPEED: Fail to constant value")
self.context_push()
self.context_collect("STATUSTEXT")
# create an airspeed sensor error by freezing to the current airspeed then changing the groundspeed
old_parameter = self.get_parameter("SIM_ARSPD_FAIL")
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
# create an airspeed sensor error by freezing to the current airspeed then changing the groundspeed
self.set_parameter("SIM_ARSPD_FAIL", m.airspeed)
self.change_mode("GUIDED")
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
0,
0,
0,
12345, # lat*1e7
12345, # lon*1e7
50 # alt
)
self.delay_sim_time(10)
new_target_groundspeed = m.groundspeed + 5
self.run_cmd(
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
1, # groundspeed
new_target_groundspeed,
-1, # throttle / no change
0, # absolute values
0,
0,
0
)
self.wait_statustext(text="EKF3 lane switch", timeout=30)
def change_speed():
self.change_mode("GUIDED")
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
0,
0,
0,
12345, # lat*1e7
12345, # lon*1e7
50 # alt
)
self.delay_sim_time(5)
new_target_groundspeed = m.groundspeed + 5
self.run_cmd(
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
1, # groundspeed
new_target_groundspeed,
-1, # throttle / no change
0, # absolute values
0,
0,
0
)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=change_speed(), check_context=True)
if self.lane_switches != [1, 0, 1, 0, 1]:
raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.context_pop()
self.change_mode('CIRCLE')
self.context_pop()
self.context_clear_collection("STATUSTEXT")
self.wait_heading(0, accuracy=10, timeout=60)
self.wait_heading(180, accuracy=10, timeout=60)
###################################################################################################
#####################################################################################################################################################
self.progress("GYROSCOPE: Change Y-Axis Offset")
self.context_collect("STATUSTEXT")
# create a gyroscope error by changing the Y-axis offset
old_parameter = self.get_parameter("INS_GYR2OFFS_Y")
self.set_parameter("INS_GYR2OFFS_Y", old_parameter + 1)
self.wait_statustext(text="EKF3 lane switch", timeout=30)
self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=self.set_parameter("INS_GYR2OFFS_Y", old_parameter + 1), check_context=True)
if self.lane_switches != [1, 0, 1, 0, 1, 0]:
raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1]))
# Cleanup
self.set_parameter("INS_GYR2OFFS_Y", old_parameter)
###################################################################################################
self.context_clear_collection("STATUSTEXT")
#####################################################################################################################################################
self.disarm_vehicle()

Write Preview
Loading…
Cancel
Save