Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

pysim: update the multicopter simulator with correct acceleration

Browse Source 
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 2012-03-22 22:50:47 +11:00
parent 21c8351f2f
commit ccfac19cef
5 changed files with 96 additions and 225 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,5 +1,5 @@
import euclid, math, util import math, util, rotmat
from rotmat import Vector3, Matrix3
class Aircraft(object): class Aircraft(object):
'''a basic aircraft class''' '''a basic aircraft class'''
@ -14,67 +14,35 @@ class Aircraft(object):
self.longitude = self.home_longitude self.longitude = self.home_longitude
self.altitude = self.home_altitude self.altitude = self.home_altitude
self.pitch = 0.0 # degrees self.dcm = Matrix3()
self.roll = 0.0 # degrees
self.yaw = 0.0 # degrees
# rates in earth frame # rotation rate in body frame
self.pitch_rate = 0.0 # degrees/s self.gyro = Vector3(0,0,0) # rad/s
self.roll_rate = 0.0 # degrees/s
self.yaw_rate = 0.0 # degrees/s
# rates in body frame self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
self.pDeg = 0.0 # degrees/s self.position = Vector3(0, 0, 0) # m North, East, Down
self.qDeg = 0.0 # degrees/s self.last_velocity = self.velocity.copy()
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.mass = 0.0 self.mass = 0.0
self.update_frequency = 50 # in Hz self.update_frequency = 50 # in Hz
self.gravity = 9.8 # m/s/s 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') self.wind = util.Wind('0,0,0')
def normalise(self): def update_position(self, delta_time):
'''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):
'''update lat/lon/alt from position''' '''update lat/lon/alt from position'''
radius_of_earth = 6378100.0 # in meters radius_of_earth = 6378100.0 # in meters
dlat = math.degrees(math.atan(self.position.x/radius_of_earth)) 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 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 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 # work out what the accelerometer would see
xAccel = sin(radians(self.pitch)) self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity)
yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch)) # self.accelerometer = Vector3(0,0,-self.gravity)
zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch)) self.accelerometer = self.dcm.transposed() * self.accelerometer
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel)) self.last_velocity = self.velocity.copy()
xAccel *= scale;
yAccel *= scale;
zAccel *= scale;
self.accelerometer = euclid.Vector3(xAccel, yAccel, zAccel)

89
Tools/autotest/pysim/multicopter.py
View File

@ -1,7 +1,9 @@
#!/usr/bin/env python #!/usr/bin/env python
from aircraft import Aircraft 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): class Motor(object):
def __init__(self, angle, clockwise, servo): def __init__(self, angle, clockwise, servo):
@ -83,7 +85,7 @@ class MultiCopter(Aircraft):
self.mass = mass # Kg self.mass = mass # Kg
self.hover_throttle = hover_throttle self.hover_throttle = hover_throttle
self.terminal_velocity = terminal_velocity 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 self.frame_height = frame_height
# scaling from total motor power to Newtons. Allows the copter # 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)): for i in range(0, len(self.motors)):
servo = servos[self.motors[i].servo-1] servo = servos[self.motors[i].servo-1]
if servo <= 0.0: if servo <= 0.0:
self.motor_speed[i] = 0 self.motor_speed[i] = 0
else: else:
self.motor_speed[i] = servo self.motor_speed[i] = servo
@ -107,78 +110,58 @@ class MultiCopter(Aircraft):
delta_time = t - self.last_time delta_time = t - self.last_time
self.last_time = t self.last_time = t
# rotational acceleration, in degrees/s/s, in body frame # rotational acceleration, in rad/s/s, in body frame
roll_accel = 0.0 rot_accel = Vector3(0,0,0)
pitch_accel = 0.0
yaw_accel = 0.0
thrust = 0.0 thrust = 0.0
for i in range(len(self.motors)): for i in range(len(self.motors)):
roll_accel += -5000.0 * math.sin(math.radians(self.motors[i].angle)) * m[i] rot_accel.x += -radians(5000.0) * math.sin(radians(self.motors[i].angle)) * m[i]
pitch_accel += 5000.0 * math.cos(math.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: if self.motors[i].clockwise:
yaw_accel -= m[i] * 400.0 rot_accel.z -= m[i] * radians(400.0)
else: else:
yaw_accel += m[i] * 400.0 rot_accel.z += m[i] * radians(400.0)
thrust += m[i] * self.thrust_scale # newtons thrust += m[i] * self.thrust_scale # newtons
# rotational resistance # rotational air resistance
roll_accel -= (self.pDeg / self.terminal_rotation_rate) * 5000.0 rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate
pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0 rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0 rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate
# update rotational rates in body frame # update rotational rates in body frame
self.pDeg += roll_accel * delta_time self.gyro += rot_accel * delta_time
self.qDeg += pitch_accel * delta_time
self.rDeg += yaw_accel * delta_time
# calculate rates in earth frame # update attitude
(self.roll_rate, self.dcm.rotate(self.gyro * delta_time)
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
# air resistance # air resistance
air_resistance = - self.velocity * (self.gravity/self.terminal_velocity) air_resistance = - self.velocity * (self.gravity/self.terminal_velocity)
# normalise rotations accel_body = Vector3(0, 0, -thrust / self.mass)
self.normalise() accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
accel = thrust / self.mass accel_earth += air_resistance
accel3D = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, accel)
accel3D += euclid.Vector3(0, 0, -self.gravity)
accel3D += air_resistance
# add in some wind # add in some wind
accel3D += self.wind.accel(self.velocity) accel_earth += self.wind.accel(self.velocity)
# new velocity vector # new velocity vector
self.velocity += accel3D * delta_time self.velocity += accel_earth * delta_time
self.accel = accel3D
# new position vector # new position vector
old_position = self.position.copy() old_position = self.position.copy()
self.position += self.velocity * delta_time self.position += self.velocity * delta_time
# constrain height to the ground # constrain height to the ground
if self.position.z + self.home_altitude < self.ground_level + self.frame_height: 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: if (-old_position.z) + self.home_altitude > self.ground_level + self.frame_height:
print("Hit ground at %f m/s" % (-self.velocity.z)) print("Hit ground at %f m/s" % (self.velocity.z))
self.velocity = euclid.Vector3(0, 0, 0)
self.roll_rate = 0 self.velocity = Vector3(0, 0, 0)
self.pitch_rate = 0 # zero roll/pitch, but keep yaw
self.yaw_rate = 0 (r, p, y) = self.dcm.to_euler()
self.roll = 0 self.dcm.from_euler(0, 0, y)
self.pitch = 0
self.accel = euclid.Vector3(0, 0, 0) self.position = Vector3(self.position.x, self.position.y,
self.position = euclid.Vector3(self.position.x, self.position.y, -(self.ground_level + self.frame_height - self.home_altitude))
self.ground_level + self.frame_height - self.home_altitude)
# update lat/lon/altitude # update lat/lon/altitude
self.update_position() self.update_position(delta_time)

26
Tools/autotest/pysim/sim_multicopter.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python #!/usr/bin/env python
from multicopter import MultiCopter from multicopter import MultiCopter
import euclid, util, time, os, sys, math import util, time, os, sys, math
import socket, struct import socket, struct
import select, fgFDM, errno import select, fgFDM, errno
@ -16,16 +16,20 @@ import mavlink
def sim_send(m, a): def sim_send(m, a):
'''send flight information to mavproxy and flightgear''' '''send flight information to mavproxy and flightgear'''
global fdm 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('latitude', a.latitude, units='degrees')
fdm.set('longitude', a.longitude, units='degrees') fdm.set('longitude', a.longitude, units='degrees')
fdm.set('altitude', a.altitude, units='meters') fdm.set('altitude', a.altitude, units='meters')
fdm.set('phi', a.roll, units='degrees') fdm.set('phi', roll, units='radians')
fdm.set('theta', a.pitch, units='degrees') fdm.set('theta', pitch, units='radians')
fdm.set('psi', a.yaw, units='degrees') fdm.set('psi', yaw, units='radians')
fdm.set('phidot', a.roll_rate, units='dps') fdm.set('phidot', earth_rates.x, units='rps')
fdm.set('thetadot', a.pitch_rate, units='dps') fdm.set('thetadot', earth_rates.y, units='rps')
fdm.set('psidot', a.yaw_rate, units='dps') 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('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_north', a.velocity.x, units='mps')
fdm.set('v_east', a.velocity.y, units='mps') fdm.set('v_east', a.velocity.y, units='mps')
@ -40,11 +44,11 @@ def sim_send(m, a):
raise raise
buf = struct.pack('<16dI', 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.velocity.x, a.velocity.y,
a.accelerometer.x, a.accelerometer.y, a.accelerometer.z, -a.accelerometer.x, -a.accelerometer.y, -a.accelerometer.z,
a.roll_rate, a.pitch_rate, a.yaw_rate, degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z),
a.roll, a.pitch, a.yaw, degrees(roll), degrees(pitch), degrees(yaw),
math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
0x4c56414e) 0x4c56414e)
try: try:

4
Tools/autotest/pysim/testwind.py
View File

@ -1,12 +1,12 @@
#!/usr/bin/env python #!/usr/bin/env python
# simple test of wind generation code # simple test of wind generation code
import util, euclid, time, random import util, time, random
wind = util.Wind('3,90,0.1') wind = util.Wind('3,90,0.1')
t0 = time.time() t0 = time.time()
velocity = euclid.Vector3(0,0,0) velocity = Vector3(0,0,0)
t = 0 t = 0
deltat = 0.01 deltat = 0.01

134
Tools/autotest/pysim/util.py
View File

@ -1,90 +1,8 @@
import euclid, math import math
import os, pexpect, sys, time, random import os, pexpect, sys, time, random
from rotmat import Vector3, Matrix3
from subprocess import call, check_call,Popen, PIPE 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): def m2ft(x):
'''meters to feet''' '''meters to feet'''
return float(x) / 0.3048 return float(x) / 0.3048
@ -289,53 +207,48 @@ def check_parent(parent_pid=os.getppid()):
sys.exit(1) sys.exit(1)
def EarthRatesToBodyRates(roll, pitch, yaw, def EarthRatesToBodyRates(dcm, earth_rates):
rollRate, pitchRate, yawRate):
'''convert the angular velocities from earth frame to '''convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula 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) (phi, theta, psi) = dcm.to_euler()
theta = radians(pitch) phiDot = earth_rates.x
phiDot = radians(rollRate) thetaDot = earth_rates.y
thetaDot = radians(pitchRate) psiDot = earth_rates.z
psiDot = radians(yawRate)
p = phiDot - psiDot*sin(theta) p = phiDot - psiDot*sin(theta)
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta) q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
return Vector3(p, q, r)
return (degrees(p), degrees(q), degrees(r)) def BodyRatesToEarthRates(dcm, gyro):
def BodyRatesToEarthRates(roll, pitch, yaw, pDeg, qDeg, rDeg):
'''convert the angular velocities from body frame to '''convert the angular velocities from body frame to
earth frame. 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) p = gyro.x
q = radians(qDeg) q = gyro.y
r = radians(rDeg) r = gyro.z
phi = radians(roll) (phi, theta, psi) = dcm.to_euler()
theta = radians(pitch)
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi)) phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi))
thetaDot = q*cos(phi) - r*sin(phi) thetaDot = q*cos(phi) - r*sin(phi)
if fabs(cos(theta)) < 1.0e-20: if fabs(cos(theta)) < 1.0e-20:
theta += 1.0e-10 theta += 1.0e-10
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta) psiDot = (q*sin(phi) + r*cos(phi))/cos(theta)
return Vector3(phiDot, thetaDot, psiDot)
return (degrees(phiDot), degrees(thetaDot), degrees(psiDot))
class Wind(object): class Wind(object):
@ -382,12 +295,15 @@ class Wind(object):
'''return current wind acceleration in ground frame. The '''return current wind acceleration in ground frame. The
velocity is a Vector3 of the current velocity of the aircraft velocity is a Vector3 of the current velocity of the aircraft
in earth frame, m/s''' in earth frame, m/s'''
from math import radians
(speed, direction) = self.current(deltat=deltat) (speed, direction) = self.current(deltat=deltat)
# wind vector # wind vector
v = euclid.Vector3(speed, 0, 0) v = Vector3(speed, 0, 0)
wind = euclid.Quaternion.new_rotate_euler(0, math.radians(direction), 0) * v m = Matrix3()
m.from_euler(0, 0, radians(direction))
wind = m.transposed() * v
# relative wind vector # relative wind vector
relwind = wind - velocity relwind = wind - velocity