From d2a8107b47ef5949abd3798f5b6c8c1d3dfae04e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2011 12:46:51 +1100 Subject: [PATCH 1/8] autotest: run the sim at 200Hz --- Tools/autotest/arducopter.py | 2 +- Tools/autotest/util.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 155bfdb991..a29aa2832e 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -301,7 +301,7 @@ def fly_ArduCopter(): util.expect_setup_callback(mavproxy, expect_callback) # start hil_quad.py - hquad = pexpect.spawn(util.reltopdir('../HILTest/hil_quad.py') + ' --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION, + hquad = pexpect.spawn(util.reltopdir('../HILTest/hil_quad.py') + ' --fgout=192.168.2.15:9123 --fgrate=200 --home=%s' % HOME_LOCATION, logfile=sys.stdout, timeout=10) util.pexpect_autoclose(hquad) hquad.expect('Starting at') diff --git a/Tools/autotest/util.py b/Tools/autotest/util.py index df721ae68e..e9e4ff99d6 100644 --- a/Tools/autotest/util.py +++ b/Tools/autotest/util.py @@ -106,11 +106,12 @@ def start_SIL(atype, valgrind=False, wipe=False, CLI=False): return ret def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760', + fgrate=200, options=None, logfile=sys.stdout): '''launch mavproxy connected to a SIL instance''' global close_list MAVPROXY = reltopdir('../MAVProxy/mavproxy.py') - cmd = MAVPROXY + ' --master=%s' % master + cmd = MAVPROXY + ' --master=%s --fgrate=%u' % (master, fgrate) if setup: cmd += ' --setup' if aircraft is None: From 6b26c32e9d10cbdcb9f8b5881c5d06d1ba43afd8 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 9 Nov 2011 12:46:34 -0800 Subject: [PATCH 2/8] Lowered Nav_P based on SIM data --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2c1d0accad..23f81217b1 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -486,7 +486,7 @@ #endif #ifndef NAV_P -# define NAV_P 3.0 // +# define NAV_P 2.0 // #endif #ifndef NAV_I # define NAV_I 0.05 // Lowerd from .25 - saw lots of overshoot. From 6211cc4e5021691101037fd6a865c2e6ca6a28f2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 9 Nov 2011 13:53:26 -0800 Subject: [PATCH 3/8] back to original config for testing --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 23f81217b1..2c1d0accad 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -486,7 +486,7 @@ #endif #ifndef NAV_P -# define NAV_P 2.0 // +# define NAV_P 3.0 // #endif #ifndef NAV_I # define NAV_I 0.05 // Lowerd from .25 - saw lots of overshoot. From 6263bb89361faf96c6bec6b5a982eac488605885 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2011 12:56:20 +1100 Subject: [PATCH 4/8] autotest: fixed target altitude for RTL mission --- Tools/autotest/arducopter.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a29aa2832e..cb064aaec4 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -162,16 +162,18 @@ def wait_distance(mav, distance, accuracy=5, timeout=30): return False -def wait_location(mav, loc, accuracy=5, timeout=30, height_accuracy=-1): +def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1): '''wait for arrival at a location''' tstart = time.time() + if target_altitude is None: + target_altitude = loc.alt while time.time() < tstart + timeout: m = mav.recv_match(type='GPS_RAW', blocking=True) pos = current_location(mav) delta = get_distance(loc, pos) print("Distance %.2f meters" % delta) if delta <= accuracy: - if height_accuracy != -1 and math.fabs(pos.alt - loc.alt) > height_accuracy: + if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy: continue print("Reached location (%.2f meters)" % delta) return True @@ -238,7 +240,7 @@ def land(mavproxy, mav, timeout=60): return False -def fly_mission(mavproxy, mav, filename, height_accuracy=-1): +def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None): '''fly a mission from a file''' global homeloc mavproxy.send('wp load %s\n' % filename) @@ -248,7 +250,7 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1): mavproxy.send('switch 1\n') # auto mode mavproxy.expect('AUTO>') wait_distance(mav, 30, timeout=120) - wait_location(mav, homeloc, timeout=600, height_accuracy=height_accuracy) + wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy) def setup_rc(mavproxy): @@ -328,7 +330,7 @@ def fly_ArduCopter(): loiter(mavproxy, mav) land(mavproxy, mav) #fly_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt"), height_accuracy=0.2) - fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.2) + fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.5, target_altitude=10) land(mavproxy, mav) disarm_motors(mavproxy) except pexpect.TIMEOUT, e: From e7294ee49386b6f369c4c8d1e6289afe7ef27321 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2011 13:12:43 +1100 Subject: [PATCH 5/8] autotest: improve error checking --- Tools/autotest/arducopter.py | 49 +++++++++++++++++++++++++----------- 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index cb064aaec4..355c9e1075 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -91,6 +91,7 @@ def arm_motors(mavproxy): mavproxy.expect('APM: ARMING MOTORS') mavproxy.send('rc 4 1500\n') print("MOTORS ARMED OK") + return True def disarm_motors(mavproxy): '''disarm motors''' @@ -101,6 +102,7 @@ def disarm_motors(mavproxy): mavproxy.expect('APM: DISARMING MOTORS') mavproxy.send('rc 4 1500\n') print("MOTORS DISARMED OK") + return True def takeoff(mavproxy, mav): @@ -110,6 +112,7 @@ def takeoff(mavproxy, mav): mavproxy.send('rc 3 1500\n') wait_altitude(mav, 30, 40) print("TAKEOFF COMPLETE") + return True def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60): @@ -186,6 +189,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): mavproxy.send('switch 6\n') mavproxy.expect('STABILIZE>') tstart = time.time() + failed = False mavproxy.send('rc 3 1430\n') mavproxy.send('rc 4 1610\n') if not wait_heading(mav, 0): @@ -194,24 +198,28 @@ def fly_square(mavproxy, mav, side=50, timeout=120): print("Going north %u meters" % side) mavproxy.send('rc 2 1390\n') - ok = wait_distance(mav, side) + if not wait_distance(mav, side): + failed = True mavproxy.send('rc 2 1500\n') print("Going east %u meters" % side) mavproxy.send('rc 1 1610\n') - ok = wait_distance(mav, side) + if not wait_distance(mav, side): + failed = True mavproxy.send('rc 1 1500\n') print("Going south %u meters" % side) mavproxy.send('rc 2 1610\n') - ok = wait_distance(mav, side) + if not wait_distance(mav, side): + failed = True mavproxy.send('rc 2 1500\n') print("Going west %u meters" % side) mavproxy.send('rc 1 1390\n') - ok = wait_distance(mav, side) + if not wait_distance(mav, side): + failed = True mavproxy.send('rc 1 1500\n') - return ok + return not failed @@ -249,8 +257,11 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non mavproxy.expect('Requesting [0-9]+ waypoints') mavproxy.send('switch 1\n') # auto mode mavproxy.expect('AUTO>') - wait_distance(mav, 30, timeout=120) - wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy) + 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 + return True def setup_rc(mavproxy): @@ -324,15 +335,23 @@ def fly_ArduCopter(): mav.recv_match(type='GPS_RAW', blocking=True) setup_rc(mavproxy) homeloc = current_location(mav) - arm_motors(mavproxy) - takeoff(mavproxy, mav) - fly_square(mavproxy, mav) - loiter(mavproxy, mav) - land(mavproxy, mav) + if not arm_motors(mavproxy): + failed = True + if not takeoff(mavproxy, mav): + failed = True + if not fly_square(mavproxy, mav): + failed = True + if not loiter(mavproxy, mav): + failed = True + if not land(mavproxy, mav): + failed = True #fly_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt"), height_accuracy=0.2) - fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.5, target_altitude=10) - land(mavproxy, mav) - disarm_motors(mavproxy) + if not fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.5, target_altitude=10): + failed = True + if not land(mavproxy, mav): + failed = True + if not disarm_motors(mavproxy): + failed = True except pexpect.TIMEOUT, e: failed = True From c056c4f0cb2a5de8b86aa63449f84faea0fbc217 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2011 13:17:06 +1100 Subject: [PATCH 6/8] HIL: allow for fast HIL attitude update in HIL_MODE_ATTITUDE this runs it at the full loop rate --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 38bab85f39..06d82cb280 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1170,7 +1170,7 @@ static void read_AHRS(void) { // Perform IMU calculations and get attitude info //----------------------------------------------- - #if HIL_MODE == HIL_MODE_SENSORS + #if HIL_MODE != HIL_MODE_DISABLED // update hil before dcm update gcs_update(); #endif From 72999351045ecb2a7ae538b13e8250cb49f33c9a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2011 14:37:52 +1100 Subject: [PATCH 7/8] autotest: added some colour to passed/failed msgs --- Tools/autotest/autotest.py | 6 +- Tools/autotest/web/css/main.css | 256 ++++++++++++++++---------------- 2 files changed, 135 insertions(+), 127 deletions(-) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index d0acc02ada..125997aa92 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -222,16 +222,16 @@ def run_tests(steps): print(">>>> FAILED STEP: %s at %s" % (step, time.asctime())) passed = False failed.append(step) - results.add(step, "FAILED", time.time() - t1) + results.add(step, 'FAILED', time.time() - t1) continue except Exception, msg: passed = False failed.append(step) print(">>>> FAILED STEP: %s at %s (%s)" % (step, time.asctime(), msg)) traceback.print_exc(file=sys.stdout) - results.add(step, "FAILED", time.time() - t1) + results.add(step, 'FAILED', time.time() - t1) pass - results.add(step, "PASSED", time.time() - t1) + results.add(step, 'PASSED', time.time() - t1) print(">>>> PASSED STEP: %s at %s" % (step, time.asctime())) if not passed: print("FAILED %u tests: %s" % (len(failed), failed)) diff --git a/Tools/autotest/web/css/main.css b/Tools/autotest/web/css/main.css index 4cb4f2a53f..a9f5f3c735 100644 --- a/Tools/autotest/web/css/main.css +++ b/Tools/autotest/web/css/main.css @@ -1,124 +1,132 @@ -@charset "utf-8"; -/* CSS Document */ - -/* Background-Styles */ -html { - overflow-y: scroll; - } - -body { - font-family:Helvetica; - margin:0px; - padding:0px; - background-color: #fff; - background-image: url(images/bg.png); -} - -#logo { - background-image:url(images/logo.png); - background-repeat:no-repeat; - height: 120px; - width: 420px; - -moz-box-shadow: 2px 2px 5px #888; - -webkit-box-shadow: 2px 2px 5px #888; - box-shadow: 2px 2px 5px #888; - background-color: #000; -} - -h2 { - text-shadow: #ccc 0px 1px 0px; - text-decoration:none; - color:#D14836; - padding-top: 0px; - padding-right: 5px; - padding-bottom: 5px; - padding-left: 5px; -} - -#git { - background-color: #FFF; - font-size: 15px; - display: auto; - padding: 10px; - border: 1px solid #CCC; - position: relative; - -moz-box-shadow: 2px 2px 5px #888; - -webkit-box-shadow: 2px 2px 5px #888; - box-shadow: 2px 2px 5px #888; -} - -#git a { - color:#00F; - font-weight: normal; - } - -#git a:hover { - color:#D14836; -} - - -#main { - padding-top:0px; - min-width: 420px; - margin-top: 0; - margin-right: auto; - margin-bottom: 0; - margin-left: auto; - padding-right: 20px; - padding-bottom: 50px; - padding-left: 20px; -} - -ul#testresults { - background-color: #FFF; - display: block; - padding: 10px; - border: 1px solid #CCC; - list-style-type: none; - position: relative; - -moz-box-shadow: 2px 2px 5px #888; - -webkit-box-shadow: 2px 2px 5px #888; - box-shadow: 2px 2px 5px #888; -} - -ul#testresults li { - list-style-type: none; - display: block; - background-color: #eef1f1; - margin: 5px; - padding: 5px; - } - -ul#testlogs { - background-color: #FFF; - display: block; - padding: 10px; - border: 1px solid #CCC; - list-style-type: none; - position: relative; - -moz-box-shadow: 2px 2px 5px #888; - -webkit-box-shadow: 2px 2px 5px #888; - box-shadow: 2px 2px 5px #888; -} - -ul#testlogs li { - text-decoration:none; - list-style-type: none; - display: block; - background-color: #eef1f1; - margin: 5px; - padding: 5px; - font-size: 15px; - } - - -ul#testlogs li a { - text-decoration:none; - color:#8aa19c; - font-weight: bold; - font-size: 12px; - } - -ul#testlogs li a:hover { - color:#D14836; -} +@charset "utf-8"; +/* CSS Document */ + +/* Background-Styles */ +html { + overflow-y: scroll; + } + +body { + font-family:Helvetica; + margin:0px; + padding:0px; + background-color: #fff; + background-image: url(images/bg.png); +} + +#logo { + background-image:url(images/logo.png); + background-repeat:no-repeat; + height: 120px; + width: 420px; + -moz-box-shadow: 2px 2px 5px #888; + -webkit-box-shadow: 2px 2px 5px #888; + box-shadow: 2px 2px 5px #888; + background-color: #000; +} + +h2 { + text-shadow: #ccc 0px 1px 0px; + text-decoration:none; + color:#D14836; + padding-top: 0px; + padding-right: 5px; + padding-bottom: 5px; + padding-left: 5px; +} + +#git { + background-color: #FFF; + font-size: 15px; + display: auto; + padding: 10px; + border: 1px solid #CCC; + position: relative; + -moz-box-shadow: 2px 2px 5px #888; + -webkit-box-shadow: 2px 2px 5px #888; + box-shadow: 2px 2px 5px #888; +} + +#git a { + color:#00F; + font-weight: normal; + } + +#git a:hover { + color:#D14836; +} + +.passed-text { + color:green; +} + +.failed-text { + color:red; +} + + +#main { + padding-top:0px; + min-width: 420px; + margin-top: 0; + margin-right: auto; + margin-bottom: 0; + margin-left: auto; + padding-right: 20px; + padding-bottom: 50px; + padding-left: 20px; +} + +ul#testresults { + background-color: #FFF; + display: block; + padding: 10px; + border: 1px solid #CCC; + list-style-type: none; + position: relative; + -moz-box-shadow: 2px 2px 5px #888; + -webkit-box-shadow: 2px 2px 5px #888; + box-shadow: 2px 2px 5px #888; +} + +ul#testresults li { + list-style-type: none; + display: block; + background-color: #eef1f1; + margin: 5px; + padding: 5px; + } + +ul#testlogs { + background-color: #FFF; + display: block; + padding: 10px; + border: 1px solid #CCC; + list-style-type: none; + position: relative; + -moz-box-shadow: 2px 2px 5px #888; + -webkit-box-shadow: 2px 2px 5px #888; + box-shadow: 2px 2px 5px #888; +} + +ul#testlogs li { + text-decoration:none; + list-style-type: none; + display: block; + background-color: #eef1f1; + margin: 5px; + padding: 5px; + font-size: 15px; + } + + +ul#testlogs li a { + text-decoration:none; + color:#8aa19c; + font-weight: bold; + font-size: 12px; + } + +ul#testlogs li a:hover { + color:#D14836; +} From 7229b77fcf09c61fe0e2e79c78f7c4e5618aadf3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Nov 2011 14:39:28 +1100 Subject: [PATCH 8/8] autotest: link to defaults for both builds --- Tools/autotest/autotest.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 125997aa92..3f834ebdb1 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -244,9 +244,11 @@ def run_tests(steps): results.addfile('ArduPlane build log', 'ArduPlane.txt') results.addfile('ArduPlane code size', 'ArduPlane.sizes.txt') results.addfile('ArduPlane stack sizes', 'ArduPlane.framesizes.txt') + results.addfile('ArduPlane defaults', 'ArduPlane.defaults.txt') results.addfile('ArduCopter build log', 'ArduCopter.txt') results.addfile('ArduCopter code size', 'ArduCopter.sizes.txt') results.addfile('ArduCopter stack sizes', 'ArduCopter.framesizes.txt') + results.addfile('ArduCopter defaults', 'ArduCopter.defaults.txt') write_webresults(results)