pysim: added experimental acceleration support

Tools/autotest/pysim/aircraft.py

@@ -21,14 +21,20 @@ class Aircraft(object):
 
         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 = Vector3(0, 0, self.gravity)
+        self.accelerometer = Vector3(0, 0, -self.gravity)
 
         self.wind = util.Wind('0,0,0')
 
+    def on_ground(self, position=None):
+        '''return true if we are on the ground'''
+        if position is None:
+            position = self.position
+        return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
+
+
     def update_position(self, delta_time):
         '''update lat/lon/alt from position'''
 
@@ -41,8 +47,7 @@ class Aircraft(object):
 
         self.altitude  = self.home_altitude - self.position.z
 
+        velocity_body = self.dcm.transposed() * self.velocity
+
         # work out what the accelerometer would see
-        self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity)
+        self.accelerometer = self.accel_body
-#        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

@@ -143,6 +143,15 @@ class MultiCopter(Aircraft):
         # add in some wind
         accel_earth += self.wind.accel(self.velocity)
 
+        # if we're on the ground, then our vertical acceleration is limited
+        # to zero. This effectively adds the force of the ground on the aircraft
+        if self.on_ground() and accel_earth.z > 0:
+            accel_earth.z = 0
+
+        # work out acceleration as seen by the accelerometers. It sees the kinematic
+        # acceleration (ie. real movement), plus gravity
+        self.accel_body = self.dcm.transposed() * (accel_earth + Vector3(0, 0, -self.gravity))
+
         # new velocity vector
         self.velocity += accel_earth * delta_time
 
@@ -151,8 +160,8 @@ class MultiCopter(Aircraft):
         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 self.on_ground():
-            if (-old_position.z) + self.home_altitude > self.ground_level + self.frame_height:
+            if not self.on_ground(old_position):
                 print("Hit ground at %f m/s" % (self.velocity.z))
 
             self.velocity = Vector3(0, 0, 0)

Tools/autotest/pysim/sim_multicopter.py

@@ -46,7 +46,7 @@ def sim_send(m, a):
     buf = struct.pack('<16dI',
                       a.latitude, a.longitude, a.altitude, degrees(yaw),
                       a.velocity.x, a.velocity.y,
-                      -a.accelerometer.x, -a.accelerometer.y, -a.accelerometer.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),