def start(self):
self.dataValue.lat = 50.0 + (random.random() - 0.5)
self.dataValue.lon = 20.0 + (random.random() - 0.5)
self.dataValue.alt = 40.0 + (random.random() - 0.5)*5
self.receivedCommand = 0 # Idle
self.simulatorThread = TimerThread('simulatorThread', self.simulatorThreadHandler, 0.2)
self.simulatorThread.start()
class ServerEntry(ServerID):
def __init__(self, ip, port):
super().__init__(ip, port)
self.timer_thread = TimerThread(11, self)
self.reset_timer()
self.timer_thread.start()
def reset_timer(self):
self.timer_thread.remaining_time = 11
def __init__(self, uartDev, usartBaundRate, logWriter):
self.uartController = UartController(uartDev, usartBaundRate)
self.uartOpened = True
self.logWriter = logWriter
self.onReceiveEvent = self.defaultOnReceiveEvent
self.streamProcessor = StreamProcessor(self.onReceiveControl,
self.onReceiveSignal,
self.onReceiveAutopilot)
self.receivingThread = TimerThread('receivingThread',
self.receiveThread, 0.05) # 20 Hz
self.receivingThread.start()
def __init__(self, adress, port):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((adress, port))
except:
print 'Unable to connect to the server!', adress, ':', port
sys.exit()
print 'Successfuly connected to the server', adress, ':', port
self.keepConnectionFlag = True
self.streamProcessor = StreamProcessor(self.onDebugReceive, self.onPongReceive)
self.sendingControlThread = TimerThread(self.sendingControlHandler, 0.1)
self.sendingSignalThread = TimerThread(self.sendingSignalHandler, 1.0)
def __init__(self, adress, port):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((adress, port))
except:
print 'Unable to connect to the server!', adress, ':', port
sys.exit()
print 'Successfuly connected to the server', adress, ':', port
self.keepConnectionFlag = True
self.streamProcessor = StreamProcessor(self.onDebugReceive,
self.onPongReceive)
self.sendingControlThread = TimerThread(self.sendingControlHandler,
0.1)
self.sendingSignalThread = TimerThread(self.sendingSignalHandler, 1.0)
def __init__(self,uartDev, usartBaundRate, logWriter):
self.uartController = UartController(uartDev, usartBaundRate)
self.uartOpened = True
self.logWriter = logWriter
self.onReceiveEvent = self.defaultOnReceiveEvent
self.streamProcessor = StreamProcessor(self.onReceiveControl, self.onReceiveSignal, self.onReceiveAutopilot)
self.receivingThread = TimerThread('receivingThread',self.receiveThread, 0.05) # 20 Hz
self.receivingThread.start()
def OpenSocket(self):
if not self.matches:
pass
else:
try:
self.server.connect(self.matches[self.lst.GetSelection()])
self.server.connected = True
except Exception as err:
print("Failed to connect: ", err)
self.server.connected = False
else:
self.outputfilename = increment_filename(outputfilename)
self.output_file = open(self.outputfilename, "a+")
self.output_file.write("Voltage [mV];Time [µsec]\n")
self.sendThread = TimerThread(1, 0.25, self.SendPacket)
self.sendThread.daemon = True
self.sendThread.start()
self.sendThread.start_timer()
self.recThread = TimerThread(0.00001, 0.0000025,
self.ReceivePacket)
self.recThread.daemon = True
self.recThread.start()
self.recThread.start_timer()
def run(self):
self.showHelp()
t = TimerThread('Timer', 10, self.logger)
t.start()
while True:
i = input('Type in command (or Press Enter to quit): ')
if not i:
break
#print('your input:', i)
action = i[0:3]
#print(action)
state = i[4:6]
#print(state)
if action == 'add':
self.addState(state)
if action == 'rem':
self.removeState(state)
if action == 'sho':
self.showStateList()
if action == 'hel':
self.showHelp()
print('Thank you for playing the License Plate Game, GoodBye!')
t.stop()
from TimerThread import *
import threading
import time
def reset_timer(t):
time.sleep(5)
t.remaining_time = 11
t = TimerThread()
t.start()
t1 = threading.Thread(target=reset_timer, args=(t, ))
t1.start()
def onReveiveCommDataFromIp(commData):
global droneController
droneController.sendCommData(commData)
ipController.setOnReceiveEvent(onReveiveCommDataFromIp)
def modemThreadTick():
data=modem.getModemData()
log_msg="modem: "
if data:
log_msg+=str(data)
else:
log_msg+="error"
logWriter.noteEvent(log_msg)
modemThread = TimerThread('modemThread',modemThreadTick, 2)
modemThread.start()
###########################################################################
## MAIN LOOP
###########################################################################
if SETTINGS.TCPSIMULATOR:
print('MainThread: Using port simulator')
else:
print('MainThread: Using port number ' + str(SETTINGS.PORT))
log_msg="MainThread: starting"+str(os.getpid())
logWriter.noteEvent(log_msg)
print log_msg
class DroneController:
uartController = None
receivingThread = None
logWriter = None
noDebugReceivingCounter = 0
maxTimeoutCounter = 20 # 20 * 0.05 = 1s
onReceiveEvent = None # Event to call when read data from USART
streamProcessor = None
uartOpened = None
def __init__(self, uartDev, usartBaundRate, logWriter):
self.uartController = UartController(uartDev, usartBaundRate)
self.uartOpened = True
self.logWriter = logWriter
self.onReceiveEvent = self.defaultOnReceiveEvent
self.streamProcessor = StreamProcessor(self.onReceiveControl,
self.onReceiveSignal,
self.onReceiveAutopilot)
self.receivingThread = TimerThread('receivingThread',
self.receiveThread, 0.05) # 20 Hz
self.receivingThread.start()
#onReceiveEvent - call this function when new CommData is received via UART
def setOnReceiveEvent(self, onReceiveEvent):
self.onReceiveEvent = onReceiveEvent
def defaultOnReceiveEvent(self, commData):
self.logWriter.noteEvent('DroneController: defaultOnReceiveEvent' +
str(commData))
def close(self):
print('DroneControler: close')
if self.uartOpened:
print('DroneControler: UART opened, sending STOP command')
self.sendCommData(ControlData.StopCommand())
# wait for sending and response
time.sleep(1.5)
TimerThread.kill(self.receivingThread)
self.uartController.close()
self.uartOpened = False
# Thread
# uartConnection
# handler for receiving thread
def receiveThread(self):
data = self.uartController.recv()
dataLength = len(data)
if dataLength == 0:
self.noDebugReceivingCounter += 1
if self.noDebugReceivingCounter >= self.maxTimeoutCounter:
# build ERROR_CONNECTION DebugData and call onReceiveEvent
debugData = DebugData.SomeValidDebugMessage()
debugData.setConnectionLost()
self.logWriter.noteEvent(
'DroneController: Error! Receiving thread timeout.')
self.onReceiveEvent(debugData)
self.noDebugReceivingCounter = 0
return
self.noDebugReceivingCounter = 0
log_msg = 'DroneController: received: [0x' + str(
data.encode("hex")) + ']'
self.logWriter.noteEvent(log_msg)
# push data to stream processor - when data packet is be received onReceive event will be called
self.streamProcessor.processStream(data)
# event called by StreamProcessor - on control preamble
def onReceiveControl(self, debugDataMsg):
debugData = DebugData(debugDataMsg)
if debugData.isValid():
# forward data to DronController
self.onReceiveEvent(debugData)
log_msg = 'DroneController: DebugData received: [' + str(
debugData) + ']'
else:
log_msg = 'DroneController: INVALID DebugData received: [' + debugDataMsg + ']'
self.logWriter.noteEvent(log_msg)
# event called by StreamProcessor - on signal preamble
def onReceiveSignal(self, signalPongMsg):
# this event should never be called on Uart communication side
log_msg = 'DroneController: Unexpected event received [' + str(
signalPongMsg.encode("hex")) + ']'
self.logWriter.noteEvent(log_msg)
# event called by StreamProcessor - on autopilota preamble
def onReceiveAutopilot(self, autopilotDataMsg):
autopilotData = AutopilotData(autopilotDataMsg)
if autopilotData.isValid():
# forward data to DroneController
self.onReceiveEvent(autopilotData)
log_msg = 'DroneController: AutopilotData received [0x' + str(
autopilotDataMsg.encode("hex")) + ']'
else:
log_msg = 'DroneController: INVALID AutopilotData received [0x' + autopilotDataMsg + ']'
self.logWriter.noteEvent(log_msg)
# send CommData to drone
def sendCommData(self, commData):
log_msg = 'DroneController: Forwarding CommData: [0x' + commData.data.encode(
"hex") + ']'
self.logWriter.noteEvent(log_msg)
self.uartController.send(commData.data)
def getDebugData(self):
return self.debugData
class DroneSimulator:
simulatorThread = None
logWriter = None
onReceiveEvent = None # Event to call when read data from USART
dataValue = None
aRoll = 0.3
fRoll = 0.8
aPitch = 0.2
fPitch = 1.4
aYaw = 0.08
fYaw = 0.2
mYaw = 0.05
aLat = 0.00003
fLat = 0.01
mLat = 0.000002
aLon = 0.00003
fLon = 0.01
mLon = 0.000001
aAlt = 0.1
fAlt = 2
mAlt = 0.1
receivedCommand = 0
def __init__(self, logWriter):
self.logWriter = logWriter
self.onReceiveEvent = self.defaultOnReceiveEvent
self.dataValue = DebugDataValue()
self.dataValue.controllerState = 1000
self.dataValue.battery = 10
self.dataValue.flags = 10
self.dataValue.CRC = 0
self.aRoll += random.random() * (0.5 * self.aRoll)
self.fRoll += random.random() * (0.5 * self.fRoll)
self.aPitch += random.random() * (0.5 * self.aPitch)
self.fPitch += random.random() * (0.5 * self.fPitch)
self.aYaw += random.random() * (0.5 * self.aYaw)
self.fYaw += random.random() * (0.5 * self.fYaw)
self.mYaw += random.random() * (0.5 * self.mYaw)
self.aLat += random.random() * (0.5 * self.aLat)
self.fLat += random.random() * (0.5 * self.fLat)
self.mLat += random.random() * (0.5 * self.mLat)
self.aLon += random.random() * (0.5 * self.aLon)
self.fLon += random.random() * (0.5 * self.fLon)
self.mLon += random.random() * (0.5 * self.mLon)
self.aAlt += random.random() * (0.5 * self.aAlt)
self.fAlt += random.random() * (0.5 * self.fAlt)
self.mAlt += random.random() * (0.5 * self.mAlt)
#onReceiveEvent - call this function when new CommData is received via UART
def setOnReceiveEvent(self, onReceiveEvent):
self.onReceiveEvent = onReceiveEvent
def defaultOnReceiveEvent(self, commData):
self.logWriter.noteEvent('DroneSimulator: defaultOnReceiveEvent' + str(commData))
def start(self):
self.dataValue.lat = 50.0 + (random.random() - 0.5)
self.dataValue.lon = 20.0 + (random.random() - 0.5)
self.dataValue.alt = 40.0 + (random.random() - 0.5)*5
self.receivedCommand = 0 # Idle
self.simulatorThread = TimerThread('simulatorThread', self.simulatorThreadHandler, 0.2)
self.simulatorThread.start()
def close(self):
print('DroneSimulator: close')
if self.simulatorThread is not None:
self.simulatorThread.stop()
def notifyCommData(self, commData):
if commData.typeString() == 'ControlData':
# process data as controller should do
controlDataValue = commData.getValue()
if controlDataValue.controllerCommand == 2000 or controlDataValue.throttle > 0.03:
self.receivedCommand = controlDataValue.controllerCommand
else:
self.receivedCommand = 0
elif commData.typeString() == 'AutopilotData':
# respond with ACK (same data when success at changing target) as controller should do
self.onReceiveEvent(commData)
def simulatorThreadHandler(self):
timeVal = time.time()
self.dataValue.roll = math.sin(timeVal * self.fRoll) * self.aRoll
self.dataValue.pitch = math.sin(timeVal * self.fPitch) * self.aPitch
self.dataValue.yaw += math.sin(timeVal * self.fYaw) * self.aYaw + self.mYaw
if self.dataValue.yaw > 3.14:
self.dataValue.yaw -= 2 * 3.14
if self.dataValue.yaw < -3.14:
self.dataValue.yaw += 2 * 3.14
self.dataValue.lat += math.sin(timeVal * self.fLat) * self.aLat + self.mLat
self.dataValue.lon += math.sin(timeVal * self.fLon) * self.aLon + self.mLon
self.dataValue.alt += math.sin(timeVal * self.fAlt) * self.aAlt + self.mAlt
self.dataValue.speed = self.dataValue.alt / 2.3
self.dataValue.controllerState = self.receivedCommand
temp = DebugData(self.dataValue)
self.dataValue.CRC = temp.computeCrc()
debugMessage = DebugData(self.dataValue)
self.onReceiveEvent(debugMessage)
log_msg = 'DroneSimulator: DebugData: [' + str(debugMessage) + ']'
self.logWriter.noteEvent(log_msg)
ipController.setOnReceiveEvent(onReveiveCommDataFromIp)
def modemThreadTick():
data = modem.getModemData()
log_msg = "modem: "
if data:
log_msg += str(data)
else:
log_msg += "error"
logWriter.noteEvent(log_msg)
modemThread = TimerThread('modemThread', modemThreadTick, 2)
modemThread.start()
###########################################################################
## MAIN LOOP
###########################################################################
if SETTINGS.TCPSIMULATOR:
print('MainThread: Using port simulator')
else:
print('MainThread: Using port number ' + str(SETTINGS.PORT))
log_msg = "MainThread: starting" + str(os.getpid())
logWriter.noteEvent(log_msg)
print log_msg
class Client_GUI(wx.Frame):
device_uuid = "0fd5ca36-4e7d-4f99-82ec-2868262bd4e4"
device_selected = 0
server = None
matches = None
output_file = None
sendThread = None
recThread = None
fileAccess = False
packet_set = set()
def __init__(self, parent, title):
wx.Frame.__init__(self, parent, title=title, size=(300, 150))
#self.control = wx.TextCtrl(self, style=wx.TE_MULTILINE)
mainSizer = wx.BoxSizer(wx.VERTICAL)
panel = wx.Panel(self)
box = wx.BoxSizer(wx.HORIZONTAL)
self.text = wx.TextCtrl(panel, style=wx.TE_MULTILINE)
self.device_names = []
self.lst = wx.ListBox(panel,
size=(200, -1),
choices=self.device_names,
style=wx.LB_SINGLE)
box.Add(self.lst, 0, wx.EXPAND)
box.Add(self.text, 1, wx.EXPAND)
panel.SetSizer(box)
panel.Fit()
self.Centre()
self.Bind(wx.EVT_LISTBOX, self.onListBox, self.lst)
self.Show(True)
self.CreateStatusBar() # A StatusBar in the bottom of the window
# Setting up the menu.
filemenu = wx.Menu()
# wx.ID_ABOUT and wx.ID_EXIT are standard ids provided by wxWidgets.
#menuOpen = filemenu.Append(wx.ID_OPEN, "&Open", "Open a file")
#filemenu.AppendSeparator()
menuScan = filemenu.Append(wx.ID_ABOUT, "&Scan", "Scan for servers")
menuConnect = filemenu.Append(wx.ID_ANY, "&Connect",
"Connect to selected server")
menuDisconnect = filemenu.Append(wx.ID_ANY, "&Disconnect",
"Disconnect from connected server")
menuExit = filemenu.Append(wx.ID_EXIT, "E&xit",
" Terminate the program")
# Creating the menubar.
menuBar = wx.MenuBar()
menuBar.Append(filemenu,
"&File") # Adding the "filemenu" to the MenuBar
self.SetMenuBar(menuBar) # Adding the MenuBar to the Frame content.
# Set events.
self.Bind(wx.EVT_MENU, self.OnScan, menuScan)
self.Bind(wx.EVT_MENU, self.OnConnect, menuConnect)
self.Bind(wx.EVT_MENU, self.OnDisconnect, menuDisconnect)
self.Bind(wx.EVT_MENU, self.OnExit, menuExit)
#self.Bind(wx.EVT_MENU, self.OnOpen, menuOpen)
self.Show(True)
self.server = BTServer()
try:
fh = open(filename, "rb")
except:
print("Error: can\'t open file")
thread = threading.Thread(target=self.BTScan)
thread.daemon = True
thread.start()
else:
try:
self.matches = pickle.load(fh)
except:
print("Failed to load from file")
thread = threading.Thread(target=self.BTScan)
thread.daemon = True
thread.start()
else:
names = []
for service in self.matches:
name = service["name"]
if not name is None:
name = name.decode("utf-8")
else:
name = "N/A"
host = service["host"]
if host is None:
host = "N/A"
name += ", host: "
name += host
names.append(name)
wx.CallAfter(self.lst.AppendItems, names)
fh.close()
def onListBox(self, e):
self.device_selected = self.lst.GetSelection()
self.text.AppendText("Current selection: " +
e.GetEventObject().GetStringSelection() +
" index: %d\n" % (self.device_selected))
def OnOpen(self, e):
""" Open a file"""
self.dirname = ''
dlg = wx.FileDialog(self, "Choose a file", self.dirname, "", "*.*",
wx.FD_OPEN)
if dlg.ShowModal() == wx.ID_OK:
self.filename = dlg.GetFilename()
self.dirname = dlg.GetDirectory()
f = open(os.path.join(self.dirname, self.filename), 'r')
self.control.SetValue(f.read())
f.close()
dlg.Destroy()
def OnScan(self, e):
# A message dialog box with an OK button. wx.OK is a standard ID in wxWidgets.
thread = threading.Thread(target=self.BTScan)
thread.daemon = True
thread.start()
#worker = BTDiscover(self, 0, [])
#worker.start()
#for addr in nearby_devices:
# print(" %s " % (addr))
#dlg = wx.MessageDialog( self, "A small text editor", "About Sample Editor", wx.OK)
#dlg.ShowModal() # Show it
#dlg.Destroy() # finally destroy it when finished.
def OnConnect(self, e):
if self.server.connected:
print("Already connected")
elif (not self.lst.IsEmpty()):
thread = threading.Thread(target=self.OpenSocket)
thread.daemon = True
thread.start()
else:
print("No selection")
def OnDisconnect(self, e):
if not self.server is None:
if self.server.connected:
self.sendThread.stop_timer()
self.recThread.stop_timer()
print("Closed connection to %s" % self.server.name)
if self.output_file is not None:
self.output_file.close()
self.exit()
else:
print("No server connected")
else:
print("No server connected")
def OpenSocket(self):
if not self.matches:
pass
else:
try:
self.server.connect(self.matches[self.lst.GetSelection()])
self.server.connected = True
except Exception as err:
print("Failed to connect: ", err)
self.server.connected = False
else:
self.outputfilename = increment_filename(outputfilename)
self.output_file = open(self.outputfilename, "a+")
self.output_file.write("Voltage [mV];Time [µsec]\n")
self.sendThread = TimerThread(1, 0.25, self.SendPacket)
self.sendThread.daemon = True
self.sendThread.start()
self.sendThread.start_timer()
self.recThread = TimerThread(0.00001, 0.0000025,
self.ReceivePacket)
self.recThread.daemon = True
self.recThread.start()
self.recThread.start_timer()
def countPackets(self):
print("total packets", self.server.packets)
self.server.packets = 0
def SendPacket(self):
#threading.Timer(1, self.SendPacket).start()
self.server.send("Test packet!")
def ReceivePacket(self):
data = self.server.receive(packet_length * 10)
if (data is not None):
self.fileAccess = True
# print(data)
length = len(data)
print("Receive bytes:", length)
usec_step = 1e6 / sampling_rate
added = []
# if the data returned includes multiple sequences, go over each one
for packet_num in range(0, int(length / packet_length), 1):
# get 64 bytes of unsigned usecs at the start
time = [
data[i + (packet_length * (packet_num))]
for i in range(0, 8, 1)
]
time = int.from_bytes(time, byteorder='big',
signed=False) - usec_step * 256
print(time)
if time not in self.packet_set:
self.packet_set.add(time)
# self.output_file.write("%f\n" %(time/1e6))
for i in range(
8 + (packet_num) * packet_length,
packet_length + (packet_num) * packet_length, 2):
item = [data[i + 1], data[i]]
# substract from 4096 because the data is inverted
num = 4095 - int.from_bytes(
item, byteorder='big', signed=False)
try:
self.output_file.write(
"%f;%f\n" % (num * 3300 / 4095, time / 1e6))
except Exception as err:
print("Failed to write to file:", err)
# print(num, time/(1e6))
time = time + usec_step
self.fileAccess = False
else:
if not self.output_file is None:
self.output_file.close()
raise ReceiveError()
def BTScan(self):
print("Scanning for servers")
self.matches = self.server.find(_name="BT_Sense",
_uuid=self.device_uuid)
# empty list
if (not self.matches):
wx.CallAfter(self.lst.Clear)
else:
wx.CallAfter(self.lst.Clear)
names = []
for service in self.matches:
name = service["name"]
if not name is None:
name = name.decode("utf-8")
else:
name = "N/A"
host = service["host"]
if host is None:
host = "N/A"
name += ", host: "
name += host
names.append(name)
try:
fh = open(filename, "wb+")
except:
print("Error: can\'t find file or create it")
else:
#try:
pickle.dump(self.matches, fh)
#except:
# print("failed to dump object")
#else:
print("Wrote matches to file")
fh.close()
wx.CallAfter(self.lst.AppendItems, names)
def OnExit(self, e):
self.Close(True) # Close the frame.
exit()
def exit(self):
if not self.sendThread is None:
self.sendThread.terminate()
if not self.recThread is None:
self.recThread.terminate()
if not self.output_file is None:
while (self.fileAccess):
pass
self.output_file.close()
if not self.server is None:
self.server.close()
class DroneController:
uartController = None
receivingThread = None
logWriter = None
noDebugReceivingCounter = 0
maxTimeoutCounter = 20 # 20 * 0.05 = 1s
onReceiveEvent = None # Event to call when read data from USART
streamProcessor = None
uartOpened = None
def __init__(self,uartDev, usartBaundRate, logWriter):
self.uartController = UartController(uartDev, usartBaundRate)
self.uartOpened = True
self.logWriter = logWriter
self.onReceiveEvent = self.defaultOnReceiveEvent
self.streamProcessor = StreamProcessor(self.onReceiveControl, self.onReceiveSignal, self.onReceiveAutopilot)
self.receivingThread = TimerThread('receivingThread',self.receiveThread, 0.05) # 20 Hz
self.receivingThread.start()
#onReceiveEvent - call this function when new CommData is received via UART
def setOnReceiveEvent(self, onReceiveEvent):
self.onReceiveEvent = onReceiveEvent
def defaultOnReceiveEvent(self, commData):
self.logWriter.noteEvent('DroneController: defaultOnReceiveEvent' + str(commData))
def close(self):
print('DroneControler: close')
if self.uartOpened:
print ('DroneControler: UART opened, sending STOP command')
self.sendCommData(ControlData.StopCommand())
# wait for sending and response
time.sleep(1.5)
TimerThread.kill(self.receivingThread)
self.uartController.close()
self.uartOpened = False
# Thread
# uartConnection
# handler for receiving thread
def receiveThread(self):
data = self.uartController.recv()
dataLength = len(data)
if dataLength == 0:
self.noDebugReceivingCounter += 1
if self.noDebugReceivingCounter >= self.maxTimeoutCounter:
# build ERROR_CONNECTION DebugData and call onReceiveEvent
debugData = DebugData.SomeValidDebugMessage()
debugData.setConnectionLost()
self.logWriter.noteEvent('DroneController: Error! Receiving thread timeout.')
self.onReceiveEvent(debugData)
self.noDebugReceivingCounter = 0
return
self.noDebugReceivingCounter = 0
log_msg = 'DroneController: received: [0x' + str(data.encode("hex")) + ']'
self.logWriter.noteEvent(log_msg)
# push data to stream processor - when data packet is be received onReceive event will be called
self.streamProcessor.processStream(data)
# event called by StreamProcessor - on control preamble
def onReceiveControl(self, debugDataMsg):
debugData = DebugData(debugDataMsg)
if debugData.isValid():
# forward data to DronController
self.onReceiveEvent(debugData)
log_msg = 'DroneController: DebugData received: [' + str(debugData) + ']'
else:
log_msg = 'DroneController: INVALID DebugData received: [' + debugDataMsg + ']'
self.logWriter.noteEvent(log_msg)
# event called by StreamProcessor - on signal preamble
def onReceiveSignal(self, signalPongMsg):
# this event should never be called on Uart communication side
log_msg = 'DroneController: Unexpected event received [' + str(signalPongMsg.encode("hex")) + ']'
self.logWriter.noteEvent(log_msg)
# event called by StreamProcessor - on autopilota preamble
def onReceiveAutopilot(self, autopilotDataMsg):
autopilotData = AutopilotData(autopilotDataMsg)
if autopilotData.isValid():
# forward data to DroneController
self.onReceiveEvent(autopilotData)
log_msg = 'DroneController: AutopilotData received [0x' + str(autopilotDataMsg.encode("hex")) + ']'
else:
log_msg = 'DroneController: INVALID AutopilotData received [0x' + autopilotDataMsg + ']'
self.logWriter.noteEvent(log_msg)
# send CommData to drone
def sendCommData(self, commData):
log_msg = 'DroneController: Forwarding CommData: [0x' + commData.data.encode("hex") + ']'
self.logWriter.noteEvent(log_msg)
self.uartController.send(commData.data)
def getDebugData(self):
return self.debugData
def __init__(self, ip, port):
super().__init__(ip, port)
self.timer_thread = TimerThread(11, self)
self.reset_timer()
self.timer_thread.start()
class ADdroneClient:
sock = None
streamProcessor = None
keepConnectionFlag = False
""" ControlData sending """
controlData = None
sendingControlThread = None
# method called by sending thread to send ControlData to server
def sendingControlHandler(self):
if self.controlData is not None:
self.sock.send(self.controlData.data)
""" Ping sending """
pingTimestamp = None
pingData = None
isPongReceived = True
sendingSignalThread = None
# method called by sending thread to send Ping to server
# sending is only avalible when previous Pong was received
def sendingSignalHandler(self):
if isPongReceived:
pingData = '%%%%abcd'
isPongReceived = False
pingTimestamp = datetime.datetime.now()
self.sock.send(pingData)
""" on receive handlers """
def onDebugReceive(self, data):
print onDebugReceive
pass
def onPongReceive(self, data):
if data == pingData:
now = datetime.datetime.now()
isPongReceived = True
d = now - pingTimestamp
print 'onPongReceive: ' + (d.seconds * 1000 +
d.microseconds / 1000)
else:
print 'Bad data received at signal chanell'
def __init__(self, adress, port):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((adress, port))
except:
print 'Unable to connect to the server!', adress, ':', port
sys.exit()
print 'Successfuly connected to the server', adress, ':', port
self.keepConnectionFlag = True
self.streamProcessor = StreamProcessor(self.onDebugReceive,
self.onPongReceive)
self.sendingControlThread = TimerThread(self.sendingControlHandler,
0.1)
self.sendingSignalThread = TimerThread(self.sendingSignalHandler, 1.0)
def proceedReceiving(self):
BUFFER_SIZE = 512
try:
data = self.connection.recv(BUFFER_SIZE)
except:
data = None
print 'proceedReceiving: IP receive ERROR/TIMEOUT'
if not data:
print 'IpController: client disconnected:', self.client_address
self.keepConnectionFlag = False
return
self.streamProcessor.processStream(data)
def setControlData(self, controlData):
self.controlData = controlData
def close(self):
self.sock.close()
self.sendingControlThread.close()
self.sendingSignalThread.close()
# server for IP connection
adapterServer = AdapterServer(('', 10003), 5)
# USART controller for board connection
usartController = UartController("/dev/ttyAMA0", 115200)
def usartReceivingThreadHandle():
global usartController
global adapterServer
data = usartController.recv()
dataLength = len(data)
if dataLength != 0:
adapterServer.sendData(data)
receivingThread = TimerThread('receivingThread', usartReceivingThreadHandle, 0.02) # 50 Hz
receivingThread.start()
while True:
print('MainThread: waiting for a connection')
adapterServer.acceptConnection()
while adapterServer.keepConnection():
adapterServer.forwardData(usartController)
print('MainThread: connection closed')
print "Cosing interfaces..."
endHandler()
class ADdroneClient:
sock = None
streamProcessor = None
keepConnectionFlag = False
""" ControlData sending """
controlData = None
sendingControlThread = None
# method called by sending thread to send ControlData to server
def sendingControlHandler(self):
if self.controlData is not None:
self.sock.send(self.controlData.data)
""" Ping sending """
pingTimestamp = None
pingData = None
isPongReceived = True
sendingSignalThread = None
# method called by sending thread to send Ping to server
# sending is only avalible when previous Pong was received
def sendingSignalHandler(self):
if isPongReceived:
pingData = '%%%%abcd'
isPongReceived = False
pingTimestamp = datetime.datetime.now()
self.sock.send(pingData)
""" on receive handlers """
def onDebugReceive(self, data):
print onDebugReceive
pass
def onPongReceive(self, data):
if data == pingData:
now = datetime.datetime.now()
isPongReceived = True
d = now - pingTimestamp
print 'onPongReceive: ' + (d.seconds*1000 + d.microseconds/1000)
else:
print 'Bad data received at signal chanell'
def __init__(self, adress, port):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((adress, port))
except:
print 'Unable to connect to the server!', adress, ':', port
sys.exit()
print 'Successfuly connected to the server', adress, ':', port
self.keepConnectionFlag = True
self.streamProcessor = StreamProcessor(self.onDebugReceive, self.onPongReceive)
self.sendingControlThread = TimerThread(self.sendingControlHandler, 0.1)
self.sendingSignalThread = TimerThread(self.sendingSignalHandler, 1.0)
def proceedReceiving(self):
BUFFER_SIZE = 512
try:
data = self.connection.recv(BUFFER_SIZE)
except:
data = None
print 'proceedReceiving: IP receive ERROR/TIMEOUT'
if not data:
print 'IpController: client disconnected:', self.client_address
self.keepConnectionFlag = False
return
self.streamProcessor.processStream(data)
def setControlData(self, controlData):
self.controlData = controlData
def close(self):
self.sock.close()
self.sendingControlThread.close()
self.sendingSignalThread.close()
