def test_01_make_fail_01(self):
"""Test creation w/ size error.
"""
data = 'x'*(packet.MAX_PAYLOAD_SIZE + 1) # just one byte to much...
self.assertRaises(packet.InvalidPayloadSize, packet.make_packet,
2, 0, 7, 12, data)
data = 'x'*packet.MAX_PAYLOAD_SIZE # should fit
packet.make_packet(2, 0, 7, 12, data)
return
def getInput(self, debug=False):
pygame.event.pump()
# if self.first_time:
# self.first_time = False
# return "CONNECT 192.168.43.109 65533"
roll, pitch, = self.transform_stick(self.controller.get_left_stick())
yaw, _ = self.transform_stick(self.controller.get_right_stick())
self.collective += self.controller.get_triggers() * 10
print(f"{self.collective:6.2f} {pitch:6.2f} {roll:6.2f} {yaw:6.2f}")
scale = 750
down = self.getButtonsDown()
if down == 0:
while len(
(new_pid := input("Enter new pid values: [axis] [p] [i] [d]").
split(" "))) != 4:
pass
return packet.make_packet(packet.COMMAND_PID,
tuple(map(int, new_pid)))
def test_02_parse_fail_01(self):
"""Test parsing w/ error.
"""
p = packet.make_packet(2, packet.RESET, 7, 12)
px = p[:6] # cut packet length
self.assertRaises(packet.InvalidHeaderSize, packet.unpack_packet, px)
return
def test_05_compressed_payload(self):
"""Test RmcPackets compression feature.
"""
import zlib
tdata = 'x'*500 + 'F'*100 + 'j'*33 + 'fx'*500
cdata = zlib.compress(tdata)
ps = packet.make_packet(3, packet.RESET | packet.COMPRESSED_DATA,
9, 12, cdata)
# parsed packet
p = packet.Packet.from_string(ps)
self.assertEquals(True, p.is_compressed)
self.assertEquals(3, p.id)
self.assertEquals(9, p.heartbeat)
self.assertEquals(12, p.seq)
self.assertEquals(cdata, p.raw_payload)
self.assertEquals(tdata, p.payload)
# check if to_string
s = str(p)
self.assertEquals(ps, s)
# packet from scratch
p = packet.Packet()
p.is_compressed = True
p.payload = tdata
self.assertEquals(cdata, p.raw_payload)
self.assertEquals(tdata, p.payload)
return
def test_01_make_01(self):
"""Test simple packet creation.
"""
p = packet.make_packet(1)
self.assertEquals(p[0], '\x02') # Version
self.assertEquals(p[1], '\x00') # heartbeat index
return
def test_01_make_02(self):
"""Test simple packet creation w/ network byte order.
"""
p = packet.make_packet(1, network_order=True)
self.assertEquals(p[0], '\x82') # Version
self.assertEquals(p[1], '\x00') # heartbeat index
return
def test_01_make_04(self):
"""Test packet creation w/ payload.
"""
pl = 'AABBCCDDEEFF'
p = packet.make_packet(2, 0, 7, 12, payload=pl)
self.assertEquals(p[0], '\x02') # Version
self.assertEquals(p[1], '\x07') # heartbeat
return
def test_01_make_03(self):
"""Test packet creation w/ session
"""
p = packet.make_packet(2, packet.RESET, 7, 12)
self.assertEquals(p[0], '\x02') # Version
self.assertEquals(p[1], '\x07') # heartbeat
x = p[-4:]
self.assertEquals(x[0], '\x0c') # seq no last byte
return
def test_02_parse_02(self):
"""Test parsing w/ data.
"""
p = packet.make_packet(2, packet.RESET, 7, 12)
h, payload = packet.unpack_packet(p)
self.assertEquals(2, h[0]) # Version
self.assertEquals(packet.RESET, h[3]) # flags
self.assertEquals(12, h[4]) # sequence no.
self.assertEquals('', payload)
return
def send_packet(self, pkt):
"Takes a BlinkMsg already generated and sends it via serial"
msg = make_packet()
pkt = self.sim.newSerialPacket()
pkt.setData(msg.get_data())
pkt.setType(msg.am_type)
pkt.setDestination(0)
pkt.deliver(0, self.sim.time() + 3)
self.run_some_events()
print "sended packet:\n%s" % str(msg)
def test_02_parse_03(self):
"""Test parsing w/ payload.
"""
pl = 'AABBCCDDEEFF'
p = packet.make_packet(2, packet.RESET, 7, 12, payload=pl)
h, payload = packet.unpack_packet(p)
self.assertEquals(2, h[0]) # Version
self.assertEquals(packet.RESET, h[3]) # flags
self.assertEquals(12, h[4]) # sequence no.
self.assertEquals(pl, payload)
return
def test_06_byte_order(self):
"""Test RmcPacket w/ different byte orders.
"""
ps = packet.make_packet(3, packet.RESET, 99, 456773)
## h = [hex(ord(x)) for x in ps]
## print
## print 'A|', repr(h)
p = packet.Packet.from_string(ps)
self.assertEquals(False, p.order)
self.assertEquals(3, p.id)
self.assertEquals(99, p.heartbeat)
self.assertEquals(456773, p.seq)
ps = packet.make_packet(3, packet.RESET, 99, 456773, network_order=True)
## h = [hex(ord(x)) for x in ps]
## print 'B|', repr(h)
p = packet.Packet.from_string(ps)
self.assertEquals(True, p.order)
self.assertEquals(3, p.id)
self.assertEquals(99, p.heartbeat)
self.assertEquals(456773, p.seq)
def test_02_parse_01(self):
"""Test simple parsing.
"""
p = packet.make_packet(1, 0, 7, 10000)
h, payload = packet.unpack_packet(p)
self.assertEquals(2, h[0]) # version
self.assertEquals(7, h[1]) # heartbeat
self.assertEquals(1, h[2]) # Pid
self.assertEquals(0, h[3]) # flags
self.assertEquals(10000, h[4]) # sequence no.
self.assertEquals('', payload)
return
def test_04_normal_payload(self):
"""Test RmcPacket w/ payload.
"""
tdata = 'x'*500 + 'F'*500
ps = packet.make_packet(3, packet.RESET, 8, 12, tdata)
p = packet.Packet.from_string(ps)
self.assertEquals(False, p.is_compressed)
self.assertEquals(3, p.id)
self.assertEquals(8, p.heartbeat)
self.assertEquals(12, p.seq)
self.assertEquals(tdata, p.raw_payload)
self.assertEquals(tdata, p.payload)
# check if to_string
s = str(p)
self.assertEquals(ps, s)
return
def send(self, data, sendTo, flag=0):
unacked = len(self.pkts)
packet = make_packet(self.nextseq, data, flag)
oldest_unack = (self.nextseq - unacked) & 0xffff
# fall back to fixed timeout value
self.localSocket.settimeout(0.1)
lastsent = False
if flag == 2 and unacked < self.window_size:
self.sendPkt(packet)
lastsent = True
while unacked >= self.window_size or (flag == 2 and unacked > 0):
try:
pkt, address = self.localSocket.recvfrom(MAX_SIZE)
except socket.timeout:
# resend all pkts
if time.time() - self.timer < self.TIMEOUT:
self.start_timer()
self.sendAll(sendTo)
print("go back n, resend all")
else:
csum, rsum, seq, _, _ = parse_packet(pkt)
# not corrupted
if csum == rsum:
# cumulative acknowledgement
cum_acks = seq - oldest_unack + 1
if cum_acks < 0: # seqnum restarts from zero
cum_acks = seq + 1 + 0xffff - oldest_unack + 1
self.pkts = self.pkts[cum_acks:]
#print("seq: {}, oldest: {} cum ACK {}".format(seq, oldest_unack, cum_acks))
unacked -= cum_acks
oldest_unack = (oldest_unack + cum_acks) & 0xffff
if unacked != 0:
self.start_timer()
if flag == 2 and not lastsent:
self.sendPkt(packet, sendTo)
lastsent = True
# ok to send now
if flag != 2:
self.sendPkt(packet, sendTo)
def send(self, data, sendTo, flag=0):
ack_received = False
packet = make_packet(self.seq, data, flag)
alpha = 0.875
beta = 0.25
ertt = 0.5
devrtt = 0
self.localSocket.settimeout(ertt)
retrans = 0
while not ack_received:
start = time.time()
self.localSocket.sendto(packet, sendTo)
try:
message, address = self.localSocket.recvfrom(MAX_SIZE)
except socket.timeout:
retrans += 1
print('timeout, retrans = ' + str(retrans), end='\r')
else:
csum, rsum, seqnum, flag, data = parse_packet(message)
# not corrupted and expected?
if csum == rsum and seqnum == self.seq:
#print("ACK {} received.".format(self.seq))
end = time.time()
samplertt = end - start
ertt = alpha * ertt + (1 - alpha) * samplertt
devrtt = (1 - beta) * devrtt + beta * abs(samplertt - ertt)
self.localSocket.settimeout(ertt + 4 * devrtt)
ack_received = True
else:
retrans += 1
print('timeout, retrans = ' + str(retrans), end='\r')
self.seq = 1 - self.seq
def send_pkt(self, payload):
pkt = packet.make_packet(payload)
msg = gr.message_from_string(pkt)
self.src.msgq().insert_tail(msg)
time.sleep(4)
ser.flush()
a = 0
print("Start")
supplier = inputsupplier.getSupplier()
try:
while True:
a += 1
enc = bytes()
if a == 1:
enc = packet.make_packet(packet.COMMAND_MODE, (1, ))
if a > 1:
enc = packet.make_packet(packet.COMMAND_IMU, (0, ))
# if a == 40:
# enc = packet.make_packet(packet.COMMAND_MODE, (2,))
# if a == 100:
# enc = packet.make_packet(packet.COMMAND_MODE, (4,))
# if a > 200:
# enc += supplier.getInput(False)
ser.write(enc)
ser.flush()
line = ser.read(ser.in_waiting).replace(b"\r\n", b"\n").decode("ascii")
def send(self, data, flag=0):
packet = make_packet(0, data, flag)
self.localSocket.send(packet)
self.sent += 1
def send(self, data, sendTo, flag=0):
p = make_packet(0, data, flag=flag)
self.localSocket.sendto(p, sendTo)
<http://www.gnu.org/licenses/>.
"""
import sys
import os.path
import cProfile as profile
# adjust sys.path so that we can simply import packet
sys.path.insert(0, os.path.abspath(".."))
import packet
###
RUNS = 500000
DATA1 = packet.make_packet(1, packet.RESENT, 9, 10)
DATA2 = packet.make_packet(1, packet.RESENT, 8, 10, "TT" * 5000)
###
def parse_packet_01():
for n in xrange(RUNS):
p = packet.unpack_packet(DATA1)
return
#
def parse_packet_02():
def setUp(self):
self.test_data = packet.make_packet(2, packet.RESET, 9, 12)
pass
def send_pkt(self,payload): pkt = packet.make_packet(payload) msg = gr.message_from_string(pkt) self.src.msgq().insert_tail(msg)
if down == 0:
while len(
(new_pid := input("Enter new pid values: [axis] [p] [i] [d]").
split(" "))) != 4:
pass
return packet.make_packet(packet.COMMAND_PID,
tuple(map(int, new_pid)))
elif down == 1:
return packet.make_packet(packet.COMMAND_RESETI, ())
p = packet.make_packet(packet.COMMAND_MANUAL_CONTROL, (scale, *tuple(
map(lambda x: min(2 * scale, max(0, int(x * scale + scale))), (
self.collective / scale * 0.8 + 0.2,
pitch / scale * 30,
roll / scale * 30,
yaw / scale * 200,
)))))
if down == 2:
p += packet.make_packet(packet.COMMAND_GET_BATTERY_VOLTAGES, ())
return p
def stop(self):
return False
def getSupplier():
return ControllerInputSupplier(0)
def __exit__(self, exc_type, exc_val, exc_tb):
self.ser.write(
packet.make_packet(packet.COMMAND_MODE, (packet.MODE_STANDBY, )))
self.in_context_manager = False
def build_packet_02():
pl = 'X'*3000
for n in xrange(RUNS):
p = packet.make_packet(1, packet.RESENT, 88, 33463065, pl)
return
class ControllerInputSupplier:
def __init__(self, id):
self.id = id
self.controller = xbox360.Controller(id)
self.__state_before_buttons = (False, False, False, False)
self.__state_before_pad = (False, False, False, False)
self.first_time = True
self.collective = 0
@staticmethod
def length(radians):
radians %= math.pi / 2
return 1 / math.cos(radians if radians < math.pi / 4 else math.pi / 2 -
radians)
def transform_stick(self, joystick):
old_x, old_y = joystick
old_y *= -1
if old_x == 0:
angle = 90
else:
angle = math.degrees(math.atan(abs(old_y / old_x)))
if old_x <= 0 and old_y > 0:
angle = 180 - angle
elif old_x <= 0 and old_y <= 0:
angle = 180 + angle
elif old_x > 0 and old_y <= 0:
angle = 360 - angle
else:
angle = angle
angle = math.radians(angle)
length = math.sqrt(old_x**2 + old_y**2)
desired_length_multiplier = ControllerInputSupplier.length(angle)
length *= desired_length_multiplier
# print(f"{length * math.cos(angle):5.2f}, {length * math.sin(angle):5.2f}, {old_x:8.2f}, {old_y:5.2f}, {math.degrees(angle):8.2f}, {desired_length_multiplier:8.2f}")
#print(f"{math.degrees(angle):8.2f}, {old_x:6.2f}, {old_y:6.2f}")
return length * math.cos(angle), length * math.sin(angle)
def getButtonsDown(self):
res = self.controller.get_buttons()
res = [res[i] and not self.__state_before_buttons[i] for i in range(4)]
self.__state_before_buttons = res
try:
return res.index(True)
except:
return -1
output = [
(0, 50, 50, 0),
(0, -50, 50, 0),
]
def getInput(self, debug=False):
pygame.event.pump()
# if self.first_time:
# self.first_time = False
# return "CONNECT 192.168.43.109 65533"
roll, pitch, = self.transform_stick(self.controller.get_left_stick())
yaw, _ = self.transform_stick(self.controller.get_right_stick())
self.collective += self.controller.get_triggers() * 10
print(f"{self.collective:6.2f} {pitch:6.2f} {roll:6.2f} {yaw:6.2f}")
scale = 750
down = self.getButtonsDown()
if down == 0:
while len(
(new_pid := input("Enter new pid values: [axis] [p] [i] [d]").
split(" "))) != 4:
pass
return packet.make_packet(packet.COMMAND_PID,
tuple(map(int, new_pid)))
elif down == 1:
return packet.make_packet(packet.COMMAND_RESETI, ())
time.sleep(1)
def hasTelemetry(self):
return self.telemetry != "No data"
if __name__ == "__main__":
with DroneCommunication() as c:
telemetry = DroneTelemetryManager(c)
telemetry.start()
counter = 0
enc1 = packet.CommandPackets.SOFT_INITIALIZE
enc1 += packet.make_packet(packet.COMMAND_IMU, (packet.IMU_RESET, ))
c.write(enc1)
while True:
if counter == 30:
print("resetting imu")
c.write(
packet.make_packet(packet.COMMAND_IMU,
(packet.IMU_RESET, )))
if telemetry.hasTelemetry():
counter += 1
time.sleep(0.1)
def build_packet_01():
for n in xrange(RUNS):
p = packet.make_packet(1)
return