From 464c90a03c8f094b7ccf5083950df852847e416b Mon Sep 17 00:00:00 2001 From: Harshit Kumar Sankhla Date: Thu, 16 Jul 2020 15:47:54 +0530 Subject: [PATCH] Tools: add an EKF3 lane switching test to plane --- Tools/autotest/arduplane.py | 158 ++++++++++++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index e72ec290a5..861f3e963f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -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