diff --git a/Tools/autotest/Rover.parm b/Tools/autotest/Rover.parm index 139641ed52..41dcfb2b04 100644 --- a/Tools/autotest/Rover.parm +++ b/Tools/autotest/Rover.parm @@ -1,10 +1,13 @@ +WP_RADIUS 3 LOG_BITMASK 4095 MAG_ENABLE 1 -CRUISE_SPEED 7 -CRUISE_THROTTLE 80 +CRUISE_SPEED 5 +CRUISE_THROTTLE 40 THR_MAX 100 RC3_MAX 2000 RC3_MIN 1000 +RC1_MAX 2000 +RC2_MIN 1000 RC3_TRIM 1500 MODE1 0 MODE2 0 @@ -12,4 +15,5 @@ MODE3 11 MODE4 10 MODE5 2 MODE6 0 -STEER2SRV_P 0.6 +STEER2SRV_P 1.5 +HDNG2STEER_P 1.0 diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index c944a21564..606a2d5dbd 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -8,8 +8,8 @@ import mavutil, random testdir=os.path.dirname(os.path.realpath(__file__)) -HOME=mavutil.location(-35.362938,149.165085,584,270) - +#HOME=mavutil.location(-35.362938,149.165085,584,270) +HOME=mavutil.location(40.071374969556928,-105.22978898137808,1583.702759,246) homeloc = None def drive_left_circuit(mavproxy, mav): @@ -144,15 +144,15 @@ def drive_APMrover2(viewerip=None, map=False): mav.wait_gps_fix() homeloc = mav.location() print("Home location: %s" % homeloc) - if not drive_left_circuit(mavproxy, mav): - print("Failed left circuit") - failed = True if not drive_mission(mavproxy, mav, os.path.join(testdir, "rover1.txt")): print("Failed mission") failed = True - if not drive_RTL(mavproxy, mav): - print("Failed RTL") - failed = True +# if not drive_left_circuit(mavproxy, mav): +# print("Failed left circuit") +# failed = True +# if not drive_RTL(mavproxy, mav): +# print("Failed RTL") +# failed = True except pexpect.TIMEOUT, e: print("Failed with timeout") failed = True diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py index 7b4b13a864..0c6cb564aa 100644 --- a/Tools/autotest/pysim/aircraft.py +++ b/Tools/autotest/pysim/aircraft.py @@ -48,3 +48,10 @@ class Aircraft(object): velocity_body = self.dcm.transposed() * self.velocity self.accelerometer = self.accel_body.copy() + + def set_yaw_degrees(self, yaw_degrees): + '''rotate to the given yaw''' + (roll, pitch, yaw) = self.dcm.to_euler() + yaw = math.radians(yaw_degrees) + self.dcm.from_euler(roll, pitch, yaw) + diff --git a/Tools/autotest/pysim/rover.py b/Tools/autotest/pysim/rover.py index f9c2867af7..a3c66fcb3d 100644 --- a/Tools/autotest/pysim/rover.py +++ b/Tools/autotest/pysim/rover.py @@ -11,9 +11,9 @@ from rotmat import Vector3, Matrix3 class Rover(Aircraft): '''a simple rover''' def __init__(self, - max_speed=10, + max_speed=13, max_accel=10, - max_turn_rate=45, + max_turn_rate=100, skid_steering=False): Aircraft.__init__(self) self.max_speed = max_speed diff --git a/Tools/autotest/pysim/sim_rover.py b/Tools/autotest/pysim/sim_rover.py index 8c3f36e3cd..5563db5216 100755 --- a/Tools/autotest/pysim/sim_rover.py +++ b/Tools/autotest/pysim/sim_rover.py @@ -120,6 +120,8 @@ a.yaw = float(v[3]) a.latitude = a.home_latitude a.longitude = a.home_longitude +a.set_yaw_degrees(a.yaw) + print("Starting at lat=%f lon=%f alt=%f heading=%.1f" % ( a.home_latitude, a.home_longitude, diff --git a/Tools/autotest/rover1.txt b/Tools/autotest/rover1.txt index bee0e3d31f..9664246746 100644 --- a/Tools/autotest/rover1.txt +++ b/Tools/autotest/rover1.txt @@ -1,6 +1,15 @@ QGC WPL 110 -0 1 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 584.000000 1 -1 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362049 149.164810 97.790001 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363449 149.164978 97.809998 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363430 149.165359 98.580002 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361992 149.165176 98.720001 1 +0 1 0 16 0 0 0 0 40.071375 -105.229789 1584.000000 1 +1 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071300 -105.230028 0.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071186 -105.230063 0.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070974 -105.229969 0.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070935 -105.229915 0.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070869 -105.229937 0.000000 1 +6 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070845 -105.229848 0.000000 1 +7 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070713 -105.229701 0.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070738 -105.229535 0.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070806 -105.229277 0.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 40.070910 -105.229081 0.000000 1 +11 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071251 -105.229363 0.000000 1 +12 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071403 -105.229486 0.000000 1 +13 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071371 -105.229829 0.000000 1 diff --git a/Tools/autotest/sim_rover.sh b/Tools/autotest/sim_rover.sh index b1988f2009..24b8a5bc2a 100755 --- a/Tools/autotest/sim_rover.sh +++ b/Tools/autotest/sim_rover.sh @@ -19,7 +19,7 @@ gnome-terminal -e "nice /tmp/APMrover2.build/APMrover2.elf" #gnome-terminal -e "valgrind --db-attach=yes -q /tmp/APMrover2.build/APMrover2.elf" sleep 2 rm -f $tfile -gnome-terminal -e "nice ../Tools/autotest/pysim/sim_rover.py --home=-35.362938,149.165085,584,270 --rate=400" +gnome-terminal -e "nice ../Tools/autotest/pysim/sim_rover.py --home=40.071374969556928,-105.22978898137808,1583.702759,246 --rate=400" sleep 2 popd mavproxy.py --aircraft=test --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14550 --out 127.0.0.1:14551 $*