From 420bb9cc75e5b792f34144cca730af5dfe1ef1b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Dec 2011 21:57:09 +1100 Subject: [PATCH] pysim: fixed the accelerometer calculation in the quad simulator this was causing severe drift in the attitude calculation --- Tools/autotest/pysim/aircraft.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py index 9087ec97ed..8b315238b0 100644 --- a/Tools/autotest/pysim/aircraft.py +++ b/Tools/autotest/pysim/aircraft.py @@ -65,6 +65,14 @@ class Aircraft(object): self.latitude = self.home_latitude + dlat self.longitude = self.home_longitude + dlon + from math import sin, cos, sqrt, radians + # work out what the accelerometer would see - self.accelerometer = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, 1) * -self.gravity - self.accelerometer -= self.accel + xAccel = sin(radians(self.pitch)) * cos(radians(self.roll)) + 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)