Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

pysim: fixed the accelerometer calculation in the quad simulator 

this was causing severe drift in the attitude calculation
This commit is contained in:
Andrew Tridgell 2011-12-12 21:57:09 +11:00
parent 5760239670
commit 420bb9cc75
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/autotest/pysim/aircraft.py
View File

@ -65,6 +65,14 @@ class Aircraft(object):
self.latitude = self.home_latitude + dlat self.latitude = self.home_latitude + dlat
self.longitude = self.home_longitude + dlon self.longitude = self.home_longitude + dlon
from math import sin, cos, sqrt, radians
# work out what the accelerometer would see # work out what the accelerometer would see
self.accelerometer = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, 1) * -self.gravity xAccel = sin(radians(self.pitch)) * cos(radians(self.roll))
self.accelerometer -= self.accel 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)