autotest: setup Rover autotest for Sparkfun course

this will make it easier to test around the course

Tools/autotest/Rover.parm

@@ -1,10 +1,13 @@
+WP_RADIUS 3
 LOG_BITMASK	4095
 MAG_ENABLE	1
-CRUISE_SPEED     7
+CRUISE_SPEED     5
-CRUISE_THROTTLE  80
+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

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):
+#        if not drive_left_circuit(mavproxy, mav):
-            print("Failed RTL")
+#            print("Failed left circuit")
-            failed = True
+#            failed = True
+#        if not drive_RTL(mavproxy, mav):
+#            print("Failed RTL")
+#            failed = True
     except pexpect.TIMEOUT, e:
         print("Failed with timeout")
         failed = True

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)
+        

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

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,

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
+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	-35.362049	149.164810	97.790001	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	-35.363449	149.164978	97.809998	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	-35.363430	149.165359	98.580002	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	-35.361992	149.165176	98.720001	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

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 $*