diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 6d13c40c96..a8cfeb69f2 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -45,28 +45,32 @@ def disarm_motors(mavproxy, mav): return True -def takeoff(mavproxy, mav, alt_min = 30, alt_max = 40): +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 1500\n') - wait_altitude(mav, alt_min, alt_max) + m = mav.recv_match(type='VFR_HUD', blocking=True) + if (m.alt < alt_min): + wait_altitude(mav, alt_min, (alt_min + 5)) print("TAKEOFF COMPLETE") return True - def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60): '''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 + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) - print("Altitude %u" % m.alt) + 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: @@ -75,6 +79,20 @@ def loiter(mavproxy, mav, holdtime=20, maxaltchange=10, timeout=60): 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 1800\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 1100\n') + wait_altitude(mav, (alt_min -5), alt_min) + mavproxy.send('rc 3 1430\n') + return True + def fly_square(mavproxy, mav, side=50, timeout=120): '''fly a square, flying N then E''' @@ -83,7 +101,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): tstart = time.time() failed = False - print("Save WP 1") + print("Save WP 1 and 2") save_wp(mavproxy, mav) print("turn") @@ -99,7 +117,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): failed = True mavproxy.send('rc 2 1500\n') - print("Save WP 2") + print("Save WP 3") save_wp(mavproxy, mav) print("Going east %u meters" % side) @@ -108,7 +126,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): failed = True mavproxy.send('rc 1 1500\n') - print("Save WP 3") + print("Save WP 4") save_wp(mavproxy, mav) print("Going south %u meters" % side) @@ -118,7 +136,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): mavproxy.send('rc 2 1500\n') mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) - print("Save WP 4") + print("Save WP 5") save_wp(mavproxy, mav) print("Going west %u meters" % side) @@ -127,31 +145,54 @@ def fly_square(mavproxy, mav, side=50, timeout=120): failed = True mavproxy.send('rc 1 1500\n') - print("Save WP 5") + print("Save WP 6") save_wp(mavproxy, mav) return not failed +def fly_simple(mavproxy, mav, side=60, timeout=120): + '''fly a square, flying N then E''' + mavproxy.send('switch 6\n') + wait_mode(mav, 'STABILIZE') + mavproxy.send('rc 3 1430\ n') + tstart = time.time() + 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("# Going east for 30 seconds") + mavproxy.send('rc 1 1650\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 1650\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') + return not failed + def land(mavproxy, mav, timeout=60): '''land the quad''' print("STARTING LANDING") - mavproxy.send('switch 6\n') - wait_mode(mav, 'STABILIZE') - - # start by dropping throttle till we have lost 5m - mavproxy.send('rc 3 1380\n') - m = mav.recv_match(type='VFR_HUD', blocking=True) - wait_altitude(mav, 0, m.alt-5) - - # now let it settle gently - mavproxy.send('rc 3 1400\n') - tstart = time.time() - - ret = wait_altitude(mav, -5, 0) - print("LANDING: ok=%s" % ret) + mavproxy.send('switch 2\n') + wait_mode(mav, 'LAND') + print("Entered Landing Mode") + ret = wait_altitude(mav, -5, 5) + print("LANDING: ok= %s" % ret) return ret @@ -182,7 +223,7 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None): 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) + ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) print("test: MISSION COMPLETE: passed=%s" % ret) # wait here until ready mavproxy.send('switch 5\n') @@ -308,47 +349,81 @@ def fly_ArduCopter(viewerip=None): 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 - if not takeoff(mavproxy, mav, 10, 15): + print("# Takeoff") + if not takeoff(mavproxy, mav, 10): print("takeoff failed") failed = True + # Loiter for 10 seconds + print("# Loiter for 30 seconds") + if not loiter(mavproxy, mav, 10): + 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 - # Loiter for 10 seconds - print("# Loiter for 20 seconds") - if not loiter(mavproxy, mav, 20): - print("loiter failed") - failed = True - - #Fly a circle for 60 seconds - print("# Fly a Circle") - if not circle(mavproxy, mav): - print("circle 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") @@ -392,3 +467,10 @@ def fly_ArduCopter(viewerip=None): print("FAILED: %s" % e) return False return True + + #Fly a circle for 60 seconds + #print("# Fly a Circle") + #if not circle(mavproxy, mav): + # print("circle failed") + # failed = True +