autotest: fixed body/earth frames in sim_quad physics model

Jason, can you have a look at this?

Tools/autotest/pysim/aircraft.py

@@ -17,9 +17,17 @@ class Aircraft(object):
         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
+
+        # rates in body frame
+        self.pDeg = 0.0   # degrees/s
+        self.qDeg = 0.0   # degrees/s
+        self.rDeg = 0.0   # degrees/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

Tools/autotest/pysim/quadcopter.py

@@ -34,15 +34,21 @@ class QuadCopter(Aircraft):
         delta_time = t - self.last_time
         self.last_time = t
 
-        # rotational acceleration, in degrees/s/s
+        # rotational acceleration, in degrees/s/s, in body frame
         roll_accel  = (m[1] - m[0]) * 5000.0
         pitch_accel = (m[2] - m[3]) * 5000.0
         yaw_accel   = -((m[2]+m[3]) - (m[0]+m[1])) * 400.0
 
-        # update rotational rates
-        self.roll_rate  += roll_accel * delta_time
-        self.pitch_rate += pitch_accel * delta_time
-        self.yaw_rate   += yaw_accel * delta_time
+        # 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)
 
         # update rotation
         self.roll   += self.roll_rate  * delta_time

Tools/autotest/pysim/util.py

@@ -290,6 +290,55 @@ def check_parent(parent_pid=os.getppid()):
         sys.exit(1)
 
 
+def EarthRatesToBodyRates(roll, pitch, yaw,
+                          rollRate, pitchRate, yawRate):
+    '''convert the angular velocities from earth frame to
+    body frame. Thanks to James Goppert for the formula
+
+    all inputs and outputs are in degrees
+
+    returns a tuple, (p,q,r)
+    '''
+    from math import radians, degrees, sin, cos, tan
+
+    phi      = radians(roll)
+    theta    = radians(pitch)
+    phiDot   = radians(rollRate)
+    thetaDot = radians(pitchRate)
+    psiDot   = radians(yawRate)
+
+    p = phiDot - psiDot*sin(theta)
+    q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
+    r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
+
+    return (degrees(p), degrees(q), degrees(r))
+
+def BodyRatesToEarthRates(roll, pitch, yaw, pDeg, qDeg, rDeg):
+    '''convert the angular velocities from body frame to
+    earth frame.
+
+    all inputs and outputs are in degrees
+
+    returns a tuple, (rollRate,pitchRate,yawRate)
+    '''
+    from math import radians, degrees, sin, cos, tan, fabs
+
+    p      = radians(pDeg)
+    q      = radians(qDeg)
+    r      = radians(rDeg)
+
+    phi      = radians(roll)
+    theta    = radians(pitch)
+
+    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))
+
+
 if __name__ == "__main__":
     import doctest
     doctest.testmod()