def sim():
with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as sock:
sock.bind(('', 35020))
sock.connect(('127.0.0.1', 36000))
sock.settimeout(0.01)
net = io.Network(sock)
finangle = 0
roll = [0]
for i, t in enumerate(ortime):
if not q.empty():
# finangle = lv2.servo(q.get(), t)
finangle = q.get()
print t, finangle, r
x = altitude[i]
v = velocity[i]
r = roll[-1]
aa = lv2.angular_accel(finangle, x, v, t)
if t > 2.5 and t < 2.7:
aa = 100
r += aa/or_sample_rate
roll.append(r)
if r > 399:
r = 399
if r < -399:
r = -399
# Data to pack
data = {
'VCC': 5.0,
'Gyro_X': r/360.0,
'Gyro_Y': 0,
'Gyro_Z': 1,
'Acc_X': accel[i],
'Acc_Y': 0,
'Acc_Z': 0,
'Magn_X': 53e-6,
'Magn_Y': 0,
'Magn_Z': 0,
'Temp': 20,
'Aux_ADC': 0,
}
net.send_data(ADIS, i, data)
if i == 5:
arm()
time.sleep(0.001)
def __init__(self, q):
from psas_packet import io, messages
threading.Thread.__init__(self)
self._stop = threading.Event()
self.daemon = True
self.queue = q
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.bind(('', 35003))
self.sock.settimeout(0.5)
self.net = io.Network(self.sock)
self.decode = messages
def main():
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(("127.0.0.1", 35001))
network = io.Network(sock)
packets = []
while True:
try:
for timestamp, data in network.listen():
fourcc, data = data
data["recv"] = timestamp
packets.append({fourcc: data})
except KeyboardInterrupt:
sock.close()
file = open("sent.txt", 'w')
# Lines 36-58 format the output strings to match the JavaScript
# formatting. Here are the formatting differences I observed.
# 1. Python includes a space after each colon and comma.
# 2. JavaScript converts "simple" floats into ints (ex: 0.0 -> 0).
# 3. JavaScript: 0.0000015 Python: 1.5e-06
# 4. JavaScript: 1.5e-7 Python: 1.5e-07
for packet in packets:
if "ADIS" in packet:
for key in ["Magn_X", "Magn_Y", "Magn_Z"]:
if key in packet["ADIS"]:
value = packet["ADIS"][key]
if value != 0:
if int(repr(value)[-1]) < 7:
if '.' in repr(value):
if value < 0:
dps = (len(repr(value)) - 7) +\
int(repr(value)[-1])
else:
dps = (len(repr(value)) - 6) +\
int(repr(value)[-1])
else:
dps = int(repr(value)[-1])
packet["ADIS"][key] = format(
value, ('.' + str(dps) + 'f'))
output = json.dumps(packet)
output = output.replace(' ', '')
output = output.replace(".0,", ',')
output = output.replace(".0}", '}')
output = output.replace(":\"", ':')
output = output.replace("\",", ',')
output = output.replace("e-0", "e-")
file.write(output + '\n')
file.close()
return
def listen(self):
"""Listens for incoming psas packets
network.listen() returns a tuple of timestamp and data
adds the tuple to the queue
"""
print("The telemetry server is running.")
# Starts the sender thread.
self.thread.start()
# Starts logger thread if lock and log exists
if self.lock and self.log:
self.loggerThread.start()
# Use PSAS Packet to listen for incoming telemetry data
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((self.address, self.port))
network = io.Network(sock)
while True:
try:
collection = []
for timestamp, data in network.listen():
fourcc, values = data
# Skips packet with unrecognized fourcc
if not fourcc in messages.MESSAGES:
continue
collection.append((fourcc, values, timestamp))
# Enqueues collection of packets without blocking.
if len(collection) > 0:
self.queue.put_nowait(collection)
except KeyboardInterrupt:
sock.close()
# Sets the shared event (breaks the sender thread's loop).
self.event.set()
# Waits until the sender thread terminates.
self.thread.join()
return
def __init__(self, logfile):
super(PacketListener, self).__init__(None)
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.bind(('', 35001))
#self.sock.settimeout(1)
self.net = io.Network(self.sock, logfile=logfile)
LBF2N = 4.44822
LBS2KG = 0.453592
# Seed for deterministic random behavior
random.seed(0xBADA55)
# output from JSBSim goes to this socket (port specified in output.xml)
jsb_output = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_output.bind(("127.0.0.1", 5123))
jsb_output.setblocking(0)
# Socket writer for sending PSAS formated binary data to our flight computer
fc_imu_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
fc_imu_sock.bind(('', 35020))
fc_imu_sock.connect(('127.0.0.1', 36000))
fc_imu = io.Network(fc_imu_sock)
# Run JSBSim using Popen
p = subprocess.Popen([
"JSBSim", "--realtime", "--suspend", "--nice", "--simulation-rate=1000",
"--logdirectivefile=output_UDP.xml", "--logdirectivefile=output_file.xml",
"--script=run.xml"
])
# settling time for JSBSim to initialize and start a TCP server for commands
time.sleep(0.5)
# Connect to JSBSim console with this socket (specified in run.xml: <input port="5124" />
jsb_console = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_console.connect(("127.0.0.1", 5124))
# Data type we're going to use
ADIS = messages.MESSAGES['ADIS']
# Data to pack
data = {
'VCC': 5.0,
'Gyro_X': 0.0,
'Gyro_Y': 0,
'Gyro_Z': 1,
'Acc_X': -9.8,
'Acc_Y': 0,
'Acc_Z': 0,
'Magn_X': 53e-6,
'Magn_Y': 0,
'Magn_Z': 0,
'Temp': 20,
'Aux_ADC': 0,
}
# Open a UDP socket
with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as sock:
sock.bind(('', 0))
sock.connect(('127.0.0.1', 25000))
# Network IO class, with our socket as connection
net = io.Network(sock)
# send just data (no header)
# type, sequence no, data
net.send_data(ADIS, 0, data)