class FlipdotTxWorker(QObject):
def __init__(self, parent):
QObject.__init__(self, parent)
self._url: str = 'flipdot-km'
self._port: int = 3000
self._socket = QTcpSocket(self)
self._socket.error.connect(self.__socketError)
self._socket.connected.connect(self.__socketConnected)
self._socket.disconnected.connect(self.__socketDisconnected)
self._socket.readyRead.connect(self.__socketReadyRead)
self._socket.bytesWritten.connect(self.__socketBytesWritten)
self._cmdList: List[QCommand] = []
self._rxBuffer: bytearray = bytearray()
self._closeTimer: QTimer = util.createSingleShotTimer(
1600, self.__closeTimerTimeout)
def setTarget(self, host: str, port: int):
pass
def sendCommands(self, cmds: Iterable[QCommand]) -> None:
for cmd in cmds:
self.sendCommand(cmd)
def sendCommand(self, cmd: QCommand) -> None:
if cmd.state != State.FRESH:
raise RuntimeWarning(
"Unfresh command given, the command has not been queued.")
return
if self._socket.state() == QTcpSocket.UnconnectedState:
print("re-establish")
self._rxBuffer.clear()
self._socket.connectToHost(self._url, self._port)
self._cmdList.append(cmd)
elif self._socket.state() == QTcpSocket.HostLookupState:
self._cmdList.append(cmd)
elif self._socket.state() == QTcpSocket.ConnectedState:
self._cmdList.append(cmd)
self.__sendNext()
def __socketError(self, socketError):
print(socketError)
if socketError in [
QTcpSocket.ConnectionRefusedError,
QTcpSocket.HostNotFoundError, QTcpSocket.NetworkError
]:
for cmd in filter(lambda c: c.state == State.FRESH, self._cmdList):
cmd.connectionImpossible()
self._cmdList.remove(cmd)
def _restartTimer(self):
if self._closeTimer.isActive():
self._closeTimer.stop()
self._closeTimer.start()
def __closeTimerTimeout(self):
if self._socket.isOpen():
print("closing")
self._socket.close()
self._rxBuffer.clear()
def __sendNext(self):
toSend = next(
filter(lambda cmd: cmd.state == State.FRESH, self._cmdList), None)
if toSend:
toSend.transmit(self._socket)
def __socketConnected(self):
self._restartTimer()
self.__sendNext()
def __socketBytesWritten(self, n):
self._restartTimer()
f = filter(lambda cmd: cmd.state == State.TX_PENDING, self._cmdList)
while cmd := next(f, None):
n = cmd.bytesWritten(n)
if n == 0:
break
self.__sendNext()
class MantaSocket:
def __init__(self) -> None:
self.socket = QTcpSocket()
self.timer = QTimer()
self.timer.timeout.connect(self.client_listening)
self.timer.start(1000)
self.socket.readyRead.connect(self.read_data)
self.socket.disconnected.connect(self.client_socket_disconnected)
self.writeflag = 0
def client_listening(self):
if self.socket.state() == QAbstractSocket.ConnectedState:
pass
else:
self.socket.connectToHost('192.168.7.2', 2345)
if not self.socket.waitForConnected(200):
return
print('Connected!!!')
# self.write_data()
def read_data(self):
print("read!!!")
receive_data = self.socket.readAll()
if not receive_data.isEmpty():
# print(socketdata)
self.socket_data_analysis(receive_data)
def write_data(self):
# obj=bytes('hello'.encode())
# send_str=QByteArray(obj)
send_str = 'hello yeah world'
if self.writeflag:
self.socket.writeData(send_str, len(send_str))
def client_socket_disconnected(self):
print('Socket Disconnect!!!')
def socket_data_analysis(self, receive):
'''
pos_left,pos_right,pos_left_roll,pos_right_roll,dpos_left,dpos_right,dpos_left_roll,dpos_right_roll,pos_behind_left,pos_behind_right,
roll,yaw,left_A,right_A,yaw_desire,pitch,left_behind,right_behind,pitch_desire,depth_c,depth;
'''
# print(receive)
pos_str = "$POS"
pos_str_index = receive.indexOf(pos_str.encode())
aux_num = 0
data_index = 0
data_index = pos_str_index + (4 + 1)
aux_num = 5
pos_left = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_right = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_left_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_right_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_left = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_right = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_left_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_right_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_behind_left = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_behind_right = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
yaw = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
left_A = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
right_A = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
yaw_desire = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
pitch = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
left_behind = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
right_behind = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
pitch_desire = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
depth_c = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 7
depth = receive.mid(data_index, aux_num).toFloat()[0]
# Window UI Display
window.ui.pitch_label.setText(str(pitch))
window.ui.roll_label.setText(str(roll))
window.ui.yaw_label.setText(str(yaw))
window.ui.depth_label.setText(str(depth))
