pysim: update the multicopter simulator with correct acceleration

this re-works the multicopter simulator in terms of rotation matrices,
and adds full acceleration support, which means it will include linear
acceleration affects and centripetal acceleration

Tools/autotest/pysim/aircraft.py

@@ -1,5 +1,5 @@
-import euclid, math, util
-
+import math, util, rotmat
+from rotmat import Vector3, Matrix3
 
 class Aircraft(object):
     '''a basic aircraft class'''
@@ -14,67 +14,35 @@ class Aircraft(object):
         self.longitude = self.home_longitude
         self.altitude  = self.home_altitude
 
-        self.pitch = 0.0        # degrees
-        self.roll = 0.0         # degrees
-        self.yaw = 0.0          # degrees
-
-        # rates in earth frame
-        self.pitch_rate = 0.0   # degrees/s
-        self.roll_rate = 0.0    # degrees/s
-        self.yaw_rate = 0.0     # degrees/s
+        self.dcm = Matrix3()
 
-        # rates in body frame
-        self.pDeg = 0.0   # degrees/s
-        self.qDeg = 0.0   # degrees/s
-        self.rDeg = 0.0   # degrees/s
+        # rotation rate in body frame
+        self.gyro = Vector3(0,0,0) # rad/s
 
-        self.velocity = euclid.Vector3(0, 0, 0) # m/s, North, East, Up
-        self.position = euclid.Vector3(0, 0, 0) # m North, East, Up
-        self.accel    = euclid.Vector3(0, 0, 0) # m/s/s North, East, Up
+        self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
+        self.position = Vector3(0, 0, 0) # m North, East, Down
+        self.last_velocity = self.velocity.copy()
         self.mass = 0.0
         self.update_frequency = 50 # in Hz
         self.gravity = 9.8 # m/s/s
-        self.accelerometer = euclid.Vector3(0, 0, -self.gravity)
+        self.accelerometer = Vector3(0, 0, self.gravity)
 
         self.wind = util.Wind('0,0,0')
 
-    def normalise(self):
-        '''normalise roll, pitch and yaw
-
-        roll between -180 and 180
-        pitch between -180 and 180
-        yaw between 0 and 360
-
-        '''
-        def norm(angle, min, max):
-            while angle > max:
-                angle -= 360
-            while angle < min:
-                angle += 360
-            return angle
-        self.roll  = norm(self.roll, -180, 180)
-        self.pitch = norm(self.pitch, -180, 180)
-        self.yaw   = norm(self.yaw, 0, 360)
-
-    def update_position(self):
+    def update_position(self, delta_time):
         '''update lat/lon/alt from position'''
 
         radius_of_earth = 6378100.0 # in meters
         dlat = math.degrees(math.atan(self.position.x/radius_of_earth))
-        dlon = math.degrees(math.atan(self.position.y/radius_of_earth))
-
-        self.altitude  = self.home_altitude + self.position.z
         self.latitude  = self.home_latitude + dlat
+        lon_scale = math.cos(math.radians(self.latitude));
+        dlon = math.degrees(math.atan(self.position.y/radius_of_earth))/lon_scale
         self.longitude = self.home_longitude + dlon
 
-        from math import sin, cos, sqrt, radians
-        
+        self.altitude  = self.home_altitude - self.position.z
+
         # work out what the accelerometer would see
-        xAccel = sin(radians(self.pitch))
-        yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch))
-        zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch))
-        scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))
-        xAccel *= scale;
-        yAccel *= scale;
-        zAccel *= scale;
-        self.accelerometer = euclid.Vector3(xAccel, yAccel, zAccel)
+        self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity)
+#        self.accelerometer = Vector3(0,0,-self.gravity)
+        self.accelerometer = self.dcm.transposed() * self.accelerometer
+        self.last_velocity = self.velocity.copy()

Tools/autotest/pysim/multicopter.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python
 
 from aircraft import Aircraft
-import euclid, util, time, math
+import util, time, math
+from math import degrees, radians
+from rotmat import Vector3, Matrix3
 
 class Motor(object):
     def __init__(self, angle, clockwise, servo):
@@ -83,7 +85,7 @@ class MultiCopter(Aircraft):
         self.mass = mass # Kg
         self.hover_throttle = hover_throttle
         self.terminal_velocity = terminal_velocity
-        self.terminal_rotation_rate = 4*360.0
+        self.terminal_rotation_rate = 4*radians(360.0)
         self.frame_height = frame_height
 
         # scaling from total motor power to Newtons. Allows the copter
@@ -96,6 +98,7 @@ class MultiCopter(Aircraft):
         for i in range(0, len(self.motors)):
             servo = servos[self.motors[i].servo-1]
             if servo <= 0.0:
+
                 self.motor_speed[i] = 0
             else:
                 self.motor_speed[i] = servo
