From beab1500c17f495e74cb2f633ce4d1823b031cca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Oct 2011 18:29:52 +1100 Subject: [PATCH 1/3] autotest: ArduCopter test can now fly a square the copter now takes off, yaws to north, flies a 50m square, then does a loiter test for 10s and lands --- Tools/autotest/ArduCopter.parm | 1 + Tools/autotest/arducopter.py | 154 +++++++++++++++++++++++++++++---- 2 files changed, 138 insertions(+), 17 deletions(-) diff --git a/Tools/autotest/ArduCopter.parm b/Tools/autotest/ArduCopter.parm index 6088b34a22..5e9a2a45be 100644 --- a/Tools/autotest/ArduCopter.parm +++ b/Tools/autotest/ArduCopter.parm @@ -1,3 +1,4 @@ +FRAME 0 RC1_MAX 2000.000000 RC1_MIN 1000.000000 RC1_TRIM 1500.000000 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 434966c98c..53969cd0d9 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1,12 +1,56 @@ # fly ArduCopter in SIL -import util, pexpect, sys, time, math, shutil +import util, pexpect, sys, time, math, shutil, os + +# get location of scripts +testdir=os.path.dirname(os.path.realpath(__file__)) sys.path.insert(0, 'pymavlink') import mavutil HOME_LOCATION='-35.362938,149.165085,650,270' +class location(object): + '''represent a GPS coordinate''' + def __init__(self, lat, lng, alt=0): + self.lat = lat + self.lng = lng + self.alt = alt + +def get_distance(loc1, loc2): + '''get ground distance between two locations''' + dlat = loc2.lat - loc1.lat + dlong = loc2.lng - loc1.lng + return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 + +def get_bearing(loc1, loc2): + '''get bearing from loc1 to loc2''' + off_x = loc2.lng - loc1.lng + off_y = loc2.lat - loc1.lat + bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795 + if bearing < 0: + bearing += 360.00 + return bearing; + +def current_location(mav): + '''return current location''' + return location(mav.messages['GPS_RAW'].lat, + mav.messages['GPS_RAW'].lon, + mav.messages['VFR_HUD'].alt) + +def wait_altitude(mav, alt_min, alt_max, timeout=30): + '''wait for a given altitude range''' + tstart = time.time() + print("Waiting for altitude between %u and %u" % (alt_min, alt_max)) + while time.time() < tstart + timeout: + m = mav.recv_match(type='VFR_HUD', blocking=True) + print("Altitude %u" % m.alt) + if m.alt >= alt_min and m.alt <= alt_max: + return True + print("Failed to attain altitude range") + return False + + def arm_motors(mavproxy): '''arm motors''' mavproxy.send('switch 6\n') # stabilize mode @@ -31,9 +75,7 @@ def takeoff(mavproxy, mav): mavproxy.send('switch 6\n') # stabilize mode mavproxy.expect('STABILIZE>') mavproxy.send('rc 3 1500\n') - mavproxy.send('status\n') - m = mav.recv_match(type='VFR_HUD', condition='VFR_HUD.alt>=30', blocking=True) - print("Altitude %u" % m.alt) + wait_altitude(mav, 30, 40) print("TAKEOFF COMPLETE") @@ -60,22 +102,90 @@ def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60): return False +def wait_heading(mav, heading, accuracy=5, timeout=30): + '''wait for a given heading''' + tstart = time.time() + while time.time() < tstart + timeout: + m = mav.recv_match(type='VFR_HUD', blocking=True) + print("Heading %u" % m.heading) + if math.fabs(m.heading - heading) <= accuracy: + return True + print("Failed to attain heading %u" % heading) + return False + + +def wait_distance(mav, distance, accuracy=5, timeout=30): + '''wait for flight of a given distance''' + tstart = time.time() + start = current_location(mav) + while time.time() < tstart + timeout: + m = mav.recv_match(type='GPS_RAW', blocking=True) + pos = current_location(mav) + delta = get_distance(start, pos) + print("Distance %.2f meters" % delta) + if math.fabs(delta - distance) <= accuracy: + return True + print("Failed to attain distance %u" % distance) + return False + + +def fly_square(mavproxy, mav, side=50, timeout=120): + '''fly a square, flying N then E''' + mavproxy.send('switch 6\n') + mavproxy.expect('STABILIZE>') + tstart = time.time() + mavproxy.send('rc 3 1430\n') + 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') + ok = wait_distance(mav, side) + mavproxy.send('rc 2 1500\n') + + print("Going east %u meters" % side) + mavproxy.send('rc 1 1610\n') + ok = wait_distance(mav, side) + mavproxy.send('rc 1 1500\n') + + print("Going south %u meters" % side) + mavproxy.send('rc 2 1610\n') + ok = wait_distance(mav, side) + mavproxy.send('rc 2 1500\n') + + print("Going west %u meters" % side) + mavproxy.send('rc 1 1390\n') + ok = wait_distance(mav, side) + mavproxy.send('rc 1 1500\n') + return ok + + + + def land(mavproxy, mav, timeout=60): '''land the quad''' + print("STARTING LANDING") mavproxy.send('switch 6\n') mavproxy.expect('STABILIZE>') mavproxy.send('status\n') mavproxy.expect('>') - mavproxy.send('rc 3 1300\n') + + # 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() - while time.time() < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - print("Altitude %u" % m.alt) - if m.alt <= 0: - print("LANDED OK") - return True - print("LANDING FAILED") - return False + if wait_altitude(mav, -5, 0): + print("LANDING OK") + return True + else: + print("LANDING FAILED") + return False @@ -93,22 +203,31 @@ def fly_ArduCopter(): sil = util.start_SIL('ArduCopter') mavproxy = util.start_MAVProxy_SIL('ArduCopter') mavproxy.expect('Please Run Setup') + # we need to restart it after eeprom erase mavproxy.close() sil.close() sil = util.start_SIL('ArduCopter') mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:14550 --quadcopter') + mavproxy.expect('Received [0-9]+ parameters') + + # setup test parameters + mavproxy.send("param load %s/ArduCopter.parm\n" % testdir) + mavproxy.expect('Loaded [0-9]+ parameters') + + # reboot with new parameters + mavproxy.close() + sil.close() + sil = util.start_SIL('ArduCopter') + mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:14550 --out=192.168.2.15:14550 --quadcopter --streamrate=1') mavproxy.expect('Logging to (\S+)') logfile = mavproxy.match.group(1) print("LOGFILE %s" % logfile) mavproxy.expect("Ready to FLY") mavproxy.expect('Received [0-9]+ parameters') - # setup test parameters - mavproxy.send("param load autotest/ArduCopter.parm\n") - mavproxy.expect('Loaded [0-9]+ parameters') - # start hil_quad.py + util.run_cmd('pkill -f hil_quad.py', checkfail=False) hquad = pexpect.spawn('HILTest/hil_quad.py --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION, logfile=sys.stdout, timeout=10) hquad.expect('Starting at') @@ -123,6 +242,7 @@ def fly_ArduCopter(): setup_rc(mavproxy) arm_motors(mavproxy) takeoff(mavproxy, mav) + fly_square(mavproxy, mav) loiter(mavproxy, mav) land(mavproxy, mav) disarm_motors(mavproxy) From 9eb27c257362dd57b1b57d5ef2c1a61f27fb2039 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Oct 2011 21:25:35 +1100 Subject: [PATCH 2/3] GCS: fixed timeout loading waypoints while loading waypoints we could saturate the link --- ArduPlane/GCS.h | 1 + ArduPlane/GCS_Mavlink.pde | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 5f999f6ca2..7172eeaf52 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -152,6 +152,7 @@ private: uint16_t waypoint_count; uint32_t waypoint_timelast_send; // milliseconds uint32_t waypoint_timelast_receive; // milliseconds + uint32_t waypoint_timelast_request; // milliseconds uint16_t waypoint_send_timeout; // milliseconds uint16_t waypoint_receive_timeout; // milliseconds diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 173400c571..8b9e4abc63 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -777,8 +777,15 @@ GCS_MAVLINK::update(void) send_message(MSG_NEXT_PARAM); } + if (!waypoint_receiving && !waypoint_sending) { + return; + } + + uint32_t tnow = millis(); + if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.command_total) { + waypoint_request_i <= (unsigned)g.command_total && + tnow > waypoint_timelast_request + 500) { send_message(MSG_NEXT_WAYPOINT); } @@ -1413,6 +1420,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; waypoint_receiving = true; waypoint_sending = false; waypoint_request_i = 0; @@ -1589,6 +1597,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // update waypoint receiving state machine waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; waypoint_request_i++; if (waypoint_request_i > (uint16_t)g.command_total){ From 56390c4696ec03edf8e6e4d5b5f9b5aac4be00a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Oct 2011 21:25:58 +1100 Subject: [PATCH 3/3] GCS: fixed timeout loading waypoints (ArduCopter) prevents link saturation --- ArduCopter/GCS.h | 1 + ArduCopter/GCS_Mavlink.pde | 16 +++++++++++++--- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 5f999f6ca2..7172eeaf52 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -152,6 +152,7 @@ private: uint16_t waypoint_count; uint32_t waypoint_timelast_send; // milliseconds uint32_t waypoint_timelast_receive; // milliseconds + uint32_t waypoint_timelast_request; // milliseconds uint16_t waypoint_send_timeout; // milliseconds uint16_t waypoint_receive_timeout; // milliseconds diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index ce351f2987..1f4a5c10bd 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -572,18 +572,26 @@ GCS_MAVLINK::update(void) send_message(MSG_NEXT_PARAM); } + if (!waypoint_receiving && !waypoint_sending) { + return; + } + + uint32_t tnow = millis(); + if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.waypoint_total && + tnow > waypoint_timelast_request + 500) { + waypoint_timelast_request = tnow; send_message(MSG_NEXT_WAYPOINT); } // stop waypoint sending if timeout - if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ + if (waypoint_sending && (tnow - waypoint_timelast_send) > waypoint_send_timeout){ waypoint_sending = false; } // stop waypoint receiving if timeout - if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ + if (waypoint_receiving && (tnow - waypoint_timelast_receive) > waypoint_receive_timeout){ waypoint_receiving = false; } } @@ -1151,6 +1159,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) waypoint_receiving = true; waypoint_sending = false; waypoint_request_i = 0; + waypoint_timelast_request = 0; break; } @@ -1296,6 +1305,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // update waypoint receiving state machine waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; waypoint_request_i++; if (waypoint_request_i > (uint16_t)g.waypoint_total){