Browse Source

autotest: switch to full accel modelling in multicopter simulation

the new AHRS code should allow for centripetal compensation in
multicopters
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
5cd5f40f09
  1. 9
      Tools/autotest/pysim/aircraft.py

9
Tools/autotest/pysim/aircraft.py

@ -23,7 +23,7 @@ class Aircraft(object): @@ -23,7 +23,7 @@ class Aircraft(object):
self.position = Vector3(0, 0, 0) # m North, East, Down
self.mass = 0.0
self.update_frequency = 50 # in Hz
self.gravity = 9.8 # m/s/s
self.gravity = 9.80665 # m/s/s
self.accelerometer = Vector3(0, 0, -self.gravity)
self.wind = util.Wind('0,0,0')
@ -47,9 +47,4 @@ class Aircraft(object): @@ -47,9 +47,4 @@ class Aircraft(object):
velocity_body = self.dcm.transposed() * self.velocity
# force the acceleration to mostly be from gravity. We should be using 100% accel_body,
# but right now that flies very badly as the AHRS system can't do centripetal correction
# for multicopters. This is a compromise until we get that sorted out
accel_true = self.accel_body
accel_fake = self.dcm.transposed() * Vector3(0, 0, -self.gravity)
self.accelerometer = (accel_true * 0.5) + (accel_fake * 0.5)
self.accelerometer = self.accel_body.copy()

Loading…
Cancel
Save