@@ -107,78 +110,58 @@ class MultiCopter(Aircraft):
         delta_time = t - self.last_time
         self.last_time = t
 
-        # rotational acceleration, in degrees/s/s, in body frame
-        roll_accel = 0.0
-        pitch_accel = 0.0
-        yaw_accel = 0.0
+        # rotational acceleration, in rad/s/s, in body frame
+        rot_accel = Vector3(0,0,0)
         thrust = 0.0
         for i in range(len(self.motors)):
-            roll_accel  += -5000.0 * math.sin(math.radians(self.motors[i].angle)) * m[i]
-            pitch_accel += 5000.0 * math.cos(math.radians(self.motors[i].angle)) * m[i]
+            rot_accel.x  += -radians(5000.0) * math.sin(radians(self.motors[i].angle)) * m[i]
+            rot_accel.y  +=  radians(5000.0) * math.cos(radians(self.motors[i].angle)) * m[i]
             if self.motors[i].clockwise:
-                yaw_accel -= m[i] * 400.0
+                rot_accel.z -= m[i] * radians(400.0)
             else:
-                yaw_accel += m[i] * 400.0
+                rot_accel.z += m[i] * radians(400.0)
             thrust += m[i] * self.thrust_scale # newtons
 
-        # rotational resistance
-        roll_accel  -= (self.pDeg / self.terminal_rotation_rate) * 5000.0
-        pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0
-        yaw_accel   -= (self.rDeg / self.terminal_rotation_rate) * 400.0
+        # rotational air resistance
+        rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate
+        rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
+        rot_accel.z -= self.gyro.z * radians(400.0)  / self.terminal_rotation_rate
 
         # update rotational rates in body frame
-        self.pDeg  += roll_accel * delta_time
-        self.qDeg  += pitch_accel * delta_time
-        self.rDeg  += yaw_accel * delta_time
-
-        # calculate rates in earth frame
-        (self.roll_rate,
-         self.pitch_rate,
-         self.yaw_rate) =  util.BodyRatesToEarthRates(self.roll, self.pitch, self.yaw,
-                                                      self.pDeg, self.qDeg, self.rDeg)
+        self.gyro += rot_accel * delta_time
 
-        # update rotation
-        self.roll   += self.roll_rate  * delta_time
-        self.pitch  += self.pitch_rate * delta_time
-        self.yaw    += self.yaw_rate   * delta_time
+        # update attitude
+        self.dcm.rotate(self.gyro * delta_time)
 
         # air resistance
         air_resistance = - self.velocity * (self.gravity/self.terminal_velocity)
 
-        # normalise rotations
-        self.normalise()
-
-        accel = thrust / self.mass
-
-        accel3D = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, accel)
-        accel3D += euclid.Vector3(0, 0, -self.gravity)
-        accel3D += air_resistance
-
+        accel_body = Vector3(0, 0, -thrust / self.mass)
+        accel_earth = self.dcm * accel_body
+        accel_earth += Vector3(0, 0, self.gravity)
+        accel_earth += air_resistance
         # add in some wind
-        accel3D += self.wind.accel(self.velocity)
+        accel_earth += self.wind.accel(self.velocity)
 
         # new velocity vector
-        self.velocity += accel3D * delta_time
-        self.accel     = accel3D
+        self.velocity += accel_earth * delta_time
 
         # new position vector
         old_position = self.position.copy()
         self.position += self.velocity * delta_time
 
         # constrain height to the ground
-        if self.position.z + self.home_altitude < self.ground_level + self.frame_height:
-            if old_position.z + self.home_altitude > self.ground_level + self.frame_height:
-                print("Hit ground at %f m/s" % (-self.velocity.z))
-            self.velocity = euclid.Vector3(0, 0, 0)
-            self.roll_rate = 0
-            self.pitch_rate = 0
-            self.yaw_rate = 0
-            self.roll = 0
-            self.pitch = 0
-            self.accel = euclid.Vector3(0, 0, 0)
-            self.position = euclid.Vector3(self.position.x, self.position.y,
-                                           self.ground_level + self.frame_height - self.home_altitude)
+        if (-self.position.z) + self.home_altitude < self.ground_level + self.frame_height:
+            if (-old_position.z) + self.home_altitude > self.ground_level + self.frame_height:
+                print("Hit ground at %f m/s" % (self.velocity.z))
 
-        # update lat/lon/altitude
-        self.update_position()
+            self.velocity = Vector3(0, 0, 0)
+            # zero roll/pitch, but keep yaw
+            (r, p, y) = self.dcm.to_euler()
+            self.dcm.from_euler(0, 0, y)
 
+            self.position = Vector3(self.position.x, self.position.y,
+                                    -(self.ground_level + self.frame_height - self.home_altitude))
+
+        # update lat/lon/altitude
+        self.update_position(delta_time)

