mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
pysim: update the multicopter simulator with correct acceleration
this re-works the multicopter simulator in terms of rotation matrices, and adds full acceleration support, which means it will include linear acceleration affects and centripetal acceleration
This commit is contained in:
Andrew Tridgell
parent
21c8351f2f
commit
ccfac19cef
@ -1,5 +1,5 @@
|
||||
import euclid, math, util
|
||||
|
||||
import math, util, rotmat
|
||||
from rotmat import Vector3, Matrix3
|
||||
|
||||
class Aircraft(object):
|
||||
'''a basic aircraft class'''
|
||||
@ -14,67 +14,35 @@ class Aircraft(object):
|
||||
self.longitude = self.home_longitude
|
||||
self.altitude = self.home_altitude
|
||||
|
||||
self.pitch = 0.0 # degrees
|
||||
self.roll = 0.0 # degrees
|
||||
self.yaw = 0.0 # degrees
|
||||
self.dcm = Matrix3()
|
||||
|
||||
# rates in earth frame
|
||||
self.pitch_rate = 0.0 # degrees/s
|
||||
self.roll_rate = 0.0 # degrees/s
|
||||
self.yaw_rate = 0.0 # degrees/s
|
||||
# rotation rate in body frame
|
||||
self.gyro = Vector3(0,0,0) # rad/s
|
||||
|
||||
# rates in body frame
|
||||
self.pDeg = 0.0 # degrees/s
|
||||
self.qDeg = 0.0 # degrees/s
|
||||
self.rDeg = 0.0 # degrees/s
|
||||
|
||||
self.velocity = euclid.Vector3(0, 0, 0) # m/s, North, East, Up
|
||||
self.position = euclid.Vector3(0, 0, 0) # m North, East, Up
|
||||
self.accel = euclid.Vector3(0, 0, 0) # m/s/s North, East, Up
|
||||
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 = euclid.Vector3(0, 0, -self.gravity)
|
||||
self.accelerometer = Vector3(0, 0, self.gravity)
|
||||
|
||||
self.wind = util.Wind('0,0,0')
|
||||
|
||||
def normalise(self):
|
||||
'''normalise roll, pitch and yaw
|
||||
|
||||
roll between -180 and 180
|
||||
pitch between -180 and 180
|
||||
yaw between 0 and 360
|
||||
|
||||
'''
|
||||
def norm(angle, min, max):
|
||||
while angle > max:
|
||||
angle -= 360
|
||||
while angle < min:
|
||||
angle += 360
|
||||
return angle
|
||||
self.roll = norm(self.roll, -180, 180)
|
||||
self.pitch = norm(self.pitch, -180, 180)
|
||||
self.yaw = norm(self.yaw, 0, 360)
|
||||
|
||||
def update_position(self):
|
||||
def update_position(self, delta_time):
|
||||
'''update lat/lon/alt from position'''
|
||||
|
||||
radius_of_earth = 6378100.0 # in meters
|
||||
dlat = math.degrees(math.atan(self.position.x/radius_of_earth))
|
||||
dlon = math.degrees(math.atan(self.position.y/radius_of_earth))
|
||||
|
||||
self.altitude = self.home_altitude + self.position.z
|
||||
self.latitude = self.home_latitude + dlat
|
||||
lon_scale = math.cos(math.radians(self.latitude));
|
||||
dlon = math.degrees(math.atan(self.position.y/radius_of_earth))/lon_scale
|
||||
self.longitude = self.home_longitude + dlon
|
||||
|
||||
from math import sin, cos, sqrt, radians
|
||||
self.altitude = self.home_altitude - self.position.z
|
||||
|
||||
# work out what the accelerometer would see
|
||||
xAccel = sin(radians(self.pitch))
|
||||
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)
|
||||
self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity)
|
||||
# self.accelerometer = Vector3(0,0,-self.gravity)
|
||||
self.accelerometer = self.dcm.transposed() * self.accelerometer
|
||||
self.last_velocity = self.velocity.copy()
|
||||
|
||||
@ -1,7 +1,9 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from aircraft import Aircraft
|
||||
import euclid, util, time, math
|
||||
import util, time, math
|
||||
from math import degrees, radians
|
||||
from rotmat import Vector3, Matrix3
|
||||
|
||||
class Motor(object):
|
||||
def __init__(self, angle, clockwise, servo):
|
||||
@ -83,7 +85,7 @@ class MultiCopter(Aircraft):
|
||||
self.mass = mass # Kg
|
||||
self.hover_throttle = hover_throttle
|
||||
self.terminal_velocity = terminal_velocity
|
||||
self.terminal_rotation_rate = 4*360.0
|
||||
self.terminal_rotation_rate = 4*radians(360.0)
|
||||
self.frame_height = frame_height
|
||||
|
||||
# scaling from total motor power to Newtons. Allows the copter
|
||||
@ -96,6 +98,7 @@ class MultiCopter(Aircraft):
|
||||
for i in range(0, len(self.motors)):
|
||||
servo = servos[self.motors[i].servo-1]
|
||||
if servo <= 0.0:
|
||||
|
||||
self.motor_speed[i] = 0
|
||||
else:
|
||||
self.motor_speed[i] = servo
|
||||
@ -107,78 +110,58 @@ class MultiCopter(Aircraft):
|
||||
delta_time = t - self.last_time
|
||||
self.last_time = t
|
||||
|
||||
# rotational acceleration, in degrees/s/s, in body frame
|
||||
roll_accel = 0.0
|
||||
pitch_accel = 0.0
|
||||
yaw_accel = 0.0
|
||||
# rotational acceleration, in rad/s/s, in body frame
|
||||
rot_accel = Vector3(0,0,0)
|
||||
thrust = 0.0
|
||||
for i in range(len(self.motors)):
|
||||
roll_accel += -5000.0 * math.sin(math.radians(self.motors[i].angle)) * m[i]
|
||||
pitch_accel += 5000.0 * math.cos(math.radians(self.motors[i].angle)) * m[i]
|
||||
rot_accel.x += -radians(5000.0) * math.sin(radians(self.motors[i].angle)) * m[i]
|
||||
rot_accel.y += radians(5000.0) * math.cos(radians(self.motors[i].angle)) * m[i]
|
||||
if self.motors[i].clockwise:
|
||||
yaw_accel -= m[i] * 400.0
|
||||
rot_accel.z -= m[i] * radians(400.0)
|
||||
else:
|
||||
yaw_accel += m[i] * 400.0
|
||||
rot_accel.z += m[i] * radians(400.0)
|
||||
thrust += m[i] * self.thrust_scale # newtons
|
||||
|
||||
# rotational resistance
|
||||
roll_accel -= (self.pDeg / self.terminal_rotation_rate) * 5000.0
|
||||
pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0
|
||||
yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0
|
||||
# rotational air resistance
|
||||
rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate
|
||||
rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
|
||||
rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate
|
||||
|
||||
# update rotational rates in body frame
|
||||
self.pDeg += roll_accel * delta_time
|
||||
self.qDeg += pitch_accel * delta_time
|
||||
self.rDeg += yaw_accel * delta_time
|
||||
self.gyro += rot_accel * delta_time
|
||||
|
||||
# calculate rates in earth frame
|
||||
(self.roll_rate,
|
||||
self.pitch_rate,
|
||||
self.yaw_rate) = util.BodyRatesToEarthRates(self.roll, self.pitch, self.yaw,
|
||||
self.pDeg, self.qDeg, self.rDeg)
|
||||
|
||||
# update rotation
|
||||
self.roll += self.roll_rate * delta_time
|
||||
self.pitch += self.pitch_rate * delta_time
|
||||
self.yaw += self.yaw_rate * delta_time
|
||||
# update attitude
|
||||
self.dcm.rotate(self.gyro * delta_time)
|
||||
|
||||
# air resistance
|
||||
air_resistance = - self.velocity * (self.gravity/self.terminal_velocity)
|
||||
|
||||
# normalise rotations
|
||||
self.normalise()
|
||||
|
||||
accel = thrust / self.mass
|
||||
|
||||
accel3D = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, accel)
|
||||
accel3D += euclid.Vector3(0, 0, -self.gravity)
|
||||
accel3D += air_resistance
|
||||
|
||||
accel_body = Vector3(0, 0, -thrust / self.mass)
|
||||
accel_earth = self.dcm * accel_body
|
||||
accel_earth += Vector3(0, 0, self.gravity)
|
||||
accel_earth += air_resistance
|
||||
# add in some wind
|
||||
accel3D += self.wind.accel(self.velocity)
|
||||
accel_earth += self.wind.accel(self.velocity)
|
||||
|
||||
# new velocity vector
|
||||
self.velocity += accel3D * delta_time
|
||||
self.accel = accel3D
|
||||
self.velocity += accel_earth * delta_time
|
||||
|
||||
# new position vector
|
||||
old_position = self.position.copy()
|
||||
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 old_position.z + self.home_altitude > self.ground_level + self.frame_height:
|
||||
print("Hit ground at %f m/s" % (-self.velocity.z))
|
||||
self.velocity = euclid.Vector3(0, 0, 0)
|
||||
self.roll_rate = 0
|
||||
self.pitch_rate = 0
|
||||
self.yaw_rate = 0
|
||||
self.roll = 0
|
||||
self.pitch = 0
|
||||
self.accel = euclid.Vector3(0, 0, 0)
|
||||
self.position = euclid.Vector3(self.position.x, self.position.y,
|
||||
self.ground_level + self.frame_height - self.home_altitude)
|
||||
if (-self.position.z) + self.home_altitude < self.ground_level + self.frame_height:
|
||||
if (-old_position.z) + self.home_altitude > self.ground_level + self.frame_height:
|
||||
print("Hit ground at %f m/s" % (self.velocity.z))
|
||||
|
||||
self.velocity = Vector3(0, 0, 0)
|
||||
# zero roll/pitch, but keep yaw
|
||||
(r, p, y) = self.dcm.to_euler()
|
||||
self.dcm.from_euler(0, 0, y)
|
||||
|
||||
self.position = Vector3(self.position.x, self.position.y,
|
||||
-(self.ground_level + self.frame_height - self.home_altitude))
|
||||
|
||||
# update lat/lon/altitude
|
||||
self.update_position()
|
||||
|
||||
self.update_position(delta_time)
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from multicopter import MultiCopter
|
||||
import euclid, util, time, os, sys, math
|
||||
import util, time, os, sys, math
|
||||
import socket, struct
|
||||
import select, fgFDM, errno
|
||||
|
||||
@ -16,16 +16,20 @@ import mavlink
|
||||
def sim_send(m, a):
|
||||
'''send flight information to mavproxy and flightgear'''
|
||||
global fdm
|
||||
from math import degrees
|
||||
|
||||
earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
|
||||
(roll, pitch, yaw) = a.dcm.to_euler()
|
||||
|
||||
fdm.set('latitude', a.latitude, units='degrees')
|
||||
fdm.set('longitude', a.longitude, units='degrees')
|
||||
fdm.set('altitude', a.altitude, units='meters')
|
||||
fdm.set('phi', a.roll, units='degrees')
|
||||
fdm.set('theta', a.pitch, units='degrees')
|
||||
fdm.set('psi', a.yaw, units='degrees')
|
||||
fdm.set('phidot', a.roll_rate, units='dps')
|
||||
fdm.set('thetadot', a.pitch_rate, units='dps')
|
||||
fdm.set('psidot', a.yaw_rate, units='dps')
|
||||
fdm.set('phi', roll, units='radians')
|
||||
fdm.set('theta', pitch, units='radians')
|
||||
fdm.set('psi', yaw, units='radians')
|
||||
fdm.set('phidot', earth_rates.x, units='rps')
|
||||
fdm.set('thetadot', earth_rates.y, units='rps')
|
||||
fdm.set('psidot', earth_rates.z, units='rps')
|
||||
fdm.set('vcas', math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), units='mps')
|
||||
fdm.set('v_north', a.velocity.x, units='mps')
|
||||
fdm.set('v_east', a.velocity.y, units='mps')
|
||||
@ -40,11 +44,11 @@ def sim_send(m, a):
|
||||
raise
|
||||
|
||||
buf = struct.pack('<16dI',
|
||||
a.latitude, a.longitude, a.altitude, a.yaw,
|
||||
a.latitude, a.longitude, a.altitude, degrees(yaw),
|
||||
a.velocity.x, a.velocity.y,
|
||||
a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
|
||||
a.roll_rate, a.pitch_rate, a.yaw_rate,
|
||||
a.roll, a.pitch, a.yaw,
|
||||
-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),
|
||||
0x4c56414e)
|
||||
try:
|
||||
|
||||
@ -1,12 +1,12 @@
|
||||
#!/usr/bin/env python
|
||||
# simple test of wind generation code
|
||||
|
||||
import util, euclid, time, random
|
||||
import util, time, random
|
||||
|
||||
wind = util.Wind('3,90,0.1')
|
||||
|
||||
t0 = time.time()
|
||||
velocity = euclid.Vector3(0,0,0)
|
||||
velocity = Vector3(0,0,0)
|
||||
|
||||
t = 0
|
||||
deltat = 0.01
|
||||
|
||||
@ -1,90 +1,8 @@
|
||||
import euclid, math
|
||||
import math
|
||||
import os, pexpect, sys, time, random
|
||||
from rotmat import Vector3, Matrix3
|
||||
from subprocess import call, check_call,Popen, PIPE
|
||||
|
||||
def RPY_to_XYZ(roll, pitch, yaw, length):
|
||||
'''convert roll, pitch and yaw in degrees to
|
||||
Vector3 in X, Y and Z
|
||||
|
||||
inputs:
|
||||
|
||||
roll, pitch and yaw are in degrees
|
||||
yaw == 0 when pointing North
|
||||
roll == zero when horizontal. +ve roll means tilting to the right
|
||||
pitch == zero when horizontal, +ve pitch means nose is pointing upwards
|
||||
length is in an arbitrary linear unit.
|
||||
When RPY is (0, 0, 0) then length represents distance upwards
|
||||
|
||||
outputs:
|
||||
Vector3:
|
||||
X is in units along latitude. +ve X means going North
|
||||
Y is in units along longitude +ve Y means going East
|
||||
Z is altitude in units (+ve is up)
|
||||
|
||||
test suite:
|
||||
|
||||
>>> RPY_to_XYZ(0, 0, 0, 1)
|
||||
Vector3(0.00, 0.00, 1.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, 0, 0, 2)
|
||||
Vector3(0.00, 0.00, 2.00)
|
||||
|
||||
>>> RPY_to_XYZ(90, 0, 0, 1)
|
||||
Vector3(0.00, 1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(-90, 0, 0, 1)
|
||||
Vector3(0.00, -1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, 90, 0, 1)
|
||||
Vector3(-1.00, 0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, -90, 0, 1)
|
||||
Vector3(1.00, 0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(90, 0, 180, 1)
|
||||
Vector3(-0.00, -1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(-90, 0, 180, 1)
|
||||
Vector3(0.00, 1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, 90, 180, 1)
|
||||
Vector3(1.00, -0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, -90, 180, 1)
|
||||
Vector3(-1.00, 0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(90, 0, 90, 1)
|
||||
Vector3(-1.00, 0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(-90, 0, 90, 1)
|
||||
Vector3(1.00, -0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(90, 0, 270, 1)
|
||||
Vector3(1.00, -0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(-90, 0, 270, 1)
|
||||
Vector3(-1.00, 0.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, 90, 90, 1)
|
||||
Vector3(-0.00, -1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, -90, 90, 1)
|
||||
Vector3(0.00, 1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, 90, 270, 1)
|
||||
Vector3(0.00, 1.00, 0.00)
|
||||
|
||||
>>> RPY_to_XYZ(0, -90, 270, 1)
|
||||
Vector3(-0.00, -1.00, 0.00)
|
||||
|
||||
'''
|
||||
|
||||
v = euclid.Vector3(0, 0, length)
|
||||
v = euclid.Quaternion.new_rotate_euler(-math.radians(pitch), 0, -math.radians(roll)) * v
|
||||
v = euclid.Quaternion.new_rotate_euler(0, math.radians(yaw), 0) * v
|
||||
return v
|
||||
|
||||
|
||||
def m2ft(x):
|
||||
'''meters to feet'''
|
||||
return float(x) / 0.3048
|
||||
@ -289,53 +207,48 @@ def check_parent(parent_pid=os.getppid()):
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def EarthRatesToBodyRates(roll, pitch, yaw,
|
||||
rollRate, pitchRate, yawRate):
|
||||
def EarthRatesToBodyRates(dcm, earth_rates):
|
||||
'''convert the angular velocities from earth frame to
|
||||
body frame. Thanks to James Goppert for the formula
|
||||
|
||||
all inputs and outputs are in degrees
|
||||
all inputs and outputs are in radians
|
||||
|
||||
returns a tuple, (p,q,r)
|
||||
returns a gyro vector in body frame, in rad/s
|
||||
'''
|
||||
from math import radians, degrees, sin, cos, tan
|
||||
from math import sin, cos
|
||||
|
||||
phi = radians(roll)
|
||||
theta = radians(pitch)
|
||||
phiDot = radians(rollRate)
|
||||
thetaDot = radians(pitchRate)
|
||||
psiDot = radians(yawRate)
|
||||
(phi, theta, psi) = dcm.to_euler()
|
||||
phiDot = earth_rates.x
|
||||
thetaDot = earth_rates.y
|
||||
psiDot = earth_rates.z
|
||||
|
||||
p = phiDot - psiDot*sin(theta)
|
||||
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
|
||||
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
|
||||
return Vector3(p, q, r)
|
||||
|
||||
return (degrees(p), degrees(q), degrees(r))
|
||||
|
||||
def BodyRatesToEarthRates(roll, pitch, yaw, pDeg, qDeg, rDeg):
|
||||
def BodyRatesToEarthRates(dcm, gyro):
|
||||
'''convert the angular velocities from body frame to
|
||||
earth frame.
|
||||
|
||||
all inputs and outputs are in degrees
|
||||
all inputs and outputs are in radians/s
|
||||
|
||||
returns a tuple, (rollRate,pitchRate,yawRate)
|
||||
returns a earth rate vector
|
||||
'''
|
||||
from math import radians, degrees, sin, cos, tan, fabs
|
||||
from math import sin, cos, tan, fabs
|
||||
|
||||
p = radians(pDeg)
|
||||
q = radians(qDeg)
|
||||
r = radians(rDeg)
|
||||
p = gyro.x
|
||||
q = gyro.y
|
||||
r = gyro.z
|
||||
|
||||
phi = radians(roll)
|
||||
theta = radians(pitch)
|
||||
(phi, theta, psi) = dcm.to_euler()
|
||||
|
||||
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi))
|
||||
thetaDot = q*cos(phi) - r*sin(phi)
|
||||
if fabs(cos(theta)) < 1.0e-20:
|
||||
theta += 1.0e-10
|
||||
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta)
|
||||
|
||||
return (degrees(phiDot), degrees(thetaDot), degrees(psiDot))
|
||||
return Vector3(phiDot, thetaDot, psiDot)
|
||||
|
||||
|
||||
class Wind(object):
|
||||
@ -382,12 +295,15 @@ class Wind(object):
|
||||
'''return current wind acceleration in ground frame. The
|
||||
velocity is a Vector3 of the current velocity of the aircraft
|
||||
in earth frame, m/s'''
|
||||
from math import radians
|
||||
|
||||
(speed, direction) = self.current(deltat=deltat)
|
||||
|
||||
# wind vector
|
||||
v = euclid.Vector3(speed, 0, 0)
|
||||
wind = euclid.Quaternion.new_rotate_euler(0, math.radians(direction), 0) * v
|
||||
v = Vector3(speed, 0, 0)
|
||||
m = Matrix3()
|
||||
m.from_euler(0, 0, radians(direction))
|
||||
wind = m.transposed() * v
|
||||
|
||||
# relative wind vector
|
||||
relwind = wind - velocity
|
||||
|
||||
Loading…
Reference in New Issue
Block a user