91 lines
2.3 KiB
Python
91 lines
2.3 KiB
Python
import struct
|
|
import time
|
|
|
|
import serial
|
|
|
|
import mspdef
|
|
|
|
|
|
class Link:
|
|
def __init__(self, port):
|
|
self.port = port
|
|
self.buf = None
|
|
|
|
def request(self, msg):
|
|
self.send(msg.command())
|
|
|
|
def push(self, msg):
|
|
self.send(msg.command(), mspdef.encode(msg))
|
|
|
|
def send(self, command, data=None):
|
|
data = data or b''
|
|
chk = len(data) ^ command
|
|
for ch in data:
|
|
chk ^= ch
|
|
self.write(b'$M<' + struct.pack('<BB', len(data), command) +
|
|
bytes(data) + bytes((chk, )))
|
|
|
|
def write(self, b):
|
|
self.port.write(b)
|
|
|
|
def get(self) -> int:
|
|
if not self.buf:
|
|
w = self.port.in_waiting or 1
|
|
self.buf = self.port.read(w)
|
|
print('< %r' % self.buf)
|
|
if not self.buf:
|
|
raise Exception('Read timeout')
|
|
ch, self.buf = self.buf[0], self.buf[1:]
|
|
return ch
|
|
|
|
def read(self):
|
|
while True:
|
|
if self.get() != ord('$'):
|
|
print('sync')
|
|
continue
|
|
if self.get() != ord('M'):
|
|
print('not-m')
|
|
continue
|
|
if self.get() != ord('>'):
|
|
print('not->')
|
|
continue
|
|
size = self.get()
|
|
command = self.get()
|
|
payload = []
|
|
for _ in range(size):
|
|
payload.append(self.get())
|
|
chk = command ^ size
|
|
for p in payload:
|
|
chk ^= p
|
|
if self.get() != chk:
|
|
print('chk')
|
|
continue
|
|
return command, payload
|
|
|
|
|
|
def main():
|
|
p = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
|
|
l = Link(p)
|
|
aux1 = 1000
|
|
ch1 = 1000
|
|
# print('\033[;H\033[J')
|
|
while True:
|
|
ch1 += 999
|
|
if ch1 >= 2000:
|
|
ch1 = 1000
|
|
# print('\033[;H')
|
|
for t in mspdef.Status, :
|
|
l.request(t())
|
|
command, payload = l.read()
|
|
s = mspdef.decode(t(), payload)
|
|
print(s)
|
|
if s.arming_disable_flags == 0:
|
|
aux1 = 1500
|
|
l.push(mspdef.SetRawRC([ch1, ch1, 1000, ch1, aux1]))
|
|
command, payload = l.read()
|
|
print(mspdef.decode(mspdef.SetRawRC(), payload))
|
|
time.sleep(0.1)
|
|
|
|
|
|
main()
|