Tools/autotest/pysim/sim_multicopter.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 
 from multicopter import MultiCopter
-import euclid, util, time, os, sys, math
+import util, time, os, sys, math
 import socket, struct
 import select, fgFDM, errno
 
@@ -16,16 +16,20 @@ import mavlink
 def sim_send(m, a):
     '''send flight information to mavproxy and flightgear'''
     global fdm
+    from math import degrees
+
+    earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
+    (roll, pitch, yaw) = a.dcm.to_euler()
 
     fdm.set('latitude', a.latitude, units='degrees')
     fdm.set('longitude', a.longitude, units='degrees')
     fdm.set('altitude', a.altitude, units='meters')
-    fdm.set('phi', a.roll, units='degrees')
-    fdm.set('theta', a.pitch, units='degrees')
-    fdm.set('psi', a.yaw, units='degrees')
-    fdm.set('phidot', a.roll_rate, units='dps')
-    fdm.set('thetadot', a.pitch_rate, units='dps')
-    fdm.set('psidot', a.yaw_rate, units='dps')
+    fdm.set('phi', roll, units='radians')
+    fdm.set('theta', pitch, units='radians')
+    fdm.set('psi', yaw, units='radians')
+    fdm.set('phidot', earth_rates.x, units='rps')
+    fdm.set('thetadot', earth_rates.y, units='rps')
+    fdm.set('psidot', earth_rates.z, units='rps')
     fdm.set('vcas', math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), units='mps')
     fdm.set('v_north', a.velocity.x, units='mps')
     fdm.set('v_east', a.velocity.y, units='mps')
@@ -40,11 +44,11 @@ def sim_send(m, a):
             raise
 
     buf = struct.pack('<16dI',
-                      a.latitude, a.longitude, a.altitude, a.yaw,
+                      a.latitude, a.longitude, a.altitude, degrees(yaw),
                       a.velocity.x, a.velocity.y,
-                      a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
-                      a.roll_rate, a.pitch_rate, a.yaw_rate,
-                      a.roll, a.pitch, a.yaw,
+                      -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),
                       0x4c56414e)
     try:

Tools/autotest/pysim/testwind.py

@@ -1,12 +1,12 @@
 #!/usr/bin/env python
 # simple test of wind generation code
 
-import util, euclid, time, random
+import util, time, random
 
 wind = util.Wind('3,90,0.1')
 
 t0 = time.time()
-velocity = euclid.Vector3(0,0,0)
+velocity = Vector3(0,0,0)
 
 t = 0
 deltat = 0.01

Tools/autotest/pysim/util.py

@@ -1,90 +1,8 @@
-import euclid, math
+import math
 import os, pexpect, sys, time, random
+from rotmat import Vector3, Matrix3
 from subprocess import call, check_call,Popen, PIPE
 
-def RPY_to_XYZ(roll, pitch, yaw, length):
-    '''convert roll, pitch and yaw in degrees to
-       Vector3 in X, Y and Z
-
-       inputs:
-
-       roll, pitch and yaw are in degrees
-       yaw == 0 when pointing North
-       roll == zero when horizontal. +ve roll means tilting to the right
-       pitch == zero when horizontal, +ve pitch means nose is pointing upwards
-       length is in an arbitrary linear unit.
-       When RPY is (0, 0, 0) then length represents distance upwards
-
-       outputs:
-       Vector3:
-       X is in units along latitude. +ve X means going North
-       Y is in units along longitude +ve Y means going East
-       Z is altitude in units (+ve is up)
-
-       test suite:
-
-       >>> RPY_to_XYZ(0, 0, 0, 1)
-       Vector3(0.00, 0.00, 1.00)
-
-       >>> RPY_to_XYZ(0, 0, 0, 2)
-       Vector3(0.00, 0.00, 2.00)
-
-       >>> RPY_to_XYZ(90, 0, 0, 1)
-       Vector3(0.00, 1.00, 0.00)
-
-       >>> RPY_to_XYZ(-90, 0, 0, 1)
-       Vector3(0.00, -1.00, 0.00)
-
-       >>> RPY_to_XYZ(0, 90, 0, 1)
-       Vector3(-1.00, 0.00, 0.00)
-
-       >>> RPY_to_XYZ(0, -90, 0, 1)
-       Vector3(1.00, 0.00, 0.00)
-
-       >>> RPY_to_XYZ(90, 0, 180, 1)
-       Vector3(-0.00, -1.00, 0.00)
-
-       >>> RPY_to_XYZ(-90, 0, 180, 1)
-       Vector3(0.00, 1.00, 0.00)
-
-       >>> RPY_to_XYZ(0, 90, 180, 1)
-       Vector3(1.00, -0.00, 0.00)
-
-       >>> RPY_to_XYZ(0, -90, 180, 1)
-       Vector3(-1.00, 0.00, 0.00)
-
-       >>> RPY_to_XYZ(90, 0, 90, 1)
-       Vector3(-1.00, 0.00, 0.00)
-
-       >>> RPY_to_XYZ(-90, 0, 90, 1)
-       Vector3(1.00, -0.00, 0.00)
-
-       >>> RPY_to_XYZ(90, 0, 270, 1)
-       Vector3(1.00, -0.00, 0.00)
-
-       >>> RPY_to_XYZ(-90, 0, 270, 1)
-       Vector3(-1.00, 0.00, 0.00)
-
-       >>> RPY_to_XYZ(0, 90, 90, 1)
-       Vector3(-0.00, -1.00, 0.00)
-
-       >>> RPY_to_XYZ(0, -90, 90, 1)
-       Vector3(0.00, 1.00, 0.00)
-
-       >>> RPY_to_XYZ(0, 90, 270, 1)
-       Vector3(0.00, 1.00, 0.00)
-
-       >>> RPY_to_XYZ(0, -90, 270, 1)
-       Vector3(-0.00, -1.00, 0.00)
-
-       '''
-
-    v = euclid.Vector3(0, 0, length)
-    v = euclid.Quaternion.new_rotate_euler(-math.radians(pitch), 0,  -math.radians(roll)) * v
-    v = euclid.Quaternion.new_rotate_euler(0, math.radians(yaw), 0) * v
-    return v
-
-
 def m2ft(x):
     '''meters to feet'''
     return float(x) / 0.3048
