From ffd4662ec4d0cc13a6a708d29e132a1eeaf4fa3e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 20 Apr 2015 08:36:52 +1000
Subject: [PATCH] autotest: convert rover sim to use sim_wrapper.py

---
 Tools/autotest/pysim/rover.py       |  11 +-
 Tools/autotest/pysim/sim_rover.py   | 156 ----------------------------
 Tools/autotest/pysim/sim_wrapper.py |   4 +
 Tools/autotest/sim_vehicle.sh       |  10 +-
 4 files changed, 17 insertions(+), 164 deletions(-)
 delete mode 100755 Tools/autotest/pysim/sim_rover.py

diff --git a/Tools/autotest/pysim/rover.py b/Tools/autotest/pysim/rover.py
index 8d6c76e84b..8fcfb39726 100644
--- a/Tools/autotest/pysim/rover.py
+++ b/Tools/autotest/pysim/rover.py
@@ -11,6 +11,7 @@ from rotmat import Vector3, Matrix3
 class Rover(Aircraft):
     '''a simple rover'''
     def __init__(self,
+                 frame='',
                  max_speed=20,
                  max_accel=30,
                  wheelbase=0.335,
@@ -72,17 +73,17 @@ class Rover(Aircraft):
         steer = 0.5 * lat_accel * mincircle / (speed**2)
         return steer * 35
 
-    def update(self, state):
+    def update(self, servos):
 
         # if in skid steering mode the steering and throttle values are used for motor1 and motor2
         if self.skid_steering:
-            motor1 = state.steering # left motor
-            motor2 = state.throttle # right motor
+            motor1 = 2*(servos[0]-0.5)
+            motor2 = 2*(servos[2]-0.5)
             steering = motor1 - motor2
             throttle = 0.5*(motor1 + motor2)
         else:
-            steering = state.steering
-            throttle = state.throttle
+            steering = 2*(servos[0]-0.5)
+            throttle = 2*(servos[2]-0.5)
 
         # how much time has passed?
         t = self.time_now
diff --git a/Tools/autotest/pysim/sim_rover.py b/Tools/autotest/pysim/sim_rover.py
deleted file mode 100755
index 8c2bff13dc..0000000000
--- a/Tools/autotest/pysim/sim_rover.py
+++ /dev/null
@@ -1,156 +0,0 @@
-#!/usr/bin/env python
-'''
-simple rover simulator
-'''
-
-from rover import Rover
-import util, time, os, sys, math
-import socket, struct
-import select, errno
-
-def sim_send(a):
-    '''send flight information to mavproxy'''
-    from math import degrees
-
-    earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
-    (roll, pitch, yaw) = a.dcm.to_euler()
-
-    timestamp = int((a.time_now - a.time_base) * 1e6)
-
-    buf = struct.pack('<Q17dI',
-                      timestamp,
-                      a.latitude, a.longitude, a.altitude, degrees(yaw),
-                      a.velocity.x, a.velocity.y, a.velocity.z,
-                      a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
-                      degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z),
-                      degrees(roll), degrees(pitch), degrees(yaw),
-                      math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
-                      0x4c56414f)
-    try:
-        sim_out.send(buf)
-    except socket.error as e:
-        if not e.errno in [ errno.ECONNREFUSED ]:
-            raise
-
-
-def sim_recv(state):
-    '''receive control information from SITL'''
-    try:
-        buf = sim_in.recv(28)
-    except socket.error as e:
-        print(e, e.error)
-        if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
-            raise
-        return
-        
-    if len(buf) != 28:
-        print('len=%u' % len(buf))
-        return
-    control = list(struct.unpack('<14H', buf))
-    pwm = control[0:11]
-
-    # map steering and throttle to -1/1
-    state.steering = (pwm[0]-1500)/500.0
-    state.throttle = (pwm[2]-1500)/500.0
-
-    #print("steering=%f throttle=%f pwm=%s" % (state.steering, state.throttle, str(pwm)))
-    
-
-
-def interpret_address(addrstr):
-    '''interpret a IP:port string'''
-    a = addrstr.split(':')
-    a[1] = int(a[1])
-    return tuple(a)
-
-class ControlState:
-    def __init__(self):
-        # steering from -1 to 1, where -1 is left, 1 is right
-        self.steering = 0
-        # throttle from -1 to 1, where -1 is full reverse, 1 is full forward
-        self.throttle = 0
-
-##################
-# main program
-from optparse import OptionParser
-parser = OptionParser("sim_rover.py [options]")
-parser.add_option("--simin",  dest="simin",   help="SIM input (IP:port)",       default="127.0.0.1:5502")
-parser.add_option("--simout", dest="simout",  help="SIM output (IP:port)",      default="127.0.0.1:5501")
-parser.add_option("--home", dest="home",  type='string', default=None, help="home lat,lng,alt,hdg (required)")
-parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=1000)
-parser.add_option("--skid-steering", action='store_true', default=False, help="Use skid steering")
-parser.add_option("--speedup", type='float', default=1.0, help="speedup from realtime")
-
-
-(opts, args) = parser.parse_args()
-
-for m in [ 'home' ]:
-    if not opts.__dict__[m]:
-        print("Missing required option '%s'" % m)
-        parser.print_help()
-        sys.exit(1)
-
-# UDP socket addresses
-sim_out_address = interpret_address(opts.simout)
-sim_in_address  = interpret_address(opts.simin)
-
-# setup input from SITL
-sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-sim_in.bind(sim_in_address)
-sim_in.setblocking(1)
-
-# setup output to SITL
-sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-sim_out.connect(sim_out_address)
-sim_out.setblocking(0)
-
-# create the quadcopter model
-a = Rover(skid_steering=opts.skid_steering)
-
-# initial controls state
-state = ControlState()
-
-
-# parse home
-v = opts.home.split(',')
-if len(v) != 4:
-    print("home should be lat,lng,alt,hdg")
-    sys.exit(1)
-a.home_latitude  = float(v[0])
-a.home_longitude = float(v[1])
-a.home_altitude  = float(v[2])
-a.altitude       = a.home_altitude
-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 rate=%.1f" % (
-    a.home_latitude,
-    a.home_longitude,
-    a.altitude,
-    a.yaw,
-    opts.rate))
-
-a.setup_frame_time(opts.rate, opts.speedup)
-
-counter = 0
-
-while True:
-    # receive control input from SITL
-    sim_recv(state)
-
-    # update the model
-    a.update(state)
-
-    # send model state to SITL
-    sim_send(a)
-
-    a.sync_frame_time()
-    a.time_advance(a.frame_time)
-
-    counter += 1
-    if counter == 1000:
-        print("t=%f speedup=%.1f" % ((a.time_now - a.time_base), a.achieved_rate/a.rate))
-        counter = 0
diff --git a/Tools/autotest/pysim/sim_wrapper.py b/Tools/autotest/pysim/sim_wrapper.py
index b66739c1b6..dc846f0df6 100755
--- a/Tools/autotest/pysim/sim_wrapper.py
+++ b/Tools/autotest/pysim/sim_wrapper.py
@@ -144,6 +144,10 @@ elif opts.frame.startswith('CRRCSim'):
     from crrcsim import CRRCSim
     a = CRRCSim(frame=opts.frame)
     frame_rate = 360
