mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Tools: add an EKF3 lane switching test to plane
This commit is contained in:
Harshit Kumar Sankhla
Andrew Tridgell
parent
8507c7d3b9
commit
464c90a03c
@ -1834,6 +1834,160 @@ class AutoTestPlane(AutoTest):
|
||||
|
||||
self.fly_mission("ap-terrain.txt", mission_timeout=600)
|
||||
|
||||
def ekf_lane_switch(self):
|
||||
|
||||
self.context_push()
|
||||
ex = None
|
||||
|
||||
# new lane swtich available only with EK3
|
||||
self.set_parameter("EK3_ENABLE", 1)
|
||||
self.set_parameter("EK2_ENABLE", 0)
|
||||
self.set_parameter("AHRS_EKF_TYPE", 3)
|
||||
self.set_parameter("EK3_AFFINITY", 15) # enable affinity for all sensors
|
||||
self.set_parameter("EK3_IMU_MASK", 3) # use only 2 IMUs
|
||||
self.set_parameter("GPS_TYPE2", 1)
|
||||
self.set_parameter("SIM_GPS2_DISABLE", 0)
|
||||
self.set_parameter("SIM_BARO2_DISABL", 0)
|
||||
self.set_parameter("SIM_BARO_COUNT", 2)
|
||||
self.set_parameter("ARSPD2_TYPE", 2)
|
||||
self.set_parameter("ARSPD2_USE", 1)
|
||||
self.set_parameter("ARSPD2_PIN", 2)
|
||||
|
||||
# some parameters need reboot to take effect
|
||||
self.reboot_sitl()
|
||||
|
||||
self.lane_switches = []
|
||||
# add an EKF lane switch hook
|
||||
def statustext_hook(mav, message):
|
||||
if message.get_type() != 'STATUSTEXT':
|
||||
return
|
||||
# example msg: EKF3 lane switch 1
|
||||
if not message.text.startswith("EKF3 lane switch "):
|
||||
return
|
||||
newlane = int(message.text[-1])
|
||||
self.lane_switches.append(newlane)
|
||||
self.install_message_hook(statustext_hook)
|
||||
|
||||
# get flying
|
||||
self.takeoff(alt=50)
|
||||
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
|
||||
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)
|
||||
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.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")
|
||||
# create a barometer error by inhibiting any pressure change while changing altitude
|
||||
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)
|
||||
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.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)
|
||||
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.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")
|
||||
# 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)
|
||||
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.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()
|
||||
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)
|
||||
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.wait_heading(0, accuracy=10, timeout=60)
|
||||
self.wait_heading(180, accuracy=10, timeout=60)
|
||||
###################################################################################################
|
||||
self.progress("GYROSCOPE: Change Y-Axis Offset")
|
||||
# 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)
|
||||
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.disarm_vehicle()
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Caught exception: %s" % self.get_exception_stacktrace(e))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def tests(self):
|
||||
'''return list of all tests'''
|
||||
ret = super(AutoTestPlane, self).tests()
|
||||
@ -1954,5 +2108,9 @@ class AutoTestPlane(AutoTest):
|
||||
("LogUpload",
|
||||
"Log upload",
|
||||
self.log_upload),
|
||||
|
||||
("EKFlaneswitch",
|
||||
"Test EKF3 Affinity and Lane Switching",
|
||||
self.ekf_lane_switch),
|
||||
])
|
||||
return ret
|
||||
|
||||
Loading…
Reference in New Issue
Block a user