@@ -289,53 +207,48 @@ def check_parent(parent_pid=os.getppid()):
         sys.exit(1)
 
 
-def EarthRatesToBodyRates(roll, pitch, yaw,
-                          rollRate, pitchRate, yawRate):
+def EarthRatesToBodyRates(dcm, earth_rates):
     '''convert the angular velocities from earth frame to
     body frame. Thanks to James Goppert for the formula
 
-    all inputs and outputs are in degrees
+    all inputs and outputs are in radians
 
-    returns a tuple, (p,q,r)
+    returns a gyro vector in body frame, in rad/s
     '''
-    from math import radians, degrees, sin, cos, tan
+    from math import sin, cos
 
-    phi      = radians(roll)
-    theta    = radians(pitch)
-    phiDot   = radians(rollRate)
-    thetaDot = radians(pitchRate)
-    psiDot   = radians(yawRate)
+    (phi, theta, psi) = dcm.to_euler()
+    phiDot   = earth_rates.x
+    thetaDot = earth_rates.y
+    psiDot   = earth_rates.z
 
     p = phiDot - psiDot*sin(theta)
     q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
     r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
+    return Vector3(p, q, r)
 
-    return (degrees(p), degrees(q), degrees(r))
-
-def BodyRatesToEarthRates(roll, pitch, yaw, pDeg, qDeg, rDeg):
+def BodyRatesToEarthRates(dcm, gyro):
     '''convert the angular velocities from body frame to
     earth frame.
 
-    all inputs and outputs are in degrees
+    all inputs and outputs are in radians/s
 
-    returns a tuple, (rollRate,pitchRate,yawRate)
+    returns a earth rate vector
     '''
-    from math import radians, degrees, sin, cos, tan, fabs
+    from math import sin, cos, tan, fabs
 
-    p      = radians(pDeg)
-    q      = radians(qDeg)
-    r      = radians(rDeg)
+    p      = gyro.x
+    q      = gyro.y
+    r      = gyro.z
 
-    phi      = radians(roll)
-    theta    = radians(pitch)
+    (phi, theta, psi) = dcm.to_euler()
 
     phiDot   = p + tan(theta)*(q*sin(phi) + r*cos(phi))
     thetaDot = q*cos(phi) - r*sin(phi)
     if fabs(cos(theta)) < 1.0e-20:
         theta += 1.0e-10
     psiDot   = (q*sin(phi) + r*cos(phi))/cos(theta)
-
-    return (degrees(phiDot), degrees(thetaDot), degrees(psiDot))
+    return Vector3(phiDot, thetaDot, psiDot)
 
 
 class Wind(object):
@@ -382,12 +295,15 @@ class Wind(object):
         '''return current wind acceleration in ground frame.  The
            velocity is a Vector3 of the current velocity of the aircraft
            in earth frame, m/s'''
+        from math import radians
 
         (speed, direction) = self.current(deltat=deltat)
 
         # wind vector
-        v = euclid.Vector3(speed, 0, 0)
-        wind = euclid.Quaternion.new_rotate_euler(0, math.radians(direction), 0) * v
+        v = Vector3(speed, 0, 0)
+        m = Matrix3()
+        m.from_euler(0, 0, radians(direction))
+        wind = m.transposed() * v
 
         # relative wind vector
         relwind = wind - velocity