mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
autotest: bug fixes for ekf lane switch test
This commit is contained in:
Harshit Kumar Sankhla
Peter Barker
parent
c109df3dd5
commit
f0c237d88e
@ -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()
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user