+elif opts.frame.startswith('rover'):
+    from rover import Rover
+    a = Rover(frame=opts.frame)
+    frame_rate = 360
 else:    
     from multicopter import MultiCopter
     a = MultiCopter(frame=opts.frame)
diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh
index 68c27ebda3..5f871d934e 100755
--- a/Tools/autotest/sim_vehicle.sh
+++ b/Tools/autotest/sim_vehicle.sh
@@ -173,6 +173,10 @@ set -x
     VEHICLE=$(basename $PWD)
 }
 
+[ -z "$FRAME" -a "$VEHICLE" = "APMrover2" ] && {
+    FRAME="rover"
+}
+
 EXTRA_PARM=""
 EXTRA_SIM=""
 
@@ -223,8 +227,8 @@ case $FRAME in
         EXTRA_PARM="param set VTAIL_OUTPUT 4;"
         EXTRA_SIM="$EXTRA_SIM --vtail"
 	;;
-    skid)
-        EXTRA_SIM="$EXTRA_SIM --skid-steering"
+    rover|rover-skid)
+        EXTRA_SIM="$EXTRA_SIM --frame=$FRAME"
 	;;
     obc)
         BUILD_TARGET="sitl-obc"
@@ -341,7 +345,7 @@ EOF
         PARMS="copter_params.parm"
         ;;
     APMrover2)
-        RUNSIM="nice $autotest/pysim/sim_rover.py --home=$SIMHOME --rate=400 $EXTRA_SIM"
+        RUNSIM="nice $autotest/pysim/sim_wrapper.py --home=$SIMHOME --simin=$SIMIN_PORT --simout=$SIMOUT_PORT --fgout=$FG_PORT $EXTRA_SIM"
         PARMS="Rover.parm"
         ;;
     *)