# fly ArduCopter in SIL import util, pexpect, sys, time, math, shutil, os from common import * import mavutil, mavwp, random # get location of scripts testdir=os.path.dirname(os.path.realpath(__file__)) FRAME='octa' TARGET='sitl-octa' HOME=location(-35.362938,149.165085,584,270) homeloc = None num_wp = 0 def hover(mavproxy, mav): mavproxy.send('rc 3 1395\n') return True def calibrate_level(mavproxy, mav): '''init the accelerometers''' print("Initialising accelerometers") MAV_ACTION_CALIBRATE_ACC = 19 mav.mav.action_send(mav.target_system, mav.target_component, MAV_ACTION_CALIBRATE_ACC) mavproxy.expect('APM: action received') return True def arm_motors(mavproxy, mav): '''arm motors''' print("Arming motors") mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') mavproxy.send('rc 3 1000\n') mavproxy.send('rc 4 2000\n') mavproxy.expect('APM: ARMING MOTORS') mavproxy.send('rc 4 1500\n') print("MOTORS ARMED OK") return True def disarm_motors(mavproxy, mav): '''disarm motors''' print("Disarming motors") mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('rc 3 1000\n') mavproxy.send('rc 4 1000\n') mavproxy.expect('APM: DISARMING MOTORS') mavproxy.send('rc 4 1500\n') print("MOTORS DISARMED OK") return True def takeoff(mavproxy, mav, alt_min = 30): '''takeoff get to 30m altitude''' mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') mavproxy.send('rc 3 1510\n') m = mav.recv_match(type='VFR_HUD', blocking=True) if (m.alt < alt_min): wait_altitude(mav, alt_min, (alt_min + 5)) hover(mavproxy, mav) print("TAKEOFF COMPLETE") return True def loiter(mavproxy, mav, holdtime=60, maxaltchange=20): '''hold loiter position''' mavproxy.send('switch 5\n') # loiter mode wait_mode(mav, 'LOITER') m = mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = current_location(mav) tstart = time.time() tholdstart = time.time() print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) while time.time() < tstart + holdtime: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = current_location(mav) delta = get_distance(start, pos) print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) if math.fabs(m.alt - start_altitude) > maxaltchange: tholdstart = time.time() if time.time() - tholdstart > holdtime: print("Loiter OK for %u seconds" % holdtime) return True print("Loiter FAILED") return False def change_alt(mavproxy, mav, alt_min): '''change altitude''' m = mav.recv_match(type='VFR_HUD', blocking=True) if(m.alt < alt_min): print("Rise to alt:%u from %u" % (alt_min, m.alt)) mavproxy.send('rc 3 1920\n') wait_altitude(mav, alt_min, (alt_min + 5)) else: print("Lower to alt:%u from %u" % (alt_min, m.alt)) mavproxy.send('rc 3 1120\n') wait_altitude(mav, (alt_min -5), alt_min) hover(mavproxy, mav) return True def fly_square(mavproxy, mav, side=50, timeout=120): '''fly a square, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') tstart = time.time() failed = False print("Save WP 1 and 2") save_wp(mavproxy, mav) print("turn") hover(mavproxy, mav) mavproxy.send('rc 4 1610\n') if not wait_heading(mav, 0): return False mavproxy.send('rc 4 1500\n') print("Going north %u meters" % side) mavproxy.send('rc 2 1390\n') if not wait_distance(mav, side): failed = True mavproxy.send('rc 2 1500\n') print("Save WP 3") save_wp(mavproxy, mav) print("Going east %u meters" % side) mavproxy.send('rc 1 1610\n') if not wait_distance(mav, side): failed = True mavproxy.send('rc 1 1500\n') print("Save WP 4") save_wp(mavproxy, mav) print("Going south %u meters" % side) mavproxy.send('rc 2 1610\n') if not wait_distance(mav, side): failed = True mavproxy.send('rc 2 1500\n') mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) print("Save WP 5") save_wp(mavproxy, mav) print("Going west %u meters" % side) mavproxy.send('rc 1 1390\n') if not wait_distance(mav, side): failed = True mavproxy.send('rc 1 1500\n') print("Save WP 6") save_wp(mavproxy, mav) return not failed def fly_RTL(mavproxy, mav, side=60): '''Fly, return, land''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') hover(mavproxy, mav) failed = False print("# Going forward %u meters" % side) mavproxy.send('rc 2 1350\n') if not wait_distance(mav, side, 5, 60): failed = True mavproxy.send('rc 2 1500\n') print("# Enter RTL") mavproxy.send('switch 3\n') tstart = time.time() while time.time() < tstart + 200: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = current_location(mav) #delta = get_distance(start, pos) print("Alt: %u" % m.alt) if(m.alt <= 1): return True return True def fly_failsafe(mavproxy, mav, side=60): '''Fly, Failsafe, return, land''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') hover(mavproxy, mav) failed = False print("# Going forward %u meters" % side) mavproxy.send('rc 2 1350\n') if not wait_distance(mav, side, 5, 60): failed = True mavproxy.send('rc 2 1500\n') print("# Enter Failsafe") mavproxy.send('rc 3 900\n') tstart = time.time() while time.time() < tstart + 250: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = current_location(mav) home_distance = get_distance(HOME, pos) print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) if m.alt <= 1 and home_distance < 10: print("Reached failsafe home OK") mavproxy.send('rc 3 1100\n') return True print("Failed to land on failsafe RTL - timed out after 120 seconds") return False def fly_simple(mavproxy, mav, side=60, timeout=120): '''fly Simple, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') mavproxy.send('rc 3 1400\n') tstart = time.time() failed = False print("# Going forward %u meters" % side) mavproxy.send('rc 2 1390\n') if not wait_distance(mav, side, 5, 60): failed = True mavproxy.send('rc 2 1500\n') print("# Going east for 30 seconds") mavproxy.send('rc 1 1610\n') tstart = time.time() while time.time() < (tstart + 30): m = mav.recv_match(type='VFR_HUD', blocking=True) delta = (time.time() - tstart) #print("%u" % delta) mavproxy.send('rc 1 1500\n') print("# Going back %u meters" % side) mavproxy.send('rc 2 1610\n') if not wait_distance(mav, side, 5, 60): failed = True mavproxy.send('rc 2 1500\n') #restore to default mavproxy.send('param set SIMPLE 0\n') hover(mavproxy, mav) return not failed def land(mavproxy, mav, timeout=60): '''land the quad''' print("STARTING LANDING") mavproxy.send('switch 2\n') wait_mode(mav, 'LAND') print("Entered Landing Mode") ret = wait_altitude(mav, -5, 1) print("LANDING: ok= %s" % ret) return ret def circle(mavproxy, mav, maxaltchange=10, holdtime=90, timeout=35): '''fly circle''' print("FLY CIRCLE") mavproxy.send('switch 1\n') # CIRCLE mode wait_mode(mav, 'CIRCLE') m = mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt tstart = time.time() tholdstart = time.time() print("Circle at %u meters for %u seconds" % (start_altitude, holdtime)) while time.time() < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) print("heading %u" % m.heading) print("CIRCLE OK for %u seconds" % holdtime) return True def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None): '''fly a mission from a file''' global homeloc global num_wp print("test: Fly a mission from 1 to %u" % num_wp) mavproxy.send('wp set 1\n') mavproxy.send('switch 4\n') # auto mode wait_mode(mav, 'AUTO') #wait_altitude(mav, 30, 40) ret = wait_waypoint(mav, 0, num_wp, timeout=500) print("test: MISSION COMPLETE: passed=%s" % ret) # wait here until ready mavproxy.send('switch 5\n') wait_mode(mav, 'LOITER') return ret #if not wait_distance(mav, 30, timeout=120): # return False #if not wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy): # return False def load_mission_from_file(mavproxy, mav, filename): '''load a mission from a file''' global num_wp wploader = mavwp.MAVWPLoader() wploader.load(filename) num_wp = wploader.count() print("loaded mission with %u waypoints" % num_wp) return True def upload_mission_from_file(mavproxy, mav, filename): '''Upload a mission from a file to APM''' global num_wp mavproxy.send('wp load %s\n' % filename) mavproxy.expect('flight plan received') mavproxy.send('wp list\n') mavproxy.expect('Requesting [0-9]+ waypoints') return True def save_mission_to_file(mavproxy, mav, filename): global num_wp mavproxy.send('wp save %s\n' % filename) mavproxy.expect('Saved ([0-9]+) waypoints') num_wp = int(mavproxy.match.group(1)) print("num_wp: %d" % num_wp) return True def setup_rc(mavproxy): '''setup RC override control''' for chan in range(1,9): mavproxy.send('rc %u 1500\n' % chan) # zero throttle mavproxy.send('rc 3 1000\n') def fly_ArduCopter(viewerip=None): '''fly ArduCopter in SIL you can pass viewerip as an IP address to optionally send fg and mavproxy packets too for local viewing of the flight in real time ''' global homeloc if TARGET != 'sitl': util.build_SIL('ArduCopter', target=TARGET) sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % ( FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading) sim_cmd += ' --wind=6,45,.3' if viewerip: sim_cmd += ' --fgout=%s:5503' % viewerip sil = util.start_SIL('ArduCopter', wipe=True) mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter') mavproxy.expect('Received [0-9]+ parameters') # setup test parameters mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200)) mavproxy.send("param load %s/ArduCopter.parm\n" % testdir) mavproxy.expect('Loaded [0-9]+ parameters') # reboot with new parameters util.pexpect_close(mavproxy) util.pexpect_close(sil) sil = util.start_SIL('ArduCopter', height=HOME.alt) sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10) sim.delaybeforesend = 0 util.pexpect_autoclose(sim) options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5' if viewerip: options += ' --out=%s:14550' % viewerip mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options) mavproxy.expect('Logging to (\S+)') logfile = mavproxy.match.group(1) print("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/ArduCopter-test.mavlog") print("buildlog=%s" % buildlog) if os.path.exists(buildlog): os.unlink(buildlog) os.link(logfile, buildlog) # the received parameters can come before or after the ready to fly message mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) util.expect_setup_callback(mavproxy, expect_callback) expect_list_clear() expect_list_extend([sim, sil, mavproxy]) # get a mavlink connection going try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception, msg: print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) failed = False e = 'None' try: mav.wait_heartbeat() mav.recv_match(type='GPS_RAW', blocking=True) setup_rc(mavproxy) homeloc = current_location(mav) print("# Calibrate level") if not calibrate_level(mavproxy, mav): print("calibrate_level failed") failed = True print("# Arm motors") if not arm_motors(mavproxy, mav): print("arm_motors failed") failed = True print("# Takeoff") if not takeoff(mavproxy, mav, 10): print("takeoff failed") failed = True print("# Test RTL") if not fly_RTL(mavproxy, mav): print("RTL failed") failed = True print("# Takeoff") if not takeoff(mavproxy, mav, 10): print("takeoff failed") failed = True print("# Test Failsafe") if not fly_failsafe(mavproxy, mav): print("FS failed") failed = True print("# Takeoff") if not takeoff(mavproxy, mav, 10): print("takeoff failed") failed = True # Loiter for 30 seconds print("# Loiter for 45 seconds") if not loiter(mavproxy, mav, 45): print("loiter failed") failed = True print("# Change alt to 60m") if not change_alt(mavproxy, mav, 60): print("change_alt failed") failed = True print("# Change alt to 20m") if not change_alt(mavproxy, mav, 20): print("change_alt failed") failed = True print("# Change alt to 20m") if not change_alt(mavproxy, mav, 20): print("change_alt failed") failed = True print("# Fly A square") if not fly_square(mavproxy, mav): print("fly_square failed") failed = True print("# Land") if not land(mavproxy, mav): print("land failed") failed = True print("Save landing WP") save_wp(mavproxy, mav) # save the stored mission print("# Save out the C7 mission") if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")): print("save_mission_to_file failed") failed = True # save the stored mission print("# Fly CH 7 saved mission") if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10): print("fly_mission failed") failed = True #set SIMPLE mode mavproxy.send('param set SIMPLE 63\n') if not takeoff(mavproxy, mav, 10): print("takeoff failed") failed = True print("# Fly in SIMPLE mode") if not fly_simple(mavproxy, mav): print("fly_simple failed") failed = True print("# Upload mission1") if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")): print("upload_mission_from_file failed") failed = True # this grabs our mission count print("# store mission1 locally") if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")): print("load_mission_from_file failed") failed = True print("# Fly mission 2") if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10): print("fly_mission failed") failed = True else: print("Flew mission2 OK") print("# Land") if not land(mavproxy, mav): print("land failed") failed = True #print("# disarm motors") #if not disarm_motors(mavproxy, mav): # print("disarm_motors failed") # failed = True except pexpect.TIMEOUT, e: failed = True mav.close() util.pexpect_close(mavproxy) util.pexpect_close(sil) util.pexpect_close(sim) if os.path.exists('ArduCopter-valgrind.log'): os.chmod('ArduCopter-valgrind.log', 0644) shutil.copy("ArduCopter-valgrind.log", util.reltopdir("../buildlogs/ArduCopter-valgrind.log")) if failed: print("FAILED: %s" % e) return False return True #! mavproxy.send('rc 2 1390\n') #! #adjust till the rate is 0; #! #! mavproxy.send('rc 4 1610\n') #! if not wait_heading(mav, 0): #! return False #! mavproxy.send('rc 4 1500\n') #! #! mavproxy.send('rc 2 1455\n') #! #adjust till the rate is 0; #! pitch_test = 1455 #! roll_test = 1500 #! old_lat = 0 #! old_lon = 0 #! #! while(1): #! pos = current_location(mav) #! tmp = (pos.lat *10e7) - (old_lat *10e7) #! print("tmp %d " % tmp) #! if(tmp > 0 ): #! print("higher tmp %d " % (tmp)) #! pitch_test += 1 #! if(tmp < 0 ): #! print("lower tmp %d " % (tmp)) #! pitch_test -= 1 #! mavproxy.send('rc 2 %u\n' % math.floor(pitch_test)) #! old_lat = pos.lat #! #old_lon = pos.lon