mirror of https://github.com/ArduPilot/ardupilot
autotest: setup Rover autotest for Sparkfun course
this will make it easier to test around the course
This commit is contained in:
Andrew Tridgell
parent
80582df4be
commit
4c9cb461d6
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 $*
|
||||
|
|
|
|||
Loading…
Reference in New Issue