You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
104 lines
3.5 KiB
104 lines
3.5 KiB
#!/usr/bin/env python |
|
''' |
|
Python interface to CRRCSIM simulator |
|
''' |
|
|
|
from aircraft import Aircraft |
|
import util, time, struct |
|
import socket, select |
|
from math import degrees, radians, sin, cos |
|
from rotmat import Vector3, Matrix3 |
|
|
|
class CRRCSim(Aircraft): |
|
'''a CRRCCIM shim''' |
|
def __init__(self, frame="generic"): |
|
Aircraft.__init__(self) |
|
|
|
self.sim = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) |
|
self.sim.connect(('127.0.0.1', 9002)) |
|
self.sim.setblocking(0) |
|
self.accel_body = Vector3(0, 0, -self.gravity) |
|
self.frame = frame |
|
self.last_timestamp = 0 |
|
if self.frame.find("heli") != -1: |
|
self.decode_servos = self.decode_servos_heli |
|
else: |
|
self.decode_servos = self.decode_servos_fixed_wing |
|
|
|
def decode_servos_heli(self, servos): |
|
'''decode servos for heli''' |
|
swash1 = servos[0] |
|
swash2 = servos[1] |
|
swash3 = servos[2] |
|
tail_rotor = servos[3] |
|
rsc = servos[7] |
|
|
|
col_pitch = (swash1+swash2+swash3)/3.0 - 0.5 |
|
roll_rate = (swash1 - swash2)/2 |
|
pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2 |
|
yaw_rate = -(tail_rotor - 0.5) |
|
rsc_speed = rsc |
|
|
|
buf = struct.pack('<fffff', |
|
util.constrain(roll_rate, -0.5, 0.5), |
|
util.constrain(pitch_rate, -0.5, 0.5), |
|
util.constrain(rsc, 0, 1), |
|
util.constrain(yaw_rate, -0.5, 0.5), |
|
util.constrain(col_pitch, -0.5, 0.5)) |
|
self.sim.send(buf) |
|
|
|
def decode_servos_fixed_wing(self, servos): |
|
'''decode servos for fixed wing''' |
|
roll_rate = util.constrain(servos[0]-0.5, -0.5, 0.5) |
|
pitch_rate = util.constrain(servos[1]-0.5, -0.5, 0.5) |
|
throttle = util.constrain(servos[2], 0, 1) |
|
yaw_rate = util.constrain(servos[3]-0.5, -0.5, 0.5) |
|
|
|
buf = struct.pack('<fffff', |
|
roll_rate, |
|
pitch_rate, |
|
throttle, |
|
yaw_rate, |
|
0) |
|
self.sim.send(buf) |
|
|
|
def recv_fdm(self): |
|
'''receive FDM packet''' |
|
try: |
|
select.select([self.sim.fileno()], [], [], 1) |
|
buf = self.sim.recv(1024) |
|
except Exception as ex: |
|
return False |
|
|
|
(timestamp, |
|
latitude, longitude, |
|
altitude, |
|
heading, |
|
speedN, speedE, speedD, |
|
xAccel, yAccel, zAccel, |
|
rollRate, pitchRate, yawRate, |
|
roll, pitch, yaw, |
|
airspeed) = struct.unpack('<dddddddddddddddddd', buf) |
|
|
|
self.accel_body = Vector3(xAccel, yAccel, zAccel) |
|
self.gyro = Vector3(rollRate, pitchRate, yawRate) |
|
self.velocity = Vector3(speedN, speedE, speedD) |
|
dist = util.gps_distance(0, 0, latitude, longitude) |
|
bearing = util.gps_bearing(0, 0, latitude, longitude) |
|
self.position.x = dist * cos(radians(bearing)) |
|
self.position.y = dist * sin(radians(bearing)) |
|
self.position.z = -altitude |
|
self.dcm.from_euler(roll, pitch, yaw) |
|
self.time_now = timestamp + self.time_base |
|
|
|
# auto-adjust to crrcsim frame rate |
|
deltat = timestamp - self.last_timestamp |
|
if deltat < 0.01 and deltat > 0: |
|
self.adjust_frame_time(1.0/deltat) |
|
self.last_timestamp = timestamp |
|
|
|
def update(self, actuators): |
|
'''update model''' |
|
self.decode_servos(actuators) |
|
self.recv_fdm() |
|
self.update_position()
|
|
|