ardupilot/Tools/autotest/pysim/aircraft.py

113 lines
3.9 KiB
Python

import math
import random
import time
import util
from pymavlink.rotmat import Vector3, Matrix3
class Aircraft(object):
"""A basic aircraft class."""
def __init__(self):
self.home_latitude = 0
self.home_longitude = 0
self.home_altitude = 0
self.ground_level = 0
self.frame_height = 0.0
self.latitude = self.home_latitude
self.longitude = self.home_longitude
self.altitude = self.home_altitude
self.dcm = Matrix3()
# rotation rate in body frame
self.gyro = Vector3(0, 0, 0) # rad/s
self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
self.position = Vector3(0, 0, 0) # m North, East, Down
self.mass = 0.0
self.update_frequency = 50 # in Hz
self.gravity = 9.80665 # m/s/s
self.accelerometer = Vector3(0, 0, -self.gravity)
self.wind = util.Wind('0,0,0')
self.time_base = time.time()
self.time_now = self.time_base + 100*1.0e-6
self.gyro_noise = math.radians(0.1)
self.accel_noise = 0.3
def on_ground(self, position=None):
"""Return true if we are on the ground."""
if position is None:
position = self.position
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
def update_position(self):
"""Update lat/lon/alt from position."""
bearing = math.degrees(math.atan2(self.position.y, self.position.x))
distance = math.sqrt(self.position.x**2 + self.position.y**2)
(self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude,
bearing, distance)
self.altitude = self.home_altitude - self.position.z
velocity_body = self.dcm.transposed() * self.velocity
self.accelerometer = self.accel_body.copy()
def set_yaw_degrees(self, yaw_degrees):
"""Rotate to the given yaw."""
(roll, pitch, yaw) = self.dcm.to_euler()
yaw = math.radians(yaw_degrees)
self.dcm.from_euler(roll, pitch, yaw)
def time_advance(self, deltat):
"""Advance time by deltat in seconds."""
self.time_now += deltat
def setup_frame_time(self, rate, speedup):
"""Setup frame_time calculation."""
self.rate = rate
self.speedup = speedup
self.frame_time = 1.0/rate
self.scaled_frame_time = self.frame_time/speedup
self.last_wall_time = time.time()
self.achieved_rate = rate
def adjust_frame_time(self, rate):
"""Adjust frame_time calculation."""
self.rate = rate
self.frame_time = 1.0/rate
self.scaled_frame_time = self.frame_time/self.speedup
def sync_frame_time(self):
"""Try to synchronise simulation time with wall clock time, taking
into account desired speedup."""
now = time.time()
if now < self.last_wall_time + self.scaled_frame_time:
time.sleep(self.last_wall_time+self.scaled_frame_time - now)
now = time.time()
if now > self.last_wall_time and now - self.last_wall_time < 0.1:
rate = 1.0/(now - self.last_wall_time)
self.achieved_rate = (0.98*self.achieved_rate) + (0.02*rate)
if self.achieved_rate < self.rate*self.speedup:
self.scaled_frame_time *= 0.999
else:
self.scaled_frame_time *= 1.001
self.last_wall_time = now
def add_noise(self, throttle):
"""Add noise based on throttle level (from 0..1)."""
self.gyro += Vector3(random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)) * throttle * self.gyro_noise
self.accel_body += Vector3(random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)) * throttle * self.accel_noise