mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-09 17:34:01 -04:00
autotest: fixed the calculation of the acceleration due to gravity
this fixes the attitude calculation for the multicopter simulation
This commit is contained in:
parent
b6fdf626e4
commit
5652ccd3c6
@ -70,7 +70,7 @@ class Aircraft(object):
|
|||||||
from math import sin, cos, sqrt, radians
|
from math import sin, cos, sqrt, radians
|
||||||
|
|
||||||
# work out what the accelerometer would see
|
# work out what the accelerometer would see
|
||||||
xAccel = sin(radians(self.pitch)) * cos(radians(self.roll))
|
xAccel = sin(radians(self.pitch))
|
||||||
yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch))
|
yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch))
|
||||||
zAccel = -cos(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))
|
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))
|
||||||
|
Loading…
Reference in New Issue
Block a user