Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/Tools/autotest/arducopter.py
Views: 1798
'''1Fly Copter in SITL23AP_FLAKE8_CLEAN4'''56from __future__ import print_function7import copy8import math9import os10import shutil11import time12import numpy1314from pymavlink import quaternion15from pymavlink import mavutil16from pymavlink import mavextra17from pymavlink import rotmat1819from pysim import util20from pysim import vehicleinfo2122import vehicle_test_suite2324from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException25from vehicle_test_suite import Test26from vehicle_test_suite import MAV_POS_TARGET_TYPE_MASK27from vehicle_test_suite import WaitAndMaintainArmed28from vehicle_test_suite import WaitModeTimeout2930from pymavlink.rotmat import Vector33132# get location of scripts33testdir = os.path.dirname(os.path.realpath(__file__))34SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270)3536# Flight mode switch positions are set-up in arducopter.param to be37# switch 1 = Circle38# switch 2 = Land39# switch 3 = RTL40# switch 4 = Auto41# switch 5 = Loiter42# switch 6 = Stabilize434445class AutoTestCopter(vehicle_test_suite.TestSuite):46@staticmethod47def get_not_armable_mode_list():48return ["AUTO", "AUTOTUNE", "BRAKE", "CIRCLE", "FLIP", "LAND", "RTL", "SMART_RTL", "AVOID_ADSB", "FOLLOW"]4950@staticmethod51def get_not_disarmed_settable_modes_list():52return ["FLIP", "AUTOTUNE"]5354@staticmethod55def get_no_position_not_settable_modes_list():56return []5758@staticmethod59def get_position_armable_modes_list():60return ["DRIFT", "GUIDED", "LOITER", "POSHOLD", "THROW"]6162@staticmethod63def get_normal_armable_modes_list():64return ["ACRO", "ALT_HOLD", "STABILIZE", "GUIDED_NOGPS"]6566def log_name(self):67return "ArduCopter"6869def test_filepath(self):70return os.path.realpath(__file__)7172def default_speedup(self):73return 1007475def set_current_test_name(self, name):76self.current_test_name_directory = "ArduCopter_Tests/" + name + "/"7778def sitl_start_location(self):79return SITL_START_LOCATION8081def mavproxy_options(self):82ret = super(AutoTestCopter, self).mavproxy_options()83if self.frame != 'heli':84ret.append('--quadcopter')85return ret8687def sitl_streamrate(self):88return 58990def vehicleinfo_key(self):91return 'ArduCopter'9293def default_frame(self):94return "+"9596def apply_defaultfile_parameters(self):97# Copter passes in a defaults_filepath in place of applying98# parameters afterwards.99pass100101def defaults_filepath(self):102return self.model_defaults_filepath(self.frame)103104def wait_disarmed_default_wait_time(self):105return 120106107def close(self):108super(AutoTestCopter, self).close()109110# [2014/05/07] FC Because I'm doing a cross machine build111# (source is on host, build is on guest VM) I cannot hard link112# This flag tells me that I need to copy the data out113if self.copy_tlog:114shutil.copy(self.logfile, self.buildlog)115116def is_copter(self):117return True118119def get_stick_arming_channel(self):120return int(self.get_parameter("RCMAP_YAW"))121122def get_disarm_delay(self):123return int(self.get_parameter("DISARM_DELAY"))124125def set_autodisarm_delay(self, delay):126self.set_parameter("DISARM_DELAY", delay)127128def takeoff(self,129alt_min=30,130takeoff_throttle=1700,131require_absolute=True,132mode="STABILIZE",133timeout=120,134max_err=5):135"""Takeoff get to 30m altitude."""136self.progress("TAKEOFF")137self.change_mode(mode)138if not self.armed():139self.wait_ready_to_arm(require_absolute=require_absolute, timeout=timeout)140self.zero_throttle()141self.arm_vehicle()142if mode == 'GUIDED':143self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err)144else:145self.set_rc(3, takeoff_throttle)146self.wait_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout)147self.hover()148self.progress("TAKEOFF COMPLETE")149150def land_and_disarm(self, timeout=60):151"""Land the quad."""152self.progress("STARTING LANDING")153self.change_mode("LAND")154self.wait_landed_and_disarmed(timeout=timeout)155156def wait_landed_and_disarmed(self, min_alt=6, timeout=60):157"""Wait to be landed and disarmed"""158m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)159alt = m.relative_alt / 1000.0 # mm -> m160if alt > min_alt:161self.wait_altitude(min_alt-1, min_alt+5, relative=True, timeout=timeout)162# self.wait_statustext("SIM Hit ground", timeout=timeout)163self.wait_disarmed()164165def hover(self, hover_throttle=1500):166self.set_rc(3, hover_throttle)167168# Climb/descend to a given altitude169def setAlt(self, desiredAlt=50):170pos = self.mav.location(relative_alt=True)171if pos.alt > desiredAlt:172self.set_rc(3, 1300)173self.wait_altitude((desiredAlt-5), desiredAlt, relative=True)174if pos.alt < (desiredAlt-5):175self.set_rc(3, 1800)176self.wait_altitude((desiredAlt-5), desiredAlt, relative=True)177self.hover()178179# Takeoff, climb to given altitude, and fly east for 10 seconds180def takeoffAndMoveAway(self, dAlt=50, dDist=50):181self.progress("Centering sticks")182self.set_rc_from_map({1831: 1500,1842: 1500,1853: 1000,1864: 1500,187})188self.takeoff(alt_min=dAlt, mode='GUIDED')189self.change_mode("ALT_HOLD")190191self.progress("Yaw to east")192self.set_rc(4, 1580)193self.wait_heading(90)194self.set_rc(4, 1500)195196self.progress("Fly eastbound away from home")197self.set_rc(2, 1800)198self.delay_sim_time(10)199self.set_rc(2, 1500)200self.hover()201self.progress("Copter staging 50 meters east of home at 50 meters altitude In mode Alt Hold")202203# loiter - fly south west, then loiter within 5m position and altitude204def ModeLoiter(self, holdtime=10, maxaltchange=5, maxdistchange=5):205"""Hold loiter position."""206self.takeoff(10, mode="LOITER")207208# first aim south east209self.progress("turn south east")210self.set_rc(4, 1580)211self.wait_heading(170)212self.set_rc(4, 1500)213214# fly south east 50m215self.set_rc(2, 1100)216self.wait_distance(50)217self.set_rc(2, 1500)218219# wait for copter to slow moving220self.wait_groundspeed(0, 2)221222m = self.mav.recv_match(type='VFR_HUD', blocking=True)223start_altitude = m.alt224start = self.mav.location()225tstart = self.get_sim_time()226self.progress("Holding loiter at %u meters for %u seconds" %227(start_altitude, holdtime))228while self.get_sim_time_cached() < tstart + holdtime:229m = self.mav.recv_match(type='VFR_HUD', blocking=True)230pos = self.mav.location()231delta = self.get_distance(start, pos)232alt_delta = math.fabs(m.alt - start_altitude)233self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))234if alt_delta > maxaltchange:235raise NotAchievedException(236"Loiter alt shifted %u meters (> limit %u)" %237(alt_delta, maxaltchange))238if delta > maxdistchange:239raise NotAchievedException(240"Loiter shifted %u meters (> limit of %u)" %241(delta, maxdistchange))242self.progress("Loiter OK for %u seconds" % holdtime)243244self.progress("Climb to 30m")245self.change_alt(30)246247self.progress("Descend to 20m")248self.change_alt(20)249250self.do_RTL()251252def ModeAltHold(self):253'''Test AltHold Mode'''254self.takeoff(10, mode="ALT_HOLD")255self.watch_altitude_maintained(altitude_min=9, altitude_max=11)256# feed in full elevator and aileron input and make sure we257# retain altitude:258self.set_rc_from_map({2591: 1000,2602: 1000,261})262self.watch_altitude_maintained(altitude_min=9, altitude_max=11)263self.set_rc_from_map({2641: 1500,2652: 1500,266})267self.do_RTL()268269def fly_to_origin(self, final_alt=10):270origin = self.poll_message("GPS_GLOBAL_ORIGIN")271self.change_mode("GUIDED")272self.guided_move_global_relative_alt(origin.latitude,273origin.longitude,274final_alt)275276def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080):277"""Change altitude."""278def adjust_altitude(current_alt, target_alt, accuracy):279if math.fabs(current_alt - target_alt) <= accuracy:280self.hover()281elif current_alt < target_alt:282self.set_rc(3, climb_throttle)283else:284self.set_rc(3, descend_throttle)285self.wait_altitude(286(alt_min - 5),287alt_min,288relative=True,289called_function=lambda current_alt, target_alt: adjust_altitude(current_alt, target_alt, 1)290)291self.hover()292293def RecordThenPlayMission(self, side=50, timeout=300):294'''Use switches to toggle in mission, then fly it'''295self.takeoff(20, mode="ALT_HOLD")296297"""Fly a square, flying N then E ."""298tstart = self.get_sim_time()299300# ensure all sticks in the middle301self.set_rc_from_map({3021: 1500,3032: 1500,3043: 1500,3054: 1500,306})307308# switch to loiter mode temporarily to stop us from rising309self.change_mode('LOITER')310311# first aim north312self.progress("turn right towards north")313self.set_rc(4, 1580)314self.wait_heading(10)315self.set_rc(4, 1500)316317# save bottom left corner of box as waypoint318self.progress("Save WP 1 & 2")319self.save_wp()320321# switch back to ALT_HOLD mode322self.change_mode('ALT_HOLD')323324# pitch forward to fly north325self.progress("Going north %u meters" % side)326self.set_rc(2, 1300)327self.wait_distance(side)328self.set_rc(2, 1500)329330# save top left corner of square as waypoint331self.progress("Save WP 3")332self.save_wp()333334# roll right to fly east335self.progress("Going east %u meters" % side)336self.set_rc(1, 1700)337self.wait_distance(side)338self.set_rc(1, 1500)339340# save top right corner of square as waypoint341self.progress("Save WP 4")342self.save_wp()343344# pitch back to fly south345self.progress("Going south %u meters" % side)346self.set_rc(2, 1700)347self.wait_distance(side)348self.set_rc(2, 1500)349350# save bottom right corner of square as waypoint351self.progress("Save WP 5")352self.save_wp()353354# roll left to fly west355self.progress("Going west %u meters" % side)356self.set_rc(1, 1300)357self.wait_distance(side)358self.set_rc(1, 1500)359360# save bottom left corner of square (should be near home) as waypoint361self.progress("Save WP 6")362self.save_wp()363364# reduce throttle again365self.set_rc(3, 1500)366367# descend to 10m368self.progress("Descend to 10m in Loiter")369self.change_mode('LOITER')370self.set_rc(3, 1200)371time_left = timeout - (self.get_sim_time() - tstart)372self.progress("timeleft = %u" % time_left)373if time_left < 20:374time_left = 20375self.wait_altitude(-10, 10, timeout=time_left, relative=True)376self.set_rc(3, 1500)377self.save_wp()378379# save the stored mission to file380mavproxy = self.start_mavproxy()381num_wp = self.save_mission_to_file_using_mavproxy(382mavproxy,383os.path.join(testdir, "ch7_mission.txt"))384self.stop_mavproxy(mavproxy)385if not num_wp:386raise NotAchievedException("save_mission_to_file failed")387388self.progress("test: Fly a mission from 1 to %u" % num_wp)389self.change_mode('AUTO')390self.set_current_waypoint(1)391self.wait_waypoint(0, num_wp-1, timeout=500)392self.progress("test: MISSION COMPLETE: passed!")393self.land_and_disarm()394395# enter RTL mode and wait for the vehicle to disarm396def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250, quiet=False):397"""Enter RTL mode and wait for the vehicle to disarm at Home."""398self.change_mode("RTL")399self.hover()400self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout, quiet=True)401402def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250, quiet=False):403"""Wait for RTL to reach home and disarm"""404self.progress("Waiting RTL to reach Home and disarm")405tstart = self.get_sim_time()406while self.get_sim_time_cached() < tstart + timeout:407m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)408alt = m.relative_alt / 1000.0 # mm -> m409home_distance = self.distance_to_home(use_cached_home=True)410home = ""411alt_valid = alt <= 1412distance_valid = home_distance < distance_max413if check_alt:414if alt_valid and distance_valid:415home = "HOME"416else:417if distance_valid:418home = "HOME"419if not quiet:420self.progress("Alt: %.02f HomeDist: %.02f %s" %421(alt, home_distance, home))422423# our post-condition is that we are disarmed:424if not self.armed():425if home == "":426raise NotAchievedException("Did not get home")427# success!428return429430raise AutoTestTimeoutException("Did not get home and disarm")431432def LoiterToAlt(self):433"""Loiter-To-Alt"""434435self.context_push()436437self.set_parameters({438"PLND_ENABLED": 1,439"PLND_TYPE": 4,440})441442self.set_analog_rangefinder_parameters()443444self.reboot_sitl()445446num_wp = self.load_mission("copter_loiter_to_alt.txt")447448self.change_mode('LOITER')449450self.install_terrain_handlers_context()451self.wait_ready_to_arm()452453self.arm_vehicle()454455self.change_mode('AUTO')456457self.set_rc(3, 1550)458459self.wait_current_waypoint(2)460461self.set_rc(3, 1500)462463self.wait_waypoint(0, num_wp-1, timeout=500)464465self.wait_disarmed()466467self.context_pop()468self.reboot_sitl()469470# Tests all actions and logic behind the radio failsafe471def ThrottleFailsafe(self, side=60, timeout=360):472'''Test Throttle Failsafe'''473self.start_subtest("If you haven't taken off yet RC failure should be instant disarm")474self.change_mode("STABILIZE")475self.set_parameter("DISARM_DELAY", 0)476self.arm_vehicle()477self.set_parameter("SIM_RC_FAIL", 1)478self.disarm_wait(timeout=1)479self.set_parameter("SIM_RC_FAIL", 0)480self.set_parameter("DISARM_DELAY", 10)481482# Trigger an RC failure with the failsafe disabled. Verify no action taken.483self.start_subtest("Radio failsafe disabled test: FS_THR_ENABLE=0 should take no failsafe action")484self.set_parameter('FS_THR_ENABLE', 0)485self.set_parameter('FS_OPTIONS', 0)486self.takeoffAndMoveAway()487self.set_parameter("SIM_RC_FAIL", 1)488self.delay_sim_time(5)489self.wait_mode("ALT_HOLD")490self.set_parameter("SIM_RC_FAIL", 0)491self.delay_sim_time(5)492self.wait_mode("ALT_HOLD")493self.end_subtest("Completed Radio failsafe disabled test")494495# Trigger an RC failure, verify radio failsafe triggers,496# restore radio, verify RC function by changing modes to cicle497# and stabilize.498self.start_subtest("Radio failsafe recovery test")499self.set_parameter('FS_THR_ENABLE', 1)500self.set_parameter("SIM_RC_FAIL", 1)501self.wait_mode("RTL")502self.delay_sim_time(5)503self.set_parameter("SIM_RC_FAIL", 0)504self.delay_sim_time(5)505self.set_rc(5, 1050)506self.wait_mode("CIRCLE")507self.set_rc(5, 1950)508self.wait_mode("STABILIZE")509self.end_subtest("Completed Radio failsafe recovery test")510511# Trigger and RC failure, verify failsafe triggers and RTL completes512self.start_subtest("Radio failsafe RTL with no options test: FS_THR_ENABLE=1 & FS_OPTIONS=0")513self.set_parameter("SIM_RC_FAIL", 1)514self.wait_mode("RTL")515self.wait_rtl_complete()516self.set_parameter("SIM_RC_FAIL", 0)517self.end_subtest("Completed Radio failsafe RTL with no options test")518519# Trigger and RC failure, verify failsafe triggers and land completes520self.start_subtest("Radio failsafe LAND with no options test: FS_THR_ENABLE=3 & FS_OPTIONS=0")521self.set_parameter('FS_THR_ENABLE', 3)522self.takeoffAndMoveAway()523self.set_parameter("SIM_RC_FAIL", 1)524self.wait_mode("LAND")525self.wait_landed_and_disarmed()526self.set_parameter("SIM_RC_FAIL", 0)527self.end_subtest("Completed Radio failsafe LAND with no options test")528529# Trigger and RC failure, verify failsafe triggers and SmartRTL completes530self.start_subtest("Radio failsafe SmartRTL->RTL with no options test: FS_THR_ENABLE=4 & FS_OPTIONS=0")531self.set_parameter('FS_THR_ENABLE', 4)532self.takeoffAndMoveAway()533self.set_parameter("SIM_RC_FAIL", 1)534self.wait_mode("SMART_RTL")535self.wait_disarmed()536self.set_parameter("SIM_RC_FAIL", 0)537self.end_subtest("Completed Radio failsafe SmartRTL->RTL with no options test")538539# Trigger and RC failure, verify failsafe triggers and SmartRTL completes540self.start_subtest("Radio failsafe SmartRTL->Land with no options test: FS_THR_ENABLE=5 & FS_OPTIONS=0")541self.set_parameter('FS_THR_ENABLE', 5)542self.takeoffAndMoveAway()543self.set_parameter("SIM_RC_FAIL", 1)544self.wait_mode("SMART_RTL")545self.wait_disarmed()546self.set_parameter("SIM_RC_FAIL", 0)547self.end_subtest("Completed Radio failsafe SmartRTL_Land with no options test")548549# Trigger a GPS failure and RC failure, verify RTL fails into550# land mode and completes551self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.")552self.set_parameter('FS_THR_ENABLE', 1)553self.takeoffAndMoveAway()554self.set_parameter('SIM_GPS1_ENABLE', 0)555self.delay_sim_time(5)556self.set_parameter("SIM_RC_FAIL", 1)557self.wait_mode("LAND")558self.wait_landed_and_disarmed()559self.set_parameter("SIM_RC_FAIL", 0)560self.set_parameter('SIM_GPS1_ENABLE', 1)561self.wait_ekf_happy()562self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.")563564# Trigger a GPS failure and RC failure, verify SmartRTL fails565# into land mode and completes566self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")567self.set_parameter('FS_THR_ENABLE', 4)568self.takeoffAndMoveAway()569self.set_parameter('SIM_GPS1_ENABLE', 0)570self.delay_sim_time(5)571self.set_parameter("SIM_RC_FAIL", 1)572self.wait_mode("LAND")573self.wait_landed_and_disarmed()574self.set_parameter("SIM_RC_FAIL", 0)575self.set_parameter('SIM_GPS1_ENABLE', 1)576self.wait_ekf_happy()577self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")578579# Trigger a GPS failure and RC failure, verify SmartRTL fails580# into land mode and completes581self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")582self.set_parameter('FS_THR_ENABLE', 5)583self.takeoffAndMoveAway()584self.set_parameter('SIM_GPS1_ENABLE', 0)585self.delay_sim_time(5)586self.set_parameter("SIM_RC_FAIL", 1)587self.wait_mode("LAND")588self.wait_landed_and_disarmed()589self.set_parameter("SIM_RC_FAIL", 0)590self.set_parameter('SIM_GPS1_ENABLE', 1)591self.wait_ekf_happy()592self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")593594# Trigger a GPS failure, then restore the GPS. Trigger an RC595# failure, verify SmartRTL fails into RTL and completes596self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")597self.set_parameter('FS_THR_ENABLE', 4)598self.takeoffAndMoveAway()599self.set_parameter('SIM_GPS1_ENABLE', 0)600self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)601self.set_parameter('SIM_GPS1_ENABLE', 1)602self.wait_ekf_happy()603self.delay_sim_time(5)604self.set_parameter("SIM_RC_FAIL", 1)605self.wait_mode("RTL")606self.wait_rtl_complete()607self.set_parameter("SIM_RC_FAIL", 0)608self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")609610# Trigger a GPS failure, then restore the GPS. Trigger an RC611# failure, verify SmartRTL fails into Land and completes612self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.")613self.set_parameter('FS_THR_ENABLE', 5)614self.takeoffAndMoveAway()615self.set_parameter('SIM_GPS1_ENABLE', 0)616self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)617self.set_parameter('SIM_GPS1_ENABLE', 1)618self.wait_ekf_happy()619self.delay_sim_time(5)620self.set_parameter("SIM_RC_FAIL", 1)621self.wait_mode("LAND")622self.wait_landed_and_disarmed()623self.set_parameter("SIM_RC_FAIL", 0)624self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to no path.")625626# Trigger an RC failure in guided mode with the option enabled627# to continue in guided. Verify no failsafe action takes place628self.start_subtest("Radio failsafe with option to continue in guided mode: FS_THR_ENABLE=1 & FS_OPTIONS=4")629self.set_parameter("SYSID_MYGCS", self.mav.source_system)630self.setGCSfailsafe(1)631self.set_parameter('FS_THR_ENABLE', 1)632self.set_parameter('FS_OPTIONS', 4)633self.takeoffAndMoveAway()634self.change_mode("GUIDED")635self.set_parameter("SIM_RC_FAIL", 1)636self.delay_sim_time(5)637self.wait_mode("GUIDED")638self.set_parameter("SIM_RC_FAIL", 0)639self.delay_sim_time(5)640self.change_mode("ALT_HOLD")641self.setGCSfailsafe(0)642# self.change_mode("RTL")643# self.wait_disarmed()644self.end_subtest("Completed Radio failsafe with option to continue in guided mode")645646# Trigger an RC failure in AUTO mode with the option enabled647# to continue the mission. Verify no failsafe action takes648# place649self.start_subtest("Radio failsafe RTL with option to continue mission: FS_THR_ENABLE=1 & FS_OPTIONS=1")650self.set_parameter('FS_OPTIONS', 1)651self.progress("# Load copter_mission")652num_wp = self.load_mission("copter_mission.txt", strict=False)653if not num_wp:654raise NotAchievedException("load copter_mission failed")655# self.takeoffAndMoveAway()656self.change_mode("AUTO")657self.set_parameter("SIM_RC_FAIL", 1)658self.delay_sim_time(5)659self.wait_mode("AUTO")660self.set_parameter("SIM_RC_FAIL", 0)661self.delay_sim_time(5)662self.wait_mode("AUTO")663# self.change_mode("RTL")664# self.wait_disarmed()665self.end_subtest("Completed Radio failsafe RTL with option to continue mission")666667# Trigger an RC failure in AUTO mode without the option668# enabled to continue. Verify failsafe triggers and RTL669# completes670self.start_subtest("Radio failsafe RTL in mission without "671"option to continue should RTL: FS_THR_ENABLE=1 & FS_OPTIONS=0")672self.set_parameter('FS_OPTIONS', 0)673self.set_parameter("SIM_RC_FAIL", 1)674self.wait_mode("RTL")675self.wait_rtl_complete()676self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)677self.set_parameter("SIM_RC_FAIL", 0)678self.end_subtest("Completed Radio failsafe RTL in mission without option to continue")679680self.progress("All radio failsafe tests complete")681self.set_parameter('FS_THR_ENABLE', 0)682self.reboot_sitl()683684def ThrottleFailsafePassthrough(self):685'''check servo passthrough on RC failsafe. Make sure it doesn't glitch to the bad RC input value'''686channel = 7687trim_value = 1450688self.set_parameters({689'RC%u_MIN' % channel: 1000,690'RC%u_MAX' % channel: 2000,691'SERVO%u_MIN' % channel: 1000,692'SERVO%u_MAX' % channel: 2000,693'SERVO%u_TRIM' % channel: trim_value,694'SERVO%u_FUNCTION' % channel: 146, # scaled passthrough for channel 7695'FS_THR_ENABLE': 1,696'RC_FS_TIMEOUT': 10,697'SERVO_RC_FS_MSK': 1 << (channel-1),698})699700self.reboot_sitl()701702self.context_set_message_rate_hz('SERVO_OUTPUT_RAW', 200)703704self.set_rc(channel, 1799)705expected_servo_output_value = 1778 # 1778 because of weird trim706self.wait_servo_channel_value(channel, expected_servo_output_value)707# receiver goes into failsafe with wild override values:708709def ensure_SERVO_values_never_input(mav, m):710if m.get_type() != "SERVO_OUTPUT_RAW":711return712value = getattr(m, "servo%u_raw" % channel)713if value != expected_servo_output_value and value != trim_value:714raise NotAchievedException("Bad servo value %u received" % value)715716self.install_message_hook_context(ensure_SERVO_values_never_input)717self.progress("Forcing receiver into failsafe")718self.set_rc_from_map({7193: 800,720channel: 1300,721})722self.wait_servo_channel_value(channel, trim_value)723self.delay_sim_time(10)724725# Tests all actions and logic behind the GCS failsafe726def GCSFailsafe(self, side=60, timeout=360):727'''Test GCS Failsafe'''728try:729self.test_gcs_failsafe(side=side, timeout=timeout)730except Exception as ex:731self.setGCSfailsafe(0)732self.set_parameter('FS_OPTIONS', 0)733self.disarm_vehicle(force=True)734self.reboot_sitl()735raise ex736737def test_gcs_failsafe(self, side=60, timeout=360):738# Test double-SmartRTL; ensure we do SmarRTL twice rather than739# landing (tests fix for actual bug)740self.set_parameter("SYSID_MYGCS", self.mav.source_system)741self.context_push()742self.start_subtest("GCS failsafe SmartRTL twice")743self.setGCSfailsafe(3)744self.set_parameter('FS_OPTIONS', 8)745self.takeoffAndMoveAway()746self.set_heartbeat_rate(0)747self.wait_mode("SMART_RTL")748self.wait_disarmed()749self.set_heartbeat_rate(self.speedup)750self.wait_statustext("GCS Failsafe Cleared", timeout=60)751752self.takeoffAndMoveAway()753self.set_heartbeat_rate(0)754self.wait_statustext("GCS Failsafe")755756def ensure_smartrtl(mav, m):757if m.get_type() != "HEARTBEAT":758return759# can't use mode_is here because we're in the message hook760print("Mode: %s" % self.mav.flightmode)761if self.mav.flightmode != "SMART_RTL":762raise NotAchievedException("Not in SMART_RTL")763self.install_message_hook_context(ensure_smartrtl)764765self.set_heartbeat_rate(self.speedup)766self.wait_statustext("GCS Failsafe Cleared", timeout=60)767self.set_heartbeat_rate(0)768self.wait_statustext("GCS Failsafe")769770self.wait_disarmed()771772self.end_subtest("GCS failsafe SmartRTL twice")773self.set_heartbeat_rate(self.speedup)774self.wait_statustext("GCS Failsafe Cleared", timeout=60)775self.context_pop()776777# Trigger telemetry loss with failsafe disabled. Verify no action taken.778self.start_subtest("GCS failsafe disabled test: FS_GCS_ENABLE=0 should take no failsafe action")779self.setGCSfailsafe(0)780self.takeoffAndMoveAway()781self.set_heartbeat_rate(0)782self.delay_sim_time(5)783self.wait_mode("ALT_HOLD")784self.set_heartbeat_rate(self.speedup)785self.delay_sim_time(5)786self.wait_mode("ALT_HOLD")787self.end_subtest("Completed GCS failsafe disabled test")788789# Trigger telemetry loss with failsafe enabled. Verify790# failsafe triggers to RTL. Restore telemetry, verify failsafe791# clears, and change modes.792self.start_subtest("GCS failsafe recovery test: FS_GCS_ENABLE=1 & FS_OPTIONS=0")793self.setGCSfailsafe(1)794self.set_parameter('FS_OPTIONS', 0)795self.set_heartbeat_rate(0)796self.wait_mode("RTL")797self.set_heartbeat_rate(self.speedup)798self.wait_statustext("GCS Failsafe Cleared", timeout=60)799self.change_mode("LOITER")800self.end_subtest("Completed GCS failsafe recovery test")801802# Trigger telemetry loss with failsafe enabled. Verify803# failsafe triggers to RTL. Restore telemetry, verify failsafe804# clears, and change modes.805self.start_subtest("GCS failsafe recovery test: FS_GCS_ENABLE=1 & FS_OPTIONS=0 & FS_GCS_TIMEOUT=10")806self.setGCSfailsafe(1)807self.set_parameter('FS_OPTIONS', 0)808old_gcs_timeout = self.get_parameter("FS_GCS_TIMEOUT")809new_gcs_timeout = old_gcs_timeout * 2810self.set_parameter("FS_GCS_TIMEOUT", new_gcs_timeout)811self.set_heartbeat_rate(0)812self.delay_sim_time(old_gcs_timeout + (new_gcs_timeout - old_gcs_timeout) / 2)813self.assert_mode("LOITER")814self.wait_mode("RTL")815self.set_heartbeat_rate(self.speedup)816self.wait_statustext("GCS Failsafe Cleared", timeout=60)817self.change_mode("LOITER")818self.set_parameter('FS_GCS_TIMEOUT', old_gcs_timeout)819self.end_subtest("Completed GCS failsafe recovery test")820821# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and RTL completes822self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1 & FS_OPTIONS=0")823self.setGCSfailsafe(1)824self.set_parameter('FS_OPTIONS', 0)825self.set_heartbeat_rate(0)826self.wait_mode("RTL")827self.wait_rtl_complete()828self.set_heartbeat_rate(self.speedup)829self.wait_statustext("GCS Failsafe Cleared", timeout=60)830self.end_subtest("Completed GCS failsafe RTL with no options test")831832# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and land completes833self.start_subtest("GCS failsafe LAND with no options test: FS_GCS_ENABLE=5 & FS_OPTIONS=0")834self.setGCSfailsafe(5)835self.takeoffAndMoveAway()836self.set_heartbeat_rate(0)837self.wait_mode("LAND")838self.wait_landed_and_disarmed()839self.set_heartbeat_rate(self.speedup)840self.wait_statustext("GCS Failsafe Cleared", timeout=60)841self.end_subtest("Completed GCS failsafe land with no options test")842843# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and SmartRTL completes844self.start_subtest("GCS failsafe SmartRTL->RTL with no options test: FS_GCS_ENABLE=3 & FS_OPTIONS=0")845self.setGCSfailsafe(3)846self.takeoffAndMoveAway()847self.set_heartbeat_rate(0)848self.wait_mode("SMART_RTL")849self.wait_disarmed()850self.set_heartbeat_rate(self.speedup)851self.wait_statustext("GCS Failsafe Cleared", timeout=60)852self.end_subtest("Completed GCS failsafe SmartRTL->RTL with no options test")853854# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and SmartRTL completes855self.start_subtest("GCS failsafe SmartRTL->Land with no options test: FS_GCS_ENABLE=4 & FS_OPTIONS=0")856self.setGCSfailsafe(4)857self.takeoffAndMoveAway()858self.set_heartbeat_rate(0)859self.wait_mode("SMART_RTL")860self.wait_disarmed()861self.set_heartbeat_rate(self.speedup)862self.wait_statustext("GCS Failsafe Cleared", timeout=60)863self.end_subtest("Completed GCS failsafe SmartRTL->Land with no options test")864865# Trigger telemetry loss with an invalid failsafe value. Verify failsafe triggers and RTL completes866self.start_subtest("GCS failsafe invalid value with no options test: FS_GCS_ENABLE=99 & FS_OPTIONS=0")867self.setGCSfailsafe(99)868self.takeoffAndMoveAway()869self.set_heartbeat_rate(0)870self.wait_mode("RTL")871self.wait_rtl_complete()872self.set_heartbeat_rate(self.speedup)873self.wait_statustext("GCS Failsafe Cleared", timeout=60)874self.end_subtest("Completed GCS failsafe invalid value with no options test")875876# Trigger telemetry loss with failsafe enabled to test FS_OPTIONS settings877self.start_subtest("GCS failsafe with option bit tests: FS_GCS_ENABLE=1 & FS_OPTIONS=64/2/16")878num_wp = self.load_mission("copter_mission.txt", strict=False)879if not num_wp:880raise NotAchievedException("load copter_mission failed")881self.setGCSfailsafe(1)882self.set_parameter('FS_OPTIONS', 16)883self.takeoffAndMoveAway()884self.progress("Testing continue in pilot controlled modes")885self.set_heartbeat_rate(0)886self.wait_statustext("GCS Failsafe - Continuing Pilot Control", timeout=60)887self.delay_sim_time(5)888self.wait_mode("ALT_HOLD")889self.set_heartbeat_rate(self.speedup)890self.wait_statustext("GCS Failsafe Cleared", timeout=60)891892self.progress("Testing continue in auto mission")893self.set_parameter('FS_OPTIONS', 2)894self.change_mode("AUTO")895self.delay_sim_time(5)896self.set_heartbeat_rate(0)897self.wait_statustext("GCS Failsafe - Continuing Auto Mode", timeout=60)898self.delay_sim_time(5)899self.wait_mode("AUTO")900self.set_heartbeat_rate(self.speedup)901self.wait_statustext("GCS Failsafe Cleared", timeout=60)902903self.progress("Testing continue landing in land mode")904self.set_parameter('FS_OPTIONS', 8)905self.change_mode("LAND")906self.delay_sim_time(5)907self.set_heartbeat_rate(0)908self.wait_statustext("GCS Failsafe - Continuing Landing", timeout=60)909self.delay_sim_time(5)910self.wait_mode("LAND")911self.wait_landed_and_disarmed()912self.set_heartbeat_rate(self.speedup)913self.wait_statustext("GCS Failsafe Cleared", timeout=60)914self.end_subtest("Completed GCS failsafe with option bits")915916self.setGCSfailsafe(0)917self.set_parameter('FS_OPTIONS', 0)918self.progress("All GCS failsafe tests complete")919920def CustomController(self, timeout=300):921'''Test Custom Controller'''922self.progress("Configure custom controller parameters")923self.set_parameters({924'CC_TYPE': 2,925'CC_AXIS_MASK': 7,926'RC6_OPTION': 109,927})928self.set_rc(6, 1000)929self.reboot_sitl()930931if self.get_parameter("CC_TYPE") != 2 :932raise NotAchievedException("Custom controller is not switched to PID backend.")933934# check if we can retrive any param inside PID backend935self.get_parameter("CC2_RAT_YAW_P")936937# takeoff in GPS mode and switch to CIRCLE938self.takeoff(10, mode="LOITER", takeoff_throttle=2000)939self.change_mode("CIRCLE")940941self.context_push()942self.context_collect('STATUSTEXT')943944# switch custom controller on945self.set_rc(6, 2000)946self.wait_statustext("Custom controller is ON", check_context=True)947948# wait 20 second to see if the custom controller destabilize the aircraft949if self.wait_altitude(7, 13, relative=True, minimum_duration=20) :950raise NotAchievedException("Custom controller is not stable.")951952# switch custom controller off953self.set_rc(6, 1000)954self.wait_statustext("Custom controller is OFF", check_context=True)955956self.context_pop()957self.do_RTL()958self.progress("Custom controller test complete")959960# Tests all actions and logic behind the battery failsafe961def BatteryFailsafe(self, timeout=300):962'''Fly Battery Failsafe'''963self.progress("Configure battery failsafe parameters")964self.set_parameters({965'SIM_SPEEDUP': 4,966'BATT_LOW_VOLT': 11.5,967'BATT_CRT_VOLT': 10.1,968'BATT_FS_LOW_ACT': 0,969'BATT_FS_CRT_ACT': 0,970'FS_OPTIONS': 0,971'SIM_BATT_VOLTAGE': 12.5,972})973974# Trigger low battery condition with failsafe disabled. Verify975# no action taken.976self.start_subtest("Batt failsafe disabled test")977self.takeoffAndMoveAway()978m = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)979if m.charge_state != mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_OK:980raise NotAchievedException("Expected state ok")981self.set_parameter('SIM_BATT_VOLTAGE', 11.4)982self.wait_statustext("Battery 1 is low", timeout=60)983m = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)984if m.charge_state != mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_LOW:985raise NotAchievedException("Expected state low")986self.delay_sim_time(5)987self.wait_mode("ALT_HOLD")988self.set_parameter('SIM_BATT_VOLTAGE', 10.0)989self.wait_statustext("Battery 1 is critical", timeout=60)990m = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)991if m.charge_state != mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_CRITICAL:992raise NotAchievedException("Expected state critical")993self.delay_sim_time(5)994self.wait_mode("ALT_HOLD")995self.change_mode("RTL")996self.wait_rtl_complete()997self.set_parameter('SIM_BATT_VOLTAGE', 12.5)998self.reboot_sitl()999self.end_subtest("Completed Batt failsafe disabled test")10001001# TWO STAGE BATTERY FAILSAFE: Trigger low battery condition,1002# then critical battery condition. Verify RTL and Land actions1003# complete.1004self.start_subtest("Two stage battery failsafe test with RTL and Land")1005self.takeoffAndMoveAway()1006self.delay_sim_time(3)1007self.set_parameters({1008'BATT_FS_LOW_ACT': 2,1009'BATT_FS_CRT_ACT': 1,1010'SIM_BATT_VOLTAGE': 11.4,1011})1012self.wait_statustext("Battery 1 is low", timeout=60)1013self.delay_sim_time(5)1014self.wait_mode("RTL")1015self.delay_sim_time(10)1016self.set_parameter('SIM_BATT_VOLTAGE', 10.0)1017self.wait_statustext("Battery 1 is critical", timeout=60)1018self.delay_sim_time(5)1019self.wait_mode("LAND")1020self.wait_landed_and_disarmed()1021self.set_parameter('SIM_BATT_VOLTAGE', 12.5)1022self.reboot_sitl()1023self.end_subtest("Completed two stage battery failsafe test with RTL and Land")10241025# TWO STAGE BATTERY FAILSAFE: Trigger low battery condition,1026# then critical battery condition. Verify both SmartRTL1027# actions complete1028self.start_subtest("Two stage battery failsafe test with SmartRTL")1029self.takeoffAndMoveAway()1030self.set_parameter('BATT_FS_LOW_ACT', 3)1031self.set_parameter('BATT_FS_CRT_ACT', 4)1032self.delay_sim_time(10)1033self.set_parameter('SIM_BATT_VOLTAGE', 11.4)1034self.wait_statustext("Battery 1 is low", timeout=60)1035self.delay_sim_time(5)1036self.wait_mode("SMART_RTL")1037self.change_mode("LOITER")1038self.delay_sim_time(10)1039self.set_parameter('SIM_BATT_VOLTAGE', 10.0)1040self.wait_statustext("Battery 1 is critical", timeout=60)1041self.delay_sim_time(5)1042self.wait_mode("SMART_RTL")1043self.wait_disarmed()1044self.set_parameter('SIM_BATT_VOLTAGE', 12.5)1045self.reboot_sitl()1046self.end_subtest("Completed two stage battery failsafe test with SmartRTL")10471048# Trigger low battery condition in land mode with FS_OPTIONS1049# set to allow land mode to continue. Verify landing completes1050# uninterrupted.1051self.start_subtest("Battery failsafe with FS_OPTIONS set to continue landing")1052self.takeoffAndMoveAway()1053self.set_parameter('FS_OPTIONS', 8)1054self.change_mode("LAND")1055self.delay_sim_time(5)1056self.set_parameter('SIM_BATT_VOLTAGE', 11.4)1057self.wait_statustext("Battery 1 is low", timeout=60)1058self.delay_sim_time(5)1059self.wait_mode("LAND")1060self.wait_landed_and_disarmed()1061self.set_parameter('SIM_BATT_VOLTAGE', 12.5)1062self.reboot_sitl()1063self.end_subtest("Completed battery failsafe with FS_OPTIONS set to continue landing")10641065# Trigger a critical battery condition, which triggers a land1066# mode failsafe. Trigger an RC failure. Verify the RC failsafe1067# is prevented from stopping the low battery landing.1068self.start_subtest("Battery failsafe critical landing")1069self.takeoffAndMoveAway(100, 50)1070self.set_parameters({1071'FS_OPTIONS': 0,1072'BATT_FS_LOW_ACT': 1,1073'BATT_FS_CRT_ACT': 1,1074'FS_THR_ENABLE': 1,1075})1076self.delay_sim_time(5)1077self.set_parameter('SIM_BATT_VOLTAGE', 10.0)1078self.wait_statustext("Battery 1 is critical", timeout=60)1079self.wait_mode("LAND")1080self.delay_sim_time(10)1081self.set_parameter("SIM_RC_FAIL", 1)1082self.delay_sim_time(10)1083self.wait_mode("LAND")1084self.wait_landed_and_disarmed()1085self.set_parameter('SIM_BATT_VOLTAGE', 12.5)1086self.set_parameter("SIM_RC_FAIL", 0)1087self.reboot_sitl()1088self.end_subtest("Completed battery failsafe critical landing")10891090# Trigger low battery condition with failsafe set to terminate. Copter will disarm and crash.1091self.start_subtest("Battery failsafe terminate")1092self.takeoffAndMoveAway()1093self.set_parameter('BATT_FS_LOW_ACT', 5)1094self.delay_sim_time(10)1095self.set_parameter('SIM_BATT_VOLTAGE', 11.4)1096self.wait_statustext("Battery 1 is low", timeout=60)1097self.wait_disarmed()1098self.end_subtest("Completed terminate failsafe test")10991100self.progress("All Battery failsafe tests complete")11011102def BatteryMissing(self):1103''' Test battery health pre-arm and missing failsafe'''1104self.context_push()11051106# Should be good to arm with no changes1107self.wait_ready_to_arm()11081109# Make monitor unhealthy, this should result in unhealthy prearm1110self.set_parameters({1111'BATT_VOLT_PIN': -1,1112})11131114self.drain_mav()11151116# Battery should go unhealthy immediately1117self.assert_prearm_failure("Battery 1 unhealthy", other_prearm_failures_fatal=False)11181119# Return monitor to health1120self.context_pop()1121self.context_push()11221123self.wait_ready_to_arm()11241125# take off and then trigger in flight1126self.takeoff(10, mode="LOITER")1127self.set_parameters({1128'BATT_VOLT_PIN': -1,1129})11301131# Should trigger missing failsafe1132self.wait_statustext("Battery 1 is missing")11331134# Done, reset params and reboot to clear failsafe1135self.land_and_disarm()1136self.context_pop()1137self.reboot_sitl()11381139def VibrationFailsafe(self):1140'''Test Vibration Failsafe'''1141self.context_push()11421143# takeoff in Loiter to 20m1144self.takeoff(20, mode="LOITER")11451146# simulate accel bias caused by high vibration1147self.set_parameters({1148'SIM_ACC1_BIAS_Z': 2,1149'SIM_ACC2_BIAS_Z': 2,1150'SIM_ACC3_BIAS_Z': 2,1151})11521153# wait for Vibration compensation warning and change to LAND mode1154self.wait_statustext("Vibration compensation ON", timeout=30)1155self.change_mode("LAND")11561157# check vehicle descends to 2m or less within 40 seconds1158self.wait_altitude(-5, 2, timeout=50, relative=True)11591160# force disarm of vehicle (it will likely not automatically disarm)1161self.disarm_vehicle(force=True)11621163# revert simulated accel bias and reboot to restore EKF health1164self.context_pop()1165self.reboot_sitl()11661167def test_takeoff_check_mode(self, mode, user_takeoff=False):1168# stabilize check1169self.progress("Motor takeoff check in %s" % mode)1170self.change_mode(mode)1171self.zero_throttle()1172self.wait_ready_to_arm()1173self.context_push()1174self.context_collect('STATUSTEXT')1175self.arm_vehicle()1176if user_takeoff:1177self.run_cmd(1178mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,1179p7=10,1180)1181else:1182self.set_rc(3, 1700)1183# we may never see ourselves as armed in a heartbeat1184self.wait_statustext("Takeoff blocked: ESC RPM out of range", check_context=True)1185self.context_pop()1186self.zero_throttle()1187self.disarm_vehicle()1188self.wait_disarmed()11891190# Tests the motor failsafe1191def TakeoffCheck(self):1192'''Test takeoff check'''1193self.set_parameters({1194"AHRS_EKF_TYPE": 10,1195'SIM_ESC_TELEM': 1,1196'SIM_ESC_ARM_RPM': 500,1197'TKOFF_RPM_MIN': 1000,1198})11991200self.test_takeoff_check_mode("STABILIZE")1201self.test_takeoff_check_mode("ACRO")1202self.test_takeoff_check_mode("LOITER")1203self.test_takeoff_check_mode("ALT_HOLD")1204# self.test_takeoff_check_mode("FLOWHOLD")1205self.test_takeoff_check_mode("GUIDED", True)1206self.test_takeoff_check_mode("POSHOLD")1207# self.test_takeoff_check_mode("SPORT")12081209self.set_parameters({1210"AHRS_EKF_TYPE": 10,1211'SIM_ESC_TELEM': 1,1212'TKOFF_RPM_MIN': 1,1213'TKOFF_RPM_MAX': 3,1214})1215self.test_takeoff_check_mode("STABILIZE")1216self.test_takeoff_check_mode("ACRO")1217self.test_takeoff_check_mode("LOITER")1218self.test_takeoff_check_mode("ALT_HOLD")1219# self.test_takeoff_check_mode("FLOWHOLD")1220self.test_takeoff_check_mode("GUIDED", True)1221self.test_takeoff_check_mode("POSHOLD")1222# self.test_takeoff_check_mode("SPORT")12231224def assert_dataflash_message_field_level_at(self,1225mtype,1226field,1227level,1228maintain=1,1229tolerance=0.05,1230timeout=30,1231condition=None,1232dfreader_start_timestamp=None,1233verbose=False):1234'''wait for EKF's accel bias to reach a level and maintain it'''12351236if verbose:1237self.progress("current onboard log filepath: %s" % self.current_onboard_log_filepath())1238dfreader = self.dfreader_for_current_onboard_log()12391240achieve_start = None1241current_value = None1242while True:1243m = dfreader.recv_match(type=mtype, condition=condition)1244if m is None:1245raise NotAchievedException("%s.%s did not maintain %f" %1246(mtype, field, level))1247if dfreader_start_timestamp is not None:1248if m.TimeUS < dfreader_start_timestamp:1249continue1250if verbose:1251print("m=%s" % str(m))1252current_value = getattr(m, field)12531254if abs(current_value - level) > tolerance:1255if achieve_start is not None:1256self.progress("Achieve stop at %u" % m.TimeUS)1257achieve_start = None1258continue12591260dfreader_now = m.TimeUS1261if achieve_start is None:1262self.progress("Achieve start at %u (got=%f want=%f)" %1263(dfreader_now, current_value, level))1264if maintain is None:1265return1266achieve_start = m.TimeUS1267continue12681269# we're achieving....1270if dfreader_now - achieve_start > maintain*1e6:1271return dfreader_now12721273# Tests any EK3 accel bias is subtracted from the correct IMU data1274def EK3AccelBias(self):1275'''Test EK3 Accel Bias data'''1276self.context_push()12771278self.start_test("Test zero bias")1279dfreader_tstart = self.assert_dataflash_message_field_level_at(1280"XKF2",1281"AZ",12820.0,1283condition="XKF2.C==1",1284)12851286# Add 2m/s/s bias to the second IMU1287self.set_parameters({1288'SIM_ACC2_BIAS_Z': 0.7,1289})12901291self.start_subtest("Ensuring second core has bias")1292self.delay_sim_time(30)1293dfreader_tstart = self.assert_dataflash_message_field_level_at(1294"XKF2", "AZ", 0.7,1295condition="XKF2.C==1",1296)12971298self.start_subtest("Ensuring earth frame is compensated")1299self.assert_dataflash_message_field_level_at(1300"RATE", "A", 0,1301maintain=1,1302tolerance=2, # RATE.A is in cm/s/s1303dfreader_start_timestamp=dfreader_tstart,1304)13051306# now switch the EKF to only using the second core:1307self.set_parameters({1308'SIM_ACC2_BIAS_Z': 0.0,1309"EK3_IMU_MASK": 0b10,1310})1311self.reboot_sitl()13121313self.delay_sim_time(30)1314dfreader_tstart = self.assert_dataflash_message_field_level_at(1315"XKF2", "AZ", 0.0,1316condition="XKF2.C==0",1317)13181319# Add 2m/s/s bias to the second IMU1320self.set_parameters({1321'SIM_ACC2_BIAS_Z': 0.7,1322})13231324self.start_subtest("Ensuring first core now has bias")1325self.delay_sim_time(30)1326dfreader_tstart = self.assert_dataflash_message_field_level_at(1327"XKF2", "AZ", 0.7,1328condition="XKF2.C==0",1329)13301331self.start_subtest("Ensuring earth frame is compensated")1332self.assert_dataflash_message_field_level_at(1333"RATE", "A", 0,1334maintain=1,1335tolerance=2, # RATE.A is in cm/s/s1336dfreader_start_timestamp=dfreader_tstart,1337verbose=True,1338)13391340# revert simulated accel bias and reboot to restore EKF health1341self.context_pop()1342self.reboot_sitl()13431344# StabilityPatch - fly south, then hold loiter within 5m1345# position and altitude and reduce 1 motor to 60% efficiency1346def StabilityPatch(self,1347holdtime=30,1348maxaltchange=5,1349maxdistchange=10):1350'''Fly stability patch'''1351self.takeoff(10, mode="LOITER")13521353# first south1354self.progress("turn south")1355self.set_rc(4, 1580)1356self.wait_heading(180)1357self.set_rc(4, 1500)13581359# fly west 80m1360self.set_rc(2, 1100)1361self.wait_distance(80)1362self.set_rc(2, 1500)13631364# wait for copter to slow moving1365self.wait_groundspeed(0, 2)13661367m = self.mav.recv_match(type='VFR_HUD', blocking=True)1368start_altitude = m.alt1369start = self.mav.location()1370tstart = self.get_sim_time()1371self.progress("Holding loiter at %u meters for %u seconds" %1372(start_altitude, holdtime))13731374# cut motor 1's to efficiency1375self.progress("Cutting motor 1 to 65% efficiency")1376self.set_parameters({1377"SIM_ENGINE_MUL": 0.65,1378"SIM_ENGINE_FAIL": 1 << 0, # motor 11379})13801381while self.get_sim_time_cached() < tstart + holdtime:1382m = self.mav.recv_match(type='VFR_HUD', blocking=True)1383pos = self.mav.location()1384delta = self.get_distance(start, pos)1385alt_delta = math.fabs(m.alt - start_altitude)1386self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))1387if alt_delta > maxaltchange:1388raise NotAchievedException(1389"Loiter alt shifted %u meters (> limit %u)" %1390(alt_delta, maxaltchange))1391if delta > maxdistchange:1392raise NotAchievedException(1393("Loiter shifted %u meters (> limit of %u)" %1394(delta, maxdistchange)))13951396# restore motor 1 to 100% efficiency1397self.set_parameter("SIM_ENGINE_MUL", 1.0)13981399self.progress("Stability patch and Loiter OK for %us" % holdtime)14001401self.progress("RTL after stab patch")1402self.do_RTL()14031404def debug_arming_issue(self):1405while True:1406self.send_mavlink_arm_command()1407m = self.mav.recv_match(blocking=True, timeout=1)1408if m is None:1409continue1410if m.get_type() in ["STATUSTEXT", "COMMAND_ACK"]:1411print("Got: %s" % str(m))1412if self.mav.motors_armed():1413self.progress("Armed")1414return14151416# fly_fence_test - fly east until you hit the horizontal circular fence1417avoid_behave_slide = 014181419def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide):1420using_mode = "LOITER" # must be something which adjusts velocity!1421self.change_mode(using_mode)1422fence_radius = 151423fence_margin = 31424self.set_parameters({1425"FENCE_ENABLE": 1, # fence1426"FENCE_TYPE": 2, # circle1427"FENCE_RADIUS": fence_radius,1428"FENCE_MARGIN": fence_margin,1429"AVOID_ENABLE": 1,1430"AVOID_BEHAVE": avoid_behave,1431"RC10_OPTION": 40, # avoid-enable1432})1433self.wait_ready_to_arm()1434self.set_rc(10, 2000)1435home_distance = self.distance_to_home(use_cached_home=True)1436if home_distance > 5:1437raise PreconditionFailedException("Expected to be within 5m of home")1438self.zero_throttle()1439self.arm_vehicle()1440self.set_rc(3, 1700)1441self.wait_altitude(10, 100, relative=True)1442self.set_rc(3, 1500)1443self.set_rc(2, 1400)1444self.wait_distance_to_home(12, 20, timeout=30)1445tstart = self.get_sim_time()1446push_time = 70 # push against barrier for 60 seconds1447failed_max = False1448failed_min = False1449while True:1450if self.get_sim_time() - tstart > push_time:1451self.progress("Push time up")1452break1453# make sure we don't RTL:1454if not self.mode_is(using_mode):1455raise NotAchievedException("Changed mode away from %s" % using_mode)1456distance = self.distance_to_home(use_cached_home=True)1457inner_radius = fence_radius - fence_margin1458want_min = inner_radius - 1 # allow 1m either way1459want_max = inner_radius + 1 # allow 1m either way1460self.progress("Push: distance=%f %f<want<%f" %1461(distance, want_min, want_max))1462if distance < want_min:1463if failed_min is False:1464self.progress("Failed min")1465failed_min = True1466if distance > want_max:1467if failed_max is False:1468self.progress("Failed max")1469failed_max = True1470if failed_min and failed_max:1471raise NotAchievedException("Failed both min and max checks. Clever")1472if failed_min:1473raise NotAchievedException("Failed min")1474if failed_max:1475raise NotAchievedException("Failed max")1476self.set_rc(2, 1500)1477self.do_RTL()14781479def HorizontalAvoidFence(self, timeout=180):1480'''Test horizontal Avoidance fence'''1481self.fly_fence_avoid_test_radius_check(avoid_behave=1, timeout=timeout)1482self.fly_fence_avoid_test_radius_check(avoid_behave=0, timeout=timeout)14831484# fly_fence_test - fly east until you hit the horizontal circular fence1485def HorizontalFence(self, timeout=180):1486'''Test horizontal fence'''1487# enable fence, disable avoidance1488self.set_parameters({1489"FENCE_ENABLE": 1,1490"AVOID_ENABLE": 0,1491})14921493self.change_mode("LOITER")1494self.wait_ready_to_arm()14951496# fence requires home to be set:1497m = self.poll_home_position(quiet=False)14981499self.start_subtest("ensure we can't arm if outside fence")1500self.load_fence("fence-in-middle-of-nowhere.txt")15011502self.delay_sim_time(5) # let fence check run so it loads-from-eeprom1503self.assert_prearm_failure("Vehicle breaching Polygon fence")1504self.progress("Failed to arm outside fence (good!)")1505self.clear_fence()1506self.delay_sim_time(5) # let fence breach clear1507self.drain_mav()1508self.end_subtest("ensure we can't arm if outside fence")15091510self.start_subtest("ensure we can't arm with bad radius")1511self.context_push()1512self.set_parameter("FENCE_RADIUS", -1)1513self.assert_prearm_failure("Invalid Circle FENCE_RADIUS value")1514self.context_pop()1515self.progress("Failed to arm with bad radius")1516self.drain_mav()1517self.end_subtest("ensure we can't arm with bad radius")15181519self.start_subtest("ensure we can't arm with bad alt")1520self.context_push()1521self.set_parameter("FENCE_ALT_MAX", -1)1522self.assert_prearm_failure("Invalid FENCE_ALT_MAX value")1523self.context_pop()1524self.progress("Failed to arm with bad altitude")1525self.end_subtest("ensure we can't arm with bad radius")15261527self.start_subtest("Check breach-fence behaviour")1528self.set_parameter("FENCE_TYPE", 2)1529self.takeoff(10, mode="LOITER")15301531# first east1532self.progress("turn east")1533self.set_rc(4, 1580)1534self.wait_heading(160, timeout=60)1535self.set_rc(4, 1500)15361537fence_radius = self.get_parameter("FENCE_RADIUS")15381539self.progress("flying forward (east) until we hit fence")1540pitching_forward = True1541self.set_rc(2, 1100)15421543self.progress("Waiting for fence breach")1544tstart = self.get_sim_time()1545while not self.mode_is("RTL"):1546if self.get_sim_time_cached() - tstart > 30:1547raise NotAchievedException("Did not breach fence")15481549m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)1550alt = m.relative_alt / 1000.0 # mm -> m1551home_distance = self.distance_to_home(use_cached_home=True)1552self.progress("Alt: %.02f HomeDistance: %.02f (fence radius=%f)" %1553(alt, home_distance, fence_radius))15541555self.progress("Waiting until we get home and disarm")1556tstart = self.get_sim_time()1557while self.get_sim_time_cached() < tstart + timeout:1558m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)1559alt = m.relative_alt / 1000.0 # mm -> m1560home_distance = self.distance_to_home(use_cached_home=True)1561self.progress("Alt: %.02f HomeDistance: %.02f" %1562(alt, home_distance))1563# recenter pitch sticks once we're home so we don't fly off again1564if pitching_forward and home_distance < 50:1565pitching_forward = False1566self.set_rc(2, 1475)1567# disable fence1568self.set_parameter("FENCE_ENABLE", 0)1569if (alt <= 1 and home_distance < 10) or (not self.armed() and home_distance < 10):1570# reduce throttle1571self.zero_throttle()1572self.change_mode("LAND")1573self.wait_landed_and_disarmed()1574self.progress("Reached home OK")1575self.zero_throttle()1576return15771578# give we're testing RTL, doing one here probably doesn't make sense1579home_distance = self.distance_to_home(use_cached_home=True)1580raise AutoTestTimeoutException(1581"Fence test failed to reach home (%fm distance) - "1582"timed out after %u seconds" % (home_distance, timeout,))15831584# MaxAltFence - fly up until you hit the fence ceiling1585def MaxAltFence(self):1586'''Test Max Alt Fence'''1587self.takeoff(10, mode="LOITER")1588"""Hold loiter position."""15891590# enable fence, disable avoidance1591self.set_parameters({1592"FENCE_ENABLE": 1,1593"AVOID_ENABLE": 0,1594"FENCE_TYPE": 1,1595"FENCE_ENABLE" : 1,1596})15971598self.change_alt(10)15991600# first east1601self.progress("turning east")1602self.set_rc(4, 1580)1603self.wait_heading(160, timeout=60)1604self.set_rc(4, 1500)16051606self.progress("flying east 20m")1607self.set_rc(2, 1100)1608self.wait_distance(20)16091610self.progress("flying up")1611self.set_rc_from_map({16122: 1500,16133: 1800,1614})16151616# wait for fence to trigger1617self.wait_mode('RTL', timeout=120)16181619self.wait_rtl_complete()16201621self.zero_throttle()16221623# MaxAltFence - fly up and make sure fence action does not trigger1624# Also check that the vehicle will not try and descend too fast when trying to backup from a max alt fence due to avoidance1625def MaxAltFenceAvoid(self):1626'''Test Max Alt Fence Avoidance'''1627self.takeoff(10, mode="LOITER")1628"""Hold loiter position."""16291630# enable fence, only max altitude, defualt is 100m1631# No action, rely on avoidance to prevent the breach1632self.set_parameters({1633"FENCE_ENABLE": 1,1634"FENCE_TYPE": 1,1635"FENCE_ACTION": 0,1636})16371638# Try and fly past the fence1639self.set_rc(3, 1920)16401641# Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts1642try:1643self.wait_altitude(140, 150, timeout=90, relative=True)1644raise NotAchievedException("Avoid should prevent reaching altitude")1645except AutoTestTimeoutException:1646pass1647except Exception as e:1648raise e16491650# Check descent is not too fast, allow 10% above the configured backup speed1651max_descent_rate = -self.get_parameter("AVOID_BACKUP_SPD") * 1.116521653def get_climb_rate(mav, m):1654m_type = m.get_type()1655if m_type != 'VFR_HUD':1656return1657if m.climb < max_descent_rate:1658raise NotAchievedException("Decending too fast want %f got %f" % (max_descent_rate, m.climb))16591660self.context_push()1661self.install_message_hook_context(get_climb_rate)16621663# Reduce fence alt, this will result in a fence breach, but there is no action.1664# Avoid should then backup the vehicle to be under the new fence alt.1665self.set_parameters({1666"FENCE_ALT_MAX": 50,1667})1668self.wait_altitude(40, 50, timeout=90, relative=True)16691670self.context_pop()16711672self.set_rc(3, 1500)1673self.do_RTL()16741675# fly_alt_min_fence_test - fly down until you hit the fence floor1676def MinAltFence(self):1677'''Test Min Alt Fence'''1678self.takeoff(30, mode="LOITER", timeout=60)16791680# enable fence, disable avoidance1681self.set_parameters({1682"AVOID_ENABLE": 0,1683"FENCE_ENABLE" : 1,1684"FENCE_TYPE": 8,1685"FENCE_ALT_MIN": 20,1686})16871688self.change_alt(30)16891690# Activate the floor fence1691# TODO this test should run without requiring this1692self.do_fence_enable()16931694# first east1695self.progress("turn east")1696self.set_rc(4, 1580)1697self.wait_heading(160, timeout=60)1698self.set_rc(4, 1500)16991700# fly forward (east) at least 20m1701self.set_rc(2, 1100)1702self.wait_distance(20)17031704# stop flying forward and start flying down:1705self.set_rc_from_map({17062: 1500,17073: 1200,1708})17091710# wait for fence to trigger1711self.wait_mode('RTL', timeout=120)17121713self.wait_rtl_complete()17141715# Disable the fence using mavlink command to ensure cleaned up SITL state1716self.do_fence_disable()17171718self.zero_throttle()17191720# MinAltFenceAvoid - fly down and make sure fence action does not trigger1721# Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance1722def MinAltFenceAvoid(self):1723'''Test Min Alt Fence Avoidance'''17241725# enable fence, only min altitude1726# No action, rely on avoidance to prevent the breach1727self.set_parameters({1728"FENCE_ENABLE": 1,1729"FENCE_TYPE": 8,1730"FENCE_ALT_MIN": 20,1731"FENCE_ACTION": 0,1732})1733self.reboot_sitl()17341735self.takeoff(30, mode="LOITER")1736"""Hold loiter position."""17371738# Try and fly past the fence1739self.set_rc(3, 1120)17401741# Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts1742try:1743self.wait_altitude(10, 15, timeout=90, relative=True)1744raise NotAchievedException("Avoid should prevent reaching altitude")1745except AutoTestTimeoutException:1746pass1747except Exception as e:1748raise e17491750# Check ascent is not too fast, allow 10% above the configured backup speed1751max_ascent_rate = self.get_parameter("AVOID_BACKUP_SPD") * 1.117521753def get_climb_rate(mav, m):1754m_type = m.get_type()1755if m_type != 'VFR_HUD':1756return1757if m.climb > max_ascent_rate:1758raise NotAchievedException("Ascending too fast want %f got %f" % (max_ascent_rate, m.climb))17591760self.context_push()1761self.install_message_hook_context(get_climb_rate)17621763# Reduce fence alt, this will result in a fence breach, but there is no action.1764# Avoid should then backup the vehicle to be over the new fence alt.1765self.set_parameters({1766"FENCE_ALT_MIN": 30,1767})1768self.wait_altitude(30, 40, timeout=90, relative=True)17691770self.context_pop()17711772self.set_rc(3, 1500)1773self.do_RTL()17741775def FenceFloorEnabledLanding(self):1776"""Ensures we can initiate and complete an RTL while the fence is1777enabled.1778"""1779fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE17801781self.progress("Test Landing while fence floor enabled")1782self.set_parameters({1783"AVOID_ENABLE": 0,1784"FENCE_ENABLE" : 1,1785"FENCE_TYPE": 15,1786"FENCE_ALT_MIN": 20,1787"FENCE_ALT_MAX": 30,1788})17891790self.change_mode("GUIDED")1791self.wait_ready_to_arm()1792self.arm_vehicle()1793self.user_takeoff(alt_min=25)17941795# Check fence is enabled1796self.assert_fence_enabled()17971798# Change to RC controlled mode1799self.change_mode('LOITER')18001801self.set_rc(3, 1800)18021803self.wait_mode('RTL', timeout=120)1804# center throttle1805self.set_rc(3, 1500)18061807# wait until we are below the fence floor and re-enter loiter1808self.wait_altitude(5, 15, relative=True)1809self.change_mode('LOITER')1810# wait for manual recovery to expire1811self.delay_sim_time(15)18121813# lower throttle and try and land1814self.set_rc(3, 1300)1815self.wait_altitude(0, 2, relative=True)1816self.zero_throttle()1817self.wait_landed_and_disarmed()1818self.assert_fence_enabled()1819# must not be in RTL1820self.assert_mode("LOITER")18211822# Assert fence is healthy since it was enabled automatically1823self.assert_sensor_state(fence_bit, healthy=True)18241825# Disable the fence using mavlink command to ensure cleaned up SITL state1826self.do_fence_disable()1827self.assert_fence_disabled()18281829def FenceFloorAutoDisableLanding(self):1830"""Ensures we can initiate and complete an RTL while the fence is enabled"""18311832fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE18331834self.progress("Test Landing while fence floor enabled")1835self.set_parameters({1836"AVOID_ENABLE": 0,1837"FENCE_TYPE": 11,1838"FENCE_ALT_MIN": 10,1839"FENCE_ALT_MAX": 20,1840"FENCE_AUTOENABLE" : 1,1841})18421843self.change_mode("GUIDED")1844self.wait_ready_to_arm()1845self.arm_vehicle()1846self.takeoff(alt_min=15, mode="GUIDED")18471848# Check fence is enabled1849self.assert_fence_enabled()18501851# Change to RC controlled mode1852self.change_mode('LOITER')18531854self.set_rc(3, 1800)18551856self.wait_mode('RTL', timeout=120)18571858self.wait_landed_and_disarmed(0)1859# the breach should have cleared since we auto-disable the1860# fence on landing1861self.assert_fence_disabled()18621863# Assert fences have gone now that we have landed and disarmed1864self.assert_sensor_state(fence_bit, present=True, enabled=False)18651866def FenceFloorAutoEnableOnArming(self):1867"""Ensures we can auto-enable fences on arming and still takeoff and land"""18681869fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE18701871self.set_parameters({1872"AVOID_ENABLE": 0,1873"FENCE_TYPE": 11,1874"FENCE_ALT_MIN": 10,1875"FENCE_ALT_MAX": 20,1876"FENCE_AUTOENABLE" : 3,1877})18781879self.change_mode("GUIDED")1880# Check fence is not enabled1881self.assert_fence_disabled()18821883self.wait_ready_to_arm()1884self.arm_vehicle()1885self.takeoff(alt_min=15, mode="GUIDED")18861887# Check fence is enabled1888self.assert_fence_enabled()18891890# Change to RC controlled mode1891self.change_mode('LOITER')18921893self.set_rc(3, 1800)18941895self.wait_mode('RTL', timeout=120)1896# Assert fence is not healthy now that we are in RTL1897self.assert_sensor_state(fence_bit, healthy=False)18981899self.wait_landed_and_disarmed(0)1900# the breach should have cleared since we auto-disable the1901# fence on landing1902self.assert_fence_disabled()19031904# Assert fences have gone now that we have landed and disarmed1905self.assert_sensor_state(fence_bit, present=True, enabled=False)19061907# Disable the fence using mavlink command to ensure cleaned up SITL state1908self.assert_fence_disabled()19091910def GPSGlitchLoiter(self, timeout=30, max_distance=20):1911"""fly_gps_glitch_loiter_test. Fly south east in loiter and test1912reaction to gps glitch."""1913self.takeoff(10, mode="LOITER")19141915# turn on simulator display of gps and actual position1916if self.use_map:1917self.show_gps_and_sim_positions(True)19181919# set-up gps glitch array1920glitch_lat = [0.0002996,19210.0006958,19220.0009431,19230.0009991,19240.0009444,19250.0007716,19260.0006221]1927glitch_lon = [0.0000717,19280.0000912,19290.0002761,19300.0002626,19310.0002807,19320.0002049,19330.0001304]1934glitch_num = len(glitch_lat)1935self.progress("GPS Glitches:")1936for i in range(1, glitch_num):1937self.progress("glitch %d %.7f %.7f" %1938(i, glitch_lat[i], glitch_lon[i]))19391940# turn south east1941self.progress("turn south east")1942self.set_rc(4, 1580)1943try:1944self.wait_heading(150)1945self.set_rc(4, 1500)1946# fly forward (south east) at least 60m1947self.set_rc(2, 1100)1948self.wait_distance(60)1949self.set_rc(2, 1500)1950# wait for copter to slow down1951except Exception as e:1952if self.use_map:1953self.show_gps_and_sim_positions(False)1954raise e19551956# record time and position1957tstart = self.get_sim_time()1958tnow = tstart1959start_pos = self.sim_location()19601961# initialise current glitch1962glitch_current = 01963self.progress("Apply first glitch")1964self.set_parameters({1965"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],1966"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],1967})19681969# record position for 30 seconds1970while tnow < tstart + timeout:1971tnow = self.get_sim_time_cached()1972desired_glitch_num = int((tnow - tstart) * 2.2)1973if desired_glitch_num > glitch_current and glitch_current != -1:1974glitch_current = desired_glitch_num1975# turn off glitching if we've reached the end of glitch list1976if glitch_current >= glitch_num:1977glitch_current = -11978self.progress("Completed Glitches")1979self.set_parameters({1980"SIM_GPS1_GLTCH_X": 0,1981"SIM_GPS1_GLTCH_Y": 0,1982})1983else:1984self.progress("Applying glitch %u" % glitch_current)1985# move onto the next glitch1986self.set_parameters({1987"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],1988"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],1989})19901991# start displaying distance moved after all glitches applied1992if glitch_current == -1:1993m = self.mav.recv_match(type='GLOBAL_POSITION_INT',1994blocking=True)1995alt = m.alt/1000.0 # mm -> m1996curr_pos = self.sim_location()1997moved_distance = self.get_distance(curr_pos, start_pos)1998self.progress("Alt: %.02f Moved: %.0f" %1999(alt, moved_distance))2000if moved_distance > max_distance:2001raise NotAchievedException(2002"Moved over %u meters, Failed!" % max_distance)2003else:2004self.drain_mav()20052006# disable gps glitch2007if glitch_current != -1:2008self.set_parameters({2009"SIM_GPS1_GLTCH_X": 0,2010"SIM_GPS1_GLTCH_Y": 0,2011})2012if self.use_map:2013self.show_gps_and_sim_positions(False)20142015self.progress("GPS glitch test passed!"2016" stayed within %u meters for %u seconds" %2017(max_distance, timeout))2018self.do_RTL()2019# re-arming is problematic because the GPS is glitching!2020self.reboot_sitl()20212022def GPSGlitchLoiter2(self):2023"""test vehicle handles GPS glitch (aka EKF Reset) without twitching"""2024self.context_push()2025self.takeoff(10, mode="LOITER")20262027# wait for vehicle to level2028self.wait_attitude(desroll=0, despitch=0, timeout=10, tolerance=1)20292030# apply glitch2031self.set_parameter("SIM_GPS1_GLTCH_X", 0.001)20322033# check lean angles remain stable for 20 seconds2034tstart = self.get_sim_time()2035while self.get_sim_time_cached() - tstart < 20:2036m = self.mav.recv_match(type='ATTITUDE', blocking=True)2037roll_deg = math.degrees(m.roll)2038pitch_deg = math.degrees(m.pitch)2039self.progress("checking att: roll=%f pitch=%f " % (roll_deg, pitch_deg))2040if abs(roll_deg) > 2 or abs(pitch_deg) > 2:2041raise NotAchievedException("fly_gps_glitch_loiter_test2 failed, roll or pitch moved during GPS glitch")20422043# RTL, remove glitch and reboot sitl2044self.do_RTL()2045self.context_pop()2046self.reboot_sitl()20472048def GPSGlitchAuto(self, timeout=180):2049'''fly mission and test reaction to gps glitch'''2050# set-up gps glitch array2051glitch_lat = [0.0002996,20520.0006958,20530.0009431,20540.0009991,20550.0009444,20560.0007716,20570.0006221]2058glitch_lon = [0.0000717,20590.0000912,20600.0002761,20610.0002626,20620.0002807,20630.0002049,20640.0001304]2065glitch_num = len(glitch_lat)2066self.progress("GPS Glitches:")2067for i in range(1, glitch_num):2068self.progress("glitch %d %.7f %.7f" %2069(i, glitch_lat[i], glitch_lon[i]))20702071# Fly mission #12072self.progress("# Load copter_glitch_mission")2073# load the waypoint count2074num_wp = self.load_mission("copter_glitch_mission.txt", strict=False)2075if not num_wp:2076raise NotAchievedException("load copter_glitch_mission failed")20772078# turn on simulator display of gps and actual position2079if self.use_map:2080self.show_gps_and_sim_positions(True)20812082self.progress("test: Fly a mission from 1 to %u" % num_wp)2083self.set_current_waypoint(1)20842085self.change_mode("STABILIZE")2086self.wait_ready_to_arm()2087self.zero_throttle()2088self.arm_vehicle()20892090# switch into AUTO mode and raise throttle2091self.change_mode('AUTO')2092self.set_rc(3, 1500)20932094# wait until 100m from home2095try:2096self.wait_distance(100, 5, 90)2097except Exception as e:2098if self.use_map:2099self.show_gps_and_sim_positions(False)2100raise e21012102# stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode2103self.change_mode("LOITER")2104self.set_parameters({2105"SIM_GPS1_ENABLE": 0,2106})2107self.delay_sim_time(2)2108self.set_parameters({2109"SIM_GPS1_ENABLE": 1,2110})2111# regaining GPS should not result in it falling back to a non-navigation mode2112self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)2113# It should still be navigating after enougnh time has passed for any pending timeouts to activate.2114self.delay_sim_time(10)2115self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)2116self.change_mode("AUTO")21172118# record time and position2119tstart = self.get_sim_time()21202121# initialise current glitch2122glitch_current = 02123self.progress("Apply first glitch")2124self.set_parameters({2125"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],2126"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],2127})21282129# record position for 30 seconds2130while glitch_current < glitch_num:2131tnow = self.get_sim_time()2132desired_glitch_num = int((tnow - tstart) * 2.2)2133if desired_glitch_num > glitch_current and glitch_current != -1:2134glitch_current = desired_glitch_num2135# apply next glitch2136if glitch_current < glitch_num:2137self.progress("Applying glitch %u" % glitch_current)2138self.set_parameters({2139"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],2140"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],2141})21422143# turn off glitching2144self.progress("Completed Glitches")2145self.set_parameters({2146"SIM_GPS1_GLTCH_X": 0,2147"SIM_GPS1_GLTCH_Y": 0,2148})21492150# continue with the mission2151self.wait_waypoint(0, num_wp-1, timeout=500)21522153# wait for arrival back home2154self.wait_distance_to_home(0, 10, timeout=timeout)21552156# turn off simulator display of gps and actual position2157if self.use_map:2158self.show_gps_and_sim_positions(False)21592160self.progress("GPS Glitch test Auto completed: passed!")2161self.wait_disarmed()2162# re-arming is problematic because the GPS is glitching!2163self.reboot_sitl()21642165# fly_simple - assumes the simple bearing is initialised to be2166# directly north flies a box with 100m west, 15 seconds north,2167# 50 seconds east, 15 seconds south2168def SimpleMode(self, side=50):2169'''Fly in SIMPLE mode'''2170self.takeoff(10, mode="LOITER")21712172# set SIMPLE mode for all flight modes2173self.set_parameter("SIMPLE", 63)21742175# switch to stabilize mode2176self.change_mode('STABILIZE')2177self.set_rc(3, 1545)21782179# fly south 50m2180self.progress("# Flying south %u meters" % side)2181self.set_rc(1, 1300)2182self.wait_distance(side, 5, 60)2183self.set_rc(1, 1500)21842185# fly west 8 seconds2186self.progress("# Flying west for 8 seconds")2187self.set_rc(2, 1300)2188tstart = self.get_sim_time()2189while self.get_sim_time_cached() < (tstart + 8):2190self.mav.recv_match(type='VFR_HUD', blocking=True)2191self.set_rc(2, 1500)21922193# fly north 25 meters2194self.progress("# Flying north %u meters" % (side/2.0))2195self.set_rc(1, 1700)2196self.wait_distance(side/2, 5, 60)2197self.set_rc(1, 1500)21982199# fly east 8 seconds2200self.progress("# Flying east for 8 seconds")2201self.set_rc(2, 1700)2202tstart = self.get_sim_time()2203while self.get_sim_time_cached() < (tstart + 8):2204self.mav.recv_match(type='VFR_HUD', blocking=True)2205self.set_rc(2, 1500)22062207# hover in place2208self.hover()22092210self.do_RTL(timeout=500)22112212# fly_super_simple - flies a circle around home for 45 seconds2213def SuperSimpleCircle(self, timeout=45):2214'''Fly a circle in SUPER SIMPLE mode'''2215self.takeoff(10, mode="LOITER")22162217# fly forward 20m2218self.progress("# Flying forward 20 meters")2219self.set_rc(2, 1300)2220self.wait_distance(20, 5, 60)2221self.set_rc(2, 1500)22222223# set SUPER SIMPLE mode for all flight modes2224self.set_parameter("SUPER_SIMPLE", 63)22252226# switch to stabilize mode2227self.change_mode("ALT_HOLD")2228self.set_rc(3, 1500)22292230# start copter yawing slowly2231self.set_rc(4, 1550)22322233# roll left for timeout seconds2234self.progress("# rolling left from pilot's POV for %u seconds"2235% timeout)2236self.set_rc(1, 1300)2237tstart = self.get_sim_time()2238while self.get_sim_time_cached() < (tstart + timeout):2239self.mav.recv_match(type='VFR_HUD', blocking=True)22402241# stop rolling and yawing2242self.set_rc(1, 1500)2243self.set_rc(4, 1500)22442245# restore simple mode parameters to default2246self.set_parameter("SUPER_SIMPLE", 0)22472248# hover in place2249self.hover()22502251self.do_RTL()22522253# fly_circle - flies a circle with 20m radius2254def ModeCircle(self, holdtime=36):2255'''Fly CIRCLE mode'''2256# the following should not be required. But there appears to2257# be a physics failure in the simulation which is causing CI2258# to fall over a lot. -pb 2020070212092259self.reboot_sitl()22602261self.takeoff(10, mode="LOITER")22622263# face west2264self.progress("turn west")2265self.set_rc(4, 1580)2266self.wait_heading(270)2267self.set_rc(4, 1500)22682269# set CIRCLE radius2270self.set_parameter("CIRCLE_RADIUS", 3000)22712272# fly forward (east) at least 100m2273self.set_rc(2, 1100)2274self.wait_distance(100)2275# return pitch stick back to middle2276self.set_rc(2, 1500)22772278# set CIRCLE mode2279self.change_mode('CIRCLE')22802281# wait2282m = self.mav.recv_match(type='VFR_HUD', blocking=True)2283start_altitude = m.alt2284tstart = self.get_sim_time()2285self.progress("Circle at %u meters for %u seconds" %2286(start_altitude, holdtime))2287while self.get_sim_time_cached() < tstart + holdtime:2288m = self.mav.recv_match(type='VFR_HUD', blocking=True)2289self.progress("heading %d" % m.heading)22902291self.progress("CIRCLE OK for %u seconds" % holdtime)22922293self.do_RTL()22942295def CompassMot(self):2296'''test code that adjust mag field for motor interference'''2297self.run_cmd(2298mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,22990, # p123000, # p223010, # p323020, # p423030, # p523041, # p623050 # p72306)2307self.context_collect("STATUSTEXT")2308self.wait_statustext("Starting calibration", check_context=True)2309self.wait_statustext("Current", check_context=True)2310rc3_min = self.get_parameter('RC3_MIN')2311rc3_max = self.get_parameter('RC3_MAX')2312rc3_dz = self.get_parameter('RC3_DZ')23132314def set_rc3_for_throttle_pct(thr_pct):2315value = int((rc3_min+rc3_dz) + (thr_pct/100.0) * (rc3_max-(rc3_min+rc3_dz)))2316self.progress("Setting rc3 to %u" % value)2317self.set_rc(3, value)23182319throttle_in_pct = 02320set_rc3_for_throttle_pct(throttle_in_pct)2321self.assert_received_message_field_values("COMPASSMOT_STATUS", {2322"interference": 0,2323"throttle": throttle_in_pct2324}, verbose=True, very_verbose=True)2325tstart = self.get_sim_time()2326delta = 52327while True:2328if self.get_sim_time_cached() - tstart > 60:2329raise NotAchievedException("did not run through entire range")2330throttle_in_pct += delta2331self.progress("Using throttle %f%%" % throttle_in_pct)2332set_rc3_for_throttle_pct(throttle_in_pct)2333self.wait_message_field_values("COMPASSMOT_STATUS", {2334"throttle": throttle_in_pct * 10.0,2335}, verbose=True, very_verbose=True, epsilon=1)2336if throttle_in_pct == 0:2337# finished counting down2338break2339if throttle_in_pct == 100:2340# start counting down2341delta = -delta23422343m = self.wait_message_field_values("COMPASSMOT_STATUS", {2344"throttle": 0,2345}, verbose=True)2346for axis in "X", "Y", "Z":2347fieldname = "Compensation" + axis2348if getattr(m, fieldname) <= 0:2349raise NotAchievedException("Expected non-zero %s" % fieldname)23502351# it's kind of crap - but any command-ack will stop the2352# calibration2353self.mav.mav.command_ack_send(0, 1)2354self.wait_statustext("Calibration successful")23552356def MagFail(self):2357'''test failover of compass in EKF'''2358# we want both EK2 and EK32359self.set_parameters({2360"EK2_ENABLE": 1,2361"EK3_ENABLE": 1,2362})23632364self.takeoff(10, mode="LOITER")23652366self.change_mode('CIRCLE')23672368self.delay_sim_time(20)23692370self.context_collect("STATUSTEXT")23712372self.progress("Failing first compass")2373self.set_parameter("SIM_MAG1_FAIL", 1)23742375# we want for the message twice, one for EK2 and again for EK32376self.wait_statustext("EKF2 IMU0 switching to compass 1", check_context=True)2377self.wait_statustext("EKF3 IMU0 switching to compass 1", check_context=True)2378self.progress("compass switch 1 OK")23792380self.delay_sim_time(2)23812382self.context_clear_collection("STATUSTEXT")23832384self.progress("Failing 2nd compass")2385self.set_parameter("SIM_MAG2_FAIL", 1)23862387self.wait_statustext("EKF2 IMU0 switching to compass 2", check_context=True)2388self.wait_statustext("EKF3 IMU0 switching to compass 2", check_context=True)23892390self.progress("compass switch 2 OK")23912392self.delay_sim_time(2)23932394self.context_clear_collection("STATUSTEXT")23952396self.progress("Failing 3rd compass")2397self.set_parameter("SIM_MAG3_FAIL", 1)2398self.delay_sim_time(2)2399self.set_parameter("SIM_MAG1_FAIL", 0)24002401self.wait_statustext("EKF2 IMU0 switching to compass 0", check_context=True)2402self.wait_statustext("EKF3 IMU0 switching to compass 0", check_context=True)2403self.progress("compass switch 0 OK")24042405self.do_RTL()24062407def ModeFlip(self):2408'''Fly Flip Mode'''2409self.context_set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100)24102411self.takeoff(20)24122413self.progress("Flipping in roll")2414self.set_rc(1, 1700)2415self.send_cmd_do_set_mode('FLIP') # don't wait for success2416self.wait_attitude(despitch=0, desroll=45, tolerance=30)2417self.wait_attitude(despitch=0, desroll=90, tolerance=30)2418self.wait_attitude(despitch=0, desroll=-45, tolerance=30)2419self.progress("Waiting for level")2420self.set_rc(1, 1500) # can't change quickly enough!2421self.wait_attitude(despitch=0, desroll=0, tolerance=5)24222423self.progress("Regaining altitude")2424self.change_mode('ALT_HOLD')2425self.wait_altitude(19, 60, relative=True)24262427self.progress("Flipping in pitch")2428self.set_rc(2, 1700)2429self.send_cmd_do_set_mode('FLIP') # don't wait for success2430self.wait_attitude(despitch=45, desroll=0, tolerance=30)2431# can't check roll here as it flips from 0 to -180..2432self.wait_attitude(despitch=90, tolerance=30)2433self.wait_attitude(despitch=-45, tolerance=30)2434self.progress("Waiting for level")2435self.set_rc(2, 1500) # can't change quickly enough!2436self.wait_attitude(despitch=0, desroll=0, tolerance=5)24372438self.do_RTL()24392440def configure_EKFs_to_use_optical_flow_instead_of_GPS(self):2441'''configure EKF to use optical flow instead of GPS'''2442ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")2443if ahrs_ekf_type == 2:2444self.set_parameter("EK2_GPS_TYPE", 3)2445if ahrs_ekf_type == 3:2446self.set_parameters({2447"EK3_SRC1_POSXY": 0,2448"EK3_SRC1_VELXY": 5,2449"EK3_SRC1_VELZ": 0,2450})24512452def OpticalFlowLocation(self):2453'''test optical flow doesn't supply location'''24542455self.context_push()24562457self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, False, False, False, verbose=True)24582459self.start_subtest("Make sure no crash if no rangefinder")2460self.set_parameter("SIM_FLOW_ENABLE", 1)2461self.set_parameter("FLOW_TYPE", 10)24622463self.configure_EKFs_to_use_optical_flow_instead_of_GPS()24642465self.reboot_sitl()24662467self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, True, True, True, verbose=True)24682469self.change_mode('LOITER')2470self.delay_sim_time(5)2471self.wait_statustext("Need Position Estimate", timeout=300)24722473self.context_pop()24742475self.reboot_sitl()24762477def OpticalFlow(self):2478'''test OpticalFlow in flight'''2479self.start_subtest("Make sure no crash if no rangefinder")24802481self.set_parameters({2482"SIM_FLOW_ENABLE": 1,2483"FLOW_TYPE": 10,2484})24852486self.set_analog_rangefinder_parameters()24872488self.reboot_sitl()24892490self.change_mode('LOITER')24912492# ensure OPTICAL_FLOW message is reasonable:2493global flow_rate_rads2494global rangefinder_distance2495global gps_speed2496global last_debug_time2497flow_rate_rads = 02498rangefinder_distance = 02499gps_speed = 02500last_debug_time = 025012502def check_optical_flow(mav, m):2503global flow_rate_rads2504global rangefinder_distance2505global gps_speed2506global last_debug_time2507m_type = m.get_type()2508if m_type == "OPTICAL_FLOW":2509flow_rate_rads = math.sqrt(m.flow_comp_m_x**2+m.flow_comp_m_y**2)2510elif m_type == "RANGEFINDER":2511rangefinder_distance = m.distance2512elif m_type == "GPS_RAW_INT":2513gps_speed = m.vel/100.0 # cm/s -> m/s2514of_speed = flow_rate_rads * rangefinder_distance2515if abs(of_speed - gps_speed) > 3:2516raise NotAchievedException("gps=%f vs of=%f mismatch" %2517(gps_speed, of_speed))25182519now = self.get_sim_time_cached()2520if now - last_debug_time > 5:2521last_debug_time = now2522self.progress("gps=%f of=%f" % (gps_speed, of_speed))25232524self.install_message_hook_context(check_optical_flow)25252526self.fly_generic_mission("CMAC-copter-navtest.txt")25272528def OpticalFlowLimits(self):2529'''test EKF navigation limiting'''2530self.set_parameters({2531"SIM_FLOW_ENABLE": 1,2532"FLOW_TYPE": 10,2533"SIM_GPS1_ENABLE": 0,2534"SIM_TERRAIN": 0,2535})25362537self.configure_EKFs_to_use_optical_flow_instead_of_GPS()25382539self.set_analog_rangefinder_parameters()25402541self.reboot_sitl()25422543# we can't takeoff in loiter as we need flow healthy2544self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800)2545self.change_mode('LOITER')25462547# speed should be limited to <10m/s2548self.set_rc(2, 1000)25492550tstart = self.get_sim_time()2551timeout = 602552started_climb = False2553while self.get_sim_time_cached() - tstart < timeout:2554m = self.assert_receive_message('GLOBAL_POSITION_INT')2555spd = math.sqrt(m.vx**2 + m.vy**2) * 0.012556alt = m.relative_alt*0.00125572558# calculate max speed from altitude above the ground2559margin = 2.02560max_speed = alt * 1.5 + margin2561self.progress("%0.1f: Low Speed: %f (want <= %u) alt=%.1f" %2562(self.get_sim_time_cached() - tstart,2563spd,2564max_speed, alt))2565if spd > max_speed:2566raise NotAchievedException(("Speed should be limited by"2567"EKF optical flow limits"))25682569# after 30 seconds start climbing2570if not started_climb and self.get_sim_time_cached() - tstart > 30:2571started_climb = True2572self.set_rc(3, 1900)2573self.progress("Moving higher")25742575# check altitude is not climbing above 35m2576if alt > 35:2577raise NotAchievedException("Alt should be limited by EKF optical flow limits")2578self.reboot_sitl(force=True)25792580def OpticalFlowCalibration(self):2581'''test optical flow calibration'''2582ex = None2583self.context_push()2584try:25852586self.set_parameter("SIM_FLOW_ENABLE", 1)2587self.set_parameter("FLOW_TYPE", 10)2588self.set_analog_rangefinder_parameters()25892590# RC9 starts/stops calibration2591self.set_parameter("RC9_OPTION", 158)25922593# initialise flow scaling parameters to incorrect values2594self.set_parameter("FLOW_FXSCALER", -200)2595self.set_parameter("FLOW_FYSCALER", 200)25962597self.reboot_sitl()25982599# ensure calibration is off2600self.set_rc(9, 1000)26012602# takeoff to 10m in loiter2603self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720)26042605# start calibration2606self.set_rc(9, 2000)26072608tstart = self.get_sim_time()2609timeout = 902610veh_dir_tstart = self.get_sim_time()2611veh_dir = 02612while self.get_sim_time_cached() - tstart < timeout:2613# roll and pitch vehicle until samples collected2614# change direction of movement every 2 seconds2615if self.get_sim_time_cached() - veh_dir_tstart > 2:2616veh_dir_tstart = self.get_sim_time()2617veh_dir = veh_dir + 12618if veh_dir > 3:2619veh_dir = 02620if veh_dir == 0:2621# move right2622self.set_rc(1, 1800)2623self.set_rc(2, 1500)2624if veh_dir == 1:2625# move left2626self.set_rc(1, 1200)2627self.set_rc(2, 1500)2628if veh_dir == 2:2629# move forward2630self.set_rc(1, 1500)2631self.set_rc(2, 1200)2632if veh_dir == 3:2633# move back2634self.set_rc(1, 1500)2635self.set_rc(2, 1800)26362637# return sticks to center2638self.set_rc(1, 1500)2639self.set_rc(2, 1500)26402641# stop calibration (not actually necessary)2642self.set_rc(9, 1000)26432644# check scaling parameters have been restored to values near zero2645flow_scalar_x = self.get_parameter("FLOW_FXSCALER")2646flow_scalar_y = self.get_parameter("FLOW_FYSCALER")2647if ((flow_scalar_x > 30) or (flow_scalar_x < -30)):2648raise NotAchievedException("FlowCal failed to set FLOW_FXSCALER correctly")2649if ((flow_scalar_y > 30) or (flow_scalar_y < -30)):2650raise NotAchievedException("FlowCal failed to set FLOW_FYSCALER correctly")26512652except Exception as e:2653self.print_exception_caught(e)2654ex = e26552656self.disarm_vehicle(force=True)2657self.context_pop()2658self.reboot_sitl()26592660if ex is not None:2661raise ex26622663def AutoTune(self):2664"""Test autotune mode"""26652666rlld = self.get_parameter("ATC_RAT_RLL_D")2667rlli = self.get_parameter("ATC_RAT_RLL_I")2668rllp = self.get_parameter("ATC_RAT_RLL_P")2669self.set_parameter("ATC_RAT_RLL_SMAX", 1)2670self.takeoff(10)26712672# hold position in loiter2673self.change_mode('AUTOTUNE')26742675tstart = self.get_sim_time()2676sim_time_expected = 50002677deadline = tstart + sim_time_expected2678while self.get_sim_time_cached() < deadline:2679now = self.get_sim_time_cached()2680m = self.mav.recv_match(type='STATUSTEXT',2681blocking=True,2682timeout=1)2683if m is None:2684continue2685self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))2686if "AutoTune: Success" in m.text:2687self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))2688# near enough for now:2689self.change_mode('LAND')2690self.wait_landed_and_disarmed()2691# check the original gains have been re-instated2692if (rlld != self.get_parameter("ATC_RAT_RLL_D") or2693rlli != self.get_parameter("ATC_RAT_RLL_I") or2694rllp != self.get_parameter("ATC_RAT_RLL_P")):2695raise NotAchievedException("AUTOTUNE gains still present")2696return26972698raise NotAchievedException("AUTOTUNE failed (%u seconds)" %2699(self.get_sim_time() - tstart))27002701def AutoTuneYawD(self):2702"""Test autotune mode"""27032704rlld = self.get_parameter("ATC_RAT_RLL_D")2705rlli = self.get_parameter("ATC_RAT_RLL_I")2706rllp = self.get_parameter("ATC_RAT_RLL_P")2707self.set_parameter("ATC_RAT_RLL_SMAX", 1)2708self.set_parameter("AUTOTUNE_AXES", 15)2709self.takeoff(10)27102711# hold position in loiter2712self.change_mode('AUTOTUNE')27132714tstart = self.get_sim_time()2715sim_time_expected = 50002716deadline = tstart + sim_time_expected2717while self.get_sim_time_cached() < deadline:2718now = self.get_sim_time_cached()2719m = self.mav.recv_match(type='STATUSTEXT',2720blocking=True,2721timeout=1)2722if m is None:2723continue2724self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))2725if "AutoTune: Success" in m.text:2726self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))2727# near enough for now:2728self.change_mode('LAND')2729self.wait_landed_and_disarmed()2730# check the original gains have been re-instated2731if (rlld != self.get_parameter("ATC_RAT_RLL_D") or2732rlli != self.get_parameter("ATC_RAT_RLL_I") or2733rllp != self.get_parameter("ATC_RAT_RLL_P")):2734raise NotAchievedException("AUTOTUNE gains still present")2735return27362737raise NotAchievedException("AUTOTUNE failed (%u seconds)" %2738(self.get_sim_time() - tstart))27392740def AutoTuneSwitch(self):2741"""Test autotune on a switch with gains being saved"""27422743# autotune changes a set of parameters on the vehicle which2744# are not in our context. That changes the flight2745# characterstics, which we can't afford between runs. So2746# completely reset the simulated vehicle after the run is2747# complete by "customising" the commandline here:2748self.customise_SITL_commandline([])27492750self.set_parameters({2751"RC8_OPTION": 17,2752"ATC_RAT_RLL_FLTT": 20,2753})27542755self.takeoff(10, mode='LOITER')27562757def print_gains(name, gains):2758self.progress(f"AUTOTUNE {name} gains are P:%f I:%f D:%f" % (2759gains["ATC_RAT_RLL_P"],2760gains["ATC_RAT_RLL_I"],2761gains["ATC_RAT_RLL_D"]2762))27632764def get_roll_gains(name):2765ret = self.get_parameters([2766"ATC_RAT_RLL_D",2767"ATC_RAT_RLL_I",2768"ATC_RAT_RLL_P",2769], verbose=False)2770print_gains(name, ret)2771return ret27722773def gains_same(gains1, gains2):2774for c in 'P', 'I', 'D':2775p_name = f"ATC_RAT_RLL_{c}"2776if abs(gains1[p_name] - gains2[p_name]) > 0.00001:2777return False2778return True27792780self.progress("Take a copy of original gains")2781original_gains = get_roll_gains("pre-tuning")2782scaled_original_gains = copy.copy(original_gains)2783scaled_original_gains["ATC_RAT_RLL_I"] *= 0.127842785pre_rllt = self.get_parameter("ATC_RAT_RLL_FLTT")27862787# hold position in loiter and run autotune2788self.set_rc(8, 1850)2789self.wait_mode('AUTOTUNE')27902791tstart = self.get_sim_time()2792sim_time_expected = 50002793deadline = tstart + sim_time_expected2794while self.get_sim_time_cached() < deadline:2795now = self.get_sim_time_cached()2796m = self.mav.recv_match(type='STATUSTEXT',2797blocking=True,2798timeout=1)2799if m is None:2800continue2801self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))2802if "Determination Failed" in m.text:2803break2804if "AutoTune: Success" in m.text:2805self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))2806post_gains = get_roll_gains("post")2807if gains_same(original_gains, post_gains):2808raise NotAchievedException("AUTOTUNE gains not changed")28092810# because of the way AutoTune works, once autotune is2811# complete we return the original parameters via2812# parameter-fetching, but fly on the tuned parameters2813# (both sets with the I term scaled down). This test2814# makes sure that's still the case. It would be nice2815# if the PIDs parameters were `set` on success, but2816# they aren't... Note that if we use the switch to2817# restore the original gains and then start testing2818# again (with the switch) then we see the new gains!28192820# gains are scaled during the testing phase:2821if not gains_same(scaled_original_gains, post_gains):2822raise NotAchievedException("AUTOTUNE gains were reported as just original gains in test-mode. If you're fixing this, good!") # noqa28232824self.progress("Check original gains are re-instated by switch")2825self.set_rc(8, 1100)2826self.delay_sim_time(1)2827current_gains = get_roll_gains("set-original")2828if not gains_same(original_gains, current_gains):2829raise NotAchievedException("AUTOTUNE original gains not restored")28302831self.progress("Use autotuned gains")2832self.set_rc(8, 1850)2833self.delay_sim_time(1)2834tuned_gains = get_roll_gains("tuned")2835if gains_same(tuned_gains, original_gains):2836raise NotAchievedException("AUTOTUNE tuned gains same as pre gains")2837if gains_same(tuned_gains, scaled_original_gains):2838raise NotAchievedException("AUTOTUNE tuned gains same as scaled pre gains")28392840self.progress("land without changing mode")2841self.set_rc(3, 1000)2842self.wait_altitude(-1, 5, relative=True)2843self.wait_disarmed()2844self.progress("Check gains are still there after disarm")2845disarmed_gains = get_roll_gains("post-disarm")2846if not gains_same(tuned_gains, disarmed_gains):2847raise NotAchievedException("AUTOTUNE gains not present on disarm")28482849self.reboot_sitl()2850self.progress("Check gains are still there after reboot")2851reboot_gains = get_roll_gains("post-reboot")2852if not gains_same(tuned_gains, reboot_gains):2853raise NotAchievedException("AUTOTUNE gains not present on reboot")2854self.progress("Check FLTT is unchanged")2855if pre_rllt != self.get_parameter("ATC_RAT_RLL_FLTT"):2856raise NotAchievedException("AUTOTUNE FLTT was modified")2857return28582859raise NotAchievedException("AUTOTUNE failed (%u seconds)" %2860(self.get_sim_time() - tstart))28612862def EK3_RNG_USE_HGT(self):2863'''basic tests for using rangefinder when speed and height below thresholds'''2864# this takes advantage of some code in send_status_report2865# which only reports terrain variance when using switch-height2866# and using the rangefinder2867self.context_push()28682869self.set_analog_rangefinder_parameters()2870# set use-height to 20m (the parameter is a percentage of max range)2871self.set_parameters({2872'EK3_RNG_USE_HGT': 200000 / self.get_parameter('RNGFND1_MAX_CM'),2873})2874self.reboot_sitl()28752876# add a listener that verifies rangefinder innovations look good2877global alt2878alt = None28792880def verify_innov(mav, m):2881global alt2882if m.get_type() == 'GLOBAL_POSITION_INT':2883alt = m.relative_alt * 0.001 # mm -> m2884return2885if m.get_type() != 'EKF_STATUS_REPORT':2886return2887if alt is None:2888return2889if alt > 1 and alt < 8: # 8 is very low, but it takes a long time to start to use the rangefinder again2890zero_variance_wanted = False2891elif alt > 20:2892zero_variance_wanted = True2893else:2894return2895variance = m.terrain_alt_variance2896if zero_variance_wanted and variance > 0.00001:2897raise NotAchievedException("Wanted zero variance at height %f, got %f" % (alt, variance))2898elif not zero_variance_wanted and variance == 0:2899raise NotAchievedException("Wanted non-zero variance at alt=%f, got zero" % alt)29002901self.install_message_hook_context(verify_innov)29022903self.takeoff(50, mode='GUIDED')2904current_alt = self.mav.location().alt2905target_position = mavutil.location(2906-35.362938,2907149.165185,2908current_alt,290902910)29112912self.fly_guided_move_to(target_position, timeout=300)29132914self.change_mode('LAND')2915self.wait_disarmed()29162917self.context_pop()29182919self.reboot_sitl()29202921def TerrainDBPreArm(self):2922'''test that pre-arm checks are working corrctly for terrain database'''2923self.context_push()29242925self.progress("# Load msission with terrain alt")2926# load the waypoint2927num_wp = self.load_mission("terrain_wp.txt", strict=False)2928if not num_wp:2929raise NotAchievedException("load terrain_wp failed")29302931self.set_analog_rangefinder_parameters()2932self.set_parameters({2933"WPNAV_RFND_USE": 1,2934"TERRAIN_ENABLE": 1,2935})2936self.reboot_sitl()2937self.wait_ready_to_arm()29382939# make sure we can still arm with valid rangefinder and terrain db disabled2940self.set_parameter("TERRAIN_ENABLE", 0)2941self.wait_ready_to_arm()2942self.progress("# Vehicle armed with terrain db disabled")29432944# make sure we can't arm with terrain db enabled and no rangefinder in us2945self.set_parameter("WPNAV_RFND_USE", 0)2946self.assert_prearm_failure("terrain disabled")29472948self.context_pop()29492950self.reboot_sitl()29512952def CopterMission(self):2953'''fly mission which tests a significant number of commands'''2954# Fly mission #12955self.progress("# Load copter_mission")2956# load the waypoint count2957num_wp = self.load_mission("copter_mission.txt", strict=False)2958if not num_wp:2959raise NotAchievedException("load copter_mission failed")29602961self.fly_loaded_mission(num_wp)29622963self.progress("Auto mission completed: passed!")29642965def set_origin(self, loc, timeout=60):2966'''set the GPS global origin to loc'''2967tstart = self.get_sim_time()2968while True:2969if self.get_sim_time_cached() - tstart > timeout:2970raise AutoTestTimeoutException("Did not get non-zero lat")2971target_system = 12972self.mav.mav.set_gps_global_origin_send(2973target_system,2974int(loc.lat * 1e7),2975int(loc.lng * 1e7),2976int(loc.alt * 1e3)2977)2978gpi = self.assert_receive_message('GLOBAL_POSITION_INT')2979self.progress("gpi=%s" % str(gpi))2980if gpi.lat != 0:2981break29822983def FarOrigin(self):2984'''fly a mission far from the vehicle origin'''2985# Fly mission #12986self.set_parameters({2987"SIM_GPS1_ENABLE": 0,2988})2989self.reboot_sitl()2990nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)2991self.set_origin(nz)2992self.set_parameters({2993"SIM_GPS1_ENABLE": 1,2994})2995self.progress("# Load copter_mission")2996# load the waypoint count2997num_wp = self.load_mission("copter_mission.txt", strict=False)2998if not num_wp:2999raise NotAchievedException("load copter_mission failed")30003001self.fly_loaded_mission(num_wp)30023003self.progress("Auto mission completed: passed!")30043005def fly_loaded_mission(self, num_wp):3006'''fly mission loaded on vehicle. FIXME: get num_wp from vehicle'''3007self.progress("test: Fly a mission from 1 to %u" % num_wp)3008self.set_current_waypoint(1)30093010self.change_mode("LOITER")3011self.wait_ready_to_arm()3012self.arm_vehicle()30133014# switch into AUTO mode and raise throttle3015self.change_mode("AUTO")3016self.set_rc(3, 1500)30173018# fly the mission3019self.wait_waypoint(0, num_wp-1, timeout=500)30203021# set throttle to minimum3022self.zero_throttle()30233024# wait for disarm3025self.wait_disarmed()3026self.progress("MOTORS DISARMED OK")30273028def CANGPSCopterMission(self):3029'''fly mission which tests normal operation alongside CAN GPS'''3030self.set_parameters({3031"CAN_P1_DRIVER": 1,3032"GPS1_TYPE": 9,3033"GPS2_TYPE": 9,3034# disable simulated GPS, so only via DroneCAN3035"SIM_GPS1_ENABLE": 0,3036"SIM_GPS2_ENABLE": 0,3037# this ensures we use DroneCAN baro and compass3038"SIM_BARO_COUNT" : 0,3039"SIM_MAG1_DEVID" : 0,3040"SIM_MAG2_DEVID" : 0,3041"SIM_MAG3_DEVID" : 0,3042"COMPASS_USE2" : 0,3043"COMPASS_USE3" : 0,3044# use DroneCAN rangefinder3045"RNGFND1_TYPE" : 24,3046"RNGFND1_MAX_CM" : 11000,3047# use DroneCAN battery monitoring, and enforce with a arming voltage3048"BATT_MONITOR" : 8,3049"BATT_ARM_VOLT" : 12.0,3050"SIM_SPEEDUP": 2,3051})30523053self.context_push()3054self.set_parameter("ARMING_CHECK", 1 << 3)3055self.context_collect('STATUSTEXT')30563057self.reboot_sitl()3058# Test UAVCAN GPS ordering working3059gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True)3060gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True)3061gps1_nodeid = int(gps1_det_text.split('-')[1])3062gps2_nodeid = int(gps2_det_text.split('-')[1])3063if gps1_nodeid is None or gps2_nodeid is None:3064raise NotAchievedException("GPS not ordered per the order of Node IDs")30653066self.context_stop_collecting('STATUSTEXT')30673068GPS_Order_Tests = [[gps2_nodeid, gps2_nodeid, gps2_nodeid, 0,3069"PreArm: Same Node Id {} set for multiple GPS".format(gps2_nodeid)],3070[gps1_nodeid, int(gps2_nodeid/2), gps1_nodeid, 0,3071"Selected GPS Node {} not set as instance {}".format(int(gps2_nodeid/2), 2)],3072[int(gps1_nodeid/2), gps2_nodeid, 0, gps2_nodeid,3073"Selected GPS Node {} not set as instance {}".format(int(gps1_nodeid/2), 1)],3074[gps1_nodeid, gps2_nodeid, gps1_nodeid, gps2_nodeid, ""],3075[gps2_nodeid, gps1_nodeid, gps2_nodeid, gps1_nodeid, ""],3076[gps1_nodeid, 0, gps1_nodeid, gps2_nodeid, ""],3077[0, gps2_nodeid, gps1_nodeid, gps2_nodeid, ""]]3078for case in GPS_Order_Tests:3079self.progress("############################### Trying Case: " + str(case))3080self.set_parameters({3081"GPS1_CAN_OVRIDE": case[0],3082"GPS2_CAN_OVRIDE": case[1],3083})3084self.drain_mav()3085self.context_collect('STATUSTEXT')3086self.reboot_sitl()3087gps1_det_text = None3088gps2_det_text = None3089try:3090gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True)3091except AutoTestTimeoutException:3092pass3093try:3094gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True)3095except AutoTestTimeoutException:3096pass30973098self.context_stop_collecting('STATUSTEXT')3099self.change_mode('LOITER')3100if case[2] == 0 and case[3] == 0:3101if gps1_det_text or gps2_det_text:3102raise NotAchievedException("Failed ordering for requested CASE:", case)31033104if case[2] == 0 or case[3] == 0:3105if bool(gps1_det_text is not None) == bool(gps2_det_text is not None):3106print(gps1_det_text)3107print(gps2_det_text)3108raise NotAchievedException("Failed ordering for requested CASE:", case)31093110if gps1_det_text:3111if case[2] != int(gps1_det_text.split('-')[1]):3112raise NotAchievedException("Failed ordering for requested CASE:", case)3113if gps2_det_text:3114if case[3] != int(gps2_det_text.split('-')[1]):3115raise NotAchievedException("Failed ordering for requested CASE:", case)3116if len(case[4]):3117self.context_collect('STATUSTEXT')3118self.run_cmd(3119mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,3120p1=1, # ARM3121timeout=10,3122want_result=mavutil.mavlink.MAV_RESULT_FAILED,3123)3124self.wait_statustext(case[4], check_context=True)3125self.context_stop_collecting('STATUSTEXT')3126self.progress("############################### All GPS Order Cases Tests Passed")3127self.progress("############################### Test Healthy Prearm check")3128self.set_parameter("ARMING_CHECK", 1)3129self.stop_sup_program(instance=0)3130self.start_sup_program(instance=0, args="-M")3131self.stop_sup_program(instance=1)3132self.start_sup_program(instance=1, args="-M")3133self.delay_sim_time(2)3134self.context_collect('STATUSTEXT')3135self.run_cmd(3136mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,3137p1=1, # ARM3138timeout=10,3139want_result=mavutil.mavlink.MAV_RESULT_FAILED,3140)3141self.wait_statustext(".*Node .* unhealthy", check_context=True, regex=True)3142self.stop_sup_program(instance=0)3143self.start_sup_program(instance=0)3144self.stop_sup_program(instance=1)3145self.start_sup_program(instance=1)3146self.context_stop_collecting('STATUSTEXT')3147self.context_pop()31483149self.set_parameters({3150# use DroneCAN ESCs for flight3151"CAN_D1_UC_ESC_BM" : 0x0f,3152# this stops us using local servo output, guaranteeing we are3153# flying on DroneCAN ESCs3154"SIM_CAN_SRV_MSK" : 0xFF,3155# we can do the flight faster3156"SIM_SPEEDUP" : 5,3157})31583159self.CopterMission()31603161def TakeoffAlt(self):3162'''Test Takeoff command altitude'''3163# Test case #1 (set target altitude to relative -10m from the ground, -10m is invalid, so it is set to 1m)3164self.progress("Testing relative alt from the ground")3165self.do_takeoff_alt("copter_takeoff.txt", 1, False)3166# Test case #2 (set target altitude to relative -10m during flight, -10m is invalid, so keeps current altitude)3167self.progress("Testing relative alt during flight")3168self.do_takeoff_alt("copter_takeoff.txt", 10, True)31693170self.progress("Takeoff mission completed: passed!")31713172def do_takeoff_alt(self, mission_file, target_alt, during_flight=False):3173self.progress("# Load %s" % mission_file)3174# load the waypoint count3175num_wp = self.load_mission(mission_file, strict=False)3176if not num_wp:3177raise NotAchievedException("load %s failed" % mission_file)31783179self.set_current_waypoint(1)31803181self.change_mode("GUIDED")3182self.wait_ready_to_arm()3183self.arm_vehicle()31843185if during_flight:3186self.user_takeoff(alt_min=target_alt)31873188# switch into AUTO mode and raise throttle3189self.change_mode("AUTO")3190self.set_rc(3, 1500)31913192# fly the mission3193self.wait_waypoint(0, num_wp-1, timeout=500)31943195# altitude check3196self.wait_altitude(target_alt - 1 , target_alt + 1, relative=True)31973198self.change_mode('LAND')31993200# set throttle to minimum3201self.zero_throttle()32023203# wait for disarm3204self.wait_disarmed()3205self.progress("MOTORS DISARMED OK")32063207def GuidedEKFLaneChange(self):3208'''test lane change with GPS diff on startup'''3209self.set_parameters({3210"EK3_SRC1_POSZ": 3,3211"EK3_AFFINITY" : 1,3212"GPS2_TYPE" : 1,3213"SIM_GPS2_ENABLE" : 1,3214"SIM_GPS2_GLTCH_Z" : -303215})3216self.reboot_sitl()32173218self.change_mode("GUIDED")3219self.wait_ready_to_arm()32203221self.delay_sim_time(10, reason='"both EKF lanes to init"')32223223self.set_parameters({3224"SIM_GPS2_GLTCH_Z" : 03225})32263227self.delay_sim_time(20, reason="EKF to do a position Z reset")32283229self.arm_vehicle()3230self.user_takeoff(alt_min=20)3231gps_alt = self.get_altitude(altitude_source='GPS_RAW_INT.alt')3232self.progress("Initial guided alt=%.1fm" % gps_alt)32333234self.context_collect('STATUSTEXT')3235self.progress("force a lane change")3236self.set_parameters({3237"INS_ACCOFFS_X" : 53238})3239self.wait_statustext("EKF3 lane switch 1", timeout=10, check_context=True)32403241self.watch_altitude_maintained(3242altitude_min=gps_alt-2,3243altitude_max=gps_alt+2,3244altitude_source='GPS_RAW_INT.alt',3245minimum_duration=10,3246)32473248self.disarm_vehicle(force=True)3249self.reboot_sitl()32503251def MotorFail(self, ):3252"""Test flight with reduced motor efficiency"""3253# we only expect an octocopter to survive ATM:3254self.MotorFail_test_frame('octa', 8, frame_class=3)3255# self.MotorFail_test_frame('hexa', 6, frame_class=2)3256# self.MotorFail_test_frame('y6', 6, frame_class=5)32573258def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fail_mul=0.0, holdtime=30):3259self.set_parameters({3260'FRAME_CLASS': frame_class,3261})3262self.customise_SITL_commandline([], model=model)32633264self.takeoff(25, mode="LOITER")32653266# Get initial values3267start_hud = self.assert_receive_message('VFR_HUD')3268start_attitude = self.assert_receive_message('ATTITUDE')32693270hover_time = 53271tstart = self.get_sim_time()3272int_error_alt = 03273int_error_yaw_rate = 03274int_error_yaw = 03275self.progress("Hovering for %u seconds" % hover_time)3276failed = False3277while True:3278now = self.get_sim_time_cached()3279if now - tstart > holdtime + hover_time:3280break32813282servo = self.assert_receive_message('SERVO_OUTPUT_RAW')3283hud = self.assert_receive_message('VFR_HUD')3284attitude = self.assert_receive_message('ATTITUDE')32853286if not failed and now - tstart > hover_time:3287self.progress("Killing motor %u (%u%%)" %3288(fail_servo+1, fail_mul))3289self.set_parameters({3290"SIM_ENGINE_MUL": fail_mul,3291"SIM_ENGINE_FAIL": 1 << fail_servo,3292})3293failed = True32943295if failed:3296self.progress("Hold Time: %f/%f" % (now-tstart, holdtime))32973298servo_pwm = [3299servo.servo1_raw,3300servo.servo2_raw,3301servo.servo3_raw,3302servo.servo4_raw,3303servo.servo5_raw,3304servo.servo6_raw,3305servo.servo7_raw,3306servo.servo8_raw,3307]33083309self.progress("PWM output per motor")3310for i, pwm in enumerate(servo_pwm[0:servo_count]):3311if pwm > 1900:3312state = "oversaturated"3313elif pwm < 1200:3314state = "undersaturated"3315else:3316state = "OK"33173318if failed and i == fail_servo:3319state += " (failed)"33203321self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state))33223323alt_delta = hud.alt - start_hud.alt3324yawrate_delta = attitude.yawspeed - start_attitude.yawspeed3325yaw_delta = attitude.yaw - start_attitude.yaw33263327self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta))3328self.progress("Yaw rate=%f (delta=%f) (rad/s)" %3329(attitude.yawspeed, yawrate_delta))3330self.progress("Yaw=%f (delta=%f) (deg)" %3331(attitude.yaw, yaw_delta))33323333dt = self.get_sim_time() - now3334int_error_alt += abs(alt_delta/dt)3335int_error_yaw_rate += abs(yawrate_delta/dt)3336int_error_yaw += abs(yaw_delta/dt)3337self.progress("## Error Integration ##")3338self.progress(" Altitude: %fm" % int_error_alt)3339self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate)3340self.progress(" Yaw: %f deg" % int_error_yaw)3341self.progress("----")33423343if int_error_yaw > 5:3344raise NotAchievedException("Vehicle is spinning")33453346if alt_delta < -20:3347raise NotAchievedException("Vehicle is descending")33483349self.progress("Fixing motors")3350self.set_parameter("SIM_ENGINE_FAIL", 0)33513352self.do_RTL()33533354def hover_for_interval(self, hover_time):3355'''hovers for an interval of hover_time seconds. Returns the bookend3356times for that interval (in time-since-boot frame), and the3357output throttle level at the end of the period.3358'''3359self.progress("Hovering for %u seconds" % hover_time)3360tstart = self.get_sim_time()3361self.delay_sim_time(hover_time, reason='data collection')3362vfr_hud = self.poll_message('VFR_HUD')3363tend = self.get_sim_time()3364return tstart, tend, vfr_hud.throttle33653366def MotorVibration(self):3367"""Test flight with motor vibration"""3368# magic tridge EKF type that dramatically speeds up the test3369self.set_parameters({3370"AHRS_EKF_TYPE": 10,3371"INS_LOG_BAT_MASK": 3,3372"INS_LOG_BAT_OPT": 0,3373"LOG_BITMASK": 958,3374"LOG_DISARMED": 0,3375"SIM_VIB_MOT_MAX": 350,3376# these are real values taken from a 180mm Quad:3377"SIM_GYR1_RND": 20,3378"SIM_ACC1_RND": 5,3379"SIM_ACC2_RND": 5,3380"SIM_INS_THR_MIN": 0.1,3381})3382self.reboot_sitl()33833384# do a simple up-and-down flight to gather data:3385self.takeoff(15, mode="ALT_HOLD")3386tstart, tend, hover_throttle = self.hover_for_interval(15)3387# if we don't reduce vibes here then the landing detector3388# may not trigger3389self.set_parameter("SIM_VIB_MOT_MAX", 0)3390self.do_RTL()33913392psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)3393# ignore the first 20Hz and look for a peak at -15dB or more3394# it should be at about 190Hz, each bin is 1000/1024Hz wide3395ignore_bins = int(100 * 1.024) # start at 100Hz to be safe3396freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]3397if numpy.amax(psd["X"][ignore_bins:]) < -15 or freq < 100 or freq > 300:3398raise NotAchievedException(3399"Did not detect a motor peak, found %f at %f dB" %3400(freq, numpy.amax(psd["X"][ignore_bins:])))3401else:3402self.progress("Detected motor peak at %fHz" % freq)34033404# now add a notch and check that post-filter the peak is squashed below 40dB3405self.set_parameters({3406"INS_LOG_BAT_OPT": 2,3407"INS_HNTC2_ENABLE": 1,3408"INS_HNTC2_FREQ": freq,3409"INS_HNTC2_ATT": 50,3410"INS_HNTC2_BW": freq/2,3411"INS_HNTC2_MODE": 0,3412"SIM_VIB_MOT_MAX": 350,3413})3414self.reboot_sitl()34153416# do a simple up-and-down flight to gather data:3417self.takeoff(15, mode="ALT_HOLD")3418tstart, tend, hover_throttle = self.hover_for_interval(15)3419self.set_parameter("SIM_VIB_MOT_MAX", 0)3420self.do_RTL()34213422psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)3423freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]3424peakdB = numpy.amax(psd["X"][ignore_bins:])3425if peakdB < -23:3426self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, peakdB))3427else:3428raise NotAchievedException("Detected peak %.1f Hz %.2f dB" % (freq, peakdB))34293430def VisionPosition(self):3431"""Disable GPS navigation, enable Vicon input."""3432# scribble down a location we can set origin to:34333434self.customise_SITL_commandline(["--serial5=sim:vicon:"])3435self.progress("Waiting for location")3436self.change_mode('LOITER')3437self.wait_ready_to_arm()34383439old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)3440print("old_pos=%s" % str(old_pos))34413442# configure EKF to use external nav instead of GPS3443ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")3444if ahrs_ekf_type == 2:3445self.set_parameter("EK2_GPS_TYPE", 3)3446if ahrs_ekf_type == 3:3447self.set_parameters({3448"EK3_SRC1_POSXY": 6,3449"EK3_SRC1_VELXY": 6,3450"EK3_SRC1_POSZ": 6,3451"EK3_SRC1_VELZ": 6,3452})3453self.set_parameters({3454"GPS1_TYPE": 0,3455"VISO_TYPE": 1,3456"SERIAL5_PROTOCOL": 1,3457})3458self.reboot_sitl()3459# without a GPS or some sort of external prompting, AP3460# doesn't send system_time messages. So prompt it:3461self.mav.mav.system_time_send(int(time.time() * 1000000), 0)3462self.progress("Waiting for non-zero-lat")3463tstart = self.get_sim_time()3464while True:3465if self.get_sim_time_cached() - tstart > 60:3466raise AutoTestTimeoutException("Did not get non-zero lat")3467self.mav.mav.set_gps_global_origin_send(1,3468old_pos.lat,3469old_pos.lon,3470old_pos.alt)3471gpi = self.assert_receive_message('GLOBAL_POSITION_INT')3472self.progress("gpi=%s" % str(gpi))3473if gpi.lat != 0:3474break34753476self.takeoff()3477self.set_rc(1, 1600)3478tstart = self.get_sim_time()3479while True:3480vicon_pos = self.assert_receive_message('VISION_POSITION_ESTIMATE')3481# print("vpe=%s" % str(vicon_pos))3482# gpi = self.assert_receive_message('GLOBAL_POSITION_INT')3483# self.progress("gpi=%s" % str(gpi))3484if vicon_pos.x > 40:3485break34863487if self.get_sim_time_cached() - tstart > 100:3488raise AutoTestTimeoutException("Vicon showed no movement")34893490# recenter controls:3491self.set_rc(1, 1500)3492self.progress("# Enter RTL")3493self.change_mode('RTL')3494self.set_rc(3, 1500)3495tstart = self.get_sim_time()3496# self.install_messageprinter_handlers_context(['SIMSTATE', 'GLOBAL_POSITION_INT'])3497self.wait_disarmed(timeout=200)34983499def BodyFrameOdom(self):3500"""Disable GPS navigation, enable input of VISION_POSITION_DELTA."""35013502if self.get_parameter("AHRS_EKF_TYPE") != 3:3503# only tested on this EKF3504return35053506self.customise_SITL_commandline(["--serial5=sim:vicon:"])35073508if self.current_onboard_log_contains_message("XKFD"):3509raise NotAchievedException("Found unexpected XKFD message")35103511# scribble down a location we can set origin to:3512self.progress("Waiting for location")3513self.change_mode('LOITER')3514self.wait_ready_to_arm()35153516old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)3517print("old_pos=%s" % str(old_pos))35183519# configure EKF to use external nav instead of GPS3520self.set_parameters({3521"EK3_SRC1_POSXY": 6,3522"EK3_SRC1_VELXY": 6,3523"EK3_SRC1_POSZ": 6,3524"EK3_SRC1_VELZ": 6,3525"GPS1_TYPE": 0,3526"VISO_TYPE": 1,3527"SERIAL5_PROTOCOL": 1,3528"SIM_VICON_TMASK": 8, # send VISION_POSITION_DELTA3529})3530self.reboot_sitl()3531# without a GPS or some sort of external prompting, AP3532# doesn't send system_time messages. So prompt it:3533self.mav.mav.system_time_send(int(time.time() * 1000000), 0)3534self.progress("Waiting for non-zero-lat")3535tstart = self.get_sim_time()3536while True:3537self.mav.mav.set_gps_global_origin_send(1,3538old_pos.lat,3539old_pos.lon,3540old_pos.alt)3541gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT',3542blocking=True)3543self.progress("gpi=%s" % str(gpi))3544if gpi.lat != 0:3545break35463547if self.get_sim_time_cached() - tstart > 60:3548raise AutoTestTimeoutException("Did not get non-zero lat")35493550self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800)3551self.change_mode('LAND')3552# TODO: something more elaborate here - EKF will only aid3553# relative position3554self.wait_disarmed()3555if not self.current_onboard_log_contains_message("XKFD"):3556raise NotAchievedException("Did not find expected XKFD message")35573558def FlyMissionTwice(self):3559'''fly a mission twice in a row without changing modes in between.3560Seeks to show bugs in mission state machine'''35613562self.upload_simple_relhome_mission([3563(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),3564(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20),3565(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),3566])35673568num_wp = self.get_mission_count()3569self.set_parameter("AUTO_OPTIONS", 3)3570self.change_mode('AUTO')3571self.wait_ready_to_arm()3572for i in 1, 2:3573self.progress("run %u" % i)3574self.arm_vehicle()3575self.wait_waypoint(num_wp-1, num_wp-1)3576self.wait_disarmed()3577self.delay_sim_time(20)35783579def FlyMissionTwiceWithReset(self):3580'''Fly a mission twice in a row without changing modes in between.3581Allow the mission to complete, then reset the mission state machine and restart the mission.'''35823583self.upload_simple_relhome_mission([3584(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),3585(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20),3586(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),3587])35883589num_wp = self.get_mission_count()3590self.set_parameter("AUTO_OPTIONS", 3)3591self.change_mode('AUTO')3592self.wait_ready_to_arm()35933594for i in 1, 2:3595self.progress("run %u" % i)3596# Use the "Reset Mission" param of DO_SET_MISSION_CURRENT to reset mission state machine3597self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=0, reset=1)3598self.arm_vehicle()3599self.wait_waypoint(num_wp-1, num_wp-1)3600self.wait_disarmed()3601self.delay_sim_time(20)36023603def MissionIndexValidity(self):3604'''Confirm that attempting to select an invalid mission item is rejected.'''36053606self.upload_simple_relhome_mission([3607(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),3608(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20),3609(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),3610])36113612num_wp = self.get_mission_count()3613accepted_indices = [0, 1, num_wp-1]3614denied_indices = [-1, num_wp]36153616for seq in accepted_indices:3617self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT,3618p1=seq,3619timeout=1,3620want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)36213622for seq in denied_indices:3623self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT,3624p1=seq,3625timeout=1,3626want_result=mavutil.mavlink.MAV_RESULT_DENIED)36273628def InvalidJumpTags(self):3629'''Verify the behaviour when selecting invalid jump tags.'''36303631MAX_TAG_NUM = 655353632# Jump tag is not present, so expect FAILED3633self.run_cmd(mavutil.mavlink.MAV_CMD_DO_JUMP_TAG,3634p1=MAX_TAG_NUM,3635timeout=1,3636want_result=mavutil.mavlink.MAV_RESULT_FAILED)36373638# Jump tag is too big, so expect DENIED3639self.run_cmd(mavutil.mavlink.MAV_CMD_DO_JUMP_TAG,3640p1=MAX_TAG_NUM+1,3641timeout=1,3642want_result=mavutil.mavlink.MAV_RESULT_DENIED)36433644def GPSViconSwitching(self):3645"""Fly GPS and Vicon switching test"""3646"""Setup parameters including switching to EKF3"""3647self.set_parameters({3648"VISO_TYPE": 2, # enable vicon3649"SERIAL5_PROTOCOL": 2,3650"EK3_ENABLE": 1,3651"EK3_SRC2_POSXY": 6, # External Nav3652"EK3_SRC2_POSZ": 6, # External Nav3653"EK3_SRC2_VELXY": 6, # External Nav3654"EK3_SRC2_VELZ": 6, # External Nav3655"EK3_SRC2_YAW": 6, # External Nav3656"RC7_OPTION": 80, # RC aux switch 7 set to Viso Align3657"RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector3658"EK2_ENABLE": 0,3659"AHRS_EKF_TYPE": 3,3660})3661self.customise_SITL_commandline(["--serial5=sim:vicon:"])36623663# switch to use GPS3664self.set_rc(8, 1000)36653666# ensure we can get a global position:3667self.poll_home_position(timeout=120)36683669# record starting position3670old_pos = self.get_global_position_int()3671print("old_pos=%s" % str(old_pos))36723673# align vicon yaw with ahrs heading3674self.set_rc(7, 2000)36753676# takeoff to 10m in Loiter3677self.progress("Moving to ensure location is tracked")3678self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720)36793680# fly forward in Loiter3681self.set_rc(2, 1300)36823683# disable vicon3684self.set_parameter("SIM_VICON_FAIL", 1)36853686# ensure vehicle remain in Loiter for 15 seconds3687tstart = self.get_sim_time()3688while self.get_sim_time() - tstart < 15:3689if not self.mode_is('LOITER'):3690raise NotAchievedException("Expected to stay in loiter for >15 seconds")36913692# re-enable vicon3693self.set_parameter("SIM_VICON_FAIL", 0)36943695# switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter3696self.set_rc(8, 1500)3697self.set_parameter("GPS1_TYPE", 0)36983699# ensure vehicle remain in Loiter for 15 seconds3700tstart = self.get_sim_time()3701while self.get_sim_time() - tstart < 15:3702if not self.mode_is('LOITER'):3703raise NotAchievedException("Expected to stay in loiter for >15 seconds")37043705# RTL and check vehicle arrives within 10m of home3706self.set_rc(2, 1500)3707self.do_RTL()37083709def RTLSpeed(self):3710"""Test RTL Speed parameters"""3711rtl_speed_ms = 73712wpnav_speed_ms = 43713wpnav_accel_mss = 33714tolerance = 0.53715self.load_mission("copter_rtl_speed.txt")3716self.set_parameters({3717'WPNAV_ACCEL': wpnav_accel_mss * 100,3718'RTL_SPEED': rtl_speed_ms * 100,3719'WPNAV_SPEED': wpnav_speed_ms * 100,3720})3721self.change_mode('LOITER')3722self.wait_ready_to_arm()3723self.arm_vehicle()3724self.change_mode('AUTO')3725self.set_rc(3, 1600)3726self.wait_altitude(19, 25, relative=True)3727self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)3728self.monitor_groundspeed(wpnav_speed_ms, timeout=20)3729self.change_mode('RTL')3730self.wait_groundspeed(rtl_speed_ms-tolerance, rtl_speed_ms+tolerance)3731self.monitor_groundspeed(rtl_speed_ms, timeout=5)3732self.change_mode('AUTO')3733self.wait_groundspeed(0-tolerance, 0+tolerance)3734self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)3735self.monitor_groundspeed(wpnav_speed_ms, tolerance=0.6, timeout=5)3736self.do_RTL()37373738def NavDelay(self):3739"""Fly a simple mission that has a delay in it."""37403741self.load_mission("copter_nav_delay.txt")37423743self.set_parameter("DISARM_DELAY", 0)37443745self.change_mode("LOITER")3746self.wait_ready_to_arm()37473748self.arm_vehicle()3749self.change_mode("AUTO")3750self.set_rc(3, 1600)3751count_start = -13752count_stop = -13753tstart = self.get_sim_time()3754last_mission_current_msg = 03755last_seq = None3756while self.armed(): # we RTL at end of mission3757now = self.get_sim_time_cached()3758if now - tstart > 200:3759raise AutoTestTimeoutException("Did not disarm as expected")3760m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)3761at_delay_item = ""3762if m.seq == 3:3763at_delay_item = "(At delay item)"3764if count_start == -1:3765count_start = now3766if ((now - last_mission_current_msg) > 1 or m.seq != last_seq):3767dist = None3768x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)3769if x is not None:3770dist = x.wp_dist3771self.progress("MISSION_CURRENT.seq=%u dist=%s %s" %3772(m.seq, dist, at_delay_item))3773last_mission_current_msg = self.get_sim_time_cached()3774last_seq = m.seq3775if m.seq > 3:3776if count_stop == -1:3777count_stop = now3778calculated_delay = count_stop - count_start3779want_delay = 59 # should reflect what's in the mission file3780self.progress("Stopped for %u seconds (want >=%u seconds)" %3781(calculated_delay, want_delay))3782if calculated_delay < want_delay:3783raise NotAchievedException("Did not delay for long enough")37843785def RangeFinder(self):3786'''Test RangeFinder Basic Functionality'''3787self.progress("Making sure we don't ordinarily get RANGEFINDER")3788m = self.mav.recv_match(type='RANGEFINDER',3789blocking=True,3790timeout=5)37913792if m is not None:3793raise NotAchievedException("Received unexpected RANGEFINDER msg")37943795# may need to force a rotation if some other test has used the3796# rangefinder...3797self.progress("Ensure no RFND messages in log")3798self.set_parameter("LOG_DISARMED", 1)3799if self.current_onboard_log_contains_message("RFND"):3800raise NotAchievedException("Found unexpected RFND message")38013802self.set_analog_rangefinder_parameters()3803self.set_parameter("RC9_OPTION", 10) # rangefinder3804self.set_rc(9, 2000)38053806self.reboot_sitl()38073808self.progress("Making sure we now get RANGEFINDER messages")3809m = self.assert_receive_message('RANGEFINDER', timeout=10)38103811self.progress("Checking RangeFinder is marked as enabled in mavlink")3812m = self.mav.recv_match(type='SYS_STATUS',3813blocking=True,3814timeout=10)3815flags = m.onboard_control_sensors_enabled3816if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:3817raise NotAchievedException("Laser not enabled in SYS_STATUS")3818self.progress("Disabling laser using switch")3819self.set_rc(9, 1000)3820self.delay_sim_time(1)3821self.progress("Checking RangeFinder is marked as disabled in mavlink")3822m = self.mav.recv_match(type='SYS_STATUS',3823blocking=True,3824timeout=10)3825flags = m.onboard_control_sensors_enabled3826if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:3827raise NotAchievedException("Laser enabled in SYS_STATUS")38283829self.progress("Re-enabling rangefinder")3830self.set_rc(9, 2000)3831self.delay_sim_time(1)3832m = self.mav.recv_match(type='SYS_STATUS',3833blocking=True,3834timeout=10)3835flags = m.onboard_control_sensors_enabled3836if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:3837raise NotAchievedException("Laser not enabled in SYS_STATUS")38383839self.takeoff(10, mode="LOITER")38403841m_r = self.mav.recv_match(type='RANGEFINDER',3842blocking=True)3843m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT',3844blocking=True)38453846if abs(m_r.distance - m_p.relative_alt/1000) > 1:3847raise NotAchievedException(3848"rangefinder/global position int mismatch %0.2f vs %0.2f" %3849(m_r.distance, m_p.relative_alt/1000))38503851self.land_and_disarm()38523853if not self.current_onboard_log_contains_message("RFND"):3854raise NotAchievedException("Did not see expected RFND message")38553856def SplineTerrain(self):3857'''Test Splines and Terrain'''3858self.set_parameter("TERRAIN_ENABLE", 0)3859self.fly_mission("wp.txt")38603861def WPNAV_SPEED(self):3862'''ensure resetting WPNAV_SPEED during a mission works'''38633864loc = self.poll_home_position()3865alt = 203866loc.alt = alt3867items = []38683869# 100 waypoints in a line, 10m apart in a northerly direction3870# for i in range(1, 100):3871# items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, i*10, 0, alt))38723873# 1 waypoint a long way away3874items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, alt),)38753876items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))38773878self.upload_simple_relhome_mission(items)38793880start_speed_ms = self.get_parameter('WPNAV_SPEED') / 100.038813882self.takeoff(20)3883self.change_mode('AUTO')3884self.wait_groundspeed(start_speed_ms-1, start_speed_ms+1, minimum_duration=10)38853886for speed_ms in 7, 8, 7, 8, 9, 10, 11, 7:3887self.set_parameter('WPNAV_SPEED', speed_ms*100)3888self.wait_groundspeed(speed_ms-1, speed_ms+1, minimum_duration=10)3889self.do_RTL()38903891def WPNAV_SPEED_UP(self):3892'''Change speed (up) during mission'''38933894items = []38953896# 1 waypoint a long way up3897items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20000),)38983899items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))39003901self.upload_simple_relhome_mission(items)39023903start_speed_ms = self.get_parameter('WPNAV_SPEED_UP') / 100.039043905minimum_duration = 539063907self.takeoff(20)3908self.change_mode('AUTO')3909self.wait_climbrate(start_speed_ms-1, start_speed_ms+1, minimum_duration=minimum_duration)39103911for speed_ms in 7, 8, 7, 8, 6, 2:3912self.set_parameter('WPNAV_SPEED_UP', speed_ms*100)3913self.wait_climbrate(speed_ms-1, speed_ms+1, minimum_duration=minimum_duration)3914self.do_RTL(timeout=240)39153916def WPNAV_SPEED_DN(self):3917'''Change speed (down) during mission'''39183919items = []39203921# 1 waypoint a long way back down3922items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 10),)39233924items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))39253926self.upload_simple_relhome_mission(items)39273928minimum_duration = 539293930self.takeoff(500, timeout=70)3931self.change_mode('AUTO')39323933start_speed_ms = self.get_parameter('WPNAV_SPEED_DN') / 100.03934self.wait_climbrate(-start_speed_ms-1, -start_speed_ms+1, minimum_duration=minimum_duration)39353936for speed_ms in 7, 8, 7, 8, 6, 2:3937self.set_parameter('WPNAV_SPEED_DN', speed_ms*100)3938self.wait_climbrate(-speed_ms-1, -speed_ms+1, minimum_duration=minimum_duration)3939self.do_RTL()39403941def fly_mission(self, filename, strict=True):3942num_wp = self.load_mission(filename, strict=strict)3943self.set_parameter("AUTO_OPTIONS", 3)3944self.change_mode('AUTO')3945self.wait_ready_to_arm()3946self.arm_vehicle()3947self.wait_waypoint(num_wp-1, num_wp-1)3948self.wait_disarmed()39493950def fly_generic_mission(self, filename, strict=True):3951num_wp = self.load_generic_mission(filename, strict=strict)3952self.set_parameter("AUTO_OPTIONS", 3)3953self.change_mode('AUTO')3954self.wait_ready_to_arm()3955self.arm_vehicle()3956self.wait_waypoint(num_wp-1, num_wp-1)3957self.wait_disarmed()39583959def SurfaceTracking(self):3960'''Test Surface Tracking'''3961ex = None3962self.context_push()39633964self.install_terrain_handlers_context()39653966try:3967self.set_analog_rangefinder_parameters()3968self.set_parameter("RC9_OPTION", 10) # rangefinder3969self.set_rc(9, 2000)39703971self.reboot_sitl() # needed for both rangefinder and initial position3972self.assert_vehicle_location_is_at_startup_location()39733974self.takeoff(10, mode="LOITER")3975lower_surface_pos = mavutil.location(-35.362421, 149.164534, 584, 270)3976here = self.mav.location()3977bearing = self.get_bearing(here, lower_surface_pos)39783979self.change_mode("GUIDED")3980self.guided_achieve_heading(bearing)3981self.change_mode("LOITER")3982self.delay_sim_time(2)3983m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)3984orig_absolute_alt_mm = m.alt39853986self.progress("Original alt: absolute=%f" % orig_absolute_alt_mm)39873988self.progress("Flying somewhere which surface is known lower compared to takeoff point")3989self.set_rc(2, 1450)3990tstart = self.get_sim_time()3991while True:3992if self.get_sim_time() - tstart > 200:3993raise NotAchievedException("Did not reach lower point")3994m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)3995x = mavutil.location(m.lat/1e7, m.lon/1e7, m.alt/1e3, 0)3996dist = self.get_distance(x, lower_surface_pos)3997delta = (orig_absolute_alt_mm - m.alt)/1000.039983999self.progress("Distance: %fm abs-alt-delta: %fm" %4000(dist, delta))4001if dist < 15:4002if delta < 0.8:4003raise NotAchievedException("Did not dip in altitude as expected")4004break40054006self.set_rc(2, 1500)4007self.do_RTL()40084009except Exception as e:4010self.print_exception_caught(e)4011self.disarm_vehicle(force=True)4012ex = e40134014self.context_pop()4015self.reboot_sitl()4016if ex is not None:4017raise ex40184019def test_rangefinder_switchover(self):4020"""test that the EKF correctly handles the switchover between baro and rangefinder"""4021ex = None4022self.context_push()40234024try:4025self.set_analog_rangefinder_parameters()40264027self.set_parameters({4028"RNGFND1_MAX_CM": 15004029})40304031# configure EKF to use rangefinder for altitude at low altitudes4032ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")4033if ahrs_ekf_type == 2:4034self.set_parameter("EK2_RNG_USE_HGT", 70)4035if ahrs_ekf_type == 3:4036self.set_parameter("EK3_RNG_USE_HGT", 70)40374038self.reboot_sitl() # needed for both rangefinder and initial position4039self.assert_vehicle_location_is_at_startup_location()40404041self.change_mode("LOITER")4042self.wait_ready_to_arm()4043self.arm_vehicle()4044self.set_rc(3, 1800)4045self.set_rc(2, 1200)4046# wait till we get to 50m4047self.wait_altitude(50, 52, True, 60)40484049self.change_mode("RTL")4050# wait till we get to 25m4051self.wait_altitude(25, 27, True, 120)40524053# level up4054self.set_rc(2, 1500)4055self.wait_altitude(14, 15, relative=True)40564057self.wait_rtl_complete()40584059except Exception as e:4060self.print_exception_caught(e)4061self.disarm_vehicle(force=True)4062ex = e4063self.context_pop()4064self.reboot_sitl()4065if ex is not None:4066raise ex40674068def _Parachute(self, command):4069'''Test Parachute Functionality using specific mavlink command'''4070self.set_rc(9, 1000)4071self.set_parameters({4072"CHUTE_ENABLED": 1,4073"CHUTE_TYPE": 10,4074"SERVO9_FUNCTION": 27,4075"SIM_PARA_ENABLE": 1,4076"SIM_PARA_PIN": 9,4077})40784079self.progress("Test triggering parachute in mission")4080self.load_mission("copter_parachute_mission.txt")4081self.change_mode('LOITER')4082self.wait_ready_to_arm()4083self.arm_vehicle()4084self.change_mode('AUTO')4085self.set_rc(3, 1600)4086self.wait_statustext('BANG', timeout=60)4087self.disarm_vehicle(force=True)4088self.reboot_sitl()40894090self.progress("Test triggering with mavlink message")4091self.takeoff(20)4092command(4093mavutil.mavlink.MAV_CMD_DO_PARACHUTE,4094p1=2, # release4095)4096self.wait_statustext('BANG', timeout=60)4097self.disarm_vehicle(force=True)4098self.reboot_sitl()40994100self.progress("Testing three-position switch")4101self.set_parameter("RC9_OPTION", 23) # parachute 3pos41024103self.progress("Test manual triggering")4104self.takeoff(20)4105self.set_rc(9, 2000)4106self.wait_statustext('BANG', timeout=60)4107self.set_rc(9, 1000)4108self.disarm_vehicle(force=True)4109self.reboot_sitl()41104111self.progress("Test mavlink triggering")4112self.takeoff(20)4113command(4114mavutil.mavlink.MAV_CMD_DO_PARACHUTE,4115p1=mavutil.mavlink.PARACHUTE_DISABLE,4116)4117ok = False4118try:4119self.wait_statustext('BANG', timeout=2)4120except AutoTestTimeoutException:4121ok = True4122if not ok:4123raise NotAchievedException("Disabled parachute fired")4124command(4125mavutil.mavlink.MAV_CMD_DO_PARACHUTE,4126p1=mavutil.mavlink.PARACHUTE_ENABLE,4127)4128ok = False4129try:4130self.wait_statustext('BANG', timeout=2)4131except AutoTestTimeoutException:4132ok = True4133if not ok:4134raise NotAchievedException("Enabled parachute fired")41354136self.set_rc(9, 1000)4137self.disarm_vehicle(force=True)4138self.reboot_sitl()41394140# parachute should not fire if you go from disabled to release:4141self.takeoff(20)4142command(4143mavutil.mavlink.MAV_CMD_DO_PARACHUTE,4144p1=mavutil.mavlink.PARACHUTE_RELEASE,4145)4146ok = False4147try:4148self.wait_statustext('BANG', timeout=2)4149except AutoTestTimeoutException:4150ok = True4151if not ok:4152raise NotAchievedException("Parachute fired when going straight from disabled to release")41534154# now enable then release parachute:4155command(4156mavutil.mavlink.MAV_CMD_DO_PARACHUTE,4157p1=mavutil.mavlink.PARACHUTE_ENABLE,4158)4159command(4160mavutil.mavlink.MAV_CMD_DO_PARACHUTE,4161p1=mavutil.mavlink.PARACHUTE_RELEASE,4162)4163self.wait_statustext('BANG! Parachute deployed', timeout=2)4164self.disarm_vehicle(force=True)4165self.reboot_sitl()41664167self.context_push()4168self.progress("Crashing with 3pos switch in enable position")4169self.takeoff(40)4170self.set_rc(9, 1500)4171self.set_parameters({4172"SIM_ENGINE_FAIL": 1 << 1, # motor 24173})4174self.wait_statustext('BANG! Parachute deployed', timeout=60)4175self.set_rc(9, 1000)4176self.disarm_vehicle(force=True)4177self.reboot_sitl()4178self.context_pop()41794180self.progress("Crashing with 3pos switch in disable position")4181loiter_alt = 104182self.takeoff(loiter_alt, mode='LOITER')4183self.set_rc(9, 1100)4184self.set_parameters({4185"SIM_ENGINE_FAIL": 1 << 1, # motor 24186})4187tstart = self.get_sim_time()4188while self.get_sim_time_cached() < tstart + 5:4189m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1)4190if m is None:4191continue4192if "BANG" in m.text:4193self.set_rc(9, 1000)4194self.reboot_sitl()4195raise NotAchievedException("Parachute deployed when disabled")4196self.set_rc(9, 1000)4197self.disarm_vehicle(force=True)4198self.reboot_sitl()41994200def Parachute(self):4201'''Test Parachute Functionality'''4202self._Parachute(self.run_cmd)4203self._Parachute(self.run_cmd_int)42044205def PrecisionLanding(self):4206"""Use PrecLand backends precision messages to land aircraft."""42074208self.context_push()42094210for backend in [4, 2]: # SITL, SITL-IRLOCK4211ex = None4212try:4213self.set_parameters({4214"PLND_ENABLED": 1,4215"PLND_TYPE": backend,4216})42174218self.set_analog_rangefinder_parameters()4219self.set_parameter("SIM_SONAR_SCALE", 12)42204221start = self.mav.location()4222target = start4223(target.lat, target.lng) = mavextra.gps_offset(start.lat, start.lng, 4, -4)4224self.progress("Setting target to %f %f" % (target.lat, target.lng))42254226self.set_parameters({4227"SIM_PLD_ENABLE": 1,4228"SIM_PLD_LAT": target.lat,4229"SIM_PLD_LON": target.lng,4230"SIM_PLD_HEIGHT": 0,4231"SIM_PLD_ALT_LMT": 15,4232"SIM_PLD_DIST_LMT": 10,4233})42344235self.reboot_sitl()42364237self.progress("Waiting for location")4238self.zero_throttle()4239self.takeoff(10, 1800, mode="LOITER")4240self.change_mode("LAND")4241self.zero_throttle()4242self.wait_landed_and_disarmed()4243self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)4244new_pos = self.mav.location()4245delta = self.get_distance(target, new_pos)4246self.progress("Landed %f metres from target position" % delta)4247max_delta = 1.54248if delta > max_delta:4249raise NotAchievedException("Did not land close enough to target position (%fm > %fm" % (delta, max_delta))42504251if not self.current_onboard_log_contains_message("PL"):4252raise NotAchievedException("Did not see expected PL message")42534254except Exception as e:4255self.print_exception_caught(e)4256ex = e4257self.reboot_sitl()4258self.zero_throttle()4259self.context_pop()4260self.reboot_sitl()4261self.progress("All done")42624263if ex is not None:4264raise ex42654266def Landing(self):4267"""Test landing the aircraft."""42684269def check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, land_speed_high_accuracy=0.1):4270self.progress("Checking landing speeds (speed_high=%f speed_low=%f alt_low=%f" %4271(land_speed_high, land_speed_low, land_alt_low))4272land_high_maintain = 54273land_low_maintain = land_alt_low / land_speed_low / 242744275takeoff_alt = (land_high_maintain * land_speed_high + land_alt_low) + 204276# this is pretty rough, but takes *so much longer* in LOITER4277self.takeoff(takeoff_alt, mode='STABILIZE', timeout=200, takeoff_throttle=2000)4278# check default landing speeds:4279self.change_mode('LAND')4280# ensure higher-alt descent rate:4281self.wait_descent_rate(land_speed_high,4282minimum_duration=land_high_maintain,4283accuracy=land_speed_high_accuracy)4284self.wait_descent_rate(land_speed_low)4285# ensure we transition to low descent rate at correct height:4286self.assert_altitude(land_alt_low, relative=True)4287# now make sure we maintain that descent rate:4288self.wait_descent_rate(land_speed_low, minimum_duration=land_low_maintain)4289self.wait_disarmed()42904291# test the defaults. By default LAND_SPEED_HIGH is 0 so4292# WPNAV_SPEED_DN is used4293check_landing_speeds(4294self.get_parameter("WPNAV_SPEED_DN") / 100, # cm/s -> m/s4295self.get_parameter("LAND_SPEED") / 100, # cm/s -> m/s4296self.get_parameter("LAND_ALT_LOW") / 100 # cm -> m4297)42984299def test_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs):4300self.set_parameters({4301"LAND_SPEED_HIGH": land_speed_high * 100, # m/s -> cm/s4302"LAND_SPEED": land_speed_low * 100, # m/s -> cm/s4303"LAND_ALT_LOW": land_alt_low * 100, # m -> cm4304})4305check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs)43064307test_landing_speeds(43085, # descent speed high43091, # descent speed low431030, # transition altitude4311land_speed_high_accuracy=0.54312)43134314def get_system_clock_utc(self, time_seconds):4315# this is a copy of ArduPilot's AP_RTC function!4316# separate time into ms, sec, min, hour and days but all expressed4317# in milliseconds4318time_ms = time_seconds * 10004319ms = time_ms % 10004320sec_ms = (time_ms % (60 * 1000)) - ms4321min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms4322hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms43234324# convert times as milliseconds into appropriate units4325secs = sec_ms / 10004326mins = min_ms / (60 * 1000)4327hours = hour_ms / (60 * 60 * 1000)4328return (hours, mins, secs, 0)43294330def calc_delay(self, seconds, delay_for_seconds):4331# delay-for-seconds has to be long enough that we're at the4332# waypoint before that time. Otherwise we'll try to wait a4333# day....4334if delay_for_seconds >= 3600:4335raise ValueError("Won't handle large delays")4336(hours,4337mins,4338secs,4339ms) = self.get_system_clock_utc(seconds)4340self.progress("Now is %uh %um %us" % (hours, mins, secs))4341secs += delay_for_seconds # add seventeen seconds4342mins += int(secs/60)4343secs %= 6043444345hours += int(mins / 60)4346mins %= 6043474348if hours > 24:4349raise ValueError("Way too big a delay")4350self.progress("Delay until %uh %um %us" %4351(hours, mins, secs))4352return (hours, mins, secs, 0)43534354def reset_delay_item(self, seq, seconds_in_future):4355frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT4356command = mavutil.mavlink.MAV_CMD_NAV_DELAY4357# retrieve mission item and check it:4358tried_set = False4359hours = None4360mins = None4361secs = None4362while True:4363self.progress("Requesting item")4364self.mav.mav.mission_request_send(1,43651,4366seq)4367st = self.mav.recv_match(type='MISSION_ITEM',4368blocking=True,4369timeout=1)4370if st is None:4371continue43724373print("Item: %s" % str(st))4374have_match = (tried_set and4375st.seq == seq and4376st.command == command and4377st.param2 == hours and4378st.param3 == mins and4379st.param4 == secs)4380if have_match:4381return43824383self.progress("Mission mismatch")43844385m = None4386tstart = self.get_sim_time()4387while True:4388if self.get_sim_time_cached() - tstart > 3:4389raise NotAchievedException(4390"Did not receive MISSION_REQUEST")4391self.mav.mav.mission_write_partial_list_send(1,43921,4393seq,4394seq)4395m = self.mav.recv_match(type='MISSION_REQUEST',4396blocking=True,4397timeout=1)4398if m is None:4399continue4400if m.seq != st.seq:4401continue4402break44034404self.progress("Sending absolute-time mission item")44054406# we have to change out the delay time...4407now = self.mav.messages["SYSTEM_TIME"]4408if now is None:4409raise PreconditionFailedException("Never got SYSTEM_TIME")4410if now.time_unix_usec == 0:4411raise PreconditionFailedException("system time is zero")4412(hours, mins, secs, ms) = self.calc_delay(now.time_unix_usec/1000000, seconds_in_future)44134414self.mav.mav.mission_item_send(44151, # target system44161, # target component4417seq, # seq4418frame, # frame4419command, # command44200, # current44211, # autocontinue44220, # p1 (relative seconds)4423hours, # p24424mins, # p34425secs, # p444260, # p544270, # p644280) # p74429tried_set = True4430ack = self.mav.recv_match(type='MISSION_ACK',4431blocking=True,4432timeout=1)4433self.progress("Received ack: %s" % str(ack))44344435def NavDelayAbsTime(self):4436"""fly a simple mission that has a delay in it"""4437self.fly_nav_delay_abstime_x(87)44384439def fly_nav_delay_abstime_x(self, delay_for, expected_delay=None):4440"""fly a simple mission that has a delay in it, expect a delay"""44414442if expected_delay is None:4443expected_delay = delay_for44444445self.load_mission("copter_nav_delay.txt")44464447self.change_mode("LOITER")44484449self.wait_ready_to_arm()44504451delay_item_seq = 34452self.reset_delay_item(delay_item_seq, delay_for)4453delay_for_seconds = delay_for4454reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True)4455reset_at = reset_at_m.time_unix_usec/100000044564457self.arm_vehicle()4458self.change_mode("AUTO")4459self.set_rc(3, 1600)4460count_stop = -14461tstart = self.get_sim_time()4462while self.armed(): # we RTL at end of mission4463now = self.get_sim_time_cached()4464if now - tstart > 240:4465raise AutoTestTimeoutException("Did not disarm as expected")4466m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)4467at_delay_item = ""4468if m.seq == delay_item_seq:4469at_delay_item = "(delay item)"4470self.progress("MISSION_CURRENT.seq=%u %s" % (m.seq, at_delay_item))4471if m.seq > delay_item_seq:4472if count_stop == -1:4473count_stop_m = self.mav.recv_match(type='SYSTEM_TIME',4474blocking=True)4475count_stop = count_stop_m.time_unix_usec/10000004476calculated_delay = count_stop - reset_at4477error = abs(calculated_delay - expected_delay)4478self.progress("Stopped for %u seconds (want >=%u seconds)" %4479(calculated_delay, delay_for_seconds))4480if error > 2:4481raise NotAchievedException("delay outside expectations")44824483def NavDelayTakeoffAbsTime(self):4484"""make sure taking off at a specific time works"""4485self.load_mission("copter_nav_delay_takeoff.txt")44864487self.change_mode("LOITER")4488self.wait_ready_to_arm()44894490delay_item_seq = 24491delay_for_seconds = 774492self.reset_delay_item(delay_item_seq, delay_for_seconds)4493reset_at = self.get_sim_time_cached()44944495self.arm_vehicle()4496self.change_mode("AUTO")44974498self.set_rc(3, 1600)44994500# should not take off for about least 77 seconds4501tstart = self.get_sim_time()4502took_off = False4503while self.armed():4504now = self.get_sim_time_cached()4505if now - tstart > 200:4506# timeout4507break4508m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)4509now = self.get_sim_time_cached()4510self.progress("%s" % str(m))4511if m.seq > delay_item_seq:4512if not took_off:4513took_off = True4514delta_time = now - reset_at4515if abs(delta_time - delay_for_seconds) > 2:4516raise NotAchievedException((4517"Did not take off on time "4518"measured=%f want=%f" %4519(delta_time, delay_for_seconds)))45204521if not took_off:4522raise NotAchievedException("Did not take off")45234524def ModeZigZag(self):4525'''test zigzag mode'''4526# set channel 8 for zigzag savewp and recentre it4527self.set_parameter("RC8_OPTION", 61)45284529self.takeoff(alt_min=5, mode='LOITER')45304531ZIGZAG = 244532j = 04533slowdown_speed = 0.3 # because Copter takes a long time to actually stop4534self.start_subtest("Conduct ZigZag test for all 4 directions")4535while j < 4:4536self.progress("## Align heading with the run-way (j=%d)##" % j)4537self.set_rc(8, 1500)4538self.set_rc(4, 1420)4539self.wait_heading(352-j*90)4540self.set_rc(4, 1500)4541self.change_mode(ZIGZAG)4542self.progress("## Record Point A ##")4543self.set_rc(8, 1100) # record point A4544self.set_rc(1, 1700) # fly side-way for 20m4545self.wait_distance(20)4546self.set_rc(1, 1500)4547self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4548self.progress("## Record Point A ##")4549self.set_rc(8, 1500) # pilot always have to cross mid position when changing for low to high position4550self.set_rc(8, 1900) # record point B45514552i = 14553while i < 2:4554self.start_subtest("Run zigzag A->B and B->A (i=%d)" % i)4555self.progress("## fly forward for 10 meter ##")4556self.set_rc(2, 1300)4557self.wait_distance(10)4558self.set_rc(2, 1500) # re-centre pitch rc control4559self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4560self.set_rc(8, 1500) # switch to mid position4561self.progress("## auto execute vector BA ##")4562self.set_rc(8, 1100)4563self.wait_distance(17) # wait for it to finish4564self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down45654566self.progress("## fly forward for 10 meter ##")4567self.set_rc(2, 1300) # fly forward for 10 meter4568self.wait_distance(10)4569self.set_rc(2, 1500) # re-centre pitch rc control4570self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4571self.set_rc(8, 1500) # switch to mid position4572self.progress("## auto execute vector AB ##")4573self.set_rc(8, 1900)4574self.wait_distance(17) # wait for it to finish4575self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4576i = i + 14577# test the case when pilot switch to manual control during the auto flight4578self.start_subtest("test the case when pilot switch to manual control during the auto flight")4579self.progress("## fly forward for 10 meter ##")4580self.set_rc(2, 1300) # fly forward for 10 meter4581self.wait_distance(10)4582self.set_rc(2, 1500) # re-centre pitch rc control4583self.wait_groundspeed(0, 0.3) # wait until the copter slows down4584self.set_rc(8, 1500) # switch to mid position4585self.progress("## auto execute vector BA ##")4586self.set_rc(8, 1100) # switch to low position, auto execute vector BA4587self.wait_distance(8) # purposely switch to manual halfway4588self.set_rc(8, 1500)4589self.wait_groundspeed(0, slowdown_speed) # copter should slow down here4590self.progress("## Manual control to fly forward ##")4591self.set_rc(2, 1300) # manual control to fly forward4592self.wait_distance(8)4593self.set_rc(2, 1500) # re-centre pitch rc control4594self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4595self.progress("## continue vector BA ##")4596self.set_rc(8, 1100) # copter should continue mission here4597self.wait_distance(8) # wait for it to finish rest of BA4598self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4599self.set_rc(8, 1500) # switch to mid position4600self.progress("## auto execute vector AB ##")4601self.set_rc(8, 1900) # switch to execute AB again4602self.wait_distance(17) # wait for it to finish4603self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down4604self.change_mode('LOITER')4605j = j + 146064607self.do_RTL()46084609def SetModesViaModeSwitch(self):4610'''Set modes via modeswitch'''4611fltmode_ch = 54612self.set_parameter("FLTMODE_CH", fltmode_ch)4613self.set_rc(fltmode_ch, 1000) # PWM for mode14614testmodes = [("FLTMODE1", 4, "GUIDED", 1165),4615("FLTMODE2", 2, "ALT_HOLD", 1295),4616("FLTMODE3", 6, "RTL", 1425),4617("FLTMODE4", 7, "CIRCLE", 1555),4618("FLTMODE5", 1, "ACRO", 1685),4619("FLTMODE6", 17, "BRAKE", 1815),4620]4621for mode in testmodes:4622(parm, parm_value, name, pwm) = mode4623self.set_parameter(parm, parm_value)46244625for mode in reversed(testmodes):4626(parm, parm_value, name, pwm) = mode4627self.set_rc(fltmode_ch, pwm)4628self.wait_mode(name)46294630for mode in testmodes:4631(parm, parm_value, name, pwm) = mode4632self.set_rc(fltmode_ch, pwm)4633self.wait_mode(name)46344635for mode in reversed(testmodes):4636(parm, parm_value, name, pwm) = mode4637self.set_rc(fltmode_ch, pwm)4638self.wait_mode(name)46394640def SetModesViaAuxSwitch(self):4641'''"Set modes via auxswitch"'''4642fltmode_ch = int(self.get_parameter("FLTMODE_CH"))4643self.set_rc(fltmode_ch, 1000)4644self.wait_mode("CIRCLE")4645self.set_rc(9, 1000)4646self.set_rc(10, 1000)4647self.set_parameters({4648"RC9_OPTION": 18, # land4649"RC10_OPTION": 55, # guided4650})4651self.set_rc(9, 1900)4652self.wait_mode("LAND")4653self.set_rc(10, 1900)4654self.wait_mode("GUIDED")4655self.set_rc(10, 1000) # this re-polls the mode switch4656self.wait_mode("CIRCLE")46574658def fly_guided_stop(self,4659timeout=20,4660groundspeed_tolerance=0.05,4661climb_tolerance=0.01):4662"""stop the vehicle moving in guided mode"""4663self.progress("Stopping vehicle")4664tstart = self.get_sim_time()4665# send a position-control command4666self.mav.mav.set_position_target_local_ned_send(46670, # timestamp46681, # target system_id46691, # target component id4670mavutil.mavlink.MAV_FRAME_BODY_NED,4671MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z46720, # x46730, # y46740, # z46750, # vx46760, # vy46770, # vz46780, # afx46790, # afy46800, # afz46810, # yaw46820, # yawrate4683)4684while True:4685if self.get_sim_time_cached() - tstart > timeout:4686raise NotAchievedException("Vehicle did not stop")4687m = self.mav.recv_match(type='VFR_HUD', blocking=True)4688print("%s" % str(m))4689if (m.groundspeed < groundspeed_tolerance and4690m.climb < climb_tolerance):4691break46924693def send_set_position_target_global_int(self, lat, lon, alt):4694self.mav.mav.set_position_target_global_int_send(46950, # timestamp46961, # target system_id46971, # target component id4698mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,4699MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # mask specifying use-only-lat-lon-alt4700lat, # lat4701lon, # lon4702alt, # alt47030, # vx47040, # vy47050, # vz47060, # afx47070, # afy47080, # afz47090, # yaw47100, # yawrate4711)47124713def fly_guided_move_global_relative_alt(self, lat, lon, alt):4714startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',4715blocking=True)47164717self.send_set_position_target_global_int(lat, lon, alt)47184719tstart = self.get_sim_time()4720while True:4721if self.get_sim_time_cached() - tstart > 200:4722raise NotAchievedException("Did not move far enough")4723# send a position-control command4724pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',4725blocking=True)4726delta = self.get_distance_int(startpos, pos)4727self.progress("delta=%f (want >10)" % delta)4728if delta > 10:4729break47304731def fly_guided_move_local(self, x, y, z_up, timeout=100):4732"""move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""4733startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)4734self.progress("startpos=%s" % str(startpos))47354736tstart = self.get_sim_time()4737# send a position-control command4738self.mav.mav.set_position_target_local_ned_send(47390, # timestamp47401, # target system_id47411, # target component id4742mavutil.mavlink.MAV_FRAME_LOCAL_NED,4743MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z4744x, # x4745y, # y4746-z_up, # z47470, # vx47480, # vy47490, # vz47500, # afx47510, # afy47520, # afz47530, # yaw47540, # yawrate4755)4756while True:4757if self.get_sim_time_cached() - tstart > timeout:4758raise NotAchievedException("Did not reach destination")4759if self.distance_to_local_position((x, y, -z_up)) < 1:4760break47614762def test_guided_local_position_target(self, x, y, z_up):4763""" Check target position being received by vehicle """4764# set POSITION_TARGET_LOCAL_NED message rate using SET_MESSAGE_INTERVAL4765self.progress("Setting local target in NED: (%f, %f, %f)" % (x, y, -z_up))4766self.progress("Setting rate to 1 Hz")4767self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 1)47684769# mask specifying use only xyz4770target_typemask = MAV_POS_TARGET_TYPE_MASK.POS_ONLY47714772# set position target4773self.mav.mav.set_position_target_local_ned_send(47740, # timestamp47751, # target system_id47761, # target component id4777mavutil.mavlink.MAV_FRAME_LOCAL_NED,4778target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,4779x, # x4780y, # y4781-z_up, # z47820, # vx47830, # vy47840, # vz47850, # afx47860, # afy47870, # afz47880, # yaw47890, # yawrate4790)4791m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2)4792self.progress("Received local target: %s" % str(m))47934794if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):4795raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %4796((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))47974798if x - m.x > 0.1:4799raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x))48004801if y - m.y > 0.1:4802raise NotAchievedException("Did not receive proper target position y: wanted=%f got=%f" % (y, m.y))48034804if z_up - (-m.z) > 0.1:4805raise NotAchievedException("Did not receive proper target position z: wanted=%f got=%f" % (z_up, -m.z))48064807def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3):4808" Check local target velocity being received by vehicle "4809self.progress("Setting local NED velocity target: (%f, %f, %f)" % (vx, vy, -vz_up))4810self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")4811self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)48124813# mask specifying use only vx,vy,vz & accel. Even though we don't test acceltargets below currently4814# a velocity only mask returns a velocity & accel mask4815target_typemask = (MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |4816MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)48174818# Drain old messages and ignore the ramp-up to the required target velocity4819tstart = self.get_sim_time()4820while self.get_sim_time_cached() - tstart < timeout:4821# send velocity-control command4822self.mav.mav.set_position_target_local_ned_send(48230, # timestamp48241, # target system_id48251, # target component id4826mavutil.mavlink.MAV_FRAME_LOCAL_NED,4827target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,48280, # x48290, # y48300, # z4831vx, # vx4832vy, # vy4833-vz_up, # vz48340, # afx48350, # afy48360, # afz48370, # yaw48380, # yawrate4839)4840m = self.assert_receive_message('POSITION_TARGET_LOCAL_NED')48414842self.progress("Received local target: %s" % str(m))48434844# Check the last received message4845if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):4846raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %4847((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))48484849if vx - m.vx > 0.1:4850raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx))48514852if vy - m.vy > 0.1:4853raise NotAchievedException("Did not receive proper target velocity vy: wanted=%f got=%f" % (vy, m.vy))48544855if vz_up - (-m.vz) > 0.1:4856raise NotAchievedException("Did not receive proper target velocity vz: wanted=%f got=%f" % (vz_up, -m.vz))48574858self.progress("Received proper target velocity commands")48594860def wait_for_local_velocity(self, vx, vy, vz_up, timeout=10):4861""" Wait for local target velocity"""48624863# debug messages4864self.progress("Waiting for local NED velocity target: (%f, %f, %f)" % (vx, vy, -vz_up))4865self.progress("Setting LOCAL_POSITION_NED message rate to 10Hz")48664867# set position local ned message stream rate4868self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED, 10)48694870# wait for position local ned message4871tstart = self.get_sim_time()4872while self.get_sim_time_cached() - tstart < timeout:48734874# get position target local ned message4875m = self.mav.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=1)48764877# could not be able to get a valid target local ned message within given time4878if m is None:48794880# raise an error that did not receive a valid target local ned message within given time4881raise NotAchievedException("Did not receive any position local ned message for 1 second!")48824883# got a valid target local ned message within given time4884else:48854886# debug message4887self.progress("Received local position ned message: %s" % str(m))48884889# check if velocity values are in range4890if vx - m.vx <= 0.1 and vy - m.vy <= 0.1 and vz_up - (-m.vz) <= 0.1:48914892# get out of function4893self.progress("Vehicle successfully reached to target velocity!")4894return48954896# raise an exception4897error_message = "Did not receive target velocities vx, vy, vz_up, wanted=(%f, %f, %f) got=(%f, %f, %f)"4898error_message = error_message % (vx, vy, vz_up, m.vx, m.vy, -m.vz)4899raise NotAchievedException(error_message)49004901def test_position_target_message_mode(self):4902" Ensure that POSITION_TARGET_LOCAL_NED messages are sent in Guided Mode only "4903self.hover()4904self.change_mode('LOITER')4905self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")4906self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)49074908tstart = self.get_sim_time()4909while self.get_sim_time_cached() < tstart + 5:4910m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)4911if m is None:4912continue49134914raise NotAchievedException("Received POSITION_TARGET message in LOITER mode: %s" % str(m))49154916self.progress("Did not receive any POSITION_TARGET_LOCAL_NED message in LOITER mode. Success")49174918def earth_to_body(self, vector):4919r = mavextra.rotation(self.mav.messages["ATTITUDE"]).invert()4920# print("r=%s" % str(r))4921return r * vector49224923def precision_loiter_to_pos(self, x, y, z, timeout=40):4924'''send landing_target messages at vehicle until it arrives at4925location to x, y, z from origin (in metres), z is *up*'''4926dest_ned = rotmat.Vector3(x, y, -z)4927tstart = self.get_sim_time()4928success_start = -14929while True:4930now = self.get_sim_time_cached()4931if now - tstart > timeout:4932raise NotAchievedException("Did not loiter to position!")4933m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED',4934blocking=True)4935pos_ned = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z)4936# print("dest_ned=%s" % str(dest_ned))4937# print("pos_ned=%s" % str(pos_ned))4938delta_ef = dest_ned - pos_ned4939# print("delta_ef=%s" % str(delta_ef))49404941# determine if we've successfully navigated to close to4942# where we should be:4943dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y)4944dist_max = 14945self.progress("dist=%f want <%f" % (dist, dist_max))4946if dist < dist_max:4947# success! We've gotten within our target distance4948if success_start == -1:4949success_start = now4950elif now - success_start > 10:4951self.progress("Yay!")4952break4953else:4954success_start = -149554956delta_bf = self.earth_to_body(delta_ef)4957# print("delta_bf=%s" % str(delta_bf))4958angle_x = math.atan2(delta_bf.y, delta_bf.z)4959angle_y = -math.atan2(delta_bf.x, delta_bf.z)4960distance = math.sqrt(delta_bf.x * delta_bf.x +4961delta_bf.y * delta_bf.y +4962delta_bf.z * delta_bf.z)4963# att = self.mav.messages["ATTITUDE"]4964# print("r=%f p=%f y=%f" % (math.degrees(att.roll), math.degrees(att.pitch), math.degrees(att.yaw)))4965# print("angle_x=%s angle_y=%s" % (str(math.degrees(angle_x)), str(math.degrees(angle_y))))4966# print("distance=%s" % str(distance))49674968self.mav.mav.landing_target_send(49690, # time_usec49701, # target_num4971mavutil.mavlink.MAV_FRAME_GLOBAL, # frame; AP ignores4972angle_x, # angle x (radians)4973angle_y, # angle y (radians)4974distance, # distance to target49750.01, # size of target in radians, X-axis49760.01 # size of target in radians, Y-axis4977)49784979def set_servo_gripper_parameters(self):4980self.set_parameters({4981"GRIP_ENABLE": 1,4982"GRIP_TYPE": 1,4983"SIM_GRPS_ENABLE": 1,4984"SIM_GRPS_PIN": 8,4985"SERVO8_FUNCTION": 28,4986})49874988def PayloadPlaceMission(self):4989"""Test payload placing in auto."""4990self.context_push()49914992self.set_analog_rangefinder_parameters()4993self.set_servo_gripper_parameters()4994self.reboot_sitl()49954996self.load_mission("copter_payload_place.txt")4997if self.mavproxy is not None:4998self.mavproxy.send('wp list\n')49995000self.set_parameter("AUTO_OPTIONS", 3)5001self.change_mode('AUTO')5002self.wait_ready_to_arm()50035004self.arm_vehicle()50055006self.wait_text("Gripper load releas", timeout=90)5007dist_limit = 15008# this is a copy of the point in the mission file:5009target_loc = mavutil.location(-35.363106,5010149.165436,50110,50120)5013dist = self.get_distance(target_loc, self.mav.location())5014self.progress("dist=%f" % (dist,))5015if dist > dist_limit:5016raise NotAchievedException("Did not honour target lat/lng (dist=%f want <%f" %5017(dist, dist_limit))50185019self.wait_disarmed()50205021self.context_pop()5022self.reboot_sitl()5023self.progress("All done")50245025def Weathervane(self):5026'''Test copter weathervaning'''5027# We test nose into wind code paths and yaw direction here and test side into wind5028# yaw direction in QuadPlane tests to reduce repetition.5029self.set_parameters({5030"SIM_WIND_SPD": 10,5031"SIM_WIND_DIR": 100,5032"GUID_OPTIONS": 129, # allow weathervaning and arming from tx in guided5033"AUTO_OPTIONS": 131, # allow arming in auto, take off without raising the stick, and weathervaning5034"WVANE_ENABLE": 1,5035"WVANE_GAIN": 3,5036"WVANE_VELZ_MAX": 1,5037"WVANE_SPD_MAX": 25038})50395040self.progress("Test weathervaning in auto")5041self.load_mission("weathervane_mission.txt", strict=False)50425043self.change_mode("AUTO")5044self.wait_ready_to_arm()5045self.arm_vehicle()50465047self.wait_statustext("Weathervane Active", timeout=60)5048self.do_RTL()5049self.wait_disarmed()5050self.change_mode("GUIDED")50515052# After take off command in guided we enter the velaccl sub mode5053self.progress("Test weathervaning in guided vel-accel")5054self.set_rc(3, 1000)5055self.wait_ready_to_arm()50565057self.arm_vehicle()5058self.user_takeoff(alt_min=15)5059# Wait for heading to match wind direction.5060self.wait_heading(100, accuracy=8, timeout=100)50615062self.progress("Test weathervaning in guided pos only")5063# Travel directly north to align heading north and build some airspeed.5064self.fly_guided_move_local(x=40, y=0, z_up=15)5065# Wait for heading to match wind direction.5066self.wait_heading(100, accuracy=8, timeout=100)5067self.do_RTL()50685069def _DO_WINCH(self, command):5070self.context_push()5071self.load_default_params_file("copter-winch.parm")5072self.reboot_sitl()5073self.wait_ready_to_arm()50745075self.start_subtest("starts relaxed")5076self.wait_servo_channel_value(9, 0)50775078self.start_subtest("rate control")5079command(5080mavutil.mavlink.MAV_CMD_DO_WINCH,5081p1=1, # instance number5082p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command5083p3=0, # length to release5084p4=1, # rate in m/s5085)5086self.wait_servo_channel_value(9, 1900)50875088self.start_subtest("relax")5089command(5090mavutil.mavlink.MAV_CMD_DO_WINCH,5091p1=1, # instance number5092p2=mavutil.mavlink.WINCH_RELAXED, # command5093p3=0, # length to release5094p4=1, # rate in m/s5095)5096self.wait_servo_channel_value(9, 0)50975098self.start_subtest("hold but zero output")5099command(5100mavutil.mavlink.MAV_CMD_DO_WINCH,5101p1=1, # instance number5102p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command5103p3=0, # length to release5104p4=0, # rate in m/s5105)5106self.wait_servo_channel_value(9, 1500)51075108self.start_subtest("relax")5109command(5110mavutil.mavlink.MAV_CMD_DO_WINCH,5111p1=1, # instance number5112p2=mavutil.mavlink.WINCH_RELAXED, # command5113p3=0, # length to release5114p4=1, # rate in m/s5115)5116self.wait_servo_channel_value(9, 0)51175118self.start_subtest("position")5119command(5120mavutil.mavlink.MAV_CMD_DO_WINCH,5121p1=1, # instance number5122p2=mavutil.mavlink.WINCH_RELATIVE_LENGTH_CONTROL, # command5123p3=2, # length to release5124p4=1, # rate in m/s5125)5126self.wait_servo_channel_value(9, 1900)5127self.wait_servo_channel_value(9, 1500, timeout=60)51285129self.context_pop()5130self.reboot_sitl()51315132def DO_WINCH(self):5133'''test mavlink DO_WINCH command'''5134self._DO_WINCH(self.run_cmd_int)5135self._DO_WINCH(self.run_cmd)51365137def GuidedSubModeChange(self):5138""""Ensure we can move around in guided after a takeoff command."""51395140'''start by disabling GCS failsafe, otherwise we immediately disarm5141due to (apparently) not receiving traffic from the GCS for5142too long. This is probably a function of --speedup'''5143self.set_parameters({5144"FS_GCS_ENABLE": 0,5145"DISARM_DELAY": 0, # until traffic problems are fixed5146})5147self.change_mode("GUIDED")5148self.wait_ready_to_arm()5149self.arm_vehicle()51505151self.user_takeoff(alt_min=10)51525153self.start_subtest("yaw through absolute angles using MAV_CMD_CONDITION_YAW")5154self.guided_achieve_heading(45)5155self.guided_achieve_heading(135)51565157self.start_subtest("move the vehicle using set_position_target_global_int")5158# the following numbers are 5-degree-latitude and 5-degrees5159# longitude - just so that we start to really move a lot.5160self.fly_guided_move_global_relative_alt(5, 5, 10)51615162self.start_subtest("move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED")5163self.fly_guided_stop(groundspeed_tolerance=0.1)5164self.fly_guided_move_local(5, 5, 10)51655166self.start_subtest("Checking that WP_YAW_BEHAVIOUR 0 works")5167self.set_parameter('WP_YAW_BEHAVIOR', 0)5168self.delay_sim_time(2)5169orig_heading = self.get_heading()5170self.fly_guided_move_local(5, 0, 10)5171# ensure our heading hasn't changed:5172self.assert_heading(orig_heading)5173self.fly_guided_move_local(0, 5, 10)5174# ensure our heading hasn't changed:5175self.assert_heading(orig_heading)51765177self.start_subtest("Check target position received by vehicle using SET_MESSAGE_INTERVAL")5178self.test_guided_local_position_target(5, 5, 10)5179self.test_guided_local_velocity_target(2, 2, 1)5180self.test_position_target_message_mode()51815182self.do_RTL()51835184def TestGripperMission(self):5185'''Test Gripper mission items'''5186num_wp = self.load_mission("copter-gripper-mission.txt")5187self.change_mode('LOITER')5188self.wait_ready_to_arm()5189self.assert_vehicle_location_is_at_startup_location()5190self.arm_vehicle()5191self.change_mode('AUTO')5192self.set_rc(3, 1500)5193self.wait_statustext("Gripper Grabbed", timeout=60)5194self.wait_statustext("Gripper Released", timeout=60)5195self.wait_waypoint(num_wp-1, num_wp-1)5196self.wait_disarmed()51975198def SplineLastWaypoint(self):5199'''Test Spline as last waypoint'''5200self.load_mission("copter-spline-last-waypoint.txt")5201self.change_mode('LOITER')5202self.wait_ready_to_arm()5203self.arm_vehicle()5204self.change_mode('AUTO')5205self.set_rc(3, 1500)5206self.wait_altitude(10, 3000, relative=True)5207self.do_RTL()52085209def ManualThrottleModeChange(self):5210'''Check manual throttle mode changes denied on high throttle'''5211self.set_parameter("FS_GCS_ENABLE", 0) # avoid GUIDED instant disarm5212self.change_mode("STABILIZE")5213self.wait_ready_to_arm()5214self.arm_vehicle()5215self.change_mode("ACRO")5216self.change_mode("STABILIZE")5217self.change_mode("GUIDED")5218self.set_rc(3, 1700)5219self.watch_altitude_maintained(altitude_min=-1, altitude_max=0.2) # should not take off in guided5220self.run_cmd_do_set_mode(5221"ACRO",5222want_result=mavutil.mavlink.MAV_RESULT_FAILED)5223self.run_cmd_do_set_mode(5224"STABILIZE",5225want_result=mavutil.mavlink.MAV_RESULT_FAILED)5226self.run_cmd_do_set_mode(5227"DRIFT",5228want_result=mavutil.mavlink.MAV_RESULT_FAILED)5229self.progress("Check setting an invalid mode")5230self.run_cmd(5231mavutil.mavlink.MAV_CMD_DO_SET_MODE,5232p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,5233p2=126,5234want_result=mavutil.mavlink.MAV_RESULT_FAILED,5235timeout=1,5236)5237self.set_rc(3, 1000)5238self.run_cmd_do_set_mode("ACRO")5239self.wait_disarmed()52405241def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1):5242PITCH_MIN = self.get_parameter("MNT%u_PITCH_MIN" % mount_instance)5243PITCH_MAX = self.get_parameter("MNT%u_PITCH_MAX" % mount_instance)5244return min(max(pitch_angle_deg, PITCH_MIN), PITCH_MAX)52455246def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True):5247tstart = self.get_sim_time()5248success_start = 052495250while True:5251now = self.get_sim_time_cached()5252if now - tstart > timeout:5253raise NotAchievedException("Mount pitch not achieved")52545255# We expect to achieve the desired pitch angle unless constrained by mount limits5256if constrained:5257despitch = self.constrained_mount_pitch(despitch)52585259'''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS'''5260mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()52615262# self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw))5263if abs(despitch - mount_pitch) > despitch_tolerance:5264self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" %5265(mount_pitch, despitch, despitch_tolerance))5266success_start = 05267continue5268self.progress("Mount pitch correct: %f degrees == %f" %5269(mount_pitch, despitch))5270if success_start == 0:5271success_start = now5272if now - success_start >= hold:5273self.progress("Mount pitch achieved")5274return52755276def do_pitch(self, pitch):5277'''pitch aircraft in guided/angle mode'''5278self.mav.mav.set_attitude_target_send(52790, # time_boot_ms52801, # target sysid52811, # target compid52820, # bitmask of things to ignore5283mavextra.euler_to_quat([0, math.radians(pitch), 0]), # att52840, # roll rate (rad/s)52850, # pitch rate (rad/s)52860, # yaw rate (rad/s)52870.5) # thrust, 0 to 1, translated to a climb/descent rate52885289def do_yaw_rate(self, yaw_rate):5290'''yaw aircraft in guided/rate mode'''5291self.run_cmd(5292mavutil.mavlink.MAV_CMD_CONDITION_YAW,5293p1=60, # target angle5294p2=0, # degrees/second5295p3=1, # -1 is counter-clockwise, 1 clockwise5296p4=1, # 1 for relative, 0 for absolute5297quiet=True,5298)52995300def setup_servo_mount(self, roll_servo=5, pitch_servo=6, yaw_servo=7):5301'''configure a rpy servo mount; caller responsible for required rebooting'''5302self.progress("Setting up servo mount")5303self.set_parameters({5304"MNT1_TYPE": 1,5305"MNT1_PITCH_MIN": -45,5306"MNT1_PITCH_MAX": 45,5307"RC6_OPTION": 213, # MOUNT1_PITCH5308"SERVO%u_FUNCTION" % roll_servo: 8, # roll5309"SERVO%u_FUNCTION" % pitch_servo: 7, # pitch5310"SERVO%u_FUNCTION" % yaw_servo: 6, # yaw5311})53125313def get_mount_roll_pitch_yaw_deg(self):5314'''return mount (aka gimbal) roll, pitch and yaw angles in degrees'''5315# wait for gimbal attitude message5316m = self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5)53175318yaw_is_absolute = m.flags & mavutil.mavlink.GIMBAL_DEVICE_FLAGS_YAW_LOCK5319# convert quaternion to euler angles and return5320q = quaternion.Quaternion(m.q)5321euler = q.euler5322return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]), yaw_is_absolute53235324def set_mount_mode(self, mount_mode):5325'''set mount mode'''5326self.run_cmd_int(5327mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,5328p1=mount_mode,5329p2=0, # stabilize roll (unsupported)5330p3=0, # stabilize pitch (unsupported)5331)5332self.run_cmd(5333mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,5334p1=mount_mode,5335p2=0, # stabilize roll (unsupported)5336p3=0, # stabilize pitch (unsupported)5337)53385339def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True):5340'''called in multipleplaces to make sure that mount RC targetting works'''5341if True:5342self.context_push()5343self.set_parameters({5344'RC6_OPTION': 0,5345'RC11_OPTION': 212, # MOUNT1_ROLL5346'RC12_OPTION': 213, # MOUNT1_PITCH5347'RC13_OPTION': 214, # MOUNT1_YAW5348'RC12_MIN': 1100,5349'RC12_MAX': 1900,5350'RC12_TRIM': 1500,5351'MNT1_PITCH_MIN': -45,5352'MNT1_PITCH_MAX': 45,5353})5354self.progress("Testing RC angular control")5355# default RC min=1100 max=19005356self.set_rc_from_map({535711: 1500,535812: 1500,535913: 1500,5360})5361self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5362self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output")5363rc12_in = 14005364rc12_min = 1100 # default5365rc12_max = 1900 # default5366mpitch_min = -45.05367mpitch_max = 45.05368expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (mpitch_max-mpitch_min)) + mpitch_min5369self.progress("expected mount pitch: %f" % expected_pitch)5370if expected_pitch != -11.25:5371raise NotAchievedException("Calculation wrong - defaults changed?!")5372self.set_rc(12, rc12_in)5373self.test_mount_pitch(-11.25, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5374self.set_rc(12, 1800)5375self.test_mount_pitch(33.75, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5376self.set_rc_from_map({537711: 1500,537812: 1500,537913: 1500,5380})53815382try:5383self.context_push()5384self.set_parameters({5385"RC12_MIN": 1000,5386"RC12_MAX": 2000,5387"MNT1_PITCH_MIN": -90,5388"MNT1_PITCH_MAX": 10,5389})5390self.set_rc(12, 1000)5391self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5392self.set_rc(12, 2000)5393self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5394self.set_rc(12, 1500)5395self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5396finally:5397self.context_pop()53985399self.set_rc(12, 1500)54005401if do_rate_tests:5402self.test_mount_rc_targetting_rate_control()54035404self.context_pop()54055406def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500):5407if True:5408self.progress("Testing RC rate control")5409self.set_parameter('MNT1_RC_RATE', 10)5410self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5411# Note that we don't constrain the desired angle in the following so that we don't5412# timeout due to fetching Mount pitch limit params.5413self.set_rc(12, 1300)5414self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5415self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5416self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5417self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5418self.set_rc(12, 1700)5419self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5420self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5421self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5422self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)5423self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)54245425self.progress("Reverting to angle mode")5426self.set_parameter('MNT1_RC_RATE', 0)5427self.set_rc(12, 1500)5428self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)54295430def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True):5431'''Test Camera/Antenna Mount - assumes a camera is set up and ready to go'''5432if True:5433# make sure we're getting gimbal device attitude status5434self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5, very_verbose=True)54355436# change mount to neutral mode (point forward, not stabilising)5437self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)54385439# test pitch is not neutral to start with5440mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()5441if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:5442raise NotAchievedException("Mount not neutral")54435444self.takeoff(30, mode='GUIDED')54455446# pitch vehicle back and confirm gimbal is still not stabilising5447despitch = 105448despitch_tolerance = 354495450self.progress("Pitching vehicle")5451self.do_pitch(despitch) # will time out!54525453self.wait_pitch(despitch, despitch_tolerance)54545455# check gimbal is still not stabilising5456mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()5457if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:5458raise NotAchievedException("Mount stabilising when not requested")54595460# center RC tilt control and change mount to RC_TARGETING mode5461self.progress("Gimbal to RC Targetting mode")5462self.set_rc(6, pitch_rc_neutral)5463self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)54645465# pitch vehicle back and confirm gimbal is stabilising5466self.progress("Pitching vehicle")5467self.do_pitch(despitch)5468self.wait_pitch(despitch, despitch_tolerance)5469self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)54705471# point gimbal at specified angle5472self.progress("Point gimbal using GIMBAL_MANAGER_PITCHYAW (ANGLE)")5473self.do_pitch(0) # level vehicle5474self.wait_pitch(0, despitch_tolerance)5475self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)5476for (method, angle) in (self.run_cmd, -20), (self.run_cmd_int, -30):5477method(5478mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,5479p1=angle, # pitch angle in degrees5480p2=0, # yaw angle in degrees5481p3=0, # pitch rate in degrees (NaN to ignore)5482p4=0, # yaw rate in degrees (NaN to ignore)5483p5=0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame)5484p6=0, # unused5485p7=0, # gimbal id5486)5487self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)54885489# this is a one-off; ArduCopter *will* time out this directive!5490self.progress("Levelling aircraft")5491self.mav.mav.set_attitude_target_send(54920, # time_boot_ms54931, # target sysid54941, # target compid54950, # bitmask of things to ignore5496mavextra.euler_to_quat([0, 0, 0]), # att54970, # roll rate (rad/s)54980, # pitch rate (rad/s)54990, # yaw rate (rad/s)55000.5) # thrust, 0 to 1, translated to a climb/descent rate55015502self.wait_groundspeed(0, 1)55035504# now test RC targetting5505self.progress("Testing mount RC targetting")55065507self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5508self.test_mount_rc_targetting(5509pitch_rc_neutral=pitch_rc_neutral,5510do_rate_tests=do_rate_tests,5511)55125513self.progress("Testing mount ROI behaviour")5514self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)5515start = self.mav.location()5516self.progress("start=%s" % str(start))5517(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,5518start.lng,551910,552020)5521roi_alt = 05522self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION")5523self.run_cmd(5524mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,5525p5=roi_lat,5526p6=roi_lon,5527p7=roi_alt,5528)5529self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)5530self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION")5531# start by pointing the gimbal elsewhere with a5532# known-working command:5533self.run_cmd(5534mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,5535p5=roi_lat + 1,5536p6=roi_lon + 1,5537p7=roi_alt,5538)5539# now point it with command_int:5540self.run_cmd_int(5541mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,5542p5=int(roi_lat * 1e7),5543p6=int(roi_lon * 1e7),5544p7=roi_alt,5545frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,5546)5547self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)55485549self.progress("Using MAV_CMD_DO_SET_ROI_NONE")5550self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE)5551self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE)5552self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)55535554start = self.mav.location()5555(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,5556start.lng,5557-100,5558-200)5559roi_alt = 05560self.progress("Using MAV_CMD_DO_SET_ROI")5561self.run_cmd(5562mavutil.mavlink.MAV_CMD_DO_SET_ROI,5563p5=roi_lat,5564p6=roi_lon,5565p7=roi_alt,5566)5567self.test_mount_pitch(-7.5, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)55685569start = self.mav.location()5570(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,5571start.lng,5572-100,5573-200)5574roi_alt = 05575self.progress("Using MAV_CMD_DO_SET_ROI (COMMAND_INT)")5576self.run_cmd_int(5577mavutil.mavlink.MAV_CMD_DO_SET_ROI,55780,55790,55800,55810,5582int(roi_lat*1e7),5583int(roi_lon*1e7),5584roi_alt,5585frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,5586)5587self.test_mount_pitch(-7.5, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)5588self.progress("Using MAV_CMD_DO_SET_ROI (COMMAND_INT), absolute-alt-frame")5589# this is pointing essentially straight down5590self.run_cmd_int(5591mavutil.mavlink.MAV_CMD_DO_SET_ROI,55920,55930,55940,55950,5596int(roi_lat*1e7),5597int(roi_lon*1e7),5598roi_alt,5599frame=mavutil.mavlink.MAV_FRAME_GLOBAL,5600)5601self.test_mount_pitch(-70, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, hold=2)56025603self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)5604self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)56055606self.progress("Testing mount roi-sysid behaviour")5607self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)5608start = self.mav.location()5609self.progress("start=%s" % str(start))5610(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,5611start.lng,561210,561320)5614roi_alt = 05615self.progress("Using MAV_CMD_DO_SET_ROI_SYSID")5616self.run_cmd(5617mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID,5618p1=self.mav.source_system,5619)5620self.mav.mav.global_position_int_send(56210, # time boot ms5622int(roi_lat * 1e7),5623int(roi_lon * 1e7),56240 * 1000, # mm alt amsl56250 * 1000, # relalt mm UP!56260, # vx56270, # vy56280, # vz56290 # heading5630)5631self.test_mount_pitch(-89, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2)56325633self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE)5634self.run_cmd_int(5635mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID,5636p1=self.mav.source_system,5637)5638self.mav.mav.global_position_int_send(56390, # time boot ms5640int(roi_lat * 1e7),5641int(roi_lon * 1e7),5642670 * 1000, # mm alt amsl5643100 * 1000, # mm UP!56440, # vx56450, # vy56460, # vz56470 # heading5648)5649self.test_mount_pitch(565068,56515,5652mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET,5653hold=2,5654constrained=constrain_sysid_target,5655)56565657self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)5658self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)56595660self.disarm_vehicle(force=True)56615662self.test_mount_body_yaw()56635664def test_mount_body_yaw(self):5665'''check reporting of yaw'''5666# change mount to neutral mode (point forward, not stabilising)5667self.takeoff(10, mode='GUIDED')56685669self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)56705671for heading in 30, 45, 150:5672self.guided_achieve_heading(heading)56735674r, p , y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()56755676if yaw_is_absolute:5677raise NotAchievedException("Expected a relative yaw")56785679if y > 1:5680raise NotAchievedException("Bad yaw (y=%f)")56815682self.do_RTL()56835684def Mount(self):5685'''test servo mount'''5686self.setup_servo_mount()5687self.reboot_sitl() # to handle MNT_TYPE changing5688self.mount_test_body()56895690def MountSolo(self):5691'''test type=2, a "Solo" mount'''5692self.set_parameters({5693"MNT1_TYPE": 2,5694"RC6_OPTION": 213, # MOUNT1_PITCH5695})5696self.customise_SITL_commandline([5697"--gimbal" # connects on port 57625698])5699self.mount_test_body(5700pitch_rc_neutral=1818,5701do_rate_tests=False, # solo can't do rate control (yet?)5702constrain_sysid_target=False, # not everything constrains all angles5703)57045705def assert_mount_rpy(self, r, p, y, tolerance=1):5706'''assert mount atttiude in degrees'''5707got_r, got_p, got_y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()5708for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"):5709if abs(want - got) > tolerance:5710raise NotAchievedException("%s incorrect; want=%f got=%f" %5711(name, want, got))57125713def neutralise_gimbal(self):5714'''put mount into neutralise mode, assert it is at zero angles'''5715self.run_cmd(5716mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5717p7=mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL,5718)5719self.test_mount_pitch(0, 0, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT)57205721def MAV_CMD_DO_MOUNT_CONTROL(self):5722'''test MAV_CMD_DO_MOUNT_CONTROL mavlink command'''57235724# setup mount parameters5725self.context_push()5726self.setup_servo_mount()5727self.reboot_sitl() # to handle MNT_TYPE changing57285729takeoff_loc = self.mav.location()57305731self.takeoff(20, mode='GUIDED')5732self.guided_achieve_heading(315)57335734self.run_cmd(5735mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5736p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT,5737)5738self.run_cmd_int(5739mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5740p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT,5741)57425743for method in self.run_cmd, self.run_cmd_int:5744self.start_subtest("MAV_MOUNT_MODE_GPS_POINT")57455746self.progress("start=%s" % str(takeoff_loc))5747t = self.offset_location_ne(takeoff_loc, 20, 0)5748self.progress("targetting=%s" % str(t))57495750# this command is *weird* as the lat/lng is *always* 1e7,5751# even when transported via COMMAND_LONG!5752x = int(t.lat * 1e7)5753y = int(t.lng * 1e7)5754method(5755mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5756p4=0, # this is a relative altitude!5757p5=x,5758p6=y,5759p7=mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT,5760)5761self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)5762self.neutralise_gimbal()57635764self.start_subtest("MAV_MOUNT_MODE_HOME_LOCATION")5765method(5766mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5767p7=mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION,5768)5769self.test_mount_pitch(-90, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION)5770self.neutralise_gimbal()57715772# try an invalid mount mode. Note that this is asserting we5773# are receiving a result code which is actually incorrect;5774# this should be MAV_RESULT_DENIED5775self.start_subtest("Invalid mode")5776method(5777mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5778p7=87,5779want_result=mavutil.mavlink.MAV_RESULT_FAILED,5780)57815782self.start_subtest("MAV_MOUNT_MODE_MAVLINK_TARGETING")5783r = 155784p = 205785y = 305786method(5787mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5788p1=p,5789p2=r,5790p3=y,5791p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,5792)5793self.delay_sim_time(2)5794self.assert_mount_rpy(r, p, y)5795self.neutralise_gimbal()57965797self.start_subtest("MAV_MOUNT_MODE_RC_TARGETING")5798method(5799mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5800p7=mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,5801)5802self.test_mount_rc_targetting()58035804self.start_subtest("MAV_MOUNT_MODE_RETRACT")5805self.context_push()5806retract_r = 135807retract_p = 235808retract_y = 335809self.set_parameters({5810"MNT1_RETRACT_X": retract_r,5811"MNT1_RETRACT_Y": retract_p,5812"MNT1_RETRACT_Z": retract_y,5813})5814method(5815mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5816p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT,5817)5818self.delay_sim_time(3)5819self.assert_mount_rpy(retract_r, retract_p, retract_y)5820self.context_pop()58215822self.do_RTL()58235824self.context_pop()5825self.reboot_sitl()58265827def AutoYawDO_MOUNT_CONTROL(self):5828'''test AutoYaw behaviour when MAV_CMD_DO_MOUNT_CONTROL sent to Mount without Yaw control'''58295830# setup mount parameters5831self.context_push()58325833yaw_servo = 75834self.setup_servo_mount(roll_servo=5, pitch_servo=6, yaw_servo=yaw_servo)5835# Disable Mount Yaw servo5836self.set_parameters({5837"SERVO%u_FUNCTION" % yaw_servo: 0,5838})5839self.reboot_sitl() # to handle MNT_TYPE changing58405841self.takeoff(20, mode='GUIDED')58425843for mount_yaw in [-45, 0, 45]:5844heading = 3305845self.guided_achieve_heading(heading)5846self.assert_heading(heading)58475848self.neutralise_gimbal()58495850r = 155851p = 205852self.run_cmd_int(5853mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,5854p1=p,5855p2=r,5856p3=mount_yaw,5857p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,5858)5859self.delay_sim_time(5)5860# We have disabled yaw servo, so expect mount yaw to be zero5861self.assert_mount_rpy(r, p, 0)5862# But we expect the copter to yaw instead5863self.assert_heading(heading + mount_yaw)58645865self.do_RTL()58665867self.context_pop()5868self.reboot_sitl()58695870def MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE(self):5871'''test MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE mavlink command'''5872# setup mount parameters5873self.context_push()5874self.setup_servo_mount()5875self.reboot_sitl() # to handle MNT_TYPE changing58765877self.context_set_message_rate_hz('GIMBAL_MANAGER_STATUS', 10)5878self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {5879"gimbal_device_id": 1,5880"primary_control_sysid": 0,5881"primary_control_compid": 0,5882})58835884for method in self.run_cmd, self.run_cmd_int:5885self.start_subtest("set_sysid-compid")5886method(5887mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,5888p1=37,5889p2=38,5890)5891self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {5892"gimbal_device_id": 1,5893"primary_control_sysid": 37,5894"primary_control_compid": 38,5895})58965897self.start_subtest("leave unchanged")5898method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-1)5899self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {5900"gimbal_device_id": 1,5901"primary_control_sysid": 37,5902"primary_control_compid": 38,5903})59045905# ardupilot currently handles this incorrectly:5906# self.start_subtest("self-controlled")5907# method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-2)5908# self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {5909# "gimbal_device_id": 1,5910# "primary_control_sysid": 1,5911# "primary_control_compid": 1,5912# })59135914self.start_subtest("release control")5915method(5916mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,5917p1=self.mav.source_system,5918p2=self.mav.source_component,5919)5920self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {5921"gimbal_device_id": 1,5922"primary_control_sysid": self.mav.source_system,5923"primary_control_compid": self.mav.source_component,5924})5925method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-3)5926self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {5927"gimbal_device_id": 1,5928"primary_control_sysid": 0,5929"primary_control_compid": 0,5930})59315932self.context_pop()5933self.reboot_sitl()59345935def MountYawVehicleForMountROI(self):5936'''Test Camera/Antenna Mount vehicle yawing for ROI'''5937self.context_push()59385939self.set_parameter("SYSID_MYGCS", self.mav.source_system)5940yaw_servo = 75941self.setup_servo_mount(yaw_servo=yaw_servo)5942self.reboot_sitl() # to handle MNT1_TYPE changing59435944self.progress("checking ArduCopter yaw-aircraft-for-roi")5945ex = None5946try:5947self.takeoff(20, mode='GUIDED')59485949m = self.mav.recv_match(type='VFR_HUD', blocking=True)5950self.progress("current heading %u" % m.heading)5951self.set_parameter("SERVO%u_FUNCTION" % yaw_servo, 0) # yaw5952self.progress("Waiting for check_servo_map to do its job")5953self.delay_sim_time(5)5954self.progress("Pointing North")5955self.guided_achieve_heading(0)5956self.delay_sim_time(5)5957start = self.mav.location()5958(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,5959start.lng,5960-100,5961-100)5962roi_alt = 05963self.progress("Using MAV_CMD_DO_SET_ROI")5964self.run_cmd(5965mavutil.mavlink.MAV_CMD_DO_SET_ROI,5966p5=roi_lat,5967p6=roi_lon,5968p7=roi_alt,5969)59705971self.progress("Waiting for vehicle to point towards ROI")5972self.wait_heading(225, timeout=600, minimum_duration=2)59735974# the following numbers are 1-degree-latitude and5975# 0-degrees longitude - just so that we start to5976# really move a lot.5977there = mavutil.location(1, 0, 0, 0)59785979self.progress("Starting to move")5980self.mav.mav.set_position_target_global_int_send(59810, # timestamp59821, # target system_id59831, # target component id5984mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,5985MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-lat-lon-alt5986there.lat, # lat5987there.lng, # lon5988there.alt, # alt59890, # vx59900, # vy59910, # vz59920, # afx59930, # afy59940, # afz59950, # yaw59960, # yawrate5997)59985999self.progress("Starting to move changes the target")6000bearing = self.bearing_to(there)6001self.wait_heading(bearing, timeout=600, minimum_duration=2)60026003self.run_cmd(6004mavutil.mavlink.MAV_CMD_DO_SET_ROI,6005p5=roi_lat,6006p6=roi_lon,6007p7=roi_alt,6008)60096010self.progress("Wait for vehicle to point sssse due to moving")6011self.wait_heading(170, timeout=600, minimum_duration=1)60126013self.do_RTL()60146015except Exception as e:6016self.print_exception_caught(e)6017ex = e60186019self.context_pop()60206021if ex is not None:6022raise ex60236024def ThrowMode(self):6025'''Fly Throw Mode'''6026# test boomerang mode:6027self.progress("Throwing vehicle away")6028self.set_parameters({6029"THROW_NEXTMODE": 6,6030"SIM_SHOVE_Z": -30,6031"SIM_SHOVE_X": -20,6032})6033self.change_mode('THROW')6034self.wait_ready_to_arm()6035self.arm_vehicle()6036try:6037self.set_parameter("SIM_SHOVE_TIME", 500)6038except ValueError:6039# the shove resets this to zero6040pass60416042tstart = self.get_sim_time()6043self.wait_mode('RTL')6044max_good_tdelta = 156045tdelta = self.get_sim_time() - tstart6046self.progress("Vehicle in RTL")6047self.wait_rtl_complete()6048self.progress("Vehicle disarmed")6049if tdelta > max_good_tdelta:6050raise NotAchievedException("Took too long to enter RTL: %fs > %fs" %6051(tdelta, max_good_tdelta))6052self.progress("Vehicle returned")60536054def hover_and_check_matched_frequency_with_fft_and_psd(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None,6055reverse=None, takeoff=True, instance=0):6056'''Takeoff and hover, checking the noise against the provided db level and returning psd'''6057# find a motor peak6058if takeoff:6059self.takeoff(10, mode="ALT_HOLD")60606061tstart, tend, hover_throttle = self.hover_for_interval(15)6062self.do_RTL()60636064psd = self.mavfft_fttd(1, instance, tstart * 1.0e6, tend * 1.0e6)60656066# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin6067freq = psd["F"][numpy.argmax(psd["X"][minhz:maxhz]) + minhz] * (1000. / 1024.)6068peakdb = numpy.amax(psd["X"][minhz:maxhz])6069if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):6070if reverse is not None:6071self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))6072else:6073raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))6074else:6075if reverse is not None:6076raise NotAchievedException(6077"Detected motor peak at %fHz, throttle %f%%, %fdB" %6078(freq, hover_throttle, peakdb))6079else:6080self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" %6081(freq, hover_throttle, peakdb))60826083return freq, hover_throttle, peakdb, psd60846085def hover_and_check_matched_frequency_with_fft(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None,6086reverse=None, takeoff=True, instance=0):6087'''Takeoff and hover, checking the noise against the provided db level and returning peak db'''6088freq, hover_throttle, peakdb, psd = \6089self.hover_and_check_matched_frequency_with_fft_and_psd(dblevel, minhz,6090maxhz, peakhz, reverse, takeoff, instance)6091return freq, hover_throttle, peakdb60926093def get_average_esc_frequency(self):6094mlog = self.dfreader_for_current_onboard_log()6095rpm_total = 06096rpm_count = 06097tho = 06098while True:6099m = mlog.recv_match()6100if m is None:6101break6102msg_type = m.get_type()6103if msg_type == "CTUN":6104tho = m.ThO6105elif msg_type == "ESC" and tho > 0.1:6106rpm_total += m.RPM6107rpm_count += 161086109esc_hz = rpm_total / (rpm_count * 60)6110return esc_hz61116112def DynamicNotches(self):6113"""Use dynamic harmonic notch to control motor noise."""6114self.progress("Flying with dynamic notches")6115self.context_push()61166117ex = None6118try:6119self.set_parameters({6120"AHRS_EKF_TYPE": 10,6121"INS_LOG_BAT_MASK": 3,6122"INS_LOG_BAT_OPT": 0,6123"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour6124"LOG_BITMASK": 958,6125"LOG_DISARMED": 0,6126"SIM_VIB_MOT_MAX": 350,6127"SIM_GYR1_RND": 20,6128})6129self.reboot_sitl()61306131self.takeoff(10, mode="ALT_HOLD")61326133# find a motor peak6134freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300)61356136# now add a dynamic notch and check that the peak is squashed6137self.set_parameters({6138"INS_LOG_BAT_OPT": 2,6139"INS_HNTCH_ENABLE": 1,6140"INS_HNTCH_FREQ": freq,6141"INS_HNTCH_REF": hover_throttle/100.,6142"INS_HNTCH_HMNCS": 5, # first and third harmonic6143"INS_HNTCH_ATT": 50,6144"INS_HNTCH_BW": freq/2,6145})6146self.reboot_sitl()61476148freq, hover_throttle, peakdb1 = \6149self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True)61506151# now add double dynamic notches and check that the peak is squashed6152self.set_parameter("INS_HNTCH_OPTS", 1)6153self.reboot_sitl()61546155freq, hover_throttle, peakdb2 = \6156self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)61576158# double-notch should do better, but check for within 5%6159if peakdb2 * 1.05 > peakdb1:6160raise NotAchievedException(6161"Double-notch peak was higher than single-notch peak %fdB > %fdB" %6162(peakdb2, peakdb1))61636164# now add triple dynamic notches and check that the peak is squashed6165self.set_parameter("INS_HNTCH_OPTS", 16)6166self.reboot_sitl()61676168freq, hover_throttle, peakdb2 = \6169self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)61706171# triple-notch should do better, but check for within 5%6172if peakdb2 * 1.05 > peakdb1:6173raise NotAchievedException(6174"Triple-notch peak was higher than single-notch peak %fdB > %fdB" %6175(peakdb2, peakdb1))61766177except Exception as e:6178self.print_exception_caught(e)6179ex = e61806181self.context_pop()61826183if ex is not None:6184raise ex61856186def DynamicRpmNotches(self):6187"""Use dynamic harmonic notch to control motor noise via ESC telemetry."""6188self.progress("Flying with ESC telemetry driven dynamic notches")61896190self.set_rc_default()6191self.set_parameters({6192"AHRS_EKF_TYPE": 10,6193"INS_LOG_BAT_MASK": 3,6194"INS_LOG_BAT_OPT": 0,6195"INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour6196"LOG_BITMASK": 958,6197"LOG_DISARMED": 0,6198"SIM_VIB_MOT_MAX": 350,6199"SIM_GYR1_RND": 20,6200"SIM_ESC_TELEM": 16201})6202self.reboot_sitl()62036204self.takeoff(10, mode="ALT_HOLD")62056206# find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe.6207# there is a second harmonic at 380Hz which should be avoided to make the test reliable6208# detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB6209freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320)62106211# now add a dynamic notch and check that the peak is squashed6212self.set_parameters({6213"INS_LOG_BAT_OPT": 4,6214"INS_HNTCH_ENABLE": 1,6215"INS_HNTCH_FREQ": 80,6216"INS_HNTCH_REF": 1.0,6217"INS_HNTCH_HMNCS": 5, # first and third harmonic6218"INS_HNTCH_ATT": 50,6219"INS_HNTCH_BW": 40,6220"INS_HNTCH_MODE": 3,6221})6222self.reboot_sitl()62236224# -10dB is pretty conservative - actual is about -25dB6225freq, hover_throttle, peakdb1, psd = \6226self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)6227# find the noise at the motor frequency6228esc_hz = self.get_average_esc_frequency()6229esc_peakdb1 = psd["X"][int(esc_hz)]62306231# now add notch-per motor and check that the peak is squashed6232self.set_parameter("INS_HNTCH_OPTS", 2)6233self.reboot_sitl()62346235freq, hover_throttle, peakdb2, psd = \6236self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)6237# find the noise at the motor frequency6238esc_hz = self.get_average_esc_frequency()6239esc_peakdb2 = psd["X"][int(esc_hz)]62406241# notch-per-motor will be better at the average ESC frequency6242if esc_peakdb2 > esc_peakdb1:6243raise NotAchievedException(6244"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %6245(esc_peakdb2, esc_peakdb1))62466247# check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily6248# testing shows this to be -58dB on average6249if esc_peakdb2 > -25:6250raise NotAchievedException(6251"Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2)62526253# Now do it again for an octacopter6254self.context_push()6255ex = None6256try:6257self.progress("Flying Octacopter with ESC telemetry driven dynamic notches")6258self.set_parameter("INS_HNTCH_OPTS", 0)6259self.customise_SITL_commandline(6260[],6261defaults_filepath=','.join(self.model_defaults_filepath("octa")),6262model="octa"6263)6264freq, hover_throttle, peakdb1, psd = \6265self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)6266# find the noise at the motor frequency6267esc_hz = self.get_average_esc_frequency()6268esc_peakdb1 = psd["X"][int(esc_hz)]62696270# now add notch-per motor and check that the peak is squashed6271self.set_parameter("INS_HNTCH_HMNCS", 1)6272self.set_parameter("INS_HNTCH_OPTS", 2)6273self.reboot_sitl()62746275freq, hover_throttle, peakdb2, psd = \6276self.hover_and_check_matched_frequency_with_fft_and_psd(-15, 50, 320, reverse=True, instance=2)6277# find the noise at the motor frequency6278esc_hz = self.get_average_esc_frequency()6279esc_peakdb2 = psd["X"][int(esc_hz)]62806281# notch-per-motor will be better at the average ESC frequency6282if esc_peakdb2 > esc_peakdb1:6283raise NotAchievedException(6284"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %6285(esc_peakdb2, esc_peakdb1))62866287except Exception as e:6288self.print_exception_caught(e)6289ex = e6290self.context_pop()6291self.reboot_sitl()6292if ex is not None:6293raise ex62946295def DynamicRpmNotchesRateThread(self):6296"""Use dynamic harmonic notch to control motor noise via ESC telemetry."""6297self.progress("Flying with ESC telemetry driven dynamic notches")6298self.context_push()6299self.set_rc_default()6300self.set_parameters({6301"AHRS_EKF_TYPE": 10,6302"INS_LOG_BAT_MASK": 3,6303"INS_LOG_BAT_OPT": 0,6304"INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour6305"LOG_BITMASK": 959,6306"LOG_DISARMED": 0,6307"SIM_VIB_MOT_MAX": 350,6308"SIM_GYR1_RND": 20,6309"SIM_ESC_TELEM": 1,6310"FSTRATE_ENABLE": 16311})6312self.reboot_sitl()63136314self.takeoff(10, mode="ALT_HOLD")63156316# find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe.6317# there is a second harmonic at 380Hz which should be avoided to make the test reliable6318# detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB6319freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320)63206321# now add a dynamic notch and check that the peak is squashed6322self.set_parameters({6323"INS_LOG_BAT_OPT": 4,6324"INS_HNTCH_ENABLE": 1,6325"INS_HNTCH_FREQ": 80,6326"INS_HNTCH_REF": 1.0,6327"INS_HNTCH_HMNCS": 5, # first and third harmonic6328"INS_HNTCH_ATT": 50,6329"INS_HNTCH_BW": 40,6330"INS_HNTCH_MODE": 3,6331"FSTRATE_ENABLE": 16332})6333self.reboot_sitl()63346335# -10dB is pretty conservative - actual is about -25dB6336freq, hover_throttle, peakdb1, psd = \6337self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)6338# find the noise at the motor frequency6339esc_hz = self.get_average_esc_frequency()6340esc_peakdb1 = psd["X"][int(esc_hz)]63416342# now add notch-per motor and check that the peak is squashed6343self.set_parameter("INS_HNTCH_OPTS", 2)6344self.reboot_sitl()63456346freq, hover_throttle, peakdb2, psd = \6347self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)6348# find the noise at the motor frequency6349esc_hz = self.get_average_esc_frequency()6350esc_peakdb2 = psd["X"][int(esc_hz)]63516352# notch-per-motor will be better at the average ESC frequency6353if esc_peakdb2 > esc_peakdb1:6354raise NotAchievedException(6355"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %6356(esc_peakdb2, esc_peakdb1))63576358# check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily6359# testing shows this to be -58dB on average6360if esc_peakdb2 > -25:6361raise NotAchievedException(6362"Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2)6363self.context_pop()6364self.reboot_sitl()63656366def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):6367'''do a simple up-and-down test flight with current vehicle state.6368Check that the onboard filter comes up with the same peak-frequency that6369post-processing does.'''6370self.takeoff(10, mode="ALT_HOLD")6371tstart, tend, hover_throttle = self.hover_for_interval(15)6372self.do_RTL()63736374psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)63756376# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin6377scale = 1000. / 1024.6378sminhz = int(minhz * scale)6379smaxhz = int(maxhz * scale)6380freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]6381peakdb = numpy.amax(psd["X"][sminhz:smaxhz])63826383self.progress("Post-processing FFT detected motor peak at %fHz/%fdB, throttle %f%%" %6384(freq, peakdb, hover_throttle))63856386if peakdb < dblevel:6387raise NotAchievedException(6388"Detected motor peak not strong enough; want=%fdB got=%fdB" %6389(peakdb, dblevel))63906391# caller can supply an expected frequency:6392if peakhz is not None and abs(freq - peakhz) / peakhz > 0.05:6393raise NotAchievedException(6394"Post-processing detected motor peak at wrong frequency; want=%fHz got=%fHz" %6395(peakhz, freq))63966397# we have a peak make sure that the onboard filter detected6398# something close logging is at 10Hz63996400# peak within resolution of FFT length6401pkAvg, nmessages = self.extract_median_FTN1_PkAvg_from_current_onboard_log(tstart, tend)6402self.progress("Onboard-FFT detected motor peak at %fHz (processed %d FTN1 messages)" % (pkAvg, nmessages))64036404# accuracy is determined by sample rate and fft length, given6405# our use of quinn we could probably use half of this6406freqDelta = 1000. / fftLength6407if abs(pkAvg - freq) > freqDelta:6408raise NotAchievedException(6409"post-processed FFT does not agree with onboard filter on peak frequency; onboard=%fHz post-processed=%fHz/%fdB" % # noqa6410(pkAvg, freq, dblevel)6411)6412return freq64136414def extract_median_FTN1_PkAvg_from_current_onboard_log(self, tstart, tend):6415'''extracts FTN1 messages from log, returns median of pkAvg values and6416the number of samples'''6417mlog = self.dfreader_for_current_onboard_log()6418freqs = []6419while True:6420m = mlog.recv_match(6421type='FTN1',6422blocking=False,6423condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))6424if m is None:6425break6426freqs.append(m.PkAvg)6427return numpy.median(numpy.asarray(freqs)), len(freqs)64286429def PIDNotches(self):6430"""Use dynamic harmonic notch to control motor noise."""6431self.progress("Flying with PID notches")6432self.set_parameters({6433"FILT1_TYPE": 1,6434"AHRS_EKF_TYPE": 10,6435"INS_LOG_BAT_MASK": 3,6436"INS_LOG_BAT_OPT": 0,6437"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour6438"LOG_BITMASK": 65535,6439"LOG_DISARMED": 0,6440"SIM_VIB_FREQ_X": 120, # roll6441"SIM_VIB_FREQ_Y": 120, # pitch6442"SIM_VIB_FREQ_Z": 180, # yaw6443"FILT1_NOTCH_FREQ": 120,6444"ATC_RAT_RLL_NEF": 1,6445"ATC_RAT_PIT_NEF": 1,6446"ATC_RAT_YAW_NEF": 1,6447"SIM_GYR1_RND": 5,6448})6449self.reboot_sitl()64506451self.hover_and_check_matched_frequency_with_fft(dblevel=5, minhz=20, maxhz=350, reverse=True)64526453def StaticNotches(self):6454"""Use static harmonic notch to control motor noise."""6455self.progress("Flying with Static notches")6456self.set_parameters({6457"AHRS_EKF_TYPE": 10,6458"INS_LOG_BAT_MASK": 3,6459"INS_LOG_BAT_OPT": 4,6460"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour6461"LOG_BITMASK": 65535,6462"LOG_DISARMED": 0,6463"SIM_VIB_FREQ_X": 120, # roll6464"SIM_VIB_FREQ_Y": 120, # pitch6465"SIM_VIB_FREQ_Z": 120, # yaw6466"SIM_VIB_MOT_MULT": 0,6467"SIM_GYR1_RND": 5,6468"INS_HNTCH_ENABLE": 1,6469"INS_HNTCH_FREQ": 120,6470"INS_HNTCH_REF": 1.0,6471"INS_HNTCH_HMNCS": 3, # first and second harmonic6472"INS_HNTCH_ATT": 50,6473"INS_HNTCH_BW": 40,6474"INS_HNTCH_MODE": 0, # static notch6475})6476self.reboot_sitl()64776478self.hover_and_check_matched_frequency_with_fft(dblevel=-15, minhz=20, maxhz=350, reverse=True, instance=2)64796480def ThrottleGainBoost(self):6481"""Use PD and Angle P boost for anti-gravity."""6482# basic gyro sample rate test6483self.progress("Flying with Throttle-Gain Boost")64846485# magic tridge EKF type that dramatically speeds up the test6486self.set_parameters({6487"AHRS_EKF_TYPE": 10,6488"EK2_ENABLE": 0,6489"EK3_ENABLE": 0,6490"INS_FAST_SAMPLE": 0,6491"LOG_BITMASK": 959,6492"LOG_DISARMED": 0,6493"ATC_THR_G_BOOST": 5.0,6494})64956496self.reboot_sitl()64976498self.takeoff(10, mode="ALT_HOLD")6499hover_time = 156500self.progress("Hovering for %u seconds" % hover_time)6501tstart = self.get_sim_time()6502while self.get_sim_time_cached() < tstart + hover_time:6503self.assert_receive_message('ATTITUDE')65046505# fly fast forrest!6506self.set_rc(3, 1900)6507self.set_rc(2, 1200)6508self.wait_groundspeed(5, 1000)6509self.set_rc(3, 1500)6510self.set_rc(2, 1500)65116512self.do_RTL()65136514def test_gyro_fft_harmonic(self, averaging):6515"""Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""6516# basic gyro sample rate test6517self.progress("Flying with gyro FFT harmonic - Gyro sample rate")6518self.context_push()6519ex = None6520# we are dealing with probabalistic scenarios involving threads6521try:6522self.start_subtest("Hover to calculate approximate hover frequency")6523# magic tridge EKF type that dramatically speeds up the test6524self.set_parameters({6525"AHRS_EKF_TYPE": 10,6526"EK2_ENABLE": 0,6527"EK3_ENABLE": 0,6528"INS_LOG_BAT_MASK": 3,6529"INS_LOG_BAT_OPT": 0,6530"INS_GYRO_FILTER": 100,6531"INS_FAST_SAMPLE": 0,6532"LOG_BITMASK": 958,6533"LOG_DISARMED": 0,6534"SIM_DRIFT_SPEED": 0,6535"SIM_DRIFT_TIME": 0,6536"FFT_THR_REF": self.get_parameter("MOT_THST_HOVER"),6537"SIM_GYR1_RND": 20, # enable a noisy gyro6538})65396540# motor peak enabling FFT will also enable the arming6541# check, self-testing the functionality6542self.set_parameters({6543"FFT_ENABLE": 1,6544"FFT_MINHZ": 50,6545"FFT_MAXHZ": 450,6546"FFT_SNR_REF": 10,6547})6548if averaging:6549self.set_parameter("FFT_NUM_FRAMES", 8)65506551# Step 1: inject actual motor noise and use the FFT to track it6552self.set_parameters({6553"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz6554"FFT_WINDOW_SIZE": 64,6555"FFT_WINDOW_OLAP": 0.75,6556})65576558self.reboot_sitl()6559freq = self.hover_and_check_matched_frequency(-15, 100, 250, 64)65606561# Step 2: add a second harmonic and check the first is still tracked6562self.start_subtest("Add a fixed frequency harmonic at twice the hover frequency "6563"and check the right harmonic is found")6564self.set_parameters({6565"SIM_VIB_FREQ_X": freq * 2,6566"SIM_VIB_FREQ_Y": freq * 2,6567"SIM_VIB_FREQ_Z": freq * 2,6568"SIM_VIB_MOT_MULT": 0.25, # halve the motor noise so that the higher harmonic dominates6569})6570self.reboot_sitl()65716572self.hover_and_check_matched_frequency(-15, 100, 250, 64, None)65736574# Step 3: switch harmonics mid flight and check for tracking6575self.start_subtest("Switch harmonics mid flight and check the right harmonic is found")6576self.set_parameter("FFT_HMNC_PEAK", 0)6577self.reboot_sitl()65786579self.takeoff(10, mode="ALT_HOLD")65806581hover_time = 106582tstart, tend_unused, hover_throttle = self.hover_for_interval(hover_time)65836584self.progress("Switching motor vibration multiplier")6585self.set_parameter("SIM_VIB_MOT_MULT", 5.0)65866587tstart_unused, tend, hover_throttle = self.hover_for_interval(hover_time)65886589self.do_RTL()65906591# peak within resolution of FFT length, the highest energy peak switched but our detection should not6592pkAvg, nmessages = self.extract_median_FTN1_PkAvg_from_current_onboard_log(tstart, tend)65936594freqDelta = 1000. / self.get_parameter("FFT_WINDOW_SIZE")65956596if abs(pkAvg - freq) > freqDelta:6597raise NotAchievedException("FFT did not detect a harmonic motor peak, found %f, wanted %f" % (pkAvg, freq))65986599# Step 4: dynamic harmonic6600self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated")6601# find a motor peak6602freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350)66036604# now add a dynamic notch and check that the peak is squashed6605self.set_parameters({6606"INS_LOG_BAT_OPT": 2,6607"INS_HNTCH_ENABLE": 1,6608"INS_HNTCH_HMNCS": 1,6609"INS_HNTCH_MODE": 4,6610"INS_HNTCH_FREQ": freq,6611"INS_HNTCH_REF": hover_throttle/100.0,6612"INS_HNTCH_ATT": 100,6613"INS_HNTCH_BW": freq/2,6614"INS_HNTCH_OPTS": 3,6615})6616self.reboot_sitl()66176618# 5db is far in excess of the attenuation that the double dynamic-harmonic notch is able6619# to provide (-7dB on average), but without the notch the peak is around 20dB so still a safe test6620self.hover_and_check_matched_frequency_with_fft(5, 100, 350, reverse=True)66216622self.set_parameters({6623"SIM_VIB_FREQ_X": 0,6624"SIM_VIB_FREQ_Y": 0,6625"SIM_VIB_FREQ_Z": 0,6626"SIM_VIB_MOT_MULT": 1.0,6627})6628# prevent update parameters from messing with the settings when we pop the context6629self.set_parameter("FFT_ENABLE", 0)6630self.reboot_sitl()66316632except Exception as e:6633self.print_exception_caught(e)6634ex = e66356636self.context_pop()66376638# need a final reboot because weird things happen to your6639# vehicle state when switching back from EKF type 10!6640self.reboot_sitl()66416642if ex is not None:6643raise ex66446645def GyroFFTHarmonic(self):6646"""Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""6647self.test_gyro_fft_harmonic(False)66486649def GyroFFTContinuousAveraging(self):6650"""Use dynamic harmonic notch with FFT averaging to control motor noise6651with harmonic matching of the first harmonic."""6652self.test_gyro_fft_harmonic(True)66536654def GyroFFT(self):6655"""Use dynamic harmonic notch to control motor noise."""6656# basic gyro sample rate test6657self.progress("Flying with gyro FFT - Gyro sample rate")6658self.context_push()66596660ex = None6661try:6662# magic tridge EKF type that dramatically speeds up the test6663self.set_parameters({6664"AHRS_EKF_TYPE": 10,6665"EK2_ENABLE": 0,6666"EK3_ENABLE": 0,6667"INS_LOG_BAT_MASK": 3,6668"INS_LOG_BAT_OPT": 4,6669"INS_GYRO_FILTER": 100,6670"INS_FAST_SAMPLE": 0,6671"LOG_BITMASK": 958,6672"LOG_DISARMED": 0,6673"SIM_DRIFT_SPEED": 0,6674"SIM_DRIFT_TIME": 0,6675"SIM_GYR1_RND": 20, # enable a noisy motor peak6676})6677# enabling FFT will also enable the arming check,6678# self-testing the functionality6679self.set_parameters({6680"FFT_ENABLE": 1,6681"FFT_MINHZ": 50,6682"FFT_MAXHZ": 450,6683"FFT_SNR_REF": 10,6684"FFT_WINDOW_SIZE": 128,6685"FFT_WINDOW_OLAP": 0.75,6686"FFT_SAMPLE_MODE": 0,6687})66886689# Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft6690# can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so6691# a 250Hz peak should be detectable within 5%6692self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")6693self.set_parameters({6694"SIM_VIB_FREQ_X": 250,6695"SIM_VIB_FREQ_Y": 250,6696"SIM_VIB_FREQ_Z": 250,6697})66986699self.reboot_sitl()67006701# find a motor peak6702self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250)67036704# Step 1b: run the same test with an FFT length of 256 which is needed to flush out a6705# whole host of bugs related to uint8_t. This also tests very accurately the frequency resolution6706self.set_parameter("FFT_WINDOW_SIZE", 256)6707self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")67086709self.reboot_sitl()67106711# find a motor peak6712self.hover_and_check_matched_frequency(-15, 100, 350, 256, 250)6713self.set_parameter("FFT_WINDOW_SIZE", 128)67146715# Step 2: inject actual motor noise and use the standard length FFT to track it6716self.start_subtest("Hover and check that the FFT can find the motor noise")6717self.set_parameters({6718"SIM_VIB_FREQ_X": 0,6719"SIM_VIB_FREQ_Y": 0,6720"SIM_VIB_FREQ_Z": 0,6721"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz6722"FFT_WINDOW_SIZE": 32,6723"FFT_WINDOW_OLAP": 0.5,6724})67256726self.reboot_sitl()6727freq = self.hover_and_check_matched_frequency(-15, 100, 250, 32)67286729self.set_parameter("SIM_VIB_MOT_MULT", 1.)67306731# Step 3: add a FFT dynamic notch and check that the peak is squashed6732self.start_subtest("Add a dynamic notch, hover and check that the noise peak is now gone")6733self.set_parameters({6734"INS_LOG_BAT_OPT": 2,6735"INS_HNTCH_ENABLE": 1,6736"INS_HNTCH_FREQ": freq,6737"INS_HNTCH_REF": 1.0,6738"INS_HNTCH_ATT": 50,6739"INS_HNTCH_BW": freq/2,6740"INS_HNTCH_MODE": 4,6741})6742self.reboot_sitl()67436744# do test flight:6745self.takeoff(10, mode="ALT_HOLD")6746tstart, tend, hover_throttle = self.hover_for_interval(15)6747# fly fast forrest!6748self.set_rc(3, 1900)6749self.set_rc(2, 1200)6750self.wait_groundspeed(5, 1000)6751self.set_rc(3, 1500)6752self.set_rc(2, 1500)6753self.do_RTL()67546755psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)67566757# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin6758scale = 1000. / 1024.6759sminhz = int(100 * scale)6760smaxhz = int(350 * scale)6761freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]6762peakdb = numpy.amax(psd["X"][sminhz:smaxhz])6763if peakdb < 0:6764self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))6765else:6766raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, peakdb))67676768# Step 4: loop sample rate test with larger window6769self.start_subtest("Hover and check that the FFT can find the motor noise when running at fast loop rate")6770# we are limited to half the loop rate for frequency detection6771self.set_parameters({6772"FFT_MAXHZ": 185,6773"INS_LOG_BAT_OPT": 4,6774"SIM_VIB_MOT_MAX": 220,6775"FFT_WINDOW_SIZE": 64,6776"FFT_WINDOW_OLAP": 0.75,6777"FFT_SAMPLE_MODE": 1,6778})6779self.reboot_sitl()67806781# do test flight:6782self.takeoff(10, mode="ALT_HOLD")6783tstart, tend, hover_throttle = self.hover_for_interval(15)6784self.do_RTL()67856786# why are we not checking the results from that flight? -pb2022061367876788# prevent update parameters from messing with the settings6789# when we pop the context6790self.set_parameter("FFT_ENABLE", 0)6791self.reboot_sitl()67926793except Exception as e:6794self.print_exception_caught(e)6795ex = e67966797self.context_pop()67986799# must reboot after we move away from EKF type 10 to EKF2 or EKF36800self.reboot_sitl()68016802if ex is not None:6803raise ex68046805def GyroFFTAverage(self):6806"""Use dynamic harmonic notch to control motor noise setup via FFT averaging."""6807# basic gyro sample rate test6808self.progress("Flying with gyro FFT harmonic - Gyro sample rate")6809self.context_push()6810ex = None6811try:6812# Step 16813self.start_subtest("Hover to calculate approximate hover frequency and see that it is tracked")6814# magic tridge EKF type that dramatically speeds up the test6815self.set_parameters({6816"INS_HNTCH_ATT": 100,6817"AHRS_EKF_TYPE": 10,6818"EK2_ENABLE": 0,6819"EK3_ENABLE": 0,6820"INS_LOG_BAT_MASK": 3,6821"INS_LOG_BAT_OPT": 2,6822"INS_GYRO_FILTER": 100,6823"INS_FAST_SAMPLE": 0,6824"LOG_BITMASK": 958,6825"LOG_DISARMED": 0,6826"SIM_DRIFT_SPEED": 0,6827"SIM_DRIFT_TIME": 0,6828"SIM_GYR1_RND": 20, # enable a noisy gyro6829})6830# motor peak enabling FFT will also enable the arming6831# check, self-testing the functionality6832self.set_parameters({6833"FFT_ENABLE": 1,6834"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable6835"FFT_SNR_REF": 10,6836"FFT_MINHZ": 80,6837"FFT_MAXHZ": 450,6838})68396840# Step 1: inject actual motor noise and use the FFT to track it6841self.set_parameters({6842"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz6843"RC7_OPTION" : 162, # FFT tune6844})68456846self.reboot_sitl()68476848# hover and engage FFT tracker6849self.takeoff(10, mode="ALT_HOLD")68506851hover_time = 6068526853# start the tune6854self.set_rc(7, 2000)68556856tstart, tend, hover_throttle = self.hover_for_interval(hover_time)68576858# finish the tune6859self.set_rc(7, 1000)68606861psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)68626863# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin6864freq = psd["F"][numpy.argmax(psd["X"][50:450]) + 50] * (1000. / 1024.)68656866detected_ref = self.get_parameter("INS_HNTCH_REF")6867detected_freq = self.get_parameter("INS_HNTCH_FREQ")6868self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref))68696870# approximate the scaled frequency6871scaled_freq_at_hover = math.sqrt((hover_throttle / 100.) / detected_ref) * detected_freq68726873# Check we matched6874if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05:6875raise NotAchievedException("Detected frequency %fHz did not match required %fHz" %6876(scaled_freq_at_hover, freq))68776878if self.get_parameter("INS_HNTCH_ENABLE") != 1:6879raise NotAchievedException("Harmonic notch was not enabled")68806881# Step 2: now rerun the test and check that the peak is squashed6882self.start_subtest("Verify that noise is suppressed by the harmonic notch")6883self.hover_and_check_matched_frequency_with_fft(0, 100, 350, reverse=True, takeoff=False)68846885# reset notch to defaults6886self.set_parameters({6887"INS_HNTCH_HMNCS": 3.0,6888"INS_HNTCH_ENABLE": 0.0,6889"INS_HNTCH_REF": 0.0,6890"INS_HNTCH_FREQ": 80,6891"INS_HNTCH_BW": 40,6892"INS_HNTCH_FM_RAT": 1.06893})68946895# Step 3: add a second harmonic and check the first is still tracked6896self.start_subtest("Add a fixed frequency harmonic at twice the hover frequency "6897"and check the right harmonic is found")6898self.set_parameters({6899"SIM_VIB_FREQ_X": detected_freq * 2,6900"SIM_VIB_FREQ_Y": detected_freq * 2,6901"SIM_VIB_FREQ_Z": detected_freq * 2,6902"SIM_VIB_MOT_MULT": 0.25, # halve the motor noise so that the higher harmonic dominates6903})6904self.reboot_sitl()69056906# hover and engage FFT tracker6907self.takeoff(10, mode="ALT_HOLD")69086909hover_time = 6069106911# start the tune6912self.set_rc(7, 2000)69136914tstart, tend, hover_throttle = self.hover_for_interval(hover_time)69156916# finish the tune6917self.set_rc(7, 1000)69186919self.do_RTL()69206921detected_ref = self.get_parameter("INS_HNTCH_REF")6922detected_freq = self.get_parameter("INS_HNTCH_FREQ")6923self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref))69246925# approximate the scaled frequency6926scaled_freq_at_hover = math.sqrt((hover_throttle / 100.) / detected_ref) * detected_freq69276928# Check we matched6929if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05:6930raise NotAchievedException("Detected frequency %fHz did not match required %fHz" %6931(scaled_freq_at_hover, freq))69326933if self.get_parameter("INS_HNTCH_ENABLE") != 1:6934raise NotAchievedException("Harmonic notch was not enabled")69356936self.set_parameters({6937"SIM_VIB_FREQ_X": 0,6938"SIM_VIB_FREQ_Y": 0,6939"SIM_VIB_FREQ_Z": 0,6940"SIM_VIB_MOT_MULT": 1.0,6941"INS_HNTCH_HMNCS": 3.0,6942"INS_HNTCH_ENABLE": 0.0,6943"INS_HNTCH_REF": 0.0,6944"INS_HNTCH_FREQ": 80,6945"INS_HNTCH_BW": 40,6946"INS_HNTCH_FM_RAT": 1.06947})6948# prevent update parameters from messing with the settings when we pop the context6949self.set_parameter("FFT_ENABLE", 0)6950self.reboot_sitl()69516952except Exception as e:6953self.print_exception_caught(e)6954ex = e69556956self.context_pop()69576958# need a final reboot because weird things happen to your6959# vehicle state when switching back from EKF type 10!6960self.reboot_sitl()69616962if ex is not None:6963raise ex69646965def GyroFFTPostFilter(self):6966"""Use FFT-driven dynamic harmonic notch to control post-RPM filter motor noise."""6967# basic gyro sample rate test6968self.progress("Flying with gyro FFT post-filter supression - Gyro sample rate")6969self.context_push()6970ex = None6971try:6972# This set of parameters creates two noise peaks one at the motor frequency and one at 250Hz6973# we then use ESC telemetry to drive the notch to clean up the motor noise and a post-filter6974# FFT notch to clean up the remaining 250Hz. If either notch fails then the test will be failed6975# due to too much noise being present6976self.set_parameters({6977"AHRS_EKF_TYPE": 10, # magic tridge EKF type that dramatically speeds up the test6978"EK2_ENABLE": 0,6979"EK3_ENABLE": 0,6980"INS_LOG_BAT_MASK": 3,6981"INS_LOG_BAT_OPT": 4,6982"INS_GYRO_FILTER": 100,6983"INS_FAST_SAMPLE": 3,6984"LOG_BITMASK": 958,6985"LOG_DISARMED": 0,6986"SIM_DRIFT_SPEED": 0,6987"SIM_DRIFT_TIME": 0,6988"SIM_GYR1_RND": 20, # enable a noisy gyro6989"INS_HNTCH_ENABLE": 1,6990"INS_HNTCH_FREQ": 80,6991"INS_HNTCH_REF": 1.0,6992"INS_HNTCH_HMNCS": 1, # first harmonic6993"INS_HNTCH_ATT": 50,6994"INS_HNTCH_BW": 30,6995"INS_HNTCH_MODE": 3, # ESC telemetry6996"INS_HNTCH_OPTS": 2, # notch-per-motor6997"INS_HNTC2_ENABLE": 1,6998"INS_HNTC2_FREQ": 80,6999"INS_HNTC2_REF": 1.0,7000"INS_HNTC2_HMNCS": 1,7001"INS_HNTC2_ATT": 50,7002"INS_HNTC2_BW": 40,7003"INS_HNTC2_MODE": 4, # in-flight FFT7004"INS_HNTC2_OPTS": 18, # triple-notch, notch-per-FFT peak7005"FFT_ENABLE": 1,7006"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable7007"FFT_OPTIONS": 1,7008"FFT_MINHZ": 50,7009"FFT_MAXHZ": 450,7010"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 145Hz7011"SIM_VIB_FREQ_X": 250, # create another peak at 250hz7012"SIM_VIB_FREQ_Y": 250,7013"SIM_VIB_FREQ_Z": 250,7014"SIM_GYR_FILE_RW": 2, # write data to a file7015})7016self.reboot_sitl()70177018# do test flight:7019self.takeoff(10, mode="ALT_HOLD")7020tstart, tend, hover_throttle = self.hover_for_interval(60)7021# fly fast forrest!7022self.set_rc(3, 1900)7023self.set_rc(2, 1200)7024self.wait_groundspeed(5, 1000)7025self.set_rc(3, 1500)7026self.set_rc(2, 1500)7027self.do_RTL()70287029psd = self.mavfft_fttd(1, 2, tstart * 1.0e6, tend * 1.0e6)70307031# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin7032scale = 1000. / 1024.7033sminhz = int(100 * scale)7034smaxhz = int(350 * scale)7035freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]7036peakdb = numpy.amax(psd["X"][sminhz:smaxhz])7037if peakdb < -5:7038self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))7039else:7040raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, peakdb))70417042# prevent update parameters from messing with the settings when we pop the context7043self.set_parameters({7044"SIM_VIB_FREQ_X": 0,7045"SIM_VIB_FREQ_Y": 0,7046"SIM_VIB_FREQ_Z": 0,7047"SIM_VIB_MOT_MULT": 1.0,7048"SIM_GYR_FILE_RW": 0, # stop writing data7049"FFT_ENABLE": 0,7050})7051self.reboot_sitl()70527053except Exception as e:7054self.print_exception_caught(e)7055ex = e70567057self.context_pop()70587059# need a final reboot because weird things happen to your7060# vehicle state when switching back from EKF type 10!7061self.reboot_sitl()70627063if ex is not None:7064raise ex70657066def GyroFFTMotorNoiseCheck(self):7067"""Use FFT to detect post-filter motor noise."""7068# basic gyro sample rate test7069self.progress("Flying with FFT motor-noise detection - Gyro sample rate")7070self.context_push()7071ex = None7072try:7073# This set of parameters creates two noise peaks one at the motor frequency and one at 250Hz7074# we then use ESC telemetry to drive the notch to clean up the motor noise and a post-filter7075# FFT notch to clean up the remaining 250Hz. If either notch fails then the test will be failed7076# due to too much noise being present7077self.set_parameters({7078"AHRS_EKF_TYPE": 10, # magic tridge EKF type that dramatically speeds up the test7079"EK2_ENABLE": 0,7080"EK3_ENABLE": 0,7081"INS_LOG_BAT_MASK": 3,7082"INS_LOG_BAT_OPT": 4,7083"INS_GYRO_FILTER": 100,7084"INS_FAST_SAMPLE": 3,7085"LOG_BITMASK": 958,7086"LOG_DISARMED": 0,7087"SIM_DRIFT_SPEED": 0,7088"SIM_DRIFT_TIME": 0,7089"SIM_GYR1_RND": 200, # enable a noisy gyro7090"INS_HNTCH_ENABLE": 1,7091"INS_HNTCH_FREQ": 80,7092"INS_HNTCH_REF": 1.0,7093"INS_HNTCH_HMNCS": 1, # first harmonic7094"INS_HNTCH_ATT": 50,7095"INS_HNTCH_BW": 30,7096"INS_HNTCH_MODE": 3, # ESC telemetry7097"INS_HNTCH_OPTS": 2, # notch-per-motor7098"INS_HNTC2_ENABLE": 1,7099"INS_HNTC2_FREQ": 80,7100"INS_HNTC2_REF": 1.0,7101"INS_HNTC2_HMNCS": 1,7102"INS_HNTC2_ATT": 50,7103"INS_HNTC2_BW": 40,7104"INS_HNTC2_MODE": 0, # istatic notch7105"INS_HNTC2_OPTS": 16, # triple-notch7106"FFT_ENABLE": 1,7107"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable7108"FFT_OPTIONS": 3,7109"FFT_MINHZ": 50,7110"FFT_MAXHZ": 450,7111"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 145Hz7112"SIM_VIB_FREQ_X": 250, # create another peak at 250hz7113"SIM_VIB_FREQ_Y": 250,7114"SIM_VIB_FREQ_Z": 250,7115"SIM_GYR_FILE_RW": 2, # write data to a file7116})7117self.reboot_sitl()71187119# do test flight:7120self.takeoff(10, mode="ALT_HOLD")7121tstart, tend, hover_throttle = self.hover_for_interval(10)7122self.wait_statustext("Noise ", timeout=20)7123self.set_parameter("SIM_GYR1_RND", 0) # stop noise so that we can get home7124self.do_RTL()71257126# prevent update parameters from messing with the settings when we pop the context7127self.set_parameters({7128"SIM_VIB_FREQ_X": 0,7129"SIM_VIB_FREQ_Y": 0,7130"SIM_VIB_FREQ_Z": 0,7131"SIM_VIB_MOT_MULT": 1.0,7132"SIM_GYR_FILE_RW": 0, # stop writing data7133"FFT_ENABLE": 0,7134})7135self.reboot_sitl()71367137except Exception as e:7138self.print_exception_caught(e)7139ex = e71407141self.context_pop()71427143# need a final reboot because weird things happen to your7144# vehicle state when switching back from EKF type 10!7145self.reboot_sitl()71467147if ex is not None:7148raise ex71497150def BrakeMode(self):7151'''Fly Brake Mode'''7152# test brake mode7153self.progress("Testing brake mode")7154self.takeoff(10, mode="LOITER")71557156self.progress("Ensuring RC inputs have no effect in brake mode")7157self.change_mode("STABILIZE")7158self.set_rc(3, 1500)7159self.set_rc(2, 1200)7160self.wait_groundspeed(5, 1000)71617162self.change_mode("BRAKE")7163self.wait_groundspeed(0, 1)71647165self.set_rc(2, 1500)71667167self.do_RTL()7168self.progress("Ran brake mode")71697170def fly_guided_move_to(self, destination, timeout=30):7171'''move to mavutil.location location; absolute altitude'''7172tstart = self.get_sim_time()7173self.mav.mav.set_position_target_global_int_send(71740, # timestamp71751, # target system_id71761, # target component id7177mavutil.mavlink.MAV_FRAME_GLOBAL_INT,7178MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-lat-lon-alt7179int(destination.lat * 1e7), # lat7180int(destination.lng * 1e7), # lon7181destination.alt, # alt71820, # vx71830, # vy71840, # vz71850, # afx71860, # afy71870, # afz71880, # yaw71890, # yawrate7190)7191while True:7192if self.get_sim_time() - tstart > timeout:7193raise NotAchievedException()7194delta = self.get_distance(self.mav.location(), destination)7195self.progress("delta=%f (want <1)" % delta)7196if delta < 1:7197break71987199def AltTypes(self):7200'''Test Different Altitude Types'''7201'''start by disabling GCS failsafe, otherwise we immediately disarm7202due to (apparently) not receiving traffic from the GCS for7203too long. This is probably a function of --speedup'''72047205'''this test flies the vehicle somewhere lower than were it started.7206It then disarms. It then arms, which should reset home to the7207new, lower altitude. This delta should be outside 1m but7208within a few metres of the old one.72097210'''72117212self.install_terrain_handlers_context()72137214self.set_parameter("FS_GCS_ENABLE", 0)7215self.change_mode('GUIDED')7216self.wait_ready_to_arm()7217self.arm_vehicle()7218m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)7219max_initial_home_alt_m = 5007220if m.relative_alt > max_initial_home_alt_m:7221raise NotAchievedException("Initial home alt too high (%fm > %fm)" %7222(m.relative_alt*1000, max_initial_home_alt_m*1000))7223orig_home_offset_mm = m.alt - m.relative_alt7224self.user_takeoff(5)72257226self.progress("Flying to low position")7227current_alt = self.mav.location().alt7228# 10m delta low_position = mavutil.location(-35.358273, 149.169165, current_alt, 0)7229low_position = mavutil.location(-35.36200016, 149.16415599, current_alt, 0)7230self.fly_guided_move_to(low_position, timeout=240)7231self.change_mode('LAND')7232# expecting home to change when disarmed7233self.wait_landed_and_disarmed()7234# wait a while for home to move (it shouldn't):7235self.delay_sim_time(10)7236m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)7237new_home_offset_mm = m.alt - m.relative_alt7238home_offset_delta_mm = orig_home_offset_mm - new_home_offset_mm7239self.progress("new home offset: %f delta=%f" %7240(new_home_offset_mm, home_offset_delta_mm))7241self.progress("gpi=%s" % str(m))7242max_home_offset_delta_mm = 107243if home_offset_delta_mm > max_home_offset_delta_mm:7244raise NotAchievedException("Large home offset delta: want<%f got=%f" %7245(max_home_offset_delta_mm, home_offset_delta_mm))7246self.progress("Ensuring home moves when we arm")7247self.change_mode('GUIDED')7248self.wait_ready_to_arm()7249self.arm_vehicle()7250m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)7251post_arming_home_offset_mm = m.alt - m.relative_alt7252self.progress("post-arming home offset: %f" % (post_arming_home_offset_mm))7253self.progress("gpi=%s" % str(m))7254min_post_arming_home_offset_delta_mm = -25007255max_post_arming_home_offset_delta_mm = -40007256delta_between_original_home_alt_offset_and_new_home_alt_offset_mm = post_arming_home_offset_mm - orig_home_offset_mm7257self.progress("delta=%f-%f=%f" % (7258post_arming_home_offset_mm,7259orig_home_offset_mm,7260delta_between_original_home_alt_offset_and_new_home_alt_offset_mm))7261self.progress("Home moved %fm vertically" % (delta_between_original_home_alt_offset_and_new_home_alt_offset_mm/1000.0))7262if delta_between_original_home_alt_offset_and_new_home_alt_offset_mm > min_post_arming_home_offset_delta_mm:7263raise NotAchievedException(7264"Home did not move vertically on arming: want<=%f got=%f" %7265(min_post_arming_home_offset_delta_mm, delta_between_original_home_alt_offset_and_new_home_alt_offset_mm))7266if delta_between_original_home_alt_offset_and_new_home_alt_offset_mm < max_post_arming_home_offset_delta_mm:7267raise NotAchievedException(7268"Home moved too far vertically on arming: want>=%f got=%f" %7269(max_post_arming_home_offset_delta_mm, delta_between_original_home_alt_offset_and_new_home_alt_offset_mm))72707271self.wait_disarmed()72727273def PrecisionLoiterCompanion(self):7274"""Use Companion PrecLand backend precision messages to loiter."""72757276self.set_parameters({7277"PLND_ENABLED": 1,7278"PLND_TYPE": 1, # enable companion backend:7279"RC7_OPTION": 39, # set up a channel switch to enable precision loiter:7280})7281self.set_analog_rangefinder_parameters()7282self.reboot_sitl()72837284self.progress("Waiting for location")7285self.change_mode('LOITER')7286self.wait_ready_to_arm()72877288# we should be doing precision loiter at this point7289start = self.assert_receive_message('LOCAL_POSITION_NED')72907291self.takeoff(20, mode='ALT_HOLD')72927293# move away a little7294self.set_rc(2, 1550)7295self.wait_distance(5, accuracy=1)7296self.set_rc(2, 1500)7297self.change_mode('LOITER')72987299# turn precision loiter on:7300self.context_collect('STATUSTEXT')7301self.set_rc(7, 2000)73027303# try to drag aircraft to a position 5 metres north-east-east:7304self.precision_loiter_to_pos(start.x + 5, start.y + 10, start.z + 10)7305self.wait_statustext("PrecLand: Target Found", check_context=True, timeout=10)7306self.wait_statustext("PrecLand: Init Complete", check_context=True, timeout=10)7307# .... then northwest7308self.precision_loiter_to_pos(start.x + 5, start.y - 10, start.z + 10)73097310self.disarm_vehicle(force=True)73117312def loiter_requires_position(self):7313# ensure we can't switch to LOITER without position7314self.progress("Ensure we can't enter LOITER without position")7315self.context_push()7316self.set_parameters({7317"GPS1_TYPE": 2,7318"SIM_GPS1_ENABLE": 0,7319})7320# if there is no GPS at all then we must direct EK3 to not use7321# it at all. Otherwise it will never initialise, as it wants7322# to calculate the lag and size its delay buffers accordingly.7323self.set_parameters({7324"EK3_SRC1_POSXY": 0,7325"EK3_SRC1_VELZ": 0,7326"EK3_SRC1_VELXY": 0,7327})7328self.reboot_sitl()7329self.delay_sim_time(30) # wait for accels/gyros to settle73307331# check for expected EKF flags7332ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")7333expected_ekf_flags = (mavutil.mavlink.ESTIMATOR_ATTITUDE |7334mavutil.mavlink.ESTIMATOR_VELOCITY_VERT |7335mavutil.mavlink.ESTIMATOR_POS_VERT_ABS |7336mavutil.mavlink.ESTIMATOR_CONST_POS_MODE)7337if ahrs_ekf_type == 2:7338expected_ekf_flags = expected_ekf_flags | mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_REL7339self.wait_ekf_flags(expected_ekf_flags, 0, timeout=120)73407341# arm in Stabilize and attempt to switch to Loiter7342self.change_mode('STABILIZE')7343self.arm_vehicle()7344self.context_collect('STATUSTEXT')7345self.run_cmd_do_set_mode(7346"LOITER",7347want_result=mavutil.mavlink.MAV_RESULT_FAILED)7348self.wait_statustext("requires position", check_context=True)7349self.disarm_vehicle()7350self.context_pop()7351self.reboot_sitl()73527353def ArmFeatures(self):7354'''Arm features'''7355self.loiter_requires_position()73567357super(AutoTestCopter, self).ArmFeatures()73587359def ParameterChecks(self):7360'''Test Arming Parameter Checks'''7361self.test_parameter_checks_poscontrol("PSC")73627363def PosHoldTakeOff(self):7364"""ensure vehicle stays put until it is ready to fly"""7365self.context_push()73667367self.set_parameter("PILOT_TKOFF_ALT", 700)7368self.change_mode('POSHOLD')7369self.set_rc(3, 1000)7370self.wait_ready_to_arm()7371self.arm_vehicle()7372self.delay_sim_time(2)7373# check we are still on the ground...7374relative_alt = self.get_altitude(relative=True)7375if relative_alt > 0.1:7376raise NotAchievedException("Took off prematurely")73777378self.progress("Pushing throttle up")7379self.set_rc(3, 1710)7380self.delay_sim_time(0.5)7381self.progress("Bringing back to hover throttle")7382self.set_rc(3, 1500)73837384# make sure we haven't already reached alt:7385relative_alt = self.get_altitude(relative=True)7386max_initial_alt = 2.07387if abs(relative_alt) > max_initial_alt:7388raise NotAchievedException("Took off too fast (%f > %f" %7389(relative_alt, max_initial_alt))73907391self.progress("Monitoring takeoff-to-alt")7392self.wait_altitude(6.9, 8, relative=True, minimum_duration=10)7393self.progress("takeoff OK")73947395self.land_and_disarm()7396self.set_rc(8, 1000)73977398self.context_pop()73997400def initial_mode(self):7401return "STABILIZE"74027403def initial_mode_switch_mode(self):7404return "STABILIZE"74057406def default_mode(self):7407return "STABILIZE"74087409def rc_defaults(self):7410ret = super(AutoTestCopter, self).rc_defaults()7411ret[3] = 10007412ret[5] = 1800 # mode switch7413return ret74147415def MANUAL_CONTROL(self):7416'''test MANUAL_CONTROL mavlink message'''7417self.set_parameter("SYSID_MYGCS", self.mav.source_system)74187419self.change_mode('STABILIZE')7420self.takeoff(10)74217422tstart = self.get_sim_time_cached()7423want_pitch_degrees = -127424while True:7425if self.get_sim_time_cached() - tstart > 10:7426raise AutoTestTimeoutException("Did not reach pitch")7427self.progress("Sending pitch-forward")7428self.mav.mav.manual_control_send(74291, # target system7430500, # x (pitch)743132767, # y (roll)743232767, # z (thrust)743332767, # r (yaw)74340) # button mask7435m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=1)7436print("m=%s" % str(m))7437if m is None:7438continue7439p = math.degrees(m.pitch)7440self.progress("pitch=%f want<=%f" % (p, want_pitch_degrees))7441if p <= want_pitch_degrees:7442break7443self.mav.mav.manual_control_send(74441, # target system744532767, # x (pitch)744632767, # y (roll)744732767, # z (thrust)744832767, # r (yaw)74490) # button mask7450self.do_RTL()74517452def check_avoidance_corners(self):7453self.takeoff(10, mode="LOITER")7454here = self.mav.location()7455self.set_rc(2, 1400)7456west_loc = mavutil.location(-35.363007,7457149.164911,7458here.alt,74590)7460self.wait_location(west_loc, accuracy=6)7461north_loc = mavutil.location(-35.362908,7462149.165051,7463here.alt,74640)7465self.reach_heading_manual(0)7466self.wait_location(north_loc, accuracy=6, timeout=200)7467self.reach_heading_manual(90)7468east_loc = mavutil.location(-35.363013,7469149.165194,7470here.alt,74710)7472self.wait_location(east_loc, accuracy=6)7473self.reach_heading_manual(225)7474self.wait_location(west_loc, accuracy=6, timeout=200)7475self.set_rc(2, 1500)7476self.do_RTL()74777478def OBSTACLE_DISTANCE_3D_test_angle(self, angle):7479now = self.get_sim_time_cached()74807481distance = 157482right = distance * math.sin(math.radians(angle))7483front = distance * math.cos(math.radians(angle))7484down = 074857486expected_distance_cm = distance * 1007487# expected orientation7488expected_orientation = int((angle+22.5)/45) % 87489self.progress("Angle %f expected orient %u" %7490(angle, expected_orientation))74917492tstart = self.get_sim_time()7493last_send = 07494m = None7495while True:7496now = self.get_sim_time_cached()7497if now - tstart > 100:7498raise NotAchievedException("Did not get correct angle back (last-message=%s)" % str(m))74997500if now - last_send > 0.1:7501self.progress("ang=%f sending front=%f right=%f" %7502(angle, front, right))7503self.mav.mav.obstacle_distance_3d_send(7504int(now*1000), # time_boot_ms7505mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER,7506mavutil.mavlink.MAV_FRAME_BODY_FRD,750765535,7508front, # x (m)7509right, # y (m)7510down, # z (m)75110, # min_distance (m)751220 # max_distance (m)7513)7514last_send = now7515m = self.mav.recv_match(type="DISTANCE_SENSOR",7516blocking=True,7517timeout=1)7518if m is None:7519continue7520# self.progress("Got (%s)" % str(m))7521if m.orientation != expected_orientation:7522# self.progress("Wrong orientation (want=%u got=%u)" %7523# (expected_orientation, m.orientation))7524continue7525if abs(m.current_distance - expected_distance_cm) > 1:7526# self.progress("Wrong distance (want=%f got=%f)" %7527# (expected_distance_cm, m.current_distance))7528continue7529self.progress("distance-at-angle good")7530break75317532def OBSTACLE_DISTANCE_3D(self):7533'''Check round-trip behaviour of distance sensors'''7534self.context_push()7535self.set_parameters({7536"SERIAL5_PROTOCOL": 1,7537"PRX1_TYPE": 2,7538"SIM_SPEEDUP": 8, # much GCS interaction7539})7540self.reboot_sitl()7541# need yaw estimate to stabilise:7542self.wait_ekf_happy(require_absolute=True)75437544for angle in range(0, 360):7545self.OBSTACLE_DISTANCE_3D_test_angle(angle)75467547self.context_pop()7548self.reboot_sitl()75497550def AC_Avoidance_Proximity(self):7551'''Test proximity avoidance slide behaviour'''75527553self.context_push()75547555self.load_fence("copter-avoidance-fence.txt")7556self.set_parameters({7557"FENCE_ENABLE": 1,7558"PRX1_TYPE": 10,7559"PRX_LOG_RAW": 1,7560"RC10_OPTION": 40, # proximity-enable7561})7562self.reboot_sitl()7563self.progress("Enabling proximity")7564self.set_rc(10, 2000)7565self.check_avoidance_corners()75667567self.assert_current_onboard_log_contains_message("PRX")7568self.assert_current_onboard_log_contains_message("PRXR")75697570self.disarm_vehicle(force=True)75717572self.context_pop()7573self.reboot_sitl()75747575def ProximitySensors(self):7576'''ensure proximity sensors return appropriate data'''75777578self.set_parameters({7579"SERIAL5_PROTOCOL": 11,7580"OA_DB_OUTPUT": 3,7581"OA_TYPE": 2,7582})7583sensors = [ # tuples of name, prx_type7584('ld06', 16, {7585mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 273,7586mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,7587mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,7588mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 696,7589mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 625,7590mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 967,7591mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760,7592mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 771,7593}),7594('sf45b', 8, {7595mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 270,7596mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 258,7597mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1146,7598mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 632,7599mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 629,7600mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 972,7601mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 774,7602mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 774,7603}),7604('rplidara2', 5, {7605mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 277,7606mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,7607mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,7608mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1288,7609mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,7610mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 970,7611mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,7612mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 790,7613}),7614('terarangertower', 3, {7615mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 450,7616mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 282,7617mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 450,7618mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 450,7619mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 450,7620mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 450,7621mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 450,7622mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 450,7623}),7624]76257626# the following is a "magic" location SITL understands which7627# has some posts near it:7628home_string = "%s,%s,%s,%s" % (51.8752066, 14.6487840, 54.15, 0)7629for (name, prx_type, expected_distances) in sensors:7630self.start_subtest("Testing %s" % name)7631self.set_parameter("PRX1_TYPE", prx_type)7632self.customise_SITL_commandline([7633"--serial5=sim:%s:" % name,7634"--home", home_string,7635])7636self.wait_ready_to_arm()7637expected_distances_copy = copy.copy(expected_distances)7638tstart = self.get_sim_time()7639failed = False7640wants = []7641gots = []7642epsilon = 207643while True:7644if self.get_sim_time_cached() - tstart > 30:7645raise AutoTestTimeoutException("Failed to get distances")7646if len(expected_distances_copy.keys()) == 0:7647break7648m = self.assert_receive_message("DISTANCE_SENSOR")7649if m.orientation not in expected_distances_copy:7650continue7651got = m.current_distance7652want = expected_distances_copy[m.orientation]7653wants.append(want)7654gots.append(got)7655if abs(want - got) > epsilon:7656failed = True7657del expected_distances_copy[m.orientation]7658if failed:7659raise NotAchievedException(7660"Distance too great (%s) (want=%s != got=%s)" %7661(name, wants, gots))76627663def AC_Avoidance_Proximity_AVOID_ALT_MIN(self):7664'''Test proximity avoidance with AVOID_ALT_MIN'''7665self.context_push()7666ex = None7667try:7668self.set_parameters({7669"PRX1_TYPE": 2,7670"AVOID_ALT_MIN": 10,7671})7672self.set_analog_rangefinder_parameters()7673self.reboot_sitl()76747675self.change_mode('LOITER')7676self.wait_ekf_happy()76777678tstart = self.get_sim_time()7679while True:7680if self.armed():7681break7682if self.get_sim_time_cached() - tstart > 60:7683raise AutoTestTimeoutException("Did not arm")7684self.mav.mav.distance_sensor_send(76850, # time_boot_ms768610, # min_distance cm7687500, # max_distance cm7688400, # current_distance cm7689mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type769026, # id7691mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation7692255 # covariance7693)7694self.send_mavlink_arm_command()76957696self.takeoff(15, mode='LOITER')7697self.progress("Poking vehicle; should avoid")76987699def shove(a, b):7700self.mav.mav.distance_sensor_send(77010, # time_boot_ms770210, # min_distance cm7703500, # max_distance cm770420, # current_distance cm7705mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type770621, # id7707mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation7708255 # covariance7709)7710self.wait_speed_vector_bf(7711Vector3(-0.4, 0.0, 0.0),7712timeout=10,7713called_function=shove,7714)77157716self.change_alt(5)77177718tstart = self.get_sim_time()7719while True:7720if self.get_sim_time_cached() - tstart > 10:7721break7722vel = self.get_body_frame_velocity()7723if vel.length() > 0.5:7724raise NotAchievedException("Moved too much (%s)" %7725(str(vel),))7726shove(None, None)77277728except Exception as e:7729self.progress("Caught exception: %s" %7730self.get_exception_stacktrace(e))7731ex = e7732self.disarm_vehicle(force=True)7733self.context_pop()7734self.reboot_sitl()7735if ex is not None:7736raise ex77377738def AC_Avoidance_Fence(self):7739'''Test fence avoidance slide behaviour'''7740self.load_fence("copter-avoidance-fence.txt")7741self.set_parameter("FENCE_ENABLE", 1)7742self.check_avoidance_corners()77437744def AvoidanceAltFence(self):7745'''Test fence avoidance at minimum and maximum altitude'''7746ex = None7747try:7748self.set_parameters({7749"FENCE_ENABLE": 1,7750"FENCE_TYPE": 9, # min and max alt fence7751"FENCE_ALT_MIN": 10,7752"FENCE_ALT_MAX": 30,7753})77547755self.change_mode('LOITER')7756self.wait_ekf_happy()77577758tstart = self.get_sim_time()7759self.takeoff(15, mode='LOITER')7760self.progress("Increasing throttle, vehicle should stay below 30m")7761self.set_rc(3, 1920)77627763tstart = self.get_sim_time()7764while True:7765if self.get_sim_time_cached() - tstart > 20:7766break7767alt = self.get_altitude(relative=True)7768self.progress("Altitude %s" % alt)7769if alt > 30:7770raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),))77717772self.progress("Decreasing, vehicle should stay above 10m")7773self.set_rc(3, 1080)7774tstart = self.get_sim_time()7775while True:7776if self.get_sim_time_cached() - tstart > 20:7777break7778alt = self.get_altitude(relative=True)7779self.progress("Altitude %s" % alt)7780if alt < 10:7781raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),))77827783except Exception as e:7784self.progress("Caught exception: %s" %7785self.get_exception_stacktrace(e))7786ex = e7787self.land_and_disarm()7788self.disarm_vehicle(force=True)7789if ex is not None:7790raise ex77917792def ModeFollow(self):7793'''Fly follow mode'''7794foll_ofs_x = 30 # metres7795self.set_parameters({7796"FOLL_ENABLE": 1,7797"FOLL_SYSID": self.mav.source_system,7798"FOLL_OFS_X": -foll_ofs_x,7799"FOLL_OFS_TYPE": 1, # relative to other vehicle heading7800})7801self.takeoff(10, mode="LOITER")7802self.context_push()7803self.set_parameter("SIM_SPEEDUP", 1)7804self.change_mode("FOLLOW")7805new_loc = self.mav.location()7806new_loc_offset_n = 207807new_loc_offset_e = 307808self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e)7809self.progress("new_loc: %s" % str(new_loc))7810heading = 07811if self.mavproxy is not None:7812self.mavproxy.send("map icon %f %f greenplane %f\n" %7813(new_loc.lat, new_loc.lng, heading))78147815expected_loc = copy.copy(new_loc)7816self.location_offset_ne(expected_loc, -foll_ofs_x, 0)7817if self.mavproxy is not None:7818self.mavproxy.send("map icon %f %f hoop\n" %7819(expected_loc.lat, expected_loc.lng))7820self.progress("expected_loc: %s" % str(expected_loc))78217822origin = self.poll_message('GPS_GLOBAL_ORIGIN')78237824last_sent = 07825tstart = self.get_sim_time()7826while True:7827now = self.get_sim_time_cached()7828if now - tstart > 60:7829raise NotAchievedException("Did not FOLLOW")7830if now - last_sent > 0.5:7831gpi = self.mav.mav.global_position_int_encode(7832int(now * 1000), # time_boot_ms7833int(new_loc.lat * 1e7),7834int(new_loc.lng * 1e7),7835int(new_loc.alt * 1000), # alt in mm7836int(new_loc.alt * 1000 - origin.altitude), # relative alt - urp.7837vx=0,7838vy=0,7839vz=0,7840hdg=heading7841)7842gpi.pack(self.mav.mav)7843self.mav.mav.send(gpi)7844self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)7845pos = self.mav.location()7846delta = self.get_distance(expected_loc, pos)7847max_delta = 37848self.progress("position delta=%f (want <%f)" % (delta, max_delta))7849if delta < max_delta:7850break7851self.context_pop()7852self.do_RTL()78537854def get_global_position_int(self, timeout=30):7855tstart = self.get_sim_time()7856while True:7857if self.get_sim_time_cached() - tstart > timeout:7858raise NotAchievedException("Did not get good global_position_int")7859m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)7860self.progress("GPI: %s" % str(m))7861if m is None:7862continue7863if m.lat != 0 or m.lon != 0:7864return m78657866def BeaconPosition(self):7867'''Fly Beacon Position'''7868self.reboot_sitl()78697870self.wait_ready_to_arm(require_absolute=True)78717872old_pos = self.get_global_position_int()7873print("old_pos=%s" % str(old_pos))78747875self.set_parameters({7876"BCN_TYPE": 10,7877"BCN_LATITUDE": SITL_START_LOCATION.lat,7878"BCN_LONGITUDE": SITL_START_LOCATION.lng,7879"BCN_ALT": SITL_START_LOCATION.alt,7880"BCN_ORIENT_YAW": 0,7881"AVOID_ENABLE": 4,7882"GPS1_TYPE": 0,7883"EK3_ENABLE": 1,7884"EK3_SRC1_POSXY": 4, # Beacon7885"EK3_SRC1_POSZ": 1, # Baro7886"EK3_SRC1_VELXY": 0, # None7887"EK3_SRC1_VELZ": 0, # None7888"EK2_ENABLE": 0,7889"AHRS_EKF_TYPE": 3,7890})7891self.reboot_sitl()78927893# turn off GPS arming checks. This may be considered a7894# bug that we need to do this.7895old_arming_check = int(self.get_parameter("ARMING_CHECK"))7896if old_arming_check == 1:7897old_arming_check = 1 ^ 25 - 17898new_arming_check = int(old_arming_check) & ~(1 << 3)7899self.set_parameter("ARMING_CHECK", new_arming_check)79007901self.reboot_sitl()79027903# require_absolute=True infers a GPS is present7904self.wait_ready_to_arm(require_absolute=False)79057906tstart = self.get_sim_time()7907timeout = 207908while True:7909if self.get_sim_time_cached() - tstart > timeout:7910raise NotAchievedException("Did not get new position like old position")7911self.progress("Fetching location")7912new_pos = self.get_global_position_int()7913pos_delta = self.get_distance_int(old_pos, new_pos)7914max_delta = 17915self.progress("delta=%u want <= %u" % (pos_delta, max_delta))7916if pos_delta <= max_delta:7917break79187919self.progress("Moving to ensure location is tracked")7920self.takeoff(10, mode="STABILIZE")7921self.change_mode("CIRCLE")79227923self.context_push()7924validator = vehicle_test_suite.TestSuite.ValidateGlobalPositionIntAgainstSimState(self, max_allowed_divergence=10)7925self.install_message_hook_context(validator)79267927self.delay_sim_time(20)7928self.progress("Tracked location just fine")7929self.context_pop()79307931self.change_mode("LOITER")7932self.wait_groundspeed(0, 0.3, timeout=120)7933self.land_and_disarm()79347935self.assert_current_onboard_log_contains_message("BCN")79367937self.disarm_vehicle(force=True)79387939def AC_Avoidance_Beacon(self):7940'''Test beacon avoidance slide behaviour'''7941self.context_push()7942ex = None7943try:7944self.set_parameters({7945"BCN_TYPE": 10,7946"BCN_LATITUDE": int(SITL_START_LOCATION.lat),7947"BCN_LONGITUDE": int(SITL_START_LOCATION.lng),7948"BCN_ORIENT_YAW": 45,7949"AVOID_ENABLE": 4,7950})7951self.reboot_sitl()79527953self.takeoff(10, mode="LOITER")7954self.set_rc(2, 1400)7955here = self.mav.location()7956west_loc = mavutil.location(-35.362919, 149.165055, here.alt, 0)7957self.wait_location(west_loc, accuracy=1)7958self.reach_heading_manual(0)7959north_loc = mavutil.location(-35.362881, 149.165103, here.alt, 0)7960self.wait_location(north_loc, accuracy=1)7961self.set_rc(2, 1500)7962self.set_rc(1, 1600)7963east_loc = mavutil.location(-35.362986, 149.165227, here.alt, 0)7964self.wait_location(east_loc, accuracy=1)7965self.set_rc(1, 1500)7966self.set_rc(2, 1600)7967south_loc = mavutil.location(-35.363025, 149.165182, here.alt, 0)7968self.wait_location(south_loc, accuracy=1)7969self.set_rc(2, 1500)7970self.do_RTL()79717972except Exception as e:7973self.print_exception_caught(e)7974ex = e7975self.context_pop()7976self.clear_fence()7977self.disarm_vehicle(force=True)7978self.reboot_sitl()7979if ex is not None:7980raise ex79817982def BaroWindCorrection(self):7983'''Test wind estimation and baro position error compensation'''7984self.context_push()7985ex = None7986try:7987self.customise_SITL_commandline(7988[],7989defaults_filepath=self.model_defaults_filepath('Callisto'),7990model="octa-quad:@ROMFS/models/Callisto.json",7991wipe=True,7992)7993wind_spd_truth = 8.07994wind_dir_truth = 90.07995self.set_parameters({7996"EK3_ENABLE": 1,7997"EK2_ENABLE": 0,7998"AHRS_EKF_TYPE": 3,7999"BARO1_WCF_ENABLE": 1.000000,8000})8001self.reboot_sitl()8002self.set_parameters({8003"BARO1_WCF_FWD": -0.300000,8004"BARO1_WCF_BCK": -0.300000,8005"BARO1_WCF_RGT": 0.300000,8006"BARO1_WCF_LFT": 0.300000,8007"BARO1_WCF_UP": 0.300000,8008"BARO1_WCF_DN": 0.300000,8009"SIM_BARO_WCF_FWD": -0.300000,8010"SIM_BARO_WCF_BAK": -0.300000,8011"SIM_BARO_WCF_RGT": 0.300000,8012"SIM_BARO_WCF_LFT": 0.300000,8013"SIM_BARO_WCF_UP": 0.300000,8014"SIM_BARO_WCF_DN": 0.300000,8015"SIM_WIND_DIR": wind_dir_truth,8016"SIM_WIND_SPD": wind_spd_truth,8017"SIM_WIND_T": 1.000000,8018})8019self.reboot_sitl()80208021# require_absolute=True infers a GPS is present8022self.wait_ready_to_arm(require_absolute=False)80238024self.progress("Climb to 20m in LOITER and yaw spin for 30 seconds")8025self.takeoff(10, mode="LOITER")8026self.set_rc(4, 1400)8027self.delay_sim_time(30)80288029# check wind esitmates8030m = self.mav.recv_match(type='WIND', blocking=True)8031speed_error = abs(m.speed - wind_spd_truth)8032angle_error = abs(m.direction - wind_dir_truth)8033if (speed_error > 1.0):8034raise NotAchievedException("Wind speed incorrect - want %f +-1 got %f m/s" % (wind_spd_truth, m.speed))8035if (angle_error > 15.0):8036raise NotAchievedException(8037"Wind direction incorrect - want %f +-15 got %f deg" %8038(wind_dir_truth, m.direction))8039self.progress("Wind estimate is good, now check height variation for 30 seconds")80408041# check height stability over another 30 seconds8042z_min = 1E68043z_max = -1E68044tstart = self.get_sim_time()8045while (self.get_sim_time() < tstart + 30):8046m = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)8047if (m.z > z_max):8048z_max = m.z8049if (m.z < z_min):8050z_min = m.z8051if (z_max-z_min > 0.5):8052raise NotAchievedException("Height variation is excessive")8053self.progress("Height variation is good")80548055self.set_rc(4, 1500)8056self.land_and_disarm()80578058except Exception as e:8059self.print_exception_caught(e)8060ex = e8061self.disarm_vehicle(force=True)8062self.reboot_sitl()8063self.context_pop()8064self.reboot_sitl()8065if ex is not None:8066raise ex80678068def wait_generator_speed_and_state(self, rpm_min, rpm_max, want_state, timeout=240):8069tstart = self.get_sim_time()8070while True:8071if self.get_sim_time_cached() - tstart > timeout:8072raise NotAchievedException("Did not move to state/speed")80738074m = self.assert_receive_message("GENERATOR_STATUS", timeout=10)80758076if m.generator_speed < rpm_min:8077self.progress("Too slow (%u<%u)" % (m.generator_speed, rpm_min))8078continue8079if m.generator_speed > rpm_max:8080self.progress("Too fast (%u>%u)" % (m.generator_speed, rpm_max))8081continue8082if m.status != want_state:8083self.progress("Wrong state (got=%u want=%u)" % (m.status, want_state))8084break8085self.progress("Got generator speed and state")80868087def RichenPower(self):8088'''Test RichenPower generator'''8089self.set_parameters({8090"SERIAL5_PROTOCOL": 30,8091"SIM_RICH_ENABLE": 1,8092"SERVO8_FUNCTION": 42,8093"SIM_RICH_CTRL": 8,8094"RC9_OPTION": 85,8095"LOG_DISARMED": 1,8096"BATT2_MONITOR": 17,8097"GEN_TYPE": 3,8098})8099self.reboot_sitl()8100self.set_rc(9, 1000) # remember this is a switch position - stop8101self.customise_SITL_commandline(["--serial5=sim:richenpower"])8102self.wait_statustext("requested state is not RUN", timeout=60)81038104self.set_message_rate_hz("GENERATOR_STATUS", 10)81058106self.wait_generator_speed_and_state(0, 0, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_OFF)81078108self.context_collect('STATUSTEXT')8109self.set_rc(9, 2000) # remember this is a switch position - run8110self.wait_statustext("Generator HIGH", check_context=True)8111self.set_rc(9, 1000) # remember this is a switch position - stop8112self.wait_statustext("requested state is not RUN", timeout=200)81138114self.set_rc(9, 1500) # remember this is a switch position - idle8115self.wait_generator_speed_and_state(3000, 8000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_IDLE)81168117self.set_rc(9, 2000) # remember this is a switch position - run8118# self.wait_generator_speed_and_state(3000, 30000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_WARMING_UP)81198120self.wait_generator_speed_and_state(8000, 30000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_GENERATING)81218122bs = self.mav.recv_match(8123type="BATTERY_STATUS",8124condition="BATTERY_STATUS.id==1", # id is zero-indexed8125timeout=1,8126blocking=True8127)8128if bs is None:8129raise NotAchievedException("Did not receive BATTERY_STATUS")8130self.progress("Received battery status: %s" % str(bs))8131want_bs_volt = 500008132if bs.voltages[0] != want_bs_volt:8133raise NotAchievedException("Battery voltage not as expected (want=%f) got=(%f)" % (want_bs_volt, bs.voltages[0],))81348135self.progress("Moving *back* to idle")8136self.set_rc(9, 1500) # remember this is a switch position - idle8137self.wait_generator_speed_and_state(3000, 10000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_IDLE)81388139self.progress("Moving *back* to run")8140self.set_rc(9, 2000) # remember this is a switch position - run8141self.wait_generator_speed_and_state(8000, 30000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_GENERATING)81428143self.set_message_rate_hz("GENERATOR_STATUS", -1)8144self.set_parameter("LOG_DISARMED", 0)8145if not self.current_onboard_log_contains_message("GEN"):8146raise NotAchievedException("Did not find expected GEN message")81478148def IE24(self):8149'''Test IntelligentEnergy 2.4kWh generator with V1 and V2 telemetry protocols'''8150protocol_ver = (1, 2)8151for ver in protocol_ver:8152self.run_IE24(ver)81538154def run_IE24(self, proto_ver):8155'''Test IntelligentEnergy 2.4kWh generator'''8156elec_battery_instance = 28157fuel_battery_instance = 18158self.set_parameters({8159"SERIAL5_PROTOCOL": 30,8160"SERIAL5_BAUD": 115200,8161"GEN_TYPE": 2,8162"BATT%u_MONITOR" % (fuel_battery_instance + 1): 18, # fuel-based generator8163"BATT%u_MONITOR" % (elec_battery_instance + 1): 17,8164"SIM_IE24_ENABLE": proto_ver,8165"LOG_DISARMED": 1,8166})81678168self.customise_SITL_commandline(["--serial5=sim:ie24"])81698170self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for electrical generator message looks right" % proto_ver)8171self.start_subsubtest("Protocol %i: Checking original voltage (electrical)" % proto_ver)8172# ArduPilot spits out essentially uninitialised battery8173# messages until we read things fromthe battery:8174self.delay_sim_time(30)8175original_elec_m = self.wait_message_field_values('BATTERY_STATUS', {8176"charge_state": mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_OK8177}, instance=elec_battery_instance)8178original_fuel_m = self.wait_message_field_values('BATTERY_STATUS', {8179"charge_state": mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_OK8180}, instance=fuel_battery_instance)81818182if original_elec_m.battery_remaining < 90:8183raise NotAchievedException("Bad original percentage")8184self.start_subsubtest("Ensure percentage is counting down")8185self.wait_message_field_values('BATTERY_STATUS', {8186"battery_remaining": original_elec_m.battery_remaining - 1,8187}, instance=elec_battery_instance)81888189self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for fuel generator message looks right" % proto_ver)8190self.start_subsubtest("Protocol %i: Checking original voltage (fuel)" % proto_ver)8191# ArduPilot spits out essentially uninitialised battery8192# messages until we read things fromthe battery:8193if original_fuel_m.battery_remaining <= 90:8194raise NotAchievedException("Bad original percentage (want=>%f got %f" % (90, original_fuel_m.battery_remaining))8195self.start_subsubtest("Protocol %i: Ensure percentage is counting down" % proto_ver)8196self.wait_message_field_values('BATTERY_STATUS', {8197"battery_remaining": original_fuel_m.battery_remaining - 1,8198}, instance=fuel_battery_instance)81998200self.wait_ready_to_arm()8201self.arm_vehicle()8202self.disarm_vehicle()82038204# Test for pre-arm check fail when state is not running8205self.start_subtest("Protocol %i: Without takeoff generator error should cause failsafe and disarm" % proto_ver)8206self.set_parameter("SIM_IE24_STATE", 8)8207self.wait_statustext("Status not running", timeout=40)8208self.try_arm(result=False,8209expect_msg="Status not running")8210self.set_parameter("SIM_IE24_STATE", 2) # Explicitly set state to running82118212# Test that error code does result in failsafe8213self.start_subtest("Protocol %i: Without taken off generator error should cause failsafe and disarm" % proto_ver)8214self.change_mode("STABILIZE")8215self.set_parameter("DISARM_DELAY", 0)8216self.arm_vehicle()8217self.set_parameter("SIM_IE24_ERROR", 30)8218self.disarm_wait(timeout=1)8219self.set_parameter("SIM_IE24_ERROR", 0)8220self.set_parameter("DISARM_DELAY", 10)82218222def AuxSwitchOptions(self):8223'''Test random aux mode options'''8224self.set_parameter("RC7_OPTION", 58) # clear waypoints8225self.load_mission("copter_loiter_to_alt.txt")8226self.set_rc(7, 1000)8227self.assert_mission_count(5)8228self.progress("Clear mission")8229self.set_rc(7, 2000)8230self.delay_sim_time(1) # allow switch to debounce8231self.assert_mission_count(0)8232self.set_rc(7, 1000)8233self.set_parameter("RC7_OPTION", 24) # reset mission8234self.delay_sim_time(2)8235self.load_mission("copter_loiter_to_alt.txt")8236set_wp = 48237self.set_current_waypoint(set_wp)8238self.wait_current_waypoint(set_wp, timeout=10)8239self.progress("Reset mission")8240self.set_rc(7, 2000)8241self.delay_sim_time(1)8242self.wait_current_waypoint(0, timeout=10)8243self.set_rc(7, 1000)82448245def AuxFunctionsInMission(self):8246'''Test use of auxilliary functions in missions'''8247self.load_mission("aux_functions.txt")8248self.change_mode('LOITER')8249self.wait_ready_to_arm()8250self.arm_vehicle()8251self.change_mode('AUTO')8252self.set_rc(3, 1500)8253self.wait_mode('ALT_HOLD')8254self.change_mode('AUTO')8255self.wait_rtl_complete()82568257def MAV_CMD_AIRFRAME_CONFIGURATION(self):8258'''deploy/retract landing gear using mavlink command'''8259self.context_push()8260self.set_parameters({8261"LGR_ENABLE": 1,8262"SERVO10_FUNCTION": 29,8263"SERVO10_MIN": 1001,8264"SERVO10_MAX": 1999,8265})8266self.reboot_sitl()82678268# starts loose:8269self.wait_servo_channel_value(10, 0)82708271# 0 is down:8272self.start_subtest("Put gear down")8273self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0)8274self.wait_servo_channel_value(10, 1999)82758276# 1 is up:8277self.start_subtest("Put gear up")8278self.run_cmd_int(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=1)8279self.wait_servo_channel_value(10, 1001)82808281# 0 is down:8282self.start_subtest("Put gear down")8283self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0)8284self.wait_servo_channel_value(10, 1999)82858286self.context_pop()8287self.reboot_sitl()82888289def WatchAlts(self):8290'''Ensure we can monitor different altitudes'''8291self.takeoff(30, mode='GUIDED')8292self.delay_sim_time(5, reason='let altitude settle')82938294self.progress("Testing absolute altitudes")8295absolute_alt = self.get_altitude(altitude_source='SIM_STATE.alt')8296self.progress("absolute_alt=%f" % absolute_alt)8297epsilon = 4 # SIM_STATE and vehicle state can be off by a bit...8298for source in ['GLOBAL_POSITION_INT.alt', 'SIM_STATE.alt', 'GPS_RAW_INT.alt']:8299self.watch_altitude_maintained(8300absolute_alt-epsilon,8301absolute_alt+epsilon,8302altitude_source=source8303)83048305self.progress("Testing absolute altitudes")8306relative_alt = self.get_altitude(relative=True)8307for source in ['GLOBAL_POSITION_INT.relative_alt']:8308self.watch_altitude_maintained(8309relative_alt-epsilon,8310relative_alt+epsilon,8311altitude_source=source8312)83138314self.do_RTL()83158316def TestTetherStuck(self):8317"""Test tethered vehicle stuck because of tether"""8318# Enable tether simulation8319self.set_parameters({8320"SIM_TETH_ENABLE": 1,8321})8322self.delay_sim_time(2)8323self.reboot_sitl()83248325# Set tether line length8326self.set_parameters({8327"SIM_TETH_LINELEN": 10,8328})8329self.delay_sim_time(2)83308331# Prepare and take off8332self.wait_ready_to_arm()8333self.arm_vehicle()8334self.takeoff(10, mode='LOITER')83358336# Simulate vehicle getting stuck by increasing RC throttle8337self.set_rc(3, 1900)8338self.delay_sim_time(5, reason='let tether get stuck')83398340# Monitor behavior for 10 seconds8341tstart = self.get_sim_time()8342initial_alt = self.get_altitude()8343stuck = True # Assume it's stuck unless proven otherwise83448345while self.get_sim_time() - tstart < 10:8346# Fetch current altitude8347current_alt = self.get_altitude()8348self.progress(f"current_alt={current_alt}")83498350# Fetch and log battery status8351battery_status = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)8352if battery_status:8353self.progress(f"Battery: {battery_status}")83548355# Check if the vehicle is stuck.8356# We assume the vehicle is stuck if the current is high and the altitude is not changing8357if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2):8358stuck = False # Vehicle moved or current is abnormal8359break83608361if not stuck:8362raise NotAchievedException("Vehicle did not get stuck as expected")83638364# Land and disarm the vehicle to ensure we can go down8365self.land_and_disarm()83668367def fly_rangefinder_drivers_fly(self, rangefinders):8368'''ensure rangefinder gives height-above-ground'''8369self.change_mode('GUIDED')8370self.wait_ready_to_arm()8371self.arm_vehicle()8372expected_alt = 58373self.user_takeoff(alt_min=expected_alt)8374rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True)8375if rf is None:8376raise NotAchievedException("Did not receive rangefinder message")8377gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)8378if gpi is None:8379raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message")8380if abs(rf.distance - gpi.relative_alt/1000.0) > 1:8381raise NotAchievedException(8382"rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" %8383(rf.distance, gpi.relative_alt/1000.0)8384)83858386for i in range(0, len(rangefinders)):8387name = rangefinders[i]8388self.progress("i=%u (%s)" % (i, name))8389ds = self.mav.recv_match(8390type="DISTANCE_SENSOR",8391timeout=2,8392blocking=True,8393condition="DISTANCE_SENSOR.id==%u" % i8394)8395if ds is None:8396raise NotAchievedException("Did not receive DISTANCE_SENSOR message for id==%u (%s)" % (i, name))8397self.progress("Got: %s" % str(ds))8398if abs(ds.current_distance/100.0 - gpi.relative_alt/1000.0) > 1:8399raise NotAchievedException(8400"distance sensor.current_distance (%f) (%s) disagrees with global-position-int.relative_alt (%s)" %8401(ds.current_distance/100.0, name, gpi.relative_alt/1000.0))84028403self.land_and_disarm()84048405self.progress("Ensure RFND messages in log")8406if not self.current_onboard_log_contains_message("RFND"):8407raise NotAchievedException("No RFND messages in log")84088409def MAVProximity(self):8410'''Test MAVLink proximity driver'''8411self.start_subtest("Test mavlink proximity sensor using DISTANCE_SENSOR messages") # noqa8412self.context_push()8413ex = None8414try:8415self.set_parameter("SERIAL5_PROTOCOL", 1)8416self.set_parameter("PRX1_TYPE", 2) # mavlink8417self.reboot_sitl()84188419self.progress("Should be unhealthy while we don't send messages")8420self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, False)84218422self.progress("Should be healthy while we're sending good messages")8423tstart = self.get_sim_time()8424while True:8425if self.get_sim_time() - tstart > 5:8426raise NotAchievedException("Sensor did not come good")8427self.mav.mav.distance_sensor_send(84280, # time_boot_ms842910, # min_distance cm843050, # max_distance cm843120, # current_distance cm8432mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type843321, # id8434mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation8435255 # covariance8436)8437if self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, True):8438self.progress("Sensor has good state")8439break8440self.delay_sim_time(0.1)84418442self.progress("Should be unhealthy again if we stop sending messages")8443self.delay_sim_time(1)8444self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, False)84458446# now make sure we get echoed back the same sorts of things we send:8447# distances are in cm8448distance_map = {8449mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 30,8450mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 35,8451mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 20,8452mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 15,8453mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 70,8454mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 80,8455mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 10,8456mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 90,8457}84588459wanted_distances = copy.copy(distance_map)8460sensor_enum = mavutil.mavlink.enums["MAV_SENSOR_ORIENTATION"]84618462def my_message_hook(mav, m):8463if m.get_type() != 'DISTANCE_SENSOR':8464return8465self.progress("Got (%s)" % str(m))8466want = distance_map[m.orientation]8467got = m.current_distance8468# ArduPilot's floating point conversions make it imprecise:8469delta = abs(want-got)8470if delta > 1:8471self.progress(8472"Wrong distance (%s): want=%f got=%f" %8473(sensor_enum[m.orientation].name, want, got))8474return8475if m.orientation not in wanted_distances:8476return8477self.progress(8478"Correct distance (%s): want=%f got=%f" %8479(sensor_enum[m.orientation].name, want, got))8480del wanted_distances[m.orientation]84818482self.install_message_hook_context(my_message_hook)8483tstart = self.get_sim_time()8484while True:8485if self.get_sim_time() - tstart > 5:8486raise NotAchievedException("Sensor did not give right distances") # noqa8487for (orient, dist) in distance_map.items():8488self.mav.mav.distance_sensor_send(84890, # time_boot_ms849010, # min_distance cm849190, # max_distance cm8492dist, # current_distance cm8493mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type849421, # id8495orient, # orientation8496255 # covariance8497)8498self.wait_heartbeat()8499if len(wanted_distances.keys()) == 0:8500break8501except Exception as e:8502self.print_exception_caught(e)8503ex = e8504self.context_pop()8505self.reboot_sitl()8506if ex is not None:8507raise ex85088509def fly_rangefinder_mavlink_distance_sensor(self):8510self.start_subtest("Test mavlink rangefinder using DISTANCE_SENSOR messages")8511self.context_push()8512self.set_parameters({8513"RTL_ALT_TYPE": 0,8514"LGR_ENABLE": 1,8515"LGR_DEPLOY_ALT": 1,8516"LGR_RETRACT_ALT": 10, # metres8517"SERVO10_FUNCTION": 298518})8519ex = None8520try:8521self.set_parameter("SERIAL5_PROTOCOL", 1)8522self.set_parameter("RNGFND1_TYPE", 10)8523self.reboot_sitl()8524self.set_parameter("RNGFND1_MAX_CM", 32767)85258526self.progress("Should be unhealthy while we don't send messages")8527self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False)85288529self.progress("Should be healthy while we're sending good messages")8530tstart = self.get_sim_time()8531while True:8532if self.get_sim_time() - tstart > 5:8533raise NotAchievedException("Sensor did not come good")8534self.mav.mav.distance_sensor_send(85350, # time_boot_ms853610, # min_distance853750, # max_distance853820, # current_distance8539mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type854021, # id8541mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation8542255 # covariance8543)8544if self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, True):8545self.progress("Sensor has good state")8546break8547self.delay_sim_time(0.1)85488549self.progress("Should be unhealthy again if we stop sending messages")8550self.delay_sim_time(1)8551self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False)85528553self.progress("Landing gear should deploy with current_distance below min_distance")8554self.change_mode('STABILIZE')8555timeout = 608556tstart = self.get_sim_time()8557while not self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK, True, True, True):8558if self.get_sim_time() - tstart > timeout:8559raise NotAchievedException("Failed to become armable after %f seconds" % timeout)8560self.mav.mav.distance_sensor_send(85610, # time_boot_ms8562100, # min_distance (cm)85632500, # max_distance (cm)8564200, # current_distance (cm)8565mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type856621, # id8567mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation8568255 # covariance8569)8570self.arm_vehicle()8571self.delay_sim_time(1) # servo function maps only periodically updated8572# self.send_debug_trap()85738574self.run_cmd(8575mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION,8576p2=0, # deploy8577)85788579self.context_collect("STATUSTEXT")8580tstart = self.get_sim_time()8581while True:8582if self.get_sim_time_cached() - tstart > 5:8583raise NotAchievedException("Retraction did not happen")8584self.mav.mav.distance_sensor_send(85850, # time_boot_ms8586100, # min_distance (cm)85876000, # max_distance (cm)85881500, # current_distance (cm)8589mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type859021, # id8591mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation8592255 # covariance8593)8594self.delay_sim_time(0.1)8595try:8596self.wait_text("LandingGear: RETRACT", check_context=True, timeout=0.1)8597except Exception:8598continue8599self.progress("Retracted")8600break8601# self.send_debug_trap()8602while True:8603if self.get_sim_time_cached() - tstart > 5:8604raise NotAchievedException("Deployment did not happen")8605self.progress("Sending distance-sensor message")8606self.mav.mav.distance_sensor_send(86070, # time_boot_ms8608300, # min_distance8609500, # max_distance8610250, # current_distance8611mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type861221, # id8613mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation8614255 # covariance8615)8616try:8617self.wait_text("LandingGear: DEPLOY", check_context=True, timeout=0.1)8618except Exception:8619continue8620self.progress("Deployed")8621break8622self.disarm_vehicle()86238624except Exception as e:8625self.print_exception_caught(e)8626ex = e8627self.context_pop()8628self.reboot_sitl()8629if ex is not None:8630raise ex86318632def GSF(self):8633'''test the Gaussian Sum filter'''8634self.context_push()8635self.set_parameter("EK2_ENABLE", 1)8636self.reboot_sitl()8637self.takeoff(20, mode='LOITER')8638self.set_rc(2, 1400)8639self.delay_sim_time(5)8640self.set_rc(2, 1500)8641self.progress("Path: %s" % self.current_onboard_log_filepath())8642dfreader = self.dfreader_for_current_onboard_log()8643self.do_RTL()8644self.context_pop()8645self.reboot_sitl()86468647# ensure log messages present8648want = set(["XKY0", "XKY1", "NKY0", "NKY1"])8649still_want = want8650while len(still_want):8651m = dfreader.recv_match(type=want)8652if m is None:8653raise NotAchievedException("Did not get %s" % want)8654still_want.remove(m.get_type())86558656def GSF_reset(self):8657'''test the Gaussian Sum filter based Emergency reset'''8658self.context_push()8659self.set_parameters({8660"COMPASS_ORIENT": 4, # yaw 1808661"COMPASS_USE2": 0, # disable backup compasses to avoid pre-arm failures8662"COMPASS_USE3": 0,8663})8664self.reboot_sitl()8665self.change_mode('GUIDED')8666self.wait_ready_to_arm()86678668# record starting position8669startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)8670self.progress("startpos=%s" % str(startpos))86718672# arm vehicle and takeoff to at least 5m8673self.arm_vehicle()8674expected_alt = 58675self.user_takeoff(alt_min=expected_alt)86768677# watch for emergency yaw reset8678self.wait_text("EKF3 IMU. emergency yaw reset", timeout=5, regex=True)86798680# record how far vehicle flew off8681endpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)8682delta_x = endpos.x - startpos.x8683delta_y = endpos.y - startpos.y8684dist_m = math.sqrt(delta_x*delta_x + delta_y*delta_y)8685self.progress("GSF yaw reset triggered at %f meters" % dist_m)86868687self.do_RTL()8688self.context_pop()8689self.reboot_sitl()86908691# ensure vehicle did not fly too far8692dist_m_max = 88693if dist_m > dist_m_max:8694raise NotAchievedException("GSF reset failed, vehicle flew too far (%f > %f)" % (dist_m, dist_m_max))86958696def fly_rangefinder_mavlink(self):8697self.fly_rangefinder_mavlink_distance_sensor()86988699# explicit test for the mavlink driver as it doesn't play so nice:8700self.set_parameters({8701"SERIAL5_PROTOCOL": 1,8702"RNGFND1_TYPE": 10,8703})8704self.customise_SITL_commandline(['--serial5=sim:rf_mavlink'])87058706self.change_mode('GUIDED')8707self.wait_ready_to_arm()8708self.arm_vehicle()8709expected_alt = 58710self.user_takeoff(alt_min=expected_alt)87118712tstart = self.get_sim_time()8713while True:8714if self.get_sim_time() - tstart > 5:8715raise NotAchievedException("Mavlink rangefinder not working")8716rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True)8717if rf is None:8718raise NotAchievedException("Did not receive rangefinder message")8719gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)8720if gpi is None:8721raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message")8722if abs(rf.distance - gpi.relative_alt/1000.0) > 1:8723print("rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" %8724(rf.distance, gpi.relative_alt/1000.0))8725continue87268727ds = self.mav.recv_match(8728type="DISTANCE_SENSOR",8729timeout=2,8730blocking=True,8731)8732if ds is None:8733raise NotAchievedException("Did not receive DISTANCE_SENSOR message")8734self.progress("Got: %s" % str(ds))8735if abs(ds.current_distance/100.0 - gpi.relative_alt/1000.0) > 1:8736print(8737"distance sensor.current_distance (%f) disagrees with global-position-int.relative_alt (%s)" %8738(ds.current_distance/100.0, gpi.relative_alt/1000.0))8739continue8740break8741self.progress("mavlink rangefinder OK")8742self.land_and_disarm()87438744def MaxBotixI2CXL(self):8745'''Test maxbotix rangefinder drivers'''8746ex = None8747try:8748self.context_push()87498750self.start_subtest("No messages")8751rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)8752if rf is not None:8753raise NotAchievedException("Receiving DISTANCE_SENSOR when I shouldn't be")87548755self.start_subtest("Default address")8756self.set_parameter("RNGFND1_TYPE", 2) # maxbotix8757self.reboot_sitl()8758self.do_timesync_roundtrip()8759rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)8760self.progress("Got (%s)" % str(rf))8761if rf is None:8762raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")87638764self.start_subtest("Explicitly set to default address")8765self.set_parameters({8766"RNGFND1_TYPE": 2, # maxbotix8767"RNGFND1_ADDR": 0x70,8768})8769self.reboot_sitl()8770self.do_timesync_roundtrip()8771rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)8772self.progress("Got (%s)" % str(rf))8773if rf is None:8774raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")87758776self.start_subtest("Explicitly set to non-default address")8777self.set_parameter("RNGFND1_ADDR", 0x71)8778self.reboot_sitl()8779self.do_timesync_roundtrip()8780rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)8781self.progress("Got (%s)" % str(rf))8782if rf is None:8783raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")87848785self.start_subtest("Two MaxBotix RangeFinders")8786self.set_parameters({8787"RNGFND1_TYPE": 2, # maxbotix8788"RNGFND1_ADDR": 0x70,8789"RNGFND1_MIN_CM": 150,8790"RNGFND2_TYPE": 2, # maxbotix8791"RNGFND2_ADDR": 0x71,8792"RNGFND2_MIN_CM": 250,8793})8794self.reboot_sitl()8795self.do_timesync_roundtrip()8796for i in [0, 1]:8797rf = self.mav.recv_match(8798type="DISTANCE_SENSOR",8799timeout=5,8800blocking=True,8801condition="DISTANCE_SENSOR.id==%u" % i8802)8803self.progress("Got id==%u (%s)" % (i, str(rf)))8804if rf is None:8805raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")8806expected_dist = 1508807if i == 1:8808expected_dist = 2508809if rf.min_distance != expected_dist:8810raise NotAchievedException("Unexpected min_cm (want=%u got=%u)" %8811(expected_dist, rf.min_distance))88128813self.context_pop()8814except Exception as e:8815self.print_exception_caught(e)8816ex = e88178818self.reboot_sitl()8819if ex is not None:8820raise ex88218822def fly_rangefinder_sitl(self):8823self.set_parameters({8824"RNGFND1_TYPE": 100,8825})8826self.reboot_sitl()8827self.fly_rangefinder_drivers_fly([("unused", "sitl")])8828self.wait_disarmed()88298830def RangeFinderDrivers(self):8831'''Test rangefinder drivers'''8832self.set_parameters({8833"RTL_ALT": 500,8834"RTL_ALT_TYPE": 1,8835})8836drivers = [8837("lightwareserial", 8), # autodetected between this and -binary8838("lightwareserial-binary", 8),8839("USD1_v0", 11),8840("USD1_v1", 11),8841("leddarone", 12),8842("maxsonarseriallv", 13),8843("nmea", 17, {"baud": 9600}),8844("wasp", 18),8845("benewake_tf02", 19),8846("blping", 23),8847("benewake_tfmini", 20),8848("lanbao", 26),8849("benewake_tf03", 27),8850("gyus42v2", 31),8851("teraranger_serial", 35),8852("nooploop_tofsense", 37),8853("ainsteinlrd1", 42),8854("rds02uf", 43),8855]8856while len(drivers):8857do_drivers = drivers[0:3]8858drivers = drivers[3:]8859command_line_args = []8860self.context_push()8861for offs in range(3):8862serial_num = offs + 48863if len(do_drivers) > offs:8864if len(do_drivers[offs]) > 2:8865(sim_name, rngfnd_param_value, kwargs) = do_drivers[offs]8866else:8867(sim_name, rngfnd_param_value) = do_drivers[offs]8868kwargs = {}8869command_line_args.append("--serial%s=sim:%s" %8870(serial_num, sim_name))8871sets = {8872"SERIAL%u_PROTOCOL" % serial_num: 9, # rangefinder8873"RNGFND%u_TYPE" % (offs+1): rngfnd_param_value,8874}8875if "baud" in kwargs:8876sets["SERIAL%u_BAUD" % serial_num] = kwargs["baud"]8877self.set_parameters(sets)8878self.customise_SITL_commandline(command_line_args)8879self.fly_rangefinder_drivers_fly([x[0] for x in do_drivers])8880self.context_pop()88818882self.fly_rangefinder_mavlink()8883self.fly_rangefinder_sitl() # i.e. type 10088848885i2c_drivers = [8886("maxbotixi2cxl", 2),8887]8888while len(i2c_drivers):8889do_drivers = i2c_drivers[0:9]8890i2c_drivers = i2c_drivers[9:]8891count = 18892p = {}8893for d in do_drivers:8894(sim_name, rngfnd_param_value) = d8895p["RNGFND%u_TYPE" % count] = rngfnd_param_value8896count += 188978898self.set_parameters(p)88998900self.reboot_sitl()8901self.fly_rangefinder_drivers_fly([x[0] for x in do_drivers])89028903def RangeFinderDriversMaxAlt(self):8904'''test max-height behaviour'''8905# lightwareserial goes to 130m when out of range8906self.set_parameters({8907"SERIAL4_PROTOCOL": 9,8908"RNGFND1_TYPE": 8,8909"WPNAV_SPEED_UP": 1000, # cm/s8910})8911self.customise_SITL_commandline([8912"--serial4=sim:lightwareserial",8913])8914self.takeoff(95, mode='GUIDED', timeout=240, max_err=0.5)8915self.assert_rangefinder_distance_between(90, 100)89168917self.wait_rangefinder_distance(90, 100)89188919rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION89208921self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)8922self.assert_distance_sensor_quality(100)89238924self.progress("Moving higher to be out of max rangefinder range")8925self.fly_guided_move_local(0, 0, 150)89268927# sensor remains healthy even out-of-range8928self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)89298930self.assert_distance_sensor_quality(1)89318932self.do_RTL()89338934def ShipTakeoff(self):8935'''Fly Simulated Ship Takeoff'''8936# test ship takeoff8937self.wait_groundspeed(0, 2)8938self.set_parameters({8939"SIM_SHIP_ENABLE": 1,8940"SIM_SHIP_SPEED": 10,8941"SIM_SHIP_DSIZE": 2,8942})8943self.wait_ready_to_arm()8944# we should be moving with the ship8945self.wait_groundspeed(9, 11)8946self.takeoff(10)8947# above ship our speed drops to 08948self.wait_groundspeed(0, 2)8949self.land_and_disarm()8950# ship will have moved on, so we land on the water which isn't moving8951self.wait_groundspeed(0, 2)89528953def ParameterValidation(self):8954'''Test parameters are checked for validity'''8955# wait 10 seconds for initialisation8956self.delay_sim_time(10)8957self.progress("invalid; min must be less than max:")8958self.set_parameters({8959"MOT_PWM_MIN": 100,8960"MOT_PWM_MAX": 50,8961})8962self.drain_mav()8963self.assert_prearm_failure("Motors: Check MOT_PWM_MIN and MOT_PWM_MAX")8964self.progress("invalid; min must be less than max (equal case):")8965self.set_parameters({8966"MOT_PWM_MIN": 100,8967"MOT_PWM_MAX": 100,8968})8969self.drain_mav()8970self.assert_prearm_failure("Motors: Check MOT_PWM_MIN and MOT_PWM_MAX")8971self.progress("Spin min more than 0.3")8972self.set_parameters({8973"MOT_PWM_MIN": 1000,8974"MOT_PWM_MAX": 2000,8975"MOT_SPIN_MIN": 0.5,8976})8977self.drain_mav()8978self.assert_prearm_failure("PreArm: Motors: MOT_SPIN_MIN too high 0.50 > 0.3")8979self.progress("Spin arm more than spin min")8980self.set_parameters({8981"MOT_SPIN_MIN": 0.1,8982"MOT_SPIN_ARM": 0.2,8983})8984self.drain_mav()8985self.assert_prearm_failure("PreArm: Motors: MOT_SPIN_ARM > MOT_SPIN_MIN")89868987def SensorErrorFlags(self):8988'''Test we get ERR messages when sensors have issues'''8989self.reboot_sitl()89908991for (param_names, param_value, expected_subsys, expected_ecode, desc) in [8992(['SIM_BARO_DISABLE', 'SIM_BAR2_DISABLE'], 1, 18, 4, 'unhealthy'),8993(['SIM_BARO_DISABLE', 'SIM_BAR2_DISABLE'], 0, 18, 0, 'healthy'),8994(['SIM_MAG1_FAIL', 'SIM_MAG2_FAIL', 'SIM_MAG3_FAIL'], 1, 3, 4, 'unhealthy'),8995(['SIM_MAG1_FAIL', 'SIM_MAG2_FAIL', 'SIM_MAG3_FAIL'], 0, 3, 0, 'healthy'),8996]:8997sp = dict()8998for name in param_names:8999sp[name] = param_value9000self.set_parameters(sp)9001self.delay_sim_time(1)9002mlog = self.dfreader_for_current_onboard_log()9003success = False9004while True:9005m = mlog.recv_match(type='ERR')9006print("Got (%s)" % str(m))9007if m is None:9008break9009if m.Subsys == expected_subsys and m.ECode == expected_ecode: # baro / ecode9010success = True9011break9012if not success:9013raise NotAchievedException("Did not find %s log message" % desc)90149015def AltEstimation(self):9016'''Test that Alt Estimation is mandatory for ALT_HOLD'''9017self.context_push()9018ex = None9019try:9020# disable barometer so there is no altitude source9021self.set_parameters({9022"SIM_BARO_DISABLE": 1,9023"SIM_BARO2_DISABL": 1,9024})90259026self.wait_gps_disable(position_vertical=True)90279028# turn off arming checks (mandatory arming checks will still be run)9029self.set_parameter("ARMING_CHECK", 0)90309031# delay 12 sec to allow EKF to lose altitude estimate9032self.delay_sim_time(12)90339034self.change_mode("ALT_HOLD")9035self.assert_prearm_failure("Need Alt Estimate")90369037# force arm vehicle in stabilize to bypass barometer pre-arm checks9038self.change_mode("STABILIZE")9039self.arm_vehicle()9040self.set_rc(3, 1700)9041try:9042self.change_mode("ALT_HOLD", timeout=10)9043except AutoTestTimeoutException:9044self.progress("PASS not able to set mode without Position : %s" % "ALT_HOLD")90459046# check that mode change to ALT_HOLD has failed (it should)9047if self.mode_is("ALT_HOLD"):9048raise NotAchievedException("Changed to ALT_HOLD with no altitude estimate")90499050except Exception as e:9051self.print_exception_caught(e)9052ex = e9053self.context_pop()9054self.disarm_vehicle(force=True)9055if ex is not None:9056raise ex90579058def EKFSource(self):9059'''Check EKF Source Prearms work'''9060self.context_push()9061ex = None9062try:9063self.set_parameters({9064"EK3_ENABLE": 1,9065"AHRS_EKF_TYPE": 3,9066})9067self.wait_ready_to_arm()90689069self.start_subtest("bad yaw source")9070self.set_parameter("EK3_SRC3_YAW", 17)9071self.assert_prearm_failure("Check EK3_SRC3_YAW")90729073self.context_push()9074self.start_subtest("missing required yaw source")9075self.set_parameters({9076"EK3_SRC3_YAW": 3, # External Yaw with Compass Fallback9077"COMPASS_USE": 0,9078"COMPASS_USE2": 0,9079"COMPASS_USE3": 0,9080})9081self.assert_prearm_failure("EK3 sources require Compass")9082self.context_pop()90839084except Exception as e:9085self.disarm_vehicle(force=True)9086self.print_exception_caught(e)9087ex = e9088self.context_pop()9089if ex is not None:9090raise ex90919092def test_replay_gps_bit(self):9093self.set_parameters({9094"LOG_REPLAY": 1,9095"LOG_DISARMED": 1,9096"EK3_ENABLE": 1,9097"EK2_ENABLE": 1,9098"AHRS_TRIM_X": 0.01,9099"AHRS_TRIM_Y": -0.03,9100"GPS2_TYPE": 1,9101"GPS1_POS_X": 0.1,9102"GPS1_POS_Y": 0.2,9103"GPS1_POS_Z": 0.3,9104"GPS2_POS_X": -0.1,9105"GPS2_POS_Y": -0.02,9106"GPS2_POS_Z": -0.31,9107"INS_POS1_X": 0.12,9108"INS_POS1_Y": 0.14,9109"INS_POS1_Z": -0.02,9110"INS_POS2_X": 0.07,9111"INS_POS2_Y": 0.012,9112"INS_POS2_Z": -0.06,9113"RNGFND1_TYPE": 1,9114"RNGFND1_PIN": 0,9115"RNGFND1_SCALING": 30,9116"RNGFND1_POS_X": 0.17,9117"RNGFND1_POS_Y": -0.07,9118"RNGFND1_POS_Z": -0.005,9119"SIM_SONAR_SCALE": 30,9120"SIM_GPS2_ENABLE": 1,9121})9122self.reboot_sitl()91239124self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_LOGGING, True, True, True)91259126current_log_filepath = self.current_onboard_log_filepath()9127self.progress("Current log path: %s" % str(current_log_filepath))91289129self.change_mode("LOITER")9130self.wait_ready_to_arm(require_absolute=True)9131self.arm_vehicle()9132self.takeoffAndMoveAway()9133self.do_RTL()91349135self.reboot_sitl()91369137return current_log_filepath91389139def test_replay_beacon_bit(self):9140self.set_parameters({9141"LOG_REPLAY": 1,9142"LOG_DISARMED": 1,9143})91449145old_onboard_logs = sorted(self.log_list())9146self.BeaconPosition()9147new_onboard_logs = sorted(self.log_list())91489149log_difference = [x for x in new_onboard_logs if x not in old_onboard_logs]9150return log_difference[2]91519152def test_replay_optical_flow_bit(self):9153self.set_parameters({9154"LOG_REPLAY": 1,9155"LOG_DISARMED": 1,9156})91579158old_onboard_logs = sorted(self.log_list())9159self.OpticalFlowLimits()9160new_onboard_logs = sorted(self.log_list())91619162log_difference = [x for x in new_onboard_logs if x not in old_onboard_logs]9163print("log difference: %s" % str(log_difference))9164return log_difference[0]91659166def GPSBlendingLog(self):9167'''Test GPS Blending'''9168'''ensure we get dataflash log messages for blended instance'''91699170self.context_push()91719172ex = None91739174try:9175# configure:9176self.set_parameters({9177"GPS2_TYPE": 1,9178"SIM_GPS2_TYPE": 1,9179"SIM_GPS2_ENABLE": 1,9180"GPS_AUTO_SWITCH": 2,9181})9182self.reboot_sitl()91839184# ensure we're seeing the second GPS:9185tstart = self.get_sim_time()9186while True:9187if self.get_sim_time_cached() - tstart > 60:9188raise NotAchievedException("Did not get good GPS2_RAW message")9189m = self.mav.recv_match(type='GPS2_RAW', blocking=True, timeout=1)9190self.progress("%s" % str(m))9191if m is None:9192continue9193if m.lat == 0:9194continue9195break91969197# create a log we can expect blended data to appear in:9198self.change_mode('LOITER')9199self.wait_ready_to_arm()9200self.arm_vehicle()9201self.delay_sim_time(5)9202self.disarm_vehicle()92039204# inspect generated log for messages:9205dfreader = self.dfreader_for_current_onboard_log()9206wanted = set([0, 1, 2])9207seen_primary_change = False9208while True:9209m = dfreader.recv_match(type=["GPS", "EV"]) # disarmed9210if m is None:9211break9212mtype = m.get_type()9213if mtype == 'GPS':9214try:9215wanted.remove(m.I)9216except KeyError:9217continue9218elif mtype == 'EV':9219if m.Id == 67: # GPS_PRIMARY_CHANGED9220seen_primary_change = True9221if len(wanted) == 0 and seen_primary_change:9222break92239224if len(wanted):9225raise NotAchievedException("Did not get all three GPS types")9226if not seen_primary_change:9227raise NotAchievedException("Did not see primary change")92289229except Exception as e:9230self.progress("Caught exception: %s" %9231self.get_exception_stacktrace(e))9232ex = e92339234self.context_pop()92359236self.reboot_sitl()92379238if ex is not None:9239raise ex92409241def GPSBlending(self):9242'''Test GPS Blending'''9243'''ensure we get dataflash log messages for blended instance'''92449245# configure:9246self.set_parameters({9247"WP_YAW_BEHAVIOR": 0, # do not yaw9248"GPS2_TYPE": 1,9249"SIM_GPS2_TYPE": 1,9250"SIM_GPS2_ENABLE": 1,9251"SIM_GPS1_POS_X": 1.0,9252"SIM_GPS1_POS_Y": -1.0,9253"SIM_GPS2_POS_X": -1.0,9254"SIM_GPS2_POS_Y": 1.0,9255"GPS_AUTO_SWITCH": 2,9256})9257self.reboot_sitl()92589259alt = 109260self.takeoff(alt, mode='GUIDED')9261self.fly_guided_move_local(30, 0, alt)9262self.fly_guided_move_local(30, 30, alt)9263self.fly_guided_move_local(0, 30, alt)9264self.fly_guided_move_local(0, 0, alt)9265self.change_mode('LAND')92669267current_log_file = self.dfreader_for_current_onboard_log()92689269self.wait_disarmed()92709271# ensure that the blended solution is always about half-way9272# between the two GPSs:9273current_ts = None9274while True:9275m = current_log_file.recv_match(type='GPS')9276if m is None:9277break9278if current_ts is None:9279if m.I != 0: # noqa9280continue9281current_ts = m.TimeUS9282measurements = {}9283if m.TimeUS != current_ts:9284current_ts = None9285continue9286measurements[m.I] = (m.Lat, m.Lng)9287if len(measurements) == 3:9288# check lat:9289for n in 0, 1:9290expected_blended = (measurements[0][n] + measurements[1][n])/29291epsilon = 0.00000029292error = abs(measurements[2][n] - expected_blended)9293if error > epsilon:9294raise NotAchievedException("Blended diverged")9295current_ts = None92969297if len(measurements) != 3:9298raise NotAchievedException("Did not see three GPS measurements!")92999300def GPSWeightedBlending(self):9301'''Test GPS Weighted Blending'''93029303self.context_push()93049305# configure:9306self.set_parameters({9307"WP_YAW_BEHAVIOR": 0, # do not yaw9308"GPS2_TYPE": 1,9309"SIM_GPS2_TYPE": 1,9310"SIM_GPS2_ENABLE": 1,9311"SIM_GPS1_POS_X": 1.0,9312"SIM_GPS1_POS_Y": -1.0,9313"SIM_GPS2_POS_X": -1.0,9314"SIM_GPS2_POS_Y": 1.0,9315"GPS_AUTO_SWITCH": 2,9316})9317# configure velocity errors such that the 1st GPS should be9318# 4/5, second GPS 1/5 of result (0.5**2)/((0.5**2)+(1.0**2))9319self.set_parameters({9320"SIM_GPS1_VERR_X": 0.3, # m/s9321"SIM_GPS1_VERR_Y": 0.4,9322"SIM_GPS2_VERR_X": 0.6, # m/s9323"SIM_GPS2_VERR_Y": 0.8,9324"GPS_BLEND_MASK": 4, # use only speed for blend calculations9325})9326self.reboot_sitl()93279328alt = 109329self.takeoff(alt, mode='GUIDED')9330self.fly_guided_move_local(30, 0, alt)9331self.fly_guided_move_local(30, 30, alt)9332self.fly_guided_move_local(0, 30, alt)9333self.fly_guided_move_local(0, 0, alt)9334self.change_mode('LAND')93359336current_log_file = self.dfreader_for_current_onboard_log()93379338self.wait_disarmed()93399340# ensure that the blended solution is always about half-way9341# between the two GPSs:9342current_ts = None9343while True:9344m = current_log_file.recv_match(type='GPS')9345if m is None:9346break9347if current_ts is None:9348if m.I != 0: # noqa9349continue9350current_ts = m.TimeUS9351measurements = {}9352if m.TimeUS != current_ts:9353current_ts = None9354continue9355measurements[m.I] = (m.Lat, m.Lng, m.Alt)9356if len(measurements) == 3:9357# check lat:9358for n in 0, 1, 2:9359expected_blended = 0.8*measurements[0][n] + 0.2*measurements[1][n]9360axis_epsilons = [0.0000002, 0.0000002, 0.2]9361epsilon = axis_epsilons[n]9362error = abs(measurements[2][n] - expected_blended)9363if error > epsilon:9364raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=}")9365current_ts = None93669367self.context_pop()9368self.reboot_sitl()93699370def GPSBlendingAffinity(self):9371'''test blending when affinity in use'''9372# configure:9373self.set_parameters({9374"WP_YAW_BEHAVIOR": 0, # do not yaw9375"GPS2_TYPE": 1,9376"SIM_GPS2_TYPE": 1,9377"SIM_GPS2_ENABLE": 1,9378"SIM_GPS1_POS_X": 1.0,9379"SIM_GPS1_POS_Y": -1.0,9380"SIM_GPS2_POS_X": -1.0,9381"SIM_GPS2_POS_Y": 1.0,9382"GPS_AUTO_SWITCH": 2,93839384"EK3_AFFINITY" : 1,9385"EK3_IMU_MASK": 7,9386"SIM_IMU_COUNT": 3,9387"INS_ACC3OFFS_X": 0.001,9388"INS_ACC3OFFS_Y": 0.001,9389"INS_ACC3OFFS_Z": 0.001,9390})9391# force-calibration of accel:9392self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, p5=76)9393self.reboot_sitl()93949395alt = 109396self.takeoff(alt, mode='GUIDED')9397self.fly_guided_move_local(30, 0, alt)9398self.fly_guided_move_local(30, 30, alt)9399self.fly_guided_move_local(0, 30, alt)9400self.fly_guided_move_local(0, 0, alt)9401self.change_mode('LAND')94029403current_log_file = self.dfreader_for_current_onboard_log()94049405self.wait_disarmed()94069407# ensure that the blended solution is always about half-way9408# between the two GPSs:9409current_ts = None9410max_errors = [0, 0, 0]9411while True:9412m = current_log_file.recv_match(type='XKF1')9413if m is None:9414break9415if current_ts is None:9416if m.C != 0: # noqa9417continue9418current_ts = m.TimeUS9419measurements = {}9420if m.TimeUS != current_ts:9421current_ts = None9422continue9423measurements[m.C] = (m.PN, m.PE, m.PD)9424if len(measurements) == 3:9425# check lat:9426for n in 0, 1, 2:9427expected_blended = 0.5*measurements[0][n] + 0.5*measurements[1][n]9428axis_epsilons = [0.02, 0.02, 0.03]9429epsilon = axis_epsilons[n]9430error = abs(measurements[2][n] - expected_blended)9431# self.progress(f"{n=} {error=}")9432if error > max_errors[n]:9433max_errors[n] = error9434if error > epsilon:9435raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=} {measurements[2][n]=} {error=}") # noqa:E5019436current_ts = None9437self.progress(f"{max_errors=}")94389439def Callisto(self):9440'''Test Callisto'''9441self.customise_SITL_commandline(9442[],9443defaults_filepath=self.model_defaults_filepath('Callisto'),9444model="octa-quad:@ROMFS/models/Callisto.json",9445wipe=True,9446)9447self.takeoff(10)9448self.do_RTL()94499450def FlyEachFrame(self):9451'''Fly each supported internal frame'''9452vinfo = vehicleinfo.VehicleInfo()9453copter_vinfo_options = vinfo.options[self.vehicleinfo_key()]9454known_broken_frames = {9455'heli-compound': "wrong binary, different takeoff regime",9456'heli-dual': "wrong binary, different takeoff regime",9457'heli': "wrong binary, different takeoff regime",9458'heli-gas': "wrong binary, different takeoff regime",9459'heli-blade360': "wrong binary, different takeoff regime",9460"quad-can" : "needs CAN periph",9461}9462for frame in sorted(copter_vinfo_options["frames"].keys()):9463self.start_subtest("Testing frame (%s)" % str(frame))9464if frame in known_broken_frames:9465self.progress("Actually, no I'm not - it is known-broken (%s)" %9466(known_broken_frames[frame]))9467continue9468frame_bits = copter_vinfo_options["frames"][frame]9469print("frame_bits: %s" % str(frame_bits))9470if frame_bits.get("external", False):9471self.progress("Actually, no I'm not - it is an external simulation")9472continue9473model = frame_bits.get("model", frame)9474defaults = self.model_defaults_filepath(frame)9475if not isinstance(defaults, list):9476defaults = [defaults]9477self.customise_SITL_commandline(9478[],9479defaults_filepath=defaults,9480model=model,9481wipe=True,9482)94839484# add a listener that verifies yaw looks good:9485def verify_yaw(mav, m):9486if m.get_type() != 'ATTITUDE':9487return9488yawspeed_thresh_rads = math.radians(20)9489if m.yawspeed > yawspeed_thresh_rads:9490raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" %9491(math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame))9492self.context_push()9493self.install_message_hook_context(verify_yaw)9494self.takeoff(10)9495self.context_pop()9496self.hover()9497self.change_mode('ALT_HOLD')9498self.delay_sim_time(1)94999500def verify_rollpitch(mav, m):9501if m.get_type() != 'ATTITUDE':9502return9503pitch_thresh_rad = math.radians(2)9504if m.pitch > pitch_thresh_rad:9505raise NotAchievedException("Excessive pitch %f deg > %f deg" %9506(math.degrees(m.pitch), math.degrees(pitch_thresh_rad)))9507roll_thresh_rad = math.radians(2)9508if m.roll > roll_thresh_rad:9509raise NotAchievedException("Excessive roll %f deg > %f deg" %9510(math.degrees(m.roll), math.degrees(roll_thresh_rad)))9511self.context_push()9512self.install_message_hook_context(verify_rollpitch)9513for i in range(5):9514self.set_rc(4, 2000)9515self.delay_sim_time(0.5)9516self.set_rc(4, 1500)9517self.delay_sim_time(5)9518self.context_pop()95199520self.do_RTL()95219522def Replay(self):9523'''test replay correctness'''9524self.progress("Building Replay")9525util.build_SITL('tool/Replay', clean=False, configure=False)9526self.set_parameters({9527"LOG_DARM_RATEMAX": 0,9528"LOG_FILE_RATEMAX": 0,9529})95309531bits = [9532('GPS', self.test_replay_gps_bit),9533('Beacon', self.test_replay_beacon_bit),9534('OpticalFlow', self.test_replay_optical_flow_bit),9535]9536for (name, func) in bits:9537self.start_subtest("%s" % name)9538self.test_replay_bit(func)95399540def test_replay_bit(self, bit):95419542self.context_push()9543current_log_filepath = bit()95449545self.progress("Running replay on (%s) (%u bytes)" % (9546(current_log_filepath, os.path.getsize(current_log_filepath))9547))95489549self.run_replay(current_log_filepath)95509551replay_log_filepath = self.current_onboard_log_filepath()95529553self.context_pop()95549555self.progress("Replay log path: %s" % str(replay_log_filepath))95569557check_replay = util.load_local_module("Tools/Replay/check_replay.py")95589559ok = check_replay.check_log(replay_log_filepath, self.progress, verbose=True)9560if not ok:9561raise NotAchievedException("check_replay (%s) failed" % current_log_filepath)95629563def DefaultIntervalsFromFiles(self):9564'''Test setting default mavlink message intervals from files'''9565ex = None9566intervals_filepath = util.reltopdir("message-intervals-chan0.txt")9567self.progress("Using filepath (%s)" % intervals_filepath)9568try:9569with open(intervals_filepath, "w") as f:9570f.write("""30 50957128 100957229 2009573""")9574f.close()95759576# other tests may have explicitly set rates, so wipe parameters:9577def custom_stream_rate_setter():9578for stream in mavutil.mavlink.MAV_DATA_STREAM_EXTRA3, mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS:9579self.set_streamrate(5, stream=stream)95809581self.customise_SITL_commandline(9582[],9583wipe=True,9584set_streamrate_callback=custom_stream_rate_setter,9585)95869587self.assert_message_rate_hz("ATTITUDE", 20)9588self.assert_message_rate_hz("SCALED_PRESSURE", 5)95899590except Exception as e:9591self.print_exception_caught(e)9592ex = e95939594os.unlink(intervals_filepath)95959596self.reboot_sitl()95979598if ex is not None:9599raise ex96009601def BaroDrivers(self):9602'''Test Baro Drivers'''9603sensors = [9604("MS5611", 2),9605]9606for (name, bus) in sensors:9607self.context_push()9608if bus is not None:9609self.set_parameter("BARO_EXT_BUS", bus)9610self.set_parameter("BARO_PROBE_EXT", 1 << 2)9611self.reboot_sitl()9612self.wait_ready_to_arm()9613self.arm_vehicle()96149615# insert listener to compare airspeeds:9616messages = [None, None, None]96179618global count9619count = 096209621def check_pressure(mav, m):9622global count9623m_type = m.get_type()9624count += 19625# if count > 500:9626# if press_abs[0] is None or press_abs[1] is None:9627# raise NotAchievedException("Not receiving messages")9628if m_type == 'SCALED_PRESSURE3':9629off = 29630elif m_type == 'SCALED_PRESSURE2':9631off = 19632elif m_type == 'SCALED_PRESSURE':9633off = 09634else:9635return96369637messages[off] = m96389639if None in messages:9640return9641first = messages[0]9642for msg in messages[1:]:9643delta_press_abs = abs(first.press_abs - msg.press_abs)9644if delta_press_abs > 0.5: # 50 Pa leeway9645raise NotAchievedException("Press_Abs mismatch (press1=%s press2=%s)" % (first, msg))9646delta_temperature = abs(first.temperature - msg.temperature)9647if delta_temperature > 300: # that's 3-degrees leeway9648raise NotAchievedException("Temperature mismatch (t1=%s t2=%s)" % (first, msg))9649self.install_message_hook_context(check_pressure)9650self.fly_mission("copter_mission.txt", strict=False)9651if None in messages:9652raise NotAchievedException("Missing a message")96539654self.context_pop()9655self.reboot_sitl()96569657def PositionWhenGPSIsZero(self):9658'''Ensure position doesn't zero when GPS lost'''9659# https://github.com/ArduPilot/ardupilot/issues/142369660self.progress("arm the vehicle and takeoff in Guided")9661self.takeoff(20, mode='GUIDED')9662self.progress("fly 50m North (or whatever)")9663old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)9664self.fly_guided_move_global_relative_alt(50, 0, 20)9665self.set_parameter('GPS1_TYPE', 0)9666self.drain_mav()9667tstart = self.get_sim_time()9668while True:9669if self.get_sim_time_cached() - tstart > 30 and self.mode_is('LAND'):9670self.progress("Bug not reproduced")9671break9672m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1, verbose=True)9673pos_delta = self.get_distance_int(old_pos, m)9674self.progress("Distance: %f" % pos_delta)9675if pos_delta < 5:9676raise NotAchievedException("Bug reproduced - returned to near origin")9677self.wait_disarmed()9678self.reboot_sitl()96799680def SMART_RTL(self):9681'''Check SMART_RTL'''9682self.progress("arm the vehicle and takeoff in Guided")9683self.takeoff(20, mode='GUIDED')9684self.progress("fly around a bit (or whatever)")9685locs = [9686(50, 0, 20),9687(-50, 50, 20),9688(-50, 0, 20),9689]9690for (lat, lng, alt) in locs:9691self.fly_guided_move_local(lat, lng, alt)96929693self.change_mode('SMART_RTL')9694for (lat, lng, alt) in reversed(locs):9695self.wait_distance_to_local_position(9696(lat, lng, -alt),96970,969810,9699timeout=609700)9701self.wait_disarmed()97029703def get_ground_effect_duration_from_current_onboard_log(self, bit, ignore_multi=False):9704'''returns a duration in seconds we were expecting to interact with9705the ground. Will die if there's more than one such block of9706time and ignore_multi is not set (will return first duration9707otherwise)9708'''9709ret = []9710dfreader = self.dfreader_for_current_onboard_log()9711seen_expected_start_TimeUS = None9712first = None9713last = None9714while True:9715m = dfreader.recv_match(type="XKF4")9716if m is None:9717break9718last = m9719if first is None:9720first = m9721# self.progress("%s" % str(m))9722expected = m.SS & (1 << bit)9723if expected:9724if seen_expected_start_TimeUS is None:9725seen_expected_start_TimeUS = m.TimeUS9726continue9727else:9728if seen_expected_start_TimeUS is not None:9729duration = (m.TimeUS - seen_expected_start_TimeUS)/1000000.09730ret.append(duration)9731seen_expected_start_TimeUS = None9732if seen_expected_start_TimeUS is not None:9733duration = (last.TimeUS - seen_expected_start_TimeUS)/1000000.09734ret.append(duration)9735return ret97369737def get_takeoffexpected_durations_from_current_onboard_log(self, ignore_multi=False):9738return self.get_ground_effect_duration_from_current_onboard_log(11, ignore_multi=ignore_multi)97399740def get_touchdownexpected_durations_from_current_onboard_log(self, ignore_multi=False):9741return self.get_ground_effect_duration_from_current_onboard_log(12, ignore_multi=ignore_multi)97429743def ThrowDoubleDrop(self):9744'''Test a more complicated drop-mode scenario'''9745self.progress("Getting a lift to altitude")9746self.set_parameters({9747"SIM_SHOVE_Z": -11,9748"THROW_TYPE": 1, # drop9749"MOT_SPOOL_TIME": 2,9750})9751self.change_mode('THROW')9752self.wait_ready_to_arm()9753self.arm_vehicle()9754try:9755self.set_parameter("SIM_SHOVE_TIME", 30000)9756except ValueError:9757# the shove resets this to zero9758pass97599760self.wait_altitude(100, 1000, timeout=100, relative=True)9761self.context_collect('STATUSTEXT')9762self.wait_statustext("throw detected - spooling motors", check_context=True, timeout=10)9763self.wait_statustext("throttle is unlimited - uprighting", check_context=True)9764self.wait_statustext("uprighted - controlling height", check_context=True)9765self.wait_statustext("height achieved - controlling position", check_context=True)9766self.progress("Waiting for still")9767self.wait_speed_vector(Vector3(0, 0, 0))9768self.change_mode('ALT_HOLD')9769self.set_rc(3, 1000)9770self.wait_disarmed(timeout=90)9771self.zero_throttle()97729773self.progress("second flight")9774self.upload_square_mission_items_around_location(self.poll_home_position())97759776self.set_parameters({9777"THROW_NEXTMODE": 3, # auto9778})97799780self.change_mode('THROW')9781self.wait_ready_to_arm()9782self.arm_vehicle()9783try:9784self.set_parameter("SIM_SHOVE_TIME", 30000)9785except ValueError:9786# the shove resets this to zero9787pass97889789self.wait_altitude(100, 1000, timeout=100, relative=True)9790self.wait_statustext("throw detected - spooling motors", check_context=True, timeout=10)9791self.wait_statustext("throttle is unlimited - uprighting", check_context=True)9792self.wait_statustext("uprighted - controlling height", check_context=True)9793self.wait_statustext("height achieved - controlling position", check_context=True)9794self.wait_mode('AUTO')9795self.wait_disarmed(timeout=240)97969797def GroundEffectCompensation_takeOffExpected(self):9798'''Test EKF's handling of takeoff-expected'''9799self.change_mode('ALT_HOLD')9800self.set_parameter("LOG_FILE_DSRMROT", 1)9801self.progress("Making sure we'll have a short log to look at")9802self.wait_ready_to_arm()9803self.arm_vehicle()9804self.disarm_vehicle()98059806# arm the vehicle and let it disarm normally. This should9807# yield a log where the EKF considers a takeoff imminent until9808# disarm9809self.start_subtest("Check ground effect compensation remains set in EKF while we're at idle on the ground")9810self.arm_vehicle()9811self.wait_disarmed()98129813durations = self.get_takeoffexpected_durations_from_current_onboard_log()9814duration = durations[0]9815want = 99816self.progress("takeoff-expected duration: %fs" % (duration,))9817if duration < want: # assumes default 10-second DISARM_DELAY9818raise NotAchievedException("Should have been expecting takeoff for longer than %fs (want>%f)" %9819(duration, want))98209821self.start_subtest("takeoffExpected should be false very soon after we launch into the air")9822self.takeoff(mode='ALT_HOLD', alt_min=5)9823self.change_mode('LAND')9824self.wait_disarmed()9825durations = self.get_takeoffexpected_durations_from_current_onboard_log(ignore_multi=True)9826self.progress("touchdown-durations: %s" % str(durations))9827duration = durations[0]9828self.progress("takeoff-expected-duration %f" % (duration,))9829want_lt = 59830if duration >= want_lt:9831raise NotAchievedException("Was expecting takeoff for longer than expected; got=%f want<=%f" %9832(duration, want_lt))98339834def _MAV_CMD_CONDITION_YAW(self, command):9835self.start_subtest("absolute")9836self.takeoff(20, mode='GUIDED')98379838m = self.mav.recv_match(type='VFR_HUD', blocking=True)9839initial_heading = m.heading98409841self.progress("Ensuring initial heading is steady")9842target = initial_heading9843command(9844mavutil.mavlink.MAV_CMD_CONDITION_YAW,9845p1=target, # target angle9846p2=10, # degrees/second9847p3=1, # -1 is counter-clockwise, 1 clockwise9848p4=0, # 1 for relative, 0 for absolute9849)9850self.wait_heading(target, minimum_duration=2, timeout=50)9851self.wait_yaw_speed(0)98529853degsecond = 298549855def rate_watcher(mav, m):9856if m.get_type() != 'ATTITUDE':9857return9858if abs(math.degrees(m.yawspeed)) > 5*degsecond:9859raise NotAchievedException("Moved too fast (%f>%f)" %9860(math.degrees(m.yawspeed), 5*degsecond))9861self.install_message_hook_context(rate_watcher)9862self.progress("Yaw CW 60 degrees")9863target = initial_heading + 609864part_way_target = initial_heading + 109865command(9866mavutil.mavlink.MAV_CMD_CONDITION_YAW,9867p1=target, # target angle9868p2=degsecond, # degrees/second9869p3=1, # -1 is counter-clockwise, 1 clockwise9870p4=0, # 1 for relative, 0 for absolute9871)9872self.wait_heading(part_way_target)9873self.wait_heading(target, minimum_duration=2)98749875self.progress("Yaw CCW 60 degrees")9876target = initial_heading9877part_way_target = initial_heading + 309878command(9879mavutil.mavlink.MAV_CMD_CONDITION_YAW,9880p1=target, # target angle9881p2=degsecond, # degrees/second9882p3=-1, # -1 is counter-clockwise, 1 clockwise9883p4=0, # 1 for relative, 0 for absolute9884)9885self.wait_heading(part_way_target)9886self.wait_heading(target, minimum_duration=2)98879888self.disarm_vehicle(force=True)9889self.reboot_sitl()98909891def MAV_CMD_CONDITION_YAW(self):9892'''Test response to MAV_CMD_CONDITION_YAW via mavlink'''9893self.context_push()9894self._MAV_CMD_CONDITION_YAW(self.run_cmd_int)9895self.context_pop()9896self.context_push()9897self._MAV_CMD_CONDITION_YAW(self.run_cmd)9898self.context_pop()98999900def GroundEffectCompensation_touchDownExpected(self):9901'''Test EKF's handling of touchdown-expected'''9902self.zero_throttle()9903self.change_mode('ALT_HOLD')9904self.set_parameter("LOG_FILE_DSRMROT", 1)9905self.progress("Making sure we'll have a short log to look at")9906self.wait_ready_to_arm()9907self.arm_vehicle()9908self.disarm_vehicle()99099910self.start_subtest("Make sure touchdown-expected duration is about right")9911self.takeoff(20, mode='ALT_HOLD')9912self.change_mode('LAND')9913self.wait_disarmed()99149915durations = self.get_touchdownexpected_durations_from_current_onboard_log(ignore_multi=True)9916self.progress("touchdown-durations: %s" % str(durations))9917duration = durations[-1]9918expected = 23 # this is the time in the final descent phase of LAND9919if abs(duration - expected) > 5:9920raise NotAchievedException("Was expecting roughly %fs of touchdown expected, got %f" % (expected, duration))99219922def upload_square_mission_items_around_location(self, loc):9923alt = 209924loc.alt = alt9925items = [9926(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt)9927]99289929for (ofs_n, ofs_e) in (20, 20), (20, -20), (-20, -20), (-20, 20), (20, 20):9930items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, ofs_n, ofs_e, alt))99319932items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))99339934self.upload_simple_relhome_mission(items)99359936def RefindGPS(self):9937'''Refind the GPS and attempt to RTL rather than continue to land'''9938# https://github.com/ArduPilot/ardupilot/issues/142369939self.progress("arm the vehicle and takeoff in Guided")9940self.takeoff(50, mode='GUIDED')9941self.progress("fly 50m North (or whatever)")9942old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)9943self.fly_guided_move_global_relative_alt(50, 0, 50)9944self.set_parameter('GPS1_TYPE', 0)9945self.drain_mav()9946tstart = self.get_sim_time()9947while True:9948if self.get_sim_time_cached() - tstart > 30 and self.mode_is('LAND'):9949self.progress("Bug not reproduced")9950break9951m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1, verbose=True)9952pos_delta = self.get_distance_int(old_pos, m)9953self.progress("Distance: %f" % pos_delta)9954if pos_delta < 5:9955raise NotAchievedException("Bug reproduced - returned to near origin")9956self.set_parameter('GPS1_TYPE', 1)9957self.do_RTL()99589959def GPSForYaw(self):9960'''Moving baseline GPS yaw'''9961self.load_default_params_file("copter-gps-for-yaw.parm")9962self.reboot_sitl()99639964self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)9965m = self.assert_receive_message("GPS2_RAW", very_verbose=True)9966want = 270009967if abs(m.yaw - want) > 500:9968raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw))9969self.wait_ready_to_arm()99709971def SMART_RTL_EnterLeave(self):9972'''check SmartRTL behaviour when entering/leaving'''9973# we had a bug where we would consume points when re-entering smartrtl99749975self.upload_simple_relhome_mission([9976# N E U9977(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),9978(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),9979])9980self.set_parameter('AUTO_OPTIONS', 3)9981self.change_mode('AUTO')9982self.wait_ready_to_arm()9983self.change_mode('ALT_HOLD')9984self.change_mode('SMART_RTL')9985self.change_mode('ALT_HOLD')9986self.change_mode('SMART_RTL')99879988def SMART_RTL_Repeat(self):9989'''Test whether Smart RTL catches the repeat'''9990self.takeoff(alt_min=10, mode='GUIDED')9991self.set_rc(3, 1500)9992self.change_mode("CIRCLE")9993self.delay_sim_time(1300)9994self.change_mode("SMART_RTL")9995self.wait_disarmed()99969997def GPSForYawCompassLearn(self):9998'''Moving baseline GPS yaw - with compass learning'''9999self.context_push()10000self.load_default_params_file("copter-gps-for-yaw.parm")10001self.set_parameter("EK3_SRC1_YAW", 3) # GPS with compass fallback10002self.reboot_sitl()1000310004self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)1000510006self.wait_ready_to_arm()1000710008self.takeoff(10, mode='GUIDED')10009tstart = self.get_sim_time()10010compass_learn_set = False10011while True:10012delta_t = self.get_sim_time_cached() - tstart10013if delta_t > 30:10014break10015if not compass_learn_set and delta_t > 10:10016self.set_parameter("COMPASS_LEARN", 3)10017compass_learn_set = True1001810019self.check_attitudes_match()10020self.delay_sim_time(1)1002110022self.context_pop()10023self.reboot_sitl()1002410025def AP_Avoidance(self):10026'''ADSB-based avoidance'''10027self.set_parameters({10028"AVD_ENABLE": 1,10029"ADSB_TYPE": 1, # mavlink10030"AVD_F_ACTION": 2, # climb or descend10031})10032self.reboot_sitl()1003310034self.wait_ready_to_arm()1003510036here = self.mav.location()1003710038self.context_push()1003910040self.start_subtest("F_ALT_MIN zero - disabled, can't arm in face of threat")10041self.set_parameters({10042"AVD_F_ALT_MIN": 0,10043})10044self.wait_ready_to_arm()10045self.test_adsb_send_threatening_adsb_message(here)10046self.delay_sim_time(1)10047self.try_arm(result=False,10048expect_msg="ADSB threat detected")1004910050self.wait_ready_to_arm(timeout=60)1005110052self.context_pop()1005310054self.start_subtest("F_ALT_MIN 16m relative - arm in face of threat")10055self.context_push()10056self.set_parameters({10057"AVD_F_ALT_MIN": int(16 + here.alt),10058})10059self.wait_ready_to_arm()10060self.test_adsb_send_threatening_adsb_message(here)10061# self.delay_sim_time(1)10062self.arm_vehicle()10063self.disarm_vehicle()10064self.context_pop()1006510066def PAUSE_CONTINUE(self):10067'''Test MAV_CMD_DO_PAUSE_CONTINUE in AUTO mode'''10068self.load_mission(filename="copter_mission.txt", strict=False)10069self.set_parameter(name="AUTO_OPTIONS", value=3)10070self.change_mode(mode="AUTO")10071self.wait_ready_to_arm()10072self.arm_vehicle()1007310074self.wait_current_waypoint(wpnum=3, timeout=500)10075self.send_pause_command()10076self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)10077self.send_resume_command()1007810079self.wait_current_waypoint(wpnum=4, timeout=500)10080self.send_pause_command()10081self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)10082self.send_resume_command()1008310084# sending a pause, or resume, to the aircraft twice, doesn't result in reporting a failure10085self.wait_current_waypoint(wpnum=5, timeout=500)10086self.send_pause_command()10087self.send_pause_command()10088self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)10089self.send_resume_command()10090self.send_resume_command()1009110092self.wait_disarmed(timeout=500)1009310094def PAUSE_CONTINUE_GUIDED(self):10095'''Test MAV_CMD_DO_PAUSE_CONTINUE in GUIDED mode'''10096self.start_subtest("Started test for Pause/Continue in GUIDED mode with LOCATION!")10097self.change_mode(mode="GUIDED")10098self.wait_ready_to_arm()10099self.arm_vehicle()10100self.set_parameter(name="GUID_TIMEOUT", value=120)10101self.user_takeoff(alt_min=30)1010210103# send vehicle to global position target10104location = self.home_relative_loc_ne(n=300, e=0)10105target_typemask = MAV_POS_TARGET_TYPE_MASK.POS_ONLY10106self.mav.mav.set_position_target_global_int_send(101070, # timestamp101081, # target system_id101091, # target component id10110mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # relative altitude frame10111target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # target typemask as pos only10112int(location.lat * 1e7), # lat10113int(location.lng * 1e7), # lon1011430, # alt101150, # vx101160, # vy101170, # vz101180, # afx101190, # afy101200, # afz101210, # yaw101220) # yawrate1012310124self.wait_distance_to_home(distance_min=100, distance_max=150, timeout=120)10125self.send_pause_command()10126self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)10127self.send_resume_command()10128self.wait_location(loc=location, timeout=120)1012910130self.end_subtest("Ended test for Pause/Continue in GUIDED mode with LOCATION!")10131self.start_subtest("Started test for Pause/Continue in GUIDED mode with DESTINATION!")10132self.guided_achieve_heading(heading=270)1013310134# move vehicle on x direction10135location = self.offset_location_ne(location=self.mav.location(), metres_north=0, metres_east=-300)10136self.mav.mav.set_position_target_global_int_send(101370, # system time in milliseconds101381, # target system101391, # target component10140mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # coordinate frame MAV_FRAME_BODY_NED10141MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # type mask (pos only)10142int(location.lat*1e7), # position x10143int(location.lng*1e7), # position y1014430, # position z101450, # velocity x101460, # velocity y101470, # velocity z101480, # accel x101490, # accel y101500, # accel z101510, # yaw101520) # yaw rate1015310154self.wait_location(loc=location, accuracy=200, timeout=120)10155self.send_pause_command()10156self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)10157self.send_resume_command()10158self.wait_location(loc=location, timeout=120)1015910160self.end_subtest("Ended test for Pause/Continue in GUIDED mode with DESTINATION!")10161self.start_subtest("Started test for Pause/Continue in GUIDED mode with VELOCITY!")1016210163# give velocity command10164vx, vy, vz_up = (5, 5, 0)10165self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10)1016610167self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10)10168self.send_pause_command()10169self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)10170self.send_resume_command()10171self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10)10172self.test_guided_local_velocity_target(vx=0, vy=0, vz_up=0, timeout=10)10173self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)1017410175self.end_subtest("Ended test for Pause/Continue in GUIDED mode with VELOCITY!")10176self.start_subtest("Started test for Pause/Continue in GUIDED mode with ACCELERATION!")1017710178# give acceleration command10179ax, ay, az_up = (1, 1, 0)10180target_typemask = (MAV_POS_TARGET_TYPE_MASK.POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |10181MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)10182self.mav.mav.set_position_target_local_ned_send(101830, # timestamp101841, # target system_id101851, # target component id10186mavutil.mavlink.MAV_FRAME_LOCAL_NED,10187target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,101880, # x101890, # y101900, # z101910, # vx101920, # vy101930, # vz10194ax, # afx10195ay, # afy10196-az_up, # afz101970, # yaw101980, # yawrate10199)1020010201self.wait_for_local_velocity(vx=5, vy=5, vz_up=0, timeout=10)10202self.send_pause_command()10203self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)10204self.send_resume_command()10205self.wait_for_local_velocity(vx=5, vy=5, vz_up=0, timeout=10)10206self.test_guided_local_velocity_target(vx=0, vy=0, vz_up=0, timeout=10)10207self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)10208self.end_subtest("Ended test for Pause/Continue in GUIDED mode with ACCELERATION!")1020910210# start pause/continue subtest with posvelaccel10211self.start_subtest("Started test for Pause/Continue in GUIDED mode with POSITION and VELOCITY and ACCELERATION!")10212self.guided_achieve_heading(heading=0)1021310214# give posvelaccel command10215x, y, z_up = (-300, 0, 30)10216target_typemask = (MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)10217self.mav.mav.set_position_target_local_ned_send(102180, # timestamp102191, # target system_id102201, # target component id10221mavutil.mavlink.MAV_FRAME_LOCAL_NED,10222target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,10223x, # x10224y, # y10225-z_up, # z102260, # vx102270, # vy102280, # vz102290, # afx102300, # afy102310, # afz102320, # yaw102330, # yawrate10234)1023510236self.wait_distance_to_local_position(local_position=(x, y, -z_up), distance_min=400, distance_max=450, timeout=120)10237self.send_pause_command()10238self.wait_for_local_velocity(0, 0, 0, timeout=10)10239self.send_resume_command()10240self.wait_distance_to_local_position(local_position=(x, y, -z_up), distance_min=0, distance_max=10, timeout=120)1024110242self.end_subtest("Ended test for Pause/Continue in GUIDED mode with POSITION and VELOCITY and ACCELERATION!")10243self.do_RTL(timeout=120)1024410245def DO_CHANGE_SPEED(self):10246'''Change speed during misison using waypoint items'''10247self.load_mission("mission.txt", strict=False)1024810249self.set_parameters({10250"AUTO_OPTIONS": 3,10251"ANGLE_MAX": 4500,10252})1025310254self.change_mode('AUTO')10255self.wait_ready_to_arm()10256self.arm_vehicle()1025710258self.wait_current_waypoint(1)10259self.wait_groundspeed(102603.5, 4.5,10261minimum_duration=5,10262timeout=60,10263)1026410265self.wait_current_waypoint(3)10266self.wait_groundspeed(1026714.5, 15.5,10268minimum_duration=10,10269timeout=60,10270)1027110272self.wait_current_waypoint(5)10273self.wait_groundspeed(102749.5, 11.5,10275minimum_duration=10,10276timeout=60,10277)1027810279self.set_parameter("ANGLE_MAX", 6000)10280self.wait_current_waypoint(7)10281self.wait_groundspeed(1028215.5, 16.5,10283minimum_duration=10,10284timeout=60,10285)1028610287self.wait_disarmed()1028810289def AUTO_LAND_TO_BRAKE(self):10290'''ensure terrain altitude is taken into account when braking'''10291self.set_parameters({10292"PLND_ACC_P_NSE": 2.500000,10293"PLND_ALT_MAX": 8.000000,10294"PLND_ALT_MIN": 0.750000,10295"PLND_BUS": -1,10296"PLND_CAM_POS_X": 0.000000,10297"PLND_CAM_POS_Y": 0.000000,10298"PLND_CAM_POS_Z": 0.000000,10299"PLND_ENABLED": 1,10300"PLND_EST_TYPE": 1,10301"PLND_LAG": 0.020000,10302"PLND_LAND_OFS_X": 0.000000,10303"PLND_LAND_OFS_Y": 0.000000,10304"PLND_OPTIONS": 0,10305"PLND_RET_BEHAVE": 0,10306"PLND_RET_MAX": 4,10307"PLND_STRICT": 1,10308"PLND_TIMEOUT": 4.000000,10309"PLND_TYPE": 4,10310"PLND_XY_DIST_MAX": 2.500000,10311"PLND_YAW_ALIGN": 0.000000,1031210313"SIM_PLD_ALT_LMT": 15.000000,10314"SIM_PLD_DIST_LMT": 10.000000,10315"SIM_PLD_ENABLE": 1,10316"SIM_PLD_HEIGHT": 0,10317"SIM_PLD_LAT": -20.558929,10318"SIM_PLD_LON": -47.415035,10319"SIM_PLD_RATE": 100,10320"SIM_PLD_TYPE": 1,10321"SIM_PLD_YAW": 87,1032210323"SIM_SONAR_SCALE": 12,10324})1032510326self.set_analog_rangefinder_parameters()1032710328self.load_mission('mission.txt')10329self.customise_SITL_commandline([10330"--home", self.sitl_home_string_from_mission("mission.txt"),10331])1033210333self.set_parameter('AUTO_OPTIONS', 3)10334self.change_mode('AUTO')10335self.wait_ready_to_arm()10336self.arm_vehicle()1033710338self.wait_current_waypoint(7)10339self.wait_altitude(10, 15, relative=True, timeout=60)10340self.change_mode('BRAKE')10341# monitor altitude here10342self.wait_altitude(10, 15, relative=True, minimum_duration=20)10343self.change_mode('AUTO')10344self.wait_disarmed()1034510346def MAVLandedStateTakeoff(self):10347'''check EXTENDED_SYS_STATE message'''10348ex = None10349try:10350self.set_message_rate_hz(id=mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz=1)10351self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,10352landed_state=mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, timeout=10)10353self.load_mission(filename="copter_mission.txt")10354self.set_parameter(name="AUTO_OPTIONS", value=3)10355self.change_mode(mode="AUTO")10356self.wait_ready_to_arm()10357self.arm_vehicle()10358self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,10359landed_state=mavutil.mavlink.MAV_LANDED_STATE_TAKEOFF, timeout=30)10360self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,10361landed_state=mavutil.mavlink.MAV_LANDED_STATE_IN_AIR, timeout=60)10362self.land_and_disarm()10363except Exception as e:10364self.print_exception_caught(e)10365ex = e10366self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -1)10367if ex is not None:10368raise ex1036910370def ATTITUDE_FAST(self):10371'''ensure that when ATTITDE_FAST is set we get many messages'''10372self.context_push()10373ex = None10374try:10375old = self.get_parameter('LOG_BITMASK')10376new = int(old) | (1 << 0) # see defines.h10377self.set_parameters({10378"LOG_BITMASK": new,10379"LOG_DISARMED": 1,10380"LOG_DARM_RATEMAX": 0,10381"LOG_FILE_RATEMAX": 0,10382})10383path = self.generate_rate_sample_log()1038410385except Exception as e:10386self.print_exception_caught(e)10387ex = e1038810389self.context_pop()1039010391self.reboot_sitl()1039210393if ex is not None:10394raise ex1039510396self.delay_sim_time(10) # NFI why this is required1039710398self.check_dflog_message_rates(path, {10399'ANG': 400,10400})1040110402def BaseLoggingRates(self):10403'''ensure messages come out at specific rates'''10404self.set_parameters({10405"LOG_DARM_RATEMAX": 0,10406"LOG_FILE_RATEMAX": 0,10407})10408path = self.generate_rate_sample_log()10409self.delay_sim_time(10) # NFI why this is required10410self.check_dflog_message_rates(path, {10411"ATT": 10,10412"IMU": 25,10413})1041410415def FETtecESC_flight(self):10416'''fly with servo outputs from FETtec ESC'''10417self.start_subtest("FETtec ESC flight")10418num_wp = self.load_mission("copter_mission.txt", strict=False)10419self.fly_loaded_mission(num_wp)1042010421def FETtecESC_esc_power_checks(self):10422'''Make sure state machine copes with ESCs rebooting'''10423self.start_subtest("FETtec ESC reboot")10424self.wait_ready_to_arm()10425self.context_collect('STATUSTEXT')10426self.progress("Turning off an ESC off ")10427mask = int(self.get_parameter("SIM_FTOWESC_POW"))1042810429for mot_id_to_kill in 1, 2:10430self.progress("Turning ESC=%u off" % mot_id_to_kill)10431self.set_parameter("SIM_FTOWESC_POW", mask & ~(1 << mot_id_to_kill))10432self.delay_sim_time(1)10433self.assert_prearm_failure("are not running")10434self.progress("Turning it back on")10435self.set_parameter("SIM_FTOWESC_POW", mask)10436self.wait_ready_to_arm()1043710438self.progress("Turning ESC=%u off (again)" % mot_id_to_kill)10439self.set_parameter("SIM_FTOWESC_POW", mask & ~(1 << mot_id_to_kill))10440self.delay_sim_time(1)10441self.assert_prearm_failure("are not running")10442self.progress("Turning it back on")10443self.set_parameter("SIM_FTOWESC_POW", mask)10444self.wait_ready_to_arm()1044510446self.progress("Turning all ESCs off")10447self.set_parameter("SIM_FTOWESC_POW", 0)10448self.delay_sim_time(1)10449self.assert_prearm_failure("are not running")10450self.progress("Turning them back on")10451self.set_parameter("SIM_FTOWESC_POW", mask)10452self.wait_ready_to_arm()1045310454def fettec_assert_bad_mask(self, mask):10455'''assert the mask is bad for fettec driver'''10456self.start_subsubtest("Checking mask (%s) is bad" % (mask,))10457self.context_push()10458self.set_parameter("SERVO_FTW_MASK", mask)10459self.reboot_sitl()10460self.delay_sim_time(12) # allow accels/gyros to be happy10461tstart = self.get_sim_time()10462while True:10463if self.get_sim_time_cached() - tstart > 20:10464raise NotAchievedException("Expected mask to be only problem within 20 seconds")10465try:10466self.assert_prearm_failure("Invalid motor mask")10467break10468except NotAchievedException:10469self.delay_sim_time(1)10470self.context_pop()10471self.reboot_sitl()1047210473def fettec_assert_good_mask(self, mask):10474'''assert the mask is bad for fettec driver'''10475self.start_subsubtest("Checking mask (%s) is good" % (mask,))10476self.context_push()10477self.set_parameter("SERVO_FTW_MASK", mask)10478self.reboot_sitl()10479self.wait_ready_to_arm()10480self.context_pop()10481self.reboot_sitl()1048210483def FETtecESC_safety_switch(self):10484mot = self.find_first_set_bit(int(self.get_parameter("SERVO_FTW_MASK"))) + 110485self.wait_esc_telem_rpm(mot, 0, 0)10486self.wait_ready_to_arm()10487self.context_push()10488self.set_parameter("DISARM_DELAY", 0)10489self.arm_vehicle()10490# we have to wait for a while for the arming tone to go out10491# before the motors will spin:10492self.wait_esc_telem_rpm(10493esc=mot,10494rpm_min=17640,10495rpm_max=17640,10496minimum_duration=2,10497timeout=5,10498)10499self.set_safetyswitch_on()10500self.wait_esc_telem_rpm(mot, 0, 0)10501self.set_safetyswitch_off()10502self.wait_esc_telem_rpm(10503esc=mot,10504rpm_min=17640,10505rpm_max=17640,10506minimum_duration=2,10507timeout=5,10508)10509self.context_pop()10510self.wait_disarmed()1051110512def FETtecESC_btw_mask_checks(self):10513'''ensure prearm checks work as expected'''10514for bad_mask in [0b1000000000000000, 0b10100000000000000]:10515self.fettec_assert_bad_mask(bad_mask)10516for good_mask in [0b00001, 0b00101, 0b110000000000]:10517self.fettec_assert_good_mask(good_mask)1051810519def FETtecESC(self):10520'''Test FETtecESC'''10521self.set_parameters({10522"SERIAL5_PROTOCOL": 38,10523"SERVO_FTW_MASK": 0b11101000,10524"SIM_FTOWESC_ENA": 1,10525"SERVO1_FUNCTION": 0,10526"SERVO2_FUNCTION": 0,10527"SERVO3_FUNCTION": 0,10528"SERVO4_FUNCTION": 33,10529"SERVO5_FUNCTION": 0,10530"SERVO6_FUNCTION": 34,10531"SERVO7_FUNCTION": 35,10532"SERVO8_FUNCTION": 36,10533"SIM_ESC_TELEM": 0,10534})10535self.customise_SITL_commandline(["--serial5=sim:fetteconewireesc"])10536self.FETtecESC_safety_switch()10537self.FETtecESC_esc_power_checks()10538self.FETtecESC_btw_mask_checks()10539self.FETtecESC_flight()1054010541def PerfInfo(self):10542'''Test Scheduler PerfInfo output'''10543self.set_parameter('SCHED_OPTIONS', 1) # enable gathering10544# sometimes we need to trigger collection....10545content = self.fetch_file_via_ftp("@SYS/tasks.txt")10546self.delay_sim_time(5)10547content = self.fetch_file_via_ftp("@SYS/tasks.txt")10548self.progress("Got content (%s)" % str(content))1054910550lines = content.split("\n")1055110552if not lines[0].startswith("TasksV2"):10553raise NotAchievedException("Expected TasksV2 as first line first not (%s)" % lines[0])10554# last line is empty, so -2 here10555if not lines[-2].startswith("AP_Vehicle::update_arming"):10556raise NotAchievedException("Expected EFI last not (%s)" % lines[-2])1055710558def RTL_TO_RALLY(self, target_system=1, target_component=1):10559'''Check RTL to rally point'''10560self.wait_ready_to_arm()10561rally_loc = self.home_relative_loc_ne(50, -25)10562rally_alt = 3710563items = [10564self.mav.mav.mission_item_int_encode(10565target_system,10566target_component,105670, # seq10568mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,10569mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,105700, # current105710, # autocontinue105720, # p1105730, # p2105740, # p3105750, # p410576int(rally_loc.lat * 1e7), # latitude10577int(rally_loc.lng * 1e7), # longitude10578rally_alt, # altitude10579mavutil.mavlink.MAV_MISSION_TYPE_RALLY),10580]10581self.upload_using_mission_protocol(10582mavutil.mavlink.MAV_MISSION_TYPE_RALLY,10583items10584)10585self.set_parameters({10586'RALLY_INCL_HOME': 0,10587})10588self.takeoff(10)10589self.change_mode('RTL')10590self.wait_location(rally_loc)10591self.assert_altitude(rally_alt, relative=True)10592self.progress("Ensuring we're descending")10593self.wait_altitude(20, 25, relative=True)10594self.change_mode('LOITER')10595self.progress("Flying home")10596self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)10597self.change_mode('RTL')10598self.wait_disarmed()10599self.assert_at_home()1060010601def NoRCOnBootPreArmFailure(self):10602'''Ensure we can't arm with no RC on boot if THR_FS_VALUE set'''10603self.context_push()10604for rc_failure_mode in 1, 2:10605self.set_parameters({10606"SIM_RC_FAIL": rc_failure_mode,10607})10608self.reboot_sitl()10609if rc_failure_mode == 1:10610self.assert_prearm_failure("RC not found",10611other_prearm_failures_fatal=False)10612elif rc_failure_mode == 2:10613self.assert_prearm_failure("Throttle below failsafe",10614other_prearm_failures_fatal=False)10615self.context_pop()10616self.reboot_sitl()1061710618def IMUConsistency(self):10619'''test IMUs must be consistent with one another'''10620self.wait_ready_to_arm()1062110622self.start_subsubtest("prearm checks for accel inconsistency")10623self.context_push()10624self.set_parameters({10625"SIM_ACC1_BIAS_X": 5,10626})10627self.assert_prearm_failure("Accels inconsistent")10628self.context_pop()10629tstart = self.get_sim_time()10630self.wait_ready_to_arm()10631if self.get_sim_time() - tstart < 8:10632raise NotAchievedException("Should take 10 seconds to become armableafter IMU upset")1063310634self.start_subsubtest("prearm checks for gyro inconsistency")10635self.context_push()10636self.set_parameters({10637"SIM_GYR1_BIAS_X": math.radians(10),10638})10639self.assert_prearm_failure("Gyros inconsistent")10640self.context_pop()10641tstart = self.get_sim_time()10642self.wait_ready_to_arm()10643if self.get_sim_time() - tstart < 8:10644raise NotAchievedException("Should take 10 seconds to become armableafter IMU upset")1064510646def Sprayer(self):10647"""Test sprayer functionality."""10648self.context_push()1064910650rc_ch = 910651pump_ch = 510652spinner_ch = 610653pump_ch_min = 105010654pump_ch_trim = 152010655pump_ch_max = 195010656spinner_ch_min = 97510657spinner_ch_trim = 151010658spinner_ch_max = 19751065910660self.set_parameters({10661"SPRAY_ENABLE": 1,1066210663"SERVO%u_FUNCTION" % pump_ch: 22,10664"SERVO%u_MIN" % pump_ch: pump_ch_min,10665"SERVO%u_TRIM" % pump_ch: pump_ch_trim,10666"SERVO%u_MAX" % pump_ch: pump_ch_max,1066710668"SERVO%u_FUNCTION" % spinner_ch: 23,10669"SERVO%u_MIN" % spinner_ch: spinner_ch_min,10670"SERVO%u_TRIM" % spinner_ch: spinner_ch_trim,10671"SERVO%u_MAX" % spinner_ch: spinner_ch_max,1067210673"SIM_SPR_ENABLE": 1,10674"SIM_SPR_PUMP": pump_ch,10675"SIM_SPR_SPIN": spinner_ch,1067610677"RC%u_OPTION" % rc_ch: 15,10678"LOG_DISARMED": 1,10679})1068010681self.reboot_sitl()1068210683self.wait_ready_to_arm()10684self.arm_vehicle()1068510686self.progress("test bootup state - it's zero-output!")10687self.wait_servo_channel_value(spinner_ch, 0)10688self.wait_servo_channel_value(pump_ch, 0)1068910690self.progress("Enable sprayer")10691self.set_rc(rc_ch, 2000)1069210693self.progress("Testing zero-speed state")10694self.wait_servo_channel_value(spinner_ch, spinner_ch_min)10695self.wait_servo_channel_value(pump_ch, pump_ch_min)1069610697self.progress("Testing turning it off")10698self.set_rc(rc_ch, 1000)10699self.wait_servo_channel_value(spinner_ch, spinner_ch_min)10700self.wait_servo_channel_value(pump_ch, pump_ch_min)1070110702self.progress("Testing turning it back on")10703self.set_rc(rc_ch, 2000)10704self.wait_servo_channel_value(spinner_ch, spinner_ch_min)10705self.wait_servo_channel_value(pump_ch, pump_ch_min)1070610707self.takeoff(30, mode='LOITER')1070810709self.progress("Testing speed-ramping")10710self.set_rc(1, 1700) # start driving forward1071110712# this is somewhat empirical...10713self.wait_servo_channel_value(10714pump_ch,107151458,10716timeout=60,10717comparator=lambda x, y : abs(x-y) < 510718)1071910720self.progress("Turning it off again")10721self.set_rc(rc_ch, 1000)10722self.wait_servo_channel_value(spinner_ch, spinner_ch_min)10723self.wait_servo_channel_value(pump_ch, pump_ch_min)1072410725self.start_subtest("Checking mavlink commands")10726self.progress("Starting Sprayer")10727self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1)1072810729self.progress("Testing speed-ramping")10730self.wait_servo_channel_value(10731pump_ch,107321458,10733timeout=60,10734comparator=lambda x, y : abs(x-y) < 510735)1073610737self.start_subtest("Stopping Sprayer")10738self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0)1073910740self.wait_servo_channel_value(pump_ch, pump_ch_min)1074110742self.disarm_vehicle(force=True)1074310744self.context_pop()1074510746self.reboot_sitl()1074710748self.progress("Sprayer OK")1074910750def tests1a(self):10751'''return list of all tests'''10752ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/vehicle_test_suite.py10753ret.extend([10754self.NavDelayTakeoffAbsTime,10755self.NavDelayAbsTime,10756self.NavDelay,10757self.GuidedSubModeChange,10758self.MAV_CMD_CONDITION_YAW,10759self.LoiterToAlt,10760self.PayloadPlaceMission,10761self.PrecisionLoiterCompanion,10762self.Landing,10763self.PrecisionLanding,10764self.SetModesViaModeSwitch,10765self.SetModesViaAuxSwitch,10766self.AuxSwitchOptions,10767self.AuxFunctionsInMission,10768self.AutoTune,10769self.AutoTuneYawD,10770self.NoRCOnBootPreArmFailure,10771])10772return ret1077310774def tests1b(self):10775'''return list of all tests'''10776ret = ([10777self.ThrowMode,10778self.BrakeMode,10779self.RecordThenPlayMission,10780self.ThrottleFailsafe,10781self.ThrottleFailsafePassthrough,10782self.GCSFailsafe,10783self.CustomController,10784])10785return ret1078610787def tests1c(self):10788'''return list of all tests'''10789ret = ([10790self.BatteryFailsafe,10791self.BatteryMissing,10792self.VibrationFailsafe,10793self.EK3AccelBias,10794self.StabilityPatch,10795self.OBSTACLE_DISTANCE_3D,10796self.AC_Avoidance_Proximity,10797self.AC_Avoidance_Proximity_AVOID_ALT_MIN,10798self.AC_Avoidance_Fence,10799self.AC_Avoidance_Beacon,10800self.AvoidanceAltFence,10801self.BaroWindCorrection,10802self.SetpointGlobalPos,10803self.ThrowDoubleDrop,10804self.SetpointGlobalVel,10805self.SetpointBadVel,10806self.SplineTerrain,10807self.TakeoffCheck,10808self.GainBackoffTakeoff,10809])10810return ret1081110812def tests1d(self):10813'''return list of all tests'''10814ret = ([10815self.HorizontalFence,10816self.HorizontalAvoidFence,10817self.MaxAltFence,10818self.MaxAltFenceAvoid,10819self.MinAltFence,10820self.MinAltFenceAvoid,10821self.FenceFloorEnabledLanding,10822self.FenceFloorAutoDisableLanding,10823self.FenceFloorAutoEnableOnArming,10824self.AutoTuneSwitch,10825self.GPSGlitchLoiter,10826self.GPSGlitchLoiter2,10827self.GPSGlitchAuto,10828self.ModeAltHold,10829self.ModeLoiter,10830self.SimpleMode,10831self.SuperSimpleCircle,10832self.ModeCircle,10833self.MagFail,10834self.OpticalFlow,10835self.OpticalFlowLocation,10836self.OpticalFlowLimits,10837self.OpticalFlowCalibration,10838self.MotorFail,10839self.ModeFlip,10840self.CopterMission,10841self.TakeoffAlt,10842self.SplineLastWaypoint,10843self.Gripper,10844self.TestLocalHomePosition,10845self.TestGripperMission,10846self.VisionPosition,10847self.ATTITUDE_FAST,10848self.BaseLoggingRates,10849self.BodyFrameOdom,10850self.GPSViconSwitching,10851])10852return ret1085310854def tests1e(self):10855'''return list of all tests'''10856ret = ([10857self.BeaconPosition,10858self.RTLSpeed,10859self.Mount,10860self.MountYawVehicleForMountROI,10861self.MAV_CMD_DO_MOUNT_CONTROL,10862self.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,10863self.AutoYawDO_MOUNT_CONTROL,10864self.Button,10865self.ShipTakeoff,10866self.RangeFinder,10867self.BaroDrivers,10868self.SurfaceTracking,10869self.Parachute,10870self.ParameterChecks,10871self.ManualThrottleModeChange,10872self.MANUAL_CONTROL,10873self.ModeZigZag,10874self.PosHoldTakeOff,10875self.ModeFollow,10876self.RangeFinderDrivers,10877self.RangeFinderDriversMaxAlt,10878self.MaxBotixI2CXL,10879self.MAVProximity,10880self.ParameterValidation,10881self.AltTypes,10882self.PAUSE_CONTINUE,10883self.PAUSE_CONTINUE_GUIDED,10884self.RichenPower,10885self.IE24,10886self.MAVLandedStateTakeoff,10887self.Weathervane,10888self.MAV_CMD_AIRFRAME_CONFIGURATION,10889self.MAV_CMD_NAV_LOITER_UNLIM,10890self.MAV_CMD_NAV_RETURN_TO_LAUNCH,10891self.MAV_CMD_NAV_VTOL_LAND,10892self.clear_roi,10893])10894return ret1089510896def tests2a(self):10897'''return list of all tests'''10898ret = ([10899# something about SITLCompassCalibration appears to fail10900# this one, so we put it first:10901self.FixedYawCalibration,1090210903# we run this single 8min-and-40s test on its own, apart10904# from requiring FixedYawCalibration right before it10905# because without it, it fails to calibrate this10906# autotest appears to interfere with10907# FixedYawCalibration, no idea why.10908self.SITLCompassCalibration,10909])10910return ret1091110912def ScriptMountPOI(self):10913'''test the MountPOI example script'''10914self.context_push()1091510916self.install_terrain_handlers_context()10917self.set_parameters({10918"SCR_ENABLE": 1,10919"RC12_OPTION": 300,10920})10921self.setup_servo_mount()10922self.reboot_sitl()10923self.set_rc(6, 1300)10924self.install_applet_script_context('mount-poi.lua')10925self.reboot_sitl()10926self.wait_ready_to_arm()10927self.context_collect('STATUSTEXT')10928self.set_rc(12, 2000)10929self.wait_statustext('POI.*-35.*149', check_context=True, regex=True)10930self.set_rc(12, 1000)1093110932self.context_pop()10933self.reboot_sitl()1093410935def ScriptCopterPosOffsets(self):10936'''test the copter-posoffset.lua example script'''10937self.context_push()1093810939# enable scripting and arming/takingoff in Auto mode10940self.set_parameters({10941"SCR_ENABLE": 1,10942"AUTO_OPTIONS": 3,10943"RC12_OPTION": 30010944})10945self.reboot_sitl()1094610947# install copter-posoffset script10948self.install_example_script_context('copter-posoffset.lua')10949self.reboot_sitl()1095010951# create simple mission with a single takeoff command10952self.upload_simple_relhome_mission([10953(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20)10954])1095510956# switch to loiter to wait for position estimate (aka GPS lock)10957self.change_mode('LOITER')10958self.wait_ready_to_arm()1095910960# arm and takeoff in Auto mode10961self.change_mode('AUTO')10962self.arm_vehicle()1096310964# wait for vehicle to climb to at least 10m10965self.wait_altitude(8, 12, relative=True)1096610967# add position offset to East and confirm vehicle moves10968self.set_parameter("PSC_OFS_POS_E", 20)10969self.set_rc(12, 2000)10970self.wait_distance(18)1097110972# remove position offset and wait for vehicle to return home10973self.set_parameter("PSC_OFS_POS_E", 0)10974self.wait_distance_to_home(distance_min=0, distance_max=4, timeout=20)1097510976# add velocity offset and confirm vehicle moves10977self.set_parameter("PSC_OFS_VEL_N", 5)10978self.wait_groundspeed(4.8, 5.2, minimum_duration=5, timeout=20)1097910980# remove velocity offset and switch to RTL10981self.set_parameter("PSC_OFS_VEL_N", 0)10982self.set_rc(12, 1000)10983self.do_RTL()10984self.context_pop()10985self.reboot_sitl()1098610987def AHRSTrimLand(self):10988'''test land detector with significant AHRS trim'''10989self.context_push()10990self.set_parameters({10991"SIM_ACC_TRIM_X": 0.12,10992"AHRS_TRIM_X": 0.12,10993})10994self.reboot_sitl()10995self.wait_ready_to_arm()10996self.takeoff(alt_min=20, mode='LOITER')10997self.do_RTL()10998self.context_pop()10999self.reboot_sitl()1100011001def GainBackoffTakeoff(self):11002'''test gain backoff on takeoff'''11003self.context_push()11004self.progress("Test gains are fully backed off")11005self.set_parameters({11006"ATC_LAND_R_MULT": 0.0,11007"ATC_LAND_P_MULT": 0.0,11008"ATC_LAND_Y_MULT": 0.0,11009"GCS_PID_MASK" : 7,11010"LOG_BITMASK": 180222,11011})11012self.reboot_sitl()11013self.wait_ready_to_arm()11014self.change_mode('ALT_HOLD')1101511016class ValidatePDZero(vehicle_test_suite.TestSuite.MessageHook):11017'''asserts correct values in PID_TUNING'''1101811019def __init__(self, suite, axis):11020super(ValidatePDZero, self).__init__(suite)11021self.pid_tuning_count = 011022self.p_sum = 011023self.d_sum = 011024self.i_sum = 011025self.axis = axis1102611027def hook_removed(self):11028if self.pid_tuning_count == 0:11029raise NotAchievedException("Did not get PID_TUNING")11030if self.i_sum == 0:11031raise ValueError("I sum is zero")11032print(f"ValidatePDZero: PID_TUNING count: {self.pid_tuning_count}")1103311034def process(self, mav, m):11035if m.get_type() != 'PID_TUNING' or m.axis != self.axis:11036return11037self.pid_tuning_count += 111038self.p_sum += m.P11039self.d_sum += m.D11040self.i_sum += m.I11041if self.p_sum > 0:11042raise ValueError("P sum is not zero")11043if self.d_sum > 0:11044raise ValueError("D sum is not zero")1104511046self.progress("Check that PD values are zero")11047self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_ROLL))11048self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_PITCH))11049self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_YAW))11050# until the context pop happens, all received PID_TUNINGS will be verified as good11051self.arm_vehicle()11052self.set_rc(3, 1500)11053self.delay_sim_time(2)11054self.set_rc(2, 1250)11055self.delay_sim_time(5)11056self.assert_receive_message('PID_TUNING', timeout=5)11057self.set_rc_default()11058self.zero_throttle()11059self.disarm_vehicle()11060self.context_pop()1106111062self.context_push()11063self.progress("Test gains are not backed off")11064self.set_parameters({11065"ATC_LAND_R_MULT": 1.0,11066"ATC_LAND_P_MULT": 1.0,11067"ATC_LAND_Y_MULT": 1.0,11068"GCS_PID_MASK" : 7,11069"LOG_BITMASK": 180222,11070})11071self.reboot_sitl()11072self.wait_ready_to_arm()11073self.change_mode('ALT_HOLD')1107411075class ValidatePDNonZero(vehicle_test_suite.TestSuite.MessageHook):11076'''asserts correct values in PID_TUNING'''1107711078def __init__(self, suite, axis):11079super(ValidatePDNonZero, self).__init__(suite)11080self.pid_tuning_count = 011081self.p_sum = 011082self.d_sum = 011083self.i_sum = 011084self.axis = axis1108511086def hook_removed(self):11087if self.pid_tuning_count == 0:11088raise NotAchievedException("Did not get PID_TUNING")11089if self.p_sum == 0:11090raise ValueError("P sum is zero")11091if self.i_sum == 0:11092raise ValueError("I sum is zero")11093print(f"ValidatePDNonZero: PID_TUNING count: {self.pid_tuning_count}")1109411095def process(self, mav, m):11096if m.get_type() != 'PID_TUNING' or m.axis != self.axis:11097return11098self.pid_tuning_count += 111099self.p_sum += m.P11100self.d_sum += m.D11101self.i_sum += m.I1110211103self.progress("Check that PD values are non-zero")11104self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_ROLL))11105self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_PITCH))11106self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_YAW))11107# until the context pop happens, all received PID_TUNINGS will be verified as good11108self.arm_vehicle()11109self.set_rc(3, 1500)11110self.delay_sim_time(2)11111self.set_rc(2, 1250)11112self.delay_sim_time(5)11113self.assert_receive_message('PID_TUNING', timeout=5)11114self.set_rc_default()11115self.zero_throttle()11116self.disarm_vehicle()1111711118self.context_pop()11119self.reboot_sitl()1112011121def turn_safety_x(self, value):11122self.mav.mav.set_mode_send(11123self.mav.target_system,11124mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY,11125value)1112611127def turn_safety_off(self):11128self.turn_safety_x(0)1112911130def turn_safety_on(self):11131self.turn_safety_x(1)1113211133def SafetySwitch(self):11134'''test safety switch behaviour'''11135self.wait_ready_to_arm()1113611137self.turn_safety_on()11138self.assert_prearm_failure("safety switch")1113911140self.turn_safety_off()11141self.wait_ready_to_arm()1114211143self.takeoff(2, mode='LOITER')11144self.turn_safety_on()1114511146self.wait_servo_channel_value(1, 0)11147self.turn_safety_off()1114811149self.change_mode('LAND')11150self.wait_disarmed()1115111152# test turning safty on/off using explicit MAVLink command:11153self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_SAFETY_SWITCH_STATE, mavutil.mavlink.SAFETY_SWITCH_STATE_SAFE)11154self.assert_prearm_failure("safety switch")11155self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_SAFETY_SWITCH_STATE, mavutil.mavlink.SAFETY_SWITCH_STATE_DANGEROUS)11156self.wait_ready_to_arm()1115711158def ArmSwitchAfterReboot(self):11159'''test that the arming switch does not trigger after a reboot'''11160self.wait_ready_to_arm()11161self.set_parameters({11162"RC8_OPTION": 153,11163})11164self.set_rc(8, 2000)11165self.wait_armed()11166self.disarm_vehicle()11167self.context_collect('STATUSTEXT')11168self.reboot_sitl()1116911170tstart = self.get_sim_time()11171while True:11172if self.get_sim_time_cached() - tstart > 60:11173break11174if self.armed():11175raise NotAchievedException("Armed after reboot with switch high")11176armmsg = self.statustext_in_collections('Arm: ')11177if armmsg is not None:11178raise NotAchievedException("statustext(%s) means we tried to arm" % armmsg.text)11179self.progress("Did not arm via arming switfch after a reboot")1118011181def GuidedYawRate(self):11182'''ensuer guided yaw rate is not affected by rate of sewt-attitude messages'''11183self.takeoff(30, mode='GUIDED')11184rates = {}11185for rate in 1, 10:11186# command huge yaw rate for a while11187tstart = self.get_sim_time()11188interval = 1/rate11189yawspeed_rads_sum = 011190yawspeed_rads_count = 011191last_sent = 011192while True:11193self.drain_mav()11194tnow = self.get_sim_time_cached()11195if tnow - last_sent > interval:11196self.do_yaw_rate(60) # this is... unlikely11197last_sent = tnow11198if tnow - tstart < 5: # let it spin up to speed first11199continue11200yawspeed_rads_sum += self.mav.messages['ATTITUDE'].yawspeed11201yawspeed_rads_count += 111202if tnow - tstart > 15: # 10 seconds of measurements11203break11204yawspeed_degs = math.degrees(yawspeed_rads_sum / yawspeed_rads_count)11205rates[rate] = yawspeed_degs11206self.progress("Input rate %u hz: average yaw rate %f deg/s" % (rate, yawspeed_degs))1120711208if rates[10] < rates[1] * 0.95:11209raise NotAchievedException("Guided yaw rate slower for higher rate updates")1121011211self.do_RTL()1121211213def test_rplidar(self, sim_device_name, expected_distances):11214'''plonks a Copter with a RPLidarA2 in the middle of a simulated field11215of posts and checks that the measurements are what we expect.'''11216self.set_parameters({11217"SERIAL5_PROTOCOL": 11,11218"PRX1_TYPE": 5,11219})11220self.customise_SITL_commandline([11221"--serial5=sim:%s:" % sim_device_name,11222"--home", "51.8752066,14.6487840,0,0", # SITL has "posts" here11223])1122411225self.wait_ready_to_arm()1122611227wanting_distances = copy.copy(expected_distances)11228tstart = self.get_sim_time()11229timeout = 6011230while True:11231now = self.get_sim_time_cached()11232if now - tstart > timeout:11233raise NotAchievedException("Did not get all distances")11234m = self.mav.recv_match(type="DISTANCE_SENSOR",11235blocking=True,11236timeout=1)11237if m is None:11238continue11239self.progress("Got (%s)" % str(m))11240if m.orientation not in wanting_distances:11241continue11242if abs(m.current_distance - wanting_distances[m.orientation]) > 5:11243self.progress("Wrong distance orient=%u want=%u got=%u" %11244(m.orientation,11245wanting_distances[m.orientation],11246m.current_distance))11247continue11248self.progress("Correct distance for orient %u (want=%u got=%u)" %11249(m.orientation,11250wanting_distances[m.orientation],11251m.current_distance))11252del wanting_distances[m.orientation]11253if len(wanting_distances.items()) == 0:11254break1125511256def RPLidarA2(self):11257'''test Raspberry Pi Lidar A2'''11258expected_distances = {11259mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276,11260mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,11261mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,11262mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1286,11263mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,11264mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 971,11265mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,11266mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792,11267}1126811269self.test_rplidar("rplidara2", expected_distances)1127011271def RPLidarA1(self):11272'''test Raspberry Pi Lidar A1'''11273return # we don't send distances when too long?11274expected_distances = {11275mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276,11276mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,11277mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 800,11278mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 800,11279mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,11280mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 800,11281mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,11282mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792,11283}1128411285self.test_rplidar("rplidara1", expected_distances)1128611287def BrakeZ(self):11288'''check jerk limit correct in Brake mode'''11289self.set_parameter('PSC_JERK_Z', 3)11290self.takeoff(50, mode='GUIDED')11291vx, vy, vz_up = (0, 0, -1)11292self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10)1129311294self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10)11295self.change_mode('BRAKE')11296self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)11297self.land_and_disarm()1129811299def MISSION_START(self):11300'''test mavlink command MAV_CMD_MISSION_START'''11301self.upload_simple_relhome_mission([11302(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 200),11303(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),11304])11305for command in self.run_cmd, self.run_cmd_int:11306self.change_mode('LOITER')11307self.set_current_waypoint(1)11308self.wait_ready_to_arm()11309self.arm_vehicle()11310self.change_mode('AUTO')11311command(mavutil.mavlink.MAV_CMD_MISSION_START)11312self.wait_altitude(20, 1000, relative=True)11313self.change_mode('RTL')11314self.wait_disarmed()1131511316def DO_CHANGE_SPEED_in_guided(self):11317'''test Copter DO_CHANGE_SPEED handling in guided mode'''11318self.takeoff(20, mode='GUIDED')1131911320new_loc = self.mav.location()11321new_loc_offset_n = 200011322new_loc_offset_e = 011323self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e)1132411325second_loc_offset_n = -100011326second_loc_offset_e = 011327second_loc = self.mav.location()11328self.location_offset_ne(second_loc, second_loc_offset_n, second_loc_offset_e)1132911330# for run_cmd we fly away from home11331for (tloc, command) in (new_loc, self.run_cmd), (second_loc, self.run_cmd_int):11332self.run_cmd_int(11333mavutil.mavlink.MAV_CMD_DO_REPOSITION,11334p1=-1, # "default"11335p2=0, # flags; none supplied here11336p3=0, # loiter radius for planes, zero ignored11337p4=float("nan"), # nan means do whatever you want to do11338p5=int(tloc.lat * 1e7),11339p6=int(tloc.lng * 1e7),11340p7=tloc.alt,11341frame=mavutil.mavlink.MAV_FRAME_GLOBAL,11342)11343for speed in [2, 10, 4]:11344command(11345mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,11346p1=1, # groundspeed,11347p2=speed,11348p3=-1, # throttle, -1 is no-change11349p4=0, # absolute value, not relative11350)11351self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=10, timeout=20)1135211353# we've made random changes to vehicle guided speeds above;11354# reboot vehicle to reset those:11355self.disarm_vehicle(force=True)11356self.reboot_sitl()1135711358def _MAV_CMD_DO_FLIGHTTERMINATION(self, command):11359self.set_parameters({11360"SYSID_MYGCS": self.mav.source_system,11361"DISARM_DELAY": 0,11362})11363self.wait_ready_to_arm()11364self.arm_vehicle()11365self.context_collect('STATUSTEXT')11366command(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, p1=1)11367self.wait_disarmed()11368self.reboot_sitl()1136911370def MAV_CMD_DO_FLIGHTTERMINATION(self):11371'''test MAV_CMD_DO_FLIGHTTERMINATION works on Copter'''11372self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd)11373self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd_int)1137411375def MAV_CMD_NAV_LOITER_UNLIM(self):11376'''ensure MAV_CMD_NAV_LOITER_UNLIM via mavlink works'''11377for command in self.run_cmd, self.run_cmd_int:11378self.change_mode('STABILIZE')11379command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)11380self.wait_mode('LOITER')1138111382def MAV_CMD_NAV_RETURN_TO_LAUNCH(self):11383'''ensure MAV_CMD_NAV_RETURN_TO_LAUNCH via mavlink works'''11384for command in self.run_cmd, self.run_cmd_int:11385self.change_mode('STABILIZE')11386command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH)11387self.wait_mode('RTL')1138811389def MAV_CMD_NAV_VTOL_LAND(self):11390'''ensure MAV_CMD_NAV_LAND via mavlink works'''11391for command in self.run_cmd, self.run_cmd_int:11392self.change_mode('STABILIZE')11393command(mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND)11394self.wait_mode('LAND')11395self.change_mode('STABILIZE')11396command(mavutil.mavlink.MAV_CMD_NAV_LAND)11397self.wait_mode('LAND')1139811399def clear_roi(self):11400'''ensure three commands that clear ROI are equivalent'''1140111402self.upload_simple_relhome_mission([11403(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),11404(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20),11405(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 20), # directly North, i.e. 0 degrees11406(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, 20), # directly North, i.e. 0 degrees11407])1140811409self.set_parameter("AUTO_OPTIONS", 3)11410self.change_mode('AUTO')11411self.wait_ready_to_arm()11412self.arm_vehicle()11413home_loc = self.mav.location()1141411415cmd_ids = [11416mavutil.mavlink.MAV_CMD_DO_SET_ROI,11417mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,11418mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE,11419]11420for command in self.run_cmd, self.run_cmd_int:11421for cmd_id in cmd_ids:11422self.wait_waypoint(2, 2)1142311424# Set an ROI at the Home location, expect to point at Home11425self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, p5=home_loc.lat, p6=home_loc.lng, p7=home_loc.alt)11426self.wait_heading(180)1142711428# Clear the ROI, expect to point at the next Waypoint11429self.progress("Clear ROI using %s(%d)" % (command.__name__, cmd_id))11430command(cmd_id)11431self.wait_heading(0)1143211433self.wait_waypoint(4, 4)11434self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=2)1143511436self.land_and_disarm()1143711438def start_flying_simple_rehome_mission(self, items):11439'''uploads items, changes mode to auto, waits ready to arm and arms11440vehicle. If the first item it a takeoff you can expect the11441vehicle to fly after this method returns11442'''1144311444self.upload_simple_relhome_mission(items)1144511446self.set_parameter("AUTO_OPTIONS", 3)11447self.change_mode('AUTO')11448self.wait_ready_to_arm()1144911450self.arm_vehicle()1145111452def _MAV_CMD_DO_LAND_START(self, run_cmd):11453alt = 511454self.start_flying_simple_rehome_mission([11455(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),11456(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, alt),11457(mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0),11458(mavutil.mavlink.MAV_CMD_DO_LAND_START, 0, 0, alt),11459(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 2000, alt),11460(mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0),11461])1146211463self.wait_current_waypoint(2)11464run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START)11465self.wait_current_waypoint(5)11466# we pretend to be in RTL mode while doing this:11467self.wait_mode("AUTO_RTL")11468self.do_RTL()1146911470def MAV_CMD_DO_LAND_START(self):11471'''test handling of mavlink-received MAV_CMD_DO_LAND_START command'''11472self._MAV_CMD_DO_LAND_START(self.run_cmd)11473self.zero_throttle()11474self._MAV_CMD_DO_LAND_START(self.run_cmd_int)1147511476def _MAV_CMD_SET_EKF_SOURCE_SET(self, run_cmd):11477run_cmd(11478mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET,1147917,11480want_result=mavutil.mavlink.MAV_RESULT_DENIED,11481)1148211483self.change_mode('LOITER')11484self.wait_ready_to_arm()1148511486run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 2)1148711488self.assert_prearm_failure('Need Position Estimate')11489run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 1)1149011491self.wait_ready_to_arm()1149211493def MAV_CMD_SET_EKF_SOURCE_SET(self):11494'''test setting of source sets using mavlink command'''11495self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd)11496self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd_int)1149711498def MAV_CMD_NAV_TAKEOFF(self):11499'''test issuing takeoff command via mavlink'''11500self.change_mode('GUIDED')11501self.wait_ready_to_arm()1150211503self.arm_vehicle()11504self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=5)11505self.wait_altitude(4.5, 5.5, minimum_duration=5, relative=True)11506self.change_mode('LAND')11507self.wait_disarmed()1150811509self.start_subtest("Check NAV_TAKEOFF is above home location, not current location")11510# reset home 20 metres above current location11511current_alt_abs = self.get_altitude(relative=False)1151211513loc = self.mav.location()1151411515home_z_ofs = 2011516self.run_cmd(11517mavutil.mavlink.MAV_CMD_DO_SET_HOME,11518p5=loc.lat,11519p6=loc.lng,11520p7=current_alt_abs + home_z_ofs,11521)1152211523self.change_mode('GUIDED')11524self.arm_vehicle()11525takeoff_alt = 511526self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=takeoff_alt)11527self.wait_altitude(11528current_alt_abs + home_z_ofs + takeoff_alt - 0.5,11529current_alt_abs + home_z_ofs + takeoff_alt + 0.5,11530minimum_duration=5,11531relative=False,11532)11533self.change_mode('LAND')11534self.wait_disarmed()1153511536self.reboot_sitl() # unlock home position1153711538def MAV_CMD_NAV_TAKEOFF_command_int(self):11539'''test issuing takeoff command via mavlink and command_int'''11540self.change_mode('GUIDED')11541self.wait_ready_to_arm()1154211543self.start_subtest("Check NAV_TAKEOFF is above home location, not current location")11544# reset home 20 metres above current location11545current_alt_abs = self.get_altitude(relative=False)1154611547loc = self.mav.location()1154811549home_z_ofs = 2011550self.run_cmd(11551mavutil.mavlink.MAV_CMD_DO_SET_HOME,11552p5=loc.lat,11553p6=loc.lng,11554p7=current_alt_abs + home_z_ofs,11555)1155611557self.change_mode('GUIDED')11558self.arm_vehicle()11559takeoff_alt = 511560self.run_cmd_int(11561mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,11562p7=takeoff_alt,11563frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,11564)11565self.wait_altitude(11566current_alt_abs + home_z_ofs + takeoff_alt - 0.5,11567current_alt_abs + home_z_ofs + takeoff_alt + 0.5,11568minimum_duration=5,11569relative=False,11570)11571self.change_mode('LAND')11572self.wait_disarmed()1157311574self.reboot_sitl() # unlock home position1157511576def Ch6TuningWPSpeed(self):11577'''test waypoint speed can be changed via Ch6 tuning knob'''11578self.set_parameters({11579"RC6_OPTION": 219, # RC6 used for tuning11580"TUNE": 10, # 10 is waypoint speed11581"TUNE_MIN": 0.02, # 20cm/s11582"TUNE_MAX": 1000, # 10m/s11583"AUTO_OPTIONS": 3,11584})11585self.set_rc(6, 2000)1158611587self.upload_simple_relhome_mission([11588(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),11589(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 20),11590(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),11591])11592self.change_mode('AUTO')1159311594self.wait_ready_to_arm()1159511596self.arm_vehicle()1159711598self.wait_groundspeed(9.5, 10.5, minimum_duration=5)1159911600self.set_rc(6, 1500)11601self.wait_groundspeed(4.5, 5.5, minimum_duration=5)1160211603self.set_rc(6, 2000)11604self.wait_groundspeed(9.5, 10.5, minimum_duration=5)1160511606self.set_rc(6, 1300)11607self.wait_groundspeed(2.5, 3.5, minimum_duration=5)1160811609self.do_RTL()1161011611def PILOT_THR_BHV(self):11612'''test the PILOT_THR_BHV parameter'''11613self.start_subtest("Test default behaviour, no disarm on land")11614self.set_parameters({11615"DISARM_DELAY": 0,11616})11617self.takeoff(2, mode='GUIDED')11618self.set_rc(3, 1500)11619self.change_mode('LOITER')11620self.set_rc(3, 1300)1162111622maintain_armed = WaitAndMaintainArmed(self, minimum_duration=20)11623maintain_armed.run()1162411625self.start_subtest("Test THR_BEHAVE_DISARM_ON_LAND_DETECT")11626self.set_parameters({11627"PILOT_THR_BHV": 4, # Disarm on land detection11628})11629self.zero_throttle()11630self.takeoff(2, mode='GUIDED')11631self.set_rc(3, 1500)11632self.change_mode('LOITER')11633self.set_rc(3, 1300)1163411635self.wait_disarmed()1163611637def CameraLogMessages(self):11638'''ensure Camera log messages are good'''11639self.set_parameter("RC12_OPTION", 9) # CameraTrigger11640self.set_parameter("CAM1_TYPE", 1) # Camera with servo trigger11641self.reboot_sitl() # needed for RC12_OPTION to take effect1164211643gpis = []11644gps_raws = []1164511646self.takeoff(10, mode='GUIDED')11647self.set_rc(12, 2000)11648gpis.append(self.assert_receive_message('GLOBAL_POSITION_INT'))11649gps_raws.append(self.assert_receive_message('GPS_RAW_INT'))11650self.set_rc(12, 1000)1165111652self.fly_guided_move_local(0, 0, 20)1165311654self.set_rc(12, 2000)11655gpis.append(self.assert_receive_message('GLOBAL_POSITION_INT'))11656gps_raws.append(self.assert_receive_message('GPS_RAW_INT'))11657self.set_rc(12, 1000)1165811659dfreader = self.dfreader_for_current_onboard_log()11660self.do_RTL()1166111662for i in range(len(gpis)):11663gpi = gpis[i]11664gps_raw = gps_raws[i]11665m = dfreader.recv_match(type="CAM")1166611667things = [11668["absalt", gpi.alt*0.001, m.Alt],11669["relalt", gpi.relative_alt*0.001, m.RelAlt],11670["gpsalt", gps_raw.alt*0.001, m.GPSAlt], # use GPS_RAW here?11671]11672for (name, want, got) in things:11673if abs(got - want) > 1:11674raise NotAchievedException(f"Incorrect {name} {want=} {got=}")11675self.progress(f"{name} {want=} {got=}")1167611677want = gpi.relative_alt*0.00111678got = m.RelAlt11679if abs(got - want) > 1:11680raise NotAchievedException(f"Incorrect relalt {want=} {got=}")1168111682def LoiterToGuidedHomeVSOrigin(self):11683'''test moving from guided to loiter mode when home is a different alt11684to origin'''11685self.set_parameters({11686"TERRAIN_ENABLE": 1,11687"SIM_TERRAIN": 1,11688})11689self.takeoff(10, mode='GUIDED')11690here = self.mav.location()11691self.set_home(here)11692self.change_mode('LOITER')11693self.wait_altitude(here.alt-1, here.alt+1, minimum_duration=10)11694self.disarm_vehicle(force=True)11695self.reboot_sitl() # to "unstick" home1169611697def GuidedModeThrust(self):11698'''test handling of option-bit-3, where mavlink commands are11699intrepreted as thrust not climb rate'''11700self.set_parameter('GUID_OPTIONS', 8)11701self.change_mode('GUIDED')11702self.wait_ready_to_arm()11703self.arm_vehicle()11704self.mav.mav.set_attitude_target_send(117050, # time_boot_ms117061, # target sysid117071, # target compid117080, # bitmask of things to ignore11709mavextra.euler_to_quat([0, 0, 0]), # att117100, # roll rate (rad/s)117110, # pitch rate (rad/s)117120, # yaw rate (rad/s)117130.511714) # thrust, 0 to 111715self.wait_altitude(0.5, 100, relative=True, timeout=10)11716self.do_RTL()1171711718def AutoRTL(self):11719'''Test Auto RTL mode using do land start and return path start mission items'''11720alt = 5011721guided_loc = self.home_relative_loc_ne(1000, 0)11722guided_loc.alt += alt1172311724# Arm, take off and fly to guided location11725self.takeoff(mode='GUIDED')11726self.fly_guided_move_to(guided_loc, timeout=240)1172711728# Try auto RTL mode, should fail with no mission11729try:11730self.change_mode('AUTO_RTL', timeout=10)11731raise NotAchievedException("Should not change mode with no mission")11732except WaitModeTimeout:11733pass11734except Exception as e:11735raise e1173611737# pymavlink does not understand the new return path command yet, at some point it will11738cmd_return_path_start = 188 # mavutil.mavlink.MAV_CMD_DO_RETURN_PATH_START1173911740# Do land start and do return path should both fail as commands too11741self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_FAILED)11742self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_FAILED)1174311744# Load mission with do land start11745self.upload_simple_relhome_mission([11746(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 111747(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 750, 0, alt), # 211748self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 311749(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, alt), # 411750(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 250, 0, alt), # 511751])1175211753# Return path should still fail11754self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_FAILED)1175511756# Do land start should jump to the waypoint following the item11757self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)11758self.drain_mav()11759self.assert_current_waypoint(4)1176011761# Back to guided location11762self.change_mode('GUIDED')11763self.fly_guided_move_to(guided_loc)1176411765# mode change to Auto RTL should do the same11766self.change_mode('AUTO_RTL')11767self.drain_mav()11768self.assert_current_waypoint(4)1176911770# Back to guided location11771self.change_mode('GUIDED')11772self.fly_guided_move_to(guided_loc)1177311774# Add a return path item11775self.upload_simple_relhome_mission([11776(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1250, 0, alt), # 111777self.create_MISSION_ITEM_INT(cmd_return_path_start), # 211778(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 900, 0, alt), # 311779(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 750, 0, alt), # 411780(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 550, 0, alt), # 511781(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, alt), # 611782self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 711783(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 250, 0, alt), # 811784(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -250, 0, alt), # 911785(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -500, 0, alt), # 1011786])1178711788# Return path should now work11789self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)11790self.drain_mav()11791self.assert_current_waypoint(3)1179211793# Back to guided location11794self.change_mode('GUIDED')11795self.fly_guided_move_to(guided_loc)1179611797# mode change to Auto RTL should join the return path11798self.change_mode('AUTO_RTL')11799self.drain_mav()11800self.assert_current_waypoint(3)1180111802# do land start should still work11803self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)11804self.drain_mav()11805self.assert_current_waypoint(8)1180611807# Move a bit closer in guided11808return_path_test = self.home_relative_loc_ne(600, 0)11809return_path_test.alt += alt11810self.change_mode('GUIDED')11811self.fly_guided_move_to(return_path_test, timeout=100)1181211813# check the mission is joined further along11814self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)11815self.drain_mav()11816self.assert_current_waypoint(5)1181711818# fly over home11819home = self.home_relative_loc_ne(0, 0)11820home.alt += alt11821self.change_mode('GUIDED')11822self.fly_guided_move_to(home, timeout=140)1182311824# Should never join return path after do land start11825self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)11826self.drain_mav()11827self.assert_current_waypoint(6)1182811829# Done11830self.land_and_disarm()1183111832def EK3_OGN_HGT_MASK(self):11833'''test baraometer-alt-compensation based on long-term GPS readings'''11834self.context_push()11835self.set_parameters({11836'EK3_OGN_HGT_MASK': 1, # compensate baro drift using GPS11837})11838self.reboot_sitl()1183911840expected_alt = 101184111842self.change_mode('GUIDED')11843self.wait_ready_to_arm()11844current_alt = self.get_altitude()1184511846expected_alt_abs = current_alt + expected_alt1184711848self.takeoff(expected_alt, mode='GUIDED')11849self.delay_sim_time(5)1185011851self.set_parameter("SIM_BARO_DRIFT", 0.01) # 1cm/second1185211853def check_altitude(mav, m):11854m_type = m.get_type()11855epsilon = 10 # in metres11856if m_type == 'GPS_RAW_INT':11857got_gps_alt = m.alt * 0.00111858if abs(expected_alt_abs - got_gps_alt) > epsilon:11859raise NotAchievedException(f"Bad GPS altitude (got={got_gps_alt} want={expected_alt_abs})")11860elif m_type == 'GLOBAL_POSITION_INT':11861got_canonical_alt = m.relative_alt * 0.00111862if abs(expected_alt - got_canonical_alt) > epsilon:11863raise NotAchievedException(f"Bad canonical altitude (got={got_canonical_alt} want={expected_alt})")1186411865self.install_message_hook_context(check_altitude)1186611867self.delay_sim_time(1500)1186811869self.disarm_vehicle(force=True)1187011871self.context_pop()1187211873self.reboot_sitl(force=True)1187411875def GuidedForceArm(self):11876'''ensure Guided acts appropriately when force-armed'''11877self.set_parameters({11878"EK3_SRC2_VELXY": 5,11879"SIM_GPS1_ENABLE": 0,11880})11881self.load_default_params_file("copter-optflow.parm")11882self.reboot_sitl()11883self.delay_sim_time(30)11884self.change_mode('GUIDED')11885self.arm_vehicle(force=True)11886self.takeoff(20, mode='GUIDED')11887location = self.offset_location_ne(self.sim_location(), metres_north=0, metres_east=-300)11888self.progress("Ensure we don't move for 10 seconds")11889tstart = self.get_sim_time()11890startpos = self.sim_location_int()11891while True:11892now = self.get_sim_time_cached()11893if now - tstart > 10:11894break11895self.send_set_position_target_global_int(int(location.lat*1e7), int(location.lng*1e7), 10)11896dist = self.get_distance_int(startpos, self.sim_location_int())11897if dist > 10:11898raise NotAchievedException("Wandered too far from start position")11899self.delay_sim_time(1)1190011901self.disarm_vehicle(force=True)11902self.reboot_sitl()1190311904def EK3_OGN_HGT_MASK_climbing(self):11905'''check combination of height bits doesn't cause climb'''11906self.context_push()11907self.set_parameters({11908'EK3_OGN_HGT_MASK': 5,11909})11910self.reboot_sitl()1191111912expected_alt = 101191311914self.change_mode('GUIDED')11915self.wait_ready_to_arm()11916current_alt = self.get_altitude()1191711918expected_alt_abs = current_alt + expected_alt1191911920self.takeoff(expected_alt, mode='GUIDED')11921self.delay_sim_time(5)1192211923def check_altitude(mav, m):11924m_type = m.get_type()11925epsilon = 10 # in metres11926if m_type == 'GPS_RAW_INT':11927got_gps_alt = m.alt * 0.00111928if abs(expected_alt_abs - got_gps_alt) > epsilon:11929raise NotAchievedException(f"Bad GPS altitude (got={got_gps_alt} want={expected_alt_abs})")11930elif m_type == 'GLOBAL_POSITION_INT':11931if abs(expected_alt - m.relative_alt * 0.001) > epsilon:11932raise NotAchievedException("Bad canonical altitude")1193311934self.install_message_hook_context(check_altitude)1193511936self.delay_sim_time(1500)1193711938self.disarm_vehicle(force=True)1193911940self.context_pop()11941self.reboot_sitl(force=True)1194211943def GuidedWeatherVane(self):11944'''check Copter Guided mode weathervane option'''11945self.set_parameters({11946'SIM_WIND_SPD': 10,11947'SIM_WIND_DIR': 90,11948'WVANE_ENABLE': 1,11949})11950self.takeoff(20, mode='GUIDED')11951self.guided_achieve_heading(0)1195211953self.set_parameter("GUID_OPTIONS", 128)11954self.wait_heading(90, timeout=60, minimum_duration=10)11955self.do_RTL()1195611957def Clamp(self):11958'''test Copter docking clamp'''11959clamp_ch = 1111960self.set_parameters({11961"SIM_CLAMP_CH": clamp_ch,11962})1196311964self.takeoff(1, mode='LOITER')1196511966self.context_push()11967self.context_collect('STATUSTEXT')11968self.progress("Ensure can't take off with clamp in place")11969self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)11970self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)11971self.arm_vehicle()11972self.set_rc(3, 2000)11973self.wait_altitude(0, 5, minimum_duration=5, relative=True)11974self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000)11975self.wait_statustext("SITL: Clamp: released vehicle", check_context=True)11976self.wait_altitude(5, 5000, minimum_duration=1, relative=True)11977self.do_RTL()11978self.set_rc(3, 1000)11979self.change_mode('LOITER')11980self.context_pop()1198111982self.progress("Same again for repeatability")11983self.context_push()11984self.context_collect('STATUSTEXT')11985self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)11986self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)11987self.arm_vehicle()11988self.set_rc(3, 2000)11989self.wait_altitude(0, 1, minimum_duration=5, relative=True)11990self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000)11991self.wait_statustext("SITL: Clamp: released vehicle", check_context=True)11992self.wait_altitude(5, 5000, minimum_duration=1, relative=True)11993self.do_RTL()11994self.set_rc(3, 1000)11995self.change_mode('LOITER')11996self.context_pop()1199711998here = self.mav.location()11999loc = self.offset_location_ne(here, 10, 0)12000self.takeoff(5, mode='GUIDED')12001self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL)12002self.wait_location(loc, timeout=120)12003self.land_and_disarm()1200412005# explicitly set home so we RTL to the right spot12006self.set_home(here)1200712008self.context_push()12009self.context_collect('STATUSTEXT')12010self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)12011self.wait_statustext("SITL: Clamp: missed vehicle", check_context=True)12012self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000)12013self.context_pop()1201412015self.takeoff(5, mode='GUIDED')12016self.do_RTL()1201712018self.takeoff(5, mode='GUIDED')12019self.land_and_disarm()1202012021self.context_push()12022self.context_collect('STATUSTEXT')12023self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)12024self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)12025self.context_pop()1202612027self.reboot_sitl() # because we set home1202812029def GripperReleaseOnThrustLoss(self):12030'''tests that gripper is released on thrust loss if option set'''1203112032self.context_push()12033self.set_servo_gripper_parameters()12034self.reboot_sitl()1203512036self.takeoff(30, mode='LOITER')12037self.context_push()12038self.context_collect('STATUSTEXT')12039self.set_parameters({12040"SIM_ENGINE_MUL": 0.5,12041"SIM_ENGINE_FAIL": 1 << 1, # motor 212042"FLIGHT_OPTIONS": 4,12043})1204412045self.wait_statustext("Gripper Load Released", timeout=60)12046self.context_pop()1204712048self.do_RTL()12049self.context_pop()12050self.reboot_sitl()1205112052def assert_home_position_not_set(self):12053try:12054self.poll_home_position()12055except NotAchievedException:12056return1205712058# if home.lng != 0: etc1205912060raise NotAchievedException("Home is set when it shouldn't be")1206112062def REQUIRE_POSITION_FOR_ARMING(self):12063'''check FlightOption::REQUIRE_POSITION_FOR_ARMING works'''12064self.context_push()12065self.set_parameters({12066"SIM_GPS1_NUMSATS": 3, # EKF does not like < 612067})12068self.reboot_sitl()12069self.change_mode('STABILIZE')12070self.wait_prearm_sys_status_healthy()12071self.assert_home_position_not_set()12072self.arm_vehicle()12073self.disarm_vehicle()12074self.change_mode('LOITER')12075self.assert_prearm_failure("waiting for home", other_prearm_failures_fatal=False)1207612077self.change_mode('STABILIZE')12078self.set_parameters({12079"FLIGHT_OPTIONS": 8,12080})12081self.assert_prearm_failure("Need Position Estimate", other_prearm_failures_fatal=False)12082self.context_pop()12083self.reboot_sitl()1208412085def AutoContinueOnRCFailsafe(self):12086'''check LOITER when entered after RC failsafe is ignored in auto'''12087self.set_parameters({12088"FS_OPTIONS": 1, # 1 is "RC continue if in auto"12089})1209012091self.upload_simple_relhome_mission([12092# N E U12093(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),12094(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10),12095(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, 10),12096(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 120, 0, 10),12097])1209812099self.takeoff(mode='LOITER')12100self.set_rc(1, 1200)12101self.delay_sim_time(1) # build up some pilot desired stuff12102self.change_mode('AUTO')12103self.wait_waypoint(2, 2)12104self.set_parameters({12105'SIM_RC_FAIL': 1,12106})12107# self.set_rc(1, 1500) # note we are still in RC fail!12108self.wait_waypoint(3, 3)12109self.assert_mode_is('AUTO')12110self.change_mode('LOITER')12111self.wait_groundspeed(0, 0.1, minimum_duration=30, timeout=450)12112self.do_RTL()1211312114def MissionRTLYawBehaviour(self):12115'''check end-of-mission yaw behaviour'''12116self.set_parameters({12117"AUTO_OPTIONS": 3,12118})1211912120self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint-except-RTL")12121self.upload_simple_relhome_mission([12122# N E U12123(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),12124(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10),12125(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),12126])12127self.change_mode('AUTO')12128self.wait_ready_to_arm()12129original_heading = self.get_heading()12130if abs(original_heading) < 5:12131raise NotAchievedException(f"Bad original heading {original_heading}")12132self.arm_vehicle()12133self.wait_current_waypoint(3)12134self.wait_rtl_complete()12135self.wait_disarmed()12136if abs(self.get_heading()) > 5:12137raise NotAchievedException("Should have yaw zero without option")1213812139# must change out of auto and back in again to reset state machine:12140self.change_mode('LOITER')12141self.change_mode('AUTO')1214212143self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint")12144self.upload_simple_relhome_mission([12145# N E U12146(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),12147(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 20, 20),12148(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),12149])12150self.set_parameters({12151"WP_YAW_BEHAVIOR": 1, # look at next waypoint (including in RTL)12152})12153self.change_mode('AUTO')12154self.wait_ready_to_arm()12155original_heading = self.get_heading()12156if abs(original_heading) > 1:12157raise NotAchievedException("Bad original heading")12158self.arm_vehicle()12159self.wait_current_waypoint(3)12160self.wait_rtl_complete()12161self.wait_disarmed()12162new_heading = self.get_heading()12163if abs(new_heading - original_heading) > 5:12164raise NotAchievedException(f"Should return to original heading want={original_heading} got={new_heading}")1216512166def BatteryInternalUseOnly(self):12167'''batteries marked as internal use only should not appear over mavlink'''12168self.set_parameters({12169"BATT_MONITOR": 4, # 4 is analog volt+curr12170"BATT2_MONITOR": 4,12171})12172self.reboot_sitl()12173self.wait_message_field_values('BATTERY_STATUS', {12174"id": 0,12175})12176self.wait_message_field_values('BATTERY_STATUS', {12177"id": 1,12178})12179self.progress("Making battery private")12180self.set_parameters({12181"BATT_OPTIONS": 256,12182})12183self.wait_message_field_values('BATTERY_STATUS', {12184"id": 1,12185})12186for i in range(10):12187self.assert_received_message_field_values('BATTERY_STATUS', {12188"id": 112189})1219012191def MAV_CMD_MISSION_START_p1_p2(self):12192'''make sure we deny MAV_CMD_MISSION_START if either p1 or p2 non-zero'''12193self.upload_simple_relhome_mission([12194(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),12195])12196self.set_parameters({12197"AUTO_OPTIONS": 3,12198})12199self.change_mode('AUTO')12200self.wait_ready_to_arm()1220112202self.run_cmd(12203mavutil.mavlink.MAV_CMD_MISSION_START,12204p1=1,12205want_result=mavutil.mavlink.MAV_RESULT_DENIED,12206)1220712208self.run_cmd(12209mavutil.mavlink.MAV_CMD_MISSION_START,12210p2=1,12211want_result=mavutil.mavlink.MAV_RESULT_DENIED,12212)1221312214self.run_cmd(12215mavutil.mavlink.MAV_CMD_MISSION_START,12216p1=1,12217p2=1,12218want_result=mavutil.mavlink.MAV_RESULT_DENIED,12219)1222012221def ScriptingAHRSSource(self):12222'''test ahrs-source.lua script'''12223self.install_example_script_context("ahrs-source.lua")12224self.set_parameters({12225"RC10_OPTION": 90,12226"SCR_ENABLE": 1,12227"SCR_USER1": 10, # something else12228"SCR_USER2": 10, # GPS something12229"SCR_USER3": 0.2, # ExtNav innovation12230})12231self.set_rc(10, 2000)12232self.reboot_sitl()12233self.context_collect('STATUSTEXT')12234self.set_rc(10, 1000)12235self.wait_statustext('Using EKF Source Set 1', check_context=True)12236self.set_rc(10, 1500)12237self.wait_statustext('Using EKF Source Set 2', check_context=True)12238self.set_rc(10, 2000)12239self.wait_statustext('Using EKF Source Set 3', check_context=True)1224012241def CommonOrigin(self):12242"""Test common origin between EKF2 and EKF3"""12243self.context_push()1224412245# start on EKF212246self.set_parameters({12247'AHRS_EKF_TYPE': 2,12248'EK2_ENABLE': 1,12249'EK3_CHECK_SCALE': 1, # make EK3 slow to get origin12250})12251self.reboot_sitl()1225212253self.context_collect('STATUSTEXT')1225412255self.wait_statustext("EKF2 IMU0 origin set", timeout=60, check_context=True)12256self.wait_statustext("EKF2 IMU0 is using GPS", timeout=60, check_context=True)12257self.wait_statustext("EKF2 active", timeout=60, check_context=True)1225812259self.context_collect('GPS_GLOBAL_ORIGIN')1226012261# get EKF2 origin12262self.run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION)12263ek2_origin = self.assert_receive_message('GPS_GLOBAL_ORIGIN', check_context=True)1226412265# switch to EKF312266self.set_parameters({12267'SIM_GPS1_GLTCH_X' : 0.001, # about 100m12268'EK3_CHECK_SCALE' : 100,12269'AHRS_EKF_TYPE' : 3})1227012271self.wait_statustext("EKF3 IMU0 is using GPS", timeout=60, check_context=True)12272self.wait_statustext("EKF3 active", timeout=60, check_context=True)1227312274self.run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION)12275ek3_origin = self.assert_receive_message('GPS_GLOBAL_ORIGIN', check_context=True)1227612277self.progress("Checking origins")12278if ek2_origin.time_usec == ek3_origin.time_usec:12279raise NotAchievedException("Did not get a new GPS_GLOBAL_ORIGIN message")1228012281print(ek2_origin, ek3_origin)1228212283if (ek2_origin.latitude != ek3_origin.latitude or12284ek2_origin.longitude != ek3_origin.longitude or12285ek2_origin.altitude != ek3_origin.altitude):12286raise NotAchievedException("Did not get matching EK2 and EK3 origins")1228712288self.context_pop()1228912290# restart GPS driver12291self.reboot_sitl()1229212293def ScriptingFlipMode(self):12294'''test adding custom mode from scripting'''12295# Really it would be nice to check for the AVAILABLE_MODES message, but pymavlink does not understand them yet.1229612297# enable scripting and install flip script12298self.set_parameters({12299"SCR_ENABLE": 1,12300})12301self.install_example_script_context('Flip_Mode.lua')12302self.reboot_sitl()1230312304# Takeoff in loiter12305self.takeoff(10, mode="LOITER")1230612307# Try and switch to flip, should not be posible from loiter12308try:12309self.change_mode(100, timeout=10)12310except AutoTestTimeoutException:12311self.progress("PASS not able to enter from loiter")1231212313# Should be alowd to enter from alt hold12314self.change_mode("ALT_HOLD")12315self.change_mode(100)1231612317# Should return to previous mode after flipping12318self.wait_mode("ALT_HOLD")1231912320# Test done12321self.land_and_disarm()1232212323def RTLYaw(self):12324'''test that vehicle yaws to original heading on RTL'''12325# 0 is WP_YAW_BEHAVIOR_NONE12326# 1 is WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP12327# 2 is WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL12328# 3 is WP_YAW_BEHAVIOR_LOOK_AHEAD12329for behaviour in 1, 3:12330self.set_parameters({12331'WP_YAW_BEHAVIOR': behaviour,12332})12333self.change_mode('GUIDED')12334original_heading = self.get_heading()12335target_heading = 10012336if original_heading - target_heading < 90:12337raise NotAchievedException("Bad initial heading")12338self.takeoff(10, mode='GUIDED')12339self.guided_achieve_heading(target_heading)12340self.change_mode('RTL')12341self.wait_heading(original_heading)12342self.wait_disarmed()1234312344def tests2b(self): # this block currently around 9.5mins here12345'''return list of all tests'''12346ret = ([12347self.MotorVibration,12348Test(self.DynamicNotches, attempts=4),12349self.PositionWhenGPSIsZero,12350self.DynamicRpmNotches, # Do not add attempts to this - failure is sign of a bug12351self.DynamicRpmNotchesRateThread,12352self.PIDNotches,12353self.StaticNotches,12354self.RefindGPS,12355Test(self.GyroFFT, attempts=1, speedup=8),12356Test(self.GyroFFTHarmonic, attempts=4, speedup=8),12357Test(self.GyroFFTAverage, attempts=1, speedup=8),12358Test(self.GyroFFTContinuousAveraging, attempts=4, speedup=8),12359self.GyroFFTPostFilter,12360self.GyroFFTMotorNoiseCheck,12361self.CompassReordering,12362self.CRSF,12363self.MotorTest,12364self.AltEstimation,12365self.EKFSource,12366self.GSF,12367self.GSF_reset,12368self.AP_Avoidance,12369self.SMART_RTL,12370self.SMART_RTL_EnterLeave,12371self.SMART_RTL_Repeat,12372self.RTL_TO_RALLY,12373self.RTLYaw,12374self.FlyEachFrame,12375self.GPSBlending,12376self.GPSWeightedBlending,12377self.GPSBlendingLog,12378self.GPSBlendingAffinity,12379self.DataFlash,12380Test(self.DataFlashErase, attempts=8),12381self.Callisto,12382self.PerfInfo,12383self.Replay,12384self.FETtecESC,12385self.ProximitySensors,12386self.GroundEffectCompensation_touchDownExpected,12387self.GroundEffectCompensation_takeOffExpected,12388self.DO_CHANGE_SPEED,12389self.MISSION_START,12390self.AUTO_LAND_TO_BRAKE,12391self.WPNAV_SPEED,12392self.WPNAV_SPEED_UP,12393self.WPNAV_SPEED_DN,12394self.DO_WINCH,12395self.SensorErrorFlags,12396self.GPSForYaw,12397self.DefaultIntervalsFromFiles,12398self.GPSTypes,12399self.MultipleGPS,12400self.WatchAlts,12401self.GuidedEKFLaneChange,12402self.Sprayer,12403self.AutoContinueOnRCFailsafe,12404self.EK3_RNG_USE_HGT,12405self.TerrainDBPreArm,12406self.ThrottleGainBoost,12407self.ScriptMountPOI,12408self.ScriptCopterPosOffsets,12409self.MountSolo,12410self.FlyMissionTwice,12411self.FlyMissionTwiceWithReset,12412self.MissionIndexValidity,12413self.InvalidJumpTags,12414self.IMUConsistency,12415self.AHRSTrimLand,12416self.IBus,12417self.GuidedYawRate,12418self.NoArmWithoutMissionItems,12419self.DO_CHANGE_SPEED_in_guided,12420self.ArmSwitchAfterReboot,12421self.RPLidarA1,12422self.RPLidarA2,12423self.SafetySwitch,12424self.BrakeZ,12425self.MAV_CMD_DO_FLIGHTTERMINATION,12426self.MAV_CMD_DO_LAND_START,12427self.MAV_CMD_SET_EKF_SOURCE_SET,12428self.MAV_CMD_NAV_TAKEOFF,12429self.MAV_CMD_NAV_TAKEOFF_command_int,12430self.Ch6TuningWPSpeed,12431self.PILOT_THR_BHV,12432self.GPSForYawCompassLearn,12433self.CameraLogMessages,12434self.LoiterToGuidedHomeVSOrigin,12435self.GuidedModeThrust,12436self.CompassMot,12437self.AutoRTL,12438self.EK3_OGN_HGT_MASK_climbing,12439self.EK3_OGN_HGT_MASK,12440self.FarOrigin,12441self.GuidedForceArm,12442self.GuidedWeatherVane,12443self.Clamp,12444self.GripperReleaseOnThrustLoss,12445self.REQUIRE_POSITION_FOR_ARMING,12446self.LoggingFormat,12447self.MissionRTLYawBehaviour,12448self.BatteryInternalUseOnly,12449self.MAV_CMD_MISSION_START_p1_p2,12450self.ScriptingAHRSSource,12451self.CommonOrigin,12452self.TestTetherStuck,12453self.ScriptingFlipMode,12454])12455return ret1245612457def testcan(self):12458ret = ([12459self.CANGPSCopterMission,12460self.TestLogDownloadMAVProxyCAN,12461])12462return ret1246312464def BattCANSplitAuxInfo(self):12465'''test CAN battery periphs'''12466self.start_subtest("Swap UAVCAN backend at runtime")12467self.set_parameters({12468"CAN_P1_DRIVER": 1,12469"BATT_MONITOR": 4, # 4 is ananlog volt+curr12470"BATT2_MONITOR": 8, # 8 is UAVCAN_BatteryInfo12471"BATT_SERIAL_NUM": 0,12472"BATT2_SERIAL_NUM": 0,12473"BATT_OPTIONS": 128, # allow split auxinfo12474"BATT2_OPTIONS": 128, # allow split auxinfo12475})12476self.reboot_sitl()12477self.delay_sim_time(2)12478self.set_parameters({12479"BATT_MONITOR": 8, # 8 is UAVCAN_BatteryInfo12480"BATT2_MONITOR": 4, # 8 is UAVCAN_BatteryInfo12481})12482self.delay_sim_time(2)12483self.set_parameters({12484"BATT_MONITOR": 4, # 8 is UAVCAN_BatteryInfo12485"BATT2_MONITOR": 8, # 8 is UAVCAN_BatteryInfo12486})12487self.delay_sim_time(2)12488self.set_parameters({12489"BATT_MONITOR": 8, # 8 is UAVCAN_BatteryInfo12490"BATT2_MONITOR": 4, # 8 is UAVCAN_BatteryInfo12491})12492self.delay_sim_time(2)1249312494def BattCANReplaceRuntime(self):12495'''test CAN battery periphs'''12496self.start_subtest("Replace UAVCAN backend at runtime")12497self.set_parameters({12498"CAN_P1_DRIVER": 1,12499"BATT_MONITOR": 11, # 4 is ananlog volt+curr12500})12501self.reboot_sitl()12502self.delay_sim_time(2)12503self.set_parameters({12504"BATT_MONITOR": 8, # 4 is UAVCAN batterinfo12505})12506self.delay_sim_time(2)1250712508def testcanbatt(self):12509ret = ([12510self.BattCANReplaceRuntime,12511self.BattCANSplitAuxInfo,12512])12513return ret1251412515def tests(self):12516ret = []12517ret.extend(self.tests1a())12518ret.extend(self.tests1b())12519ret.extend(self.tests1c())12520ret.extend(self.tests1d())12521ret.extend(self.tests1e())12522ret.extend(self.tests2a())12523ret.extend(self.tests2b())12524return ret1252512526def disabled_tests(self):12527return {12528"Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",12529"AltEstimation": "See https://github.com/ArduPilot/ardupilot/issues/15191",12530"GroundEffectCompensation_takeOffExpected": "Flapping",12531"GroundEffectCompensation_touchDownExpected": "Flapping",12532"FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561",12533"GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion",12534"CompassMot": "Cuases an arithmetic exception in the EKF",12535"SMART_RTL_EnterLeave": "Causes a panic",12536"SMART_RTL_Repeat": "Currently fails due to issue with loop detection",12537}125381253912540class AutoTestCopterTests1a(AutoTestCopter):12541def tests(self):12542return self.tests1a()125431254412545class AutoTestCopterTests1b(AutoTestCopter):12546def tests(self):12547return self.tests1b()125481254912550class AutoTestCopterTests1c(AutoTestCopter):12551def tests(self):12552return self.tests1c()125531255412555class AutoTestCopterTests1d(AutoTestCopter):12556def tests(self):12557return self.tests1d()125581255912560class AutoTestCopterTests1e(AutoTestCopter):12561def tests(self):12562return self.tests1e()125631256412565class AutoTestCopterTests2a(AutoTestCopter):12566def tests(self):12567return self.tests2a()125681256912570class AutoTestCopterTests2b(AutoTestCopter):12571def tests(self):12572return self.tests2b()125731257412575class AutoTestCAN(AutoTestCopter):1257612577def tests(self):12578return self.testcan()125791258012581class AutoTestBattCAN(AutoTestCopter):1258212583def tests(self):12584return self.testcanbatt()125851258612587