amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

lots more work on python ugv client

try-fix-comms-errors
Alex Mikhalev 6 years ago
parent
24bce4311a
commit
f475a5b962
3 changed files with 554 additions and 106 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 243
      e32_client/e32.py
  2. 351
      e32_client/messages_pb2.py
  3. 66
      e32_client/ugv.py

243
e32_client/e32.py
Unescape View File

@ -1,3 +1,5 @@
#!/usr/bin/env python3
import serial import serial
import struct import struct
@ -6,25 +8,25 @@ PARITY_ODD = 1
PARITY_EVEN = 2 PARITY_EVEN = 2
BAUD_TABLE = { BAUD_TABLE = {
0: 1200, 0: 1200,
1: 2400, 1: 2400,
2: 4800, 2: 4800,
3: 9600, 3: 9600,
4: 19200, 4: 19200,
5: 38400, 5: 38400,
6: 57600, 6: 57600,
7: 115200, 7: 115200,
} }
AIR_DATA_RATE_TABLE = { AIR_DATA_RATE_TABLE = {
0: 300, 0: 300,
1: 1200, 1: 1200,
2: 2400, 2: 2400,
3: 4800, 3: 4800,
4: 9600, 4: 9600,
5: 19200, 5: 19200,
6: 19200, 6: 19200,
7: 19200, 7: 19200,
} }
TX_MODE_TRANSPARENT = 0 TX_MODE_TRANSPARENT = 0
@ -34,103 +36,132 @@ IO_MODE_OPEN_COLLECTOR = 0
IO_MODE_PUSH_PULL = 1 IO_MODE_PUSH_PULL = 1
TX_POWER_TABLE = { TX_POWER_TABLE = {
0: 30, 0: 30,
1: 27, 1: 27,
2: 24, 2: 24,
3: 21, 3: 21,
} }
def least_gte(value, dic):
items = sorted(dic.items(), key = lambda item: item[1]) def __least_gte(value, dic):
last_k = None items = sorted(dic.items(), key=lambda item: item[1])
for k, v in items: last_k = None
last_k = k for k, v in items:
if v >= value: last_k = k
break if v >= value:
return last_k break
return last_k
class E32_Params: class E32_Params:
save: bool save: bool
address: int address: int
parity: int parity: int
baud: int baud: int
air_data_rate: int air_data_rate: int
channel: int channel: int
tx_mode: int tx_mode: int
io_mode: int io_mode: int
wake_up_time: int wake_up_time: int
fec_enabled: bool fec_enabled: bool
tx_power: int tx_power: int
def default(): @staticmethod
p = E32_Params() def default():
p.save = True p = E32_Params()
p.address = 0 p.save = True
p.parity = PARITY_NONE p.address = 0
p.baud = 9600 p.parity = PARITY_NONE
p.air_data_rate = 2400 p.baud = 9600
p.channel = 0x17 p.air_data_rate = 2400
p.tx_mode = TX_MODE_TRANSPARENT p.channel = 0x17
p.io_mode = IO_MODE_PUSH_PULL p.tx_mode = TX_MODE_TRANSPARENT
p.wake_up_time = 250 p.io_mode = IO_MODE_PUSH_PULL
p.fec_enabled = True p.wake_up_time = 250
p.tx_power = 30 p.fec_enabled = True
return p p.tx_power = 30
return p
def unpack(data):
p = E32_Params() @staticmethod
datab = bytes(data) def unpack(data):
if datab[0] == 0xC0: p = E32_Params()
p.save = True datab = bytes(data)
elif datab[0] == 0xC2: if len(datab) != 6:
p.save = False raise Exception('invalid E32_Params data length')
else: if datab[0] == 0xC0:
raise Exception('invalid E32_Params data') p.save = True
p.address = (datab[1] << 8) | datab[2] elif datab[0] == 0xC2:
p.parity = (datab[3] >> 6) & 0b11 p.save = False
p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111] else:
p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111] raise Exception('invalid E32_Params data header')
p.channel = datab[4] p.address = (datab[1] << 8) | datab[2]
p.tx_mode = (datab[5] >> 7) & 1 p.parity = (datab[3] >> 6) & 0b11
p.io_mode = (datab[5] >> 6) & 1 p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111]
p.wake_up_time = 250 * (((datab[5] >> 3) & 0b111) + 1) p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111]
p.fec_enabled = (datab[5] & (1 << 2)) != 0 p.channel = datab[4]
p.tx_power = TX_POWER_TABLE[datab[5] & 0b11] p.tx_mode = (datab[5] >> 7) & 1
return p p.io_mode = (datab[5] >> 6) & 1
p.wake_up_time = 250 * (((datab[5] >> 3) & 0b111) + 1)
def pack(self): p.fec_enabled = (datab[5] & (1 << 2)) != 0
p = self p.tx_power = TX_POWER_TABLE[datab[5] & 0b11]
datab = bytearray(range(6)) return p
if p.save:
datab[0] = 0xC0 def pack(self):
else: p = self
datab[0] = 0xC2 datab = bytearray(range(6))
datab[1] = (p.address >> 8) & 0xFF if p.save:
datab[2] = (p.address) & 0xFF datab[0] = 0xC0
datab[3] = 0 else:
datab[3] |= p.parity << 6 datab[0] = 0xC2
datab[3] |= (least_gte(p.baud, BAUD_TABLE) << 3) datab[1] = (p.address >> 8) & 0xFF
datab[3] |= (least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE)) datab[2] = (p.address) & 0xFF
datab[4] = p.channel datab[3] = 0
datab[5] = 0 datab[3] |= p.parity << 6
datab[5] |= p.tx_mode << 7 datab[3] |= (__least_gte(p.baud, BAUD_TABLE) << 3)
datab[5] |= p.io_mode << 6 datab[3] |= (__least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE))
datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3 datab[4] = p.channel
datab[5] |= p.fec_enabled << 2 datab[5] = 0
datab[5] |= least_gte(p.tx_power, TX_POWER_TABLE) datab[5] |= p.tx_mode << 7
return datab datab[5] |= p.io_mode << 6
datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3
datab[5] |= p.fec_enabled << 2
datab[5] |= __least_gte(p.tx_power, TX_POWER_TABLE)
return datab
class E32: class E32:
ser: serial.Serial ser: serial.Serial
def __init__(self, serial_port: serial.Serial):
self.ser = serial_port
def close(self):
self.ser.close()
def read_version(self):
# self.ser.flush()
self.ser.write(b'\xC3\xC3\xC3')
version = self.ser.read(size=4)
print("version: ", version)
return version
def reset(self):
# self.ser.flush()
print("writing: ", b'\xC4\xC4\xC4')
self.ser.write(b'0xC40xC40xC4')
def read_params(self):
# self.ser.flush()
self.ser.write(b'\xC1\xC1\xC1')
param_bytes = self.ser.read(size=6)
print("param_bytes: ", param_bytes)
return E32_Params.unpack(param_bytes)
def __init__(self, serial_port: serial.Serial):
self.ser = serial_port
if __name__ == "__main__": if __name__ == "__main__":
p = E32_Params.default() p = E32_Params.default()
print("params: ", p.__dict__) print("params: ", p.__dict__)
data = p.pack() data = p.pack()
print("packed data: ", ', '.join(format(x, '02x') for x in data)) print("packed data: ", ', '.join(format(x, '02x') for x in data))
p2 = E32_Params.unpack(data) p2 = E32_Params.unpack(data)
print("unpacked params: ", p2.__dict__) print("unpacked params: ", p2.__dict__)

351
e32_client/messages_pb2.py
Unescape View File

@ -0,0 +1,351 @@
# Generated by the protocol buffer compiler. DO NOT EDIT!
# source: messages.proto
import sys
_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
from google.protobuf.internal import enum_type_wrapper
from google.protobuf import descriptor as _descriptor
from google.protobuf import message as _message
from google.protobuf import reflection as _reflection
from google.protobuf import symbol_database as _symbol_database
# @@protoc_insertion_point(imports)
_sym_db = _symbol_database.Default()
DESCRIPTOR = _descriptor.FileDescriptor(
name='messages.proto',
package='uas.ugv.messages',
syntax='proto3',
serialized_options=_b('H\003'),
serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"f\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"N\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*>\n\tUGV_State\x12\x08\n\x04IDLE\x10\x00\x12\x0c\n\x08\x41QUIRING\x10\x01\x12\x0b\n\x07\x44RIVING\x10\x02\x12\x0c\n\x08\x46INISHED\x10\x03*,\n\x11GroundCommandType\x12\x0b\n\x07\x44ISABLE\x10\x00\x12\n\n\x06\x45NABLE\x10\x01\x42\x02H\x03\x62\x06proto3')
)
_UGV_STATE = _descriptor.EnumDescriptor(
name='UGV_State',
full_name='uas.ugv.messages.UGV_State',
filename=None,
file=DESCRIPTOR,
values=[
_descriptor.EnumValueDescriptor(
name='IDLE', index=0, number=0,
serialized_options=None,
type=None),
_descriptor.EnumValueDescriptor(
name='AQUIRING', index=1, number=1,
serialized_options=None,
type=None),
_descriptor.EnumValueDescriptor(
name='DRIVING', index=2, number=2,
serialized_options=None,
type=None),
_descriptor.EnumValueDescriptor(
name='FINISHED', index=3, number=3,
serialized_options=None,
type=None),
],
containing_type=None,
serialized_options=None,
serialized_start=496,
serialized_end=558,
)
_sym_db.RegisterEnumDescriptor(_UGV_STATE)
UGV_State = enum_type_wrapper.EnumTypeWrapper(_UGV_STATE)
_GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
name='GroundCommandType',
full_name='uas.ugv.messages.GroundCommandType',
filename=None,
file=DESCRIPTOR,
values=[
_descriptor.EnumValueDescriptor(
name='DISABLE', index=0, number=0,
serialized_options=None,
type=None),
_descriptor.EnumValueDescriptor(
name='ENABLE', index=1, number=1,
serialized_options=None,
type=None),
],
containing_type=None,
serialized_options=None,
serialized_start=560,
serialized_end=604,
)
_sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
GroundCommandType = enum_type_wrapper.EnumTypeWrapper(_GROUNDCOMMANDTYPE)
IDLE = 0
AQUIRING = 1
DRIVING = 2
FINISHED = 3
DISABLE = 0
ENABLE = 1
_LOCATION = _descriptor.Descriptor(
name='Location',
full_name='uas.ugv.messages.Location',
filename=None,
file=DESCRIPTOR,
containing_type=None,
fields=[
_descriptor.FieldDescriptor(
name='fix_quality', full_name='uas.ugv.messages.Location.fix_quality', index=0,
number=1, type=13, cpp_type=3, label=1,
has_default_value=False, default_value=0,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
_descriptor.FieldDescriptor(
name='latitude', full_name='uas.ugv.messages.Location.latitude', index=1,
number=2, type=2, cpp_type=6, label=1,
has_default_value=False, default_value=float(0),
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
_descriptor.FieldDescriptor(
name='longitude', full_name='uas.ugv.messages.Location.longitude', index=2,
number=3, type=2, cpp_type=6, label=1,
has_default_value=False, default_value=float(0),
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
_descriptor.FieldDescriptor(
name='altitude', full_name='uas.ugv.messages.Location.altitude', index=3,
number=4, type=2, cpp_type=6, label=1,
has_default_value=False, default_value=float(0),
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
],
extensions=[
],
nested_types=[],
enum_types=[
],
serialized_options=None,
is_extendable=False,
syntax='proto3',
extension_ranges=[],
oneofs=[
],
serialized_start=36,
serialized_end=122,
)
_UGV_STATUS = _descriptor.Descriptor(
name='UGV_Status',
full_name='uas.ugv.messages.UGV_Status',
filename=None,
file=DESCRIPTOR,
containing_type=None,
fields=[
_descriptor.FieldDescriptor(
name='state', full_name='uas.ugv.messages.UGV_Status.state', index=0,
number=1, type=14, cpp_type=8, label=1,
has_default_value=False, default_value=0,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
_descriptor.FieldDescriptor(
name='location', full_name='uas.ugv.messages.UGV_Status.location', index=1,
number=2, type=11, cpp_type=10, label=1,
has_default_value=False, default_value=None,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
],
extensions=[
],
nested_types=[],
enum_types=[
],
serialized_options=None,
is_extendable=False,
syntax='proto3',
extension_ranges=[],
oneofs=[
],
serialized_start=124,
serialized_end=226,
)
_UGV_MESSAGE = _descriptor.Descriptor(
name='UGV_Message',
full_name='uas.ugv.messages.UGV_Message',
filename=None,
file=DESCRIPTOR,
containing_type=None,
fields=[
_descriptor.FieldDescriptor(
name='status', full_name='uas.ugv.messages.UGV_Message.status', index=0,
number=1, type=11, cpp_type=10, label=1,
has_default_value=False, default_value=None,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
_descriptor.FieldDescriptor(
name='command_ack', full_name='uas.ugv.messages.UGV_Message.command_ack', index=1,
number=2, type=13, cpp_type=3, label=1,
has_default_value=False, default_value=0,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
],
extensions=[
],
nested_types=[],
enum_types=[
],
serialized_options=None,
is_extendable=False,
syntax='proto3',
extension_ranges=[],
oneofs=[
_descriptor.OneofDescriptor(
name='ugv_message', full_name='uas.ugv.messages.UGV_Message.ugv_message',
index=0, containing_type=None, fields=[]),
],
serialized_start=228,
serialized_end=327,
)
_GROUNDCOMMAND = _descriptor.Descriptor(
name='GroundCommand',
full_name='uas.ugv.messages.GroundCommand',
filename=None,
file=DESCRIPTOR,
containing_type=None,
fields=[
_descriptor.FieldDescriptor(
name='id', full_name='uas.ugv.messages.GroundCommand.id', index=0,
number=1, type=13, cpp_type=3, label=1,
has_default_value=False, default_value=0,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
_descriptor.FieldDescriptor(
name='type', full_name='uas.ugv.messages.GroundCommand.type', index=1,
number=2, type=14, cpp_type=8, label=1,
has_default_value=False, default_value=0,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
],
extensions=[
],
nested_types=[],
enum_types=[
],
serialized_options=None,
is_extendable=False,
syntax='proto3',
extension_ranges=[],
oneofs=[
],
serialized_start=329,
serialized_end=407,
)
_GROUNDMESSAGE = _descriptor.Descriptor(
name='GroundMessage',
full_name='uas.ugv.messages.GroundMessage',
filename=None,
file=DESCRIPTOR,
containing_type=None,
fields=[
_descriptor.FieldDescriptor(
name='command', full_name='uas.ugv.messages.GroundMessage.command', index=0,
number=1, type=11, cpp_type=10, label=1,
has_default_value=False, default_value=None,
message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR),
],
extensions=[
],
nested_types=[],
enum_types=[
],
serialized_options=None,
is_extendable=False,
syntax='proto3',
extension_ranges=[],
oneofs=[
_descriptor.OneofDescriptor(
name='ground_message', full_name='uas.ugv.messages.GroundMessage.ground_message',
index=0, containing_type=None, fields=[]),
],
serialized_start=409,
serialized_end=494,
)
_UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE
_UGV_STATUS.fields_by_name['location'].message_type = _LOCATION
_UGV_MESSAGE.fields_by_name['status'].message_type = _UGV_STATUS
_UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append(
_UGV_MESSAGE.fields_by_name['status'])
_UGV_MESSAGE.fields_by_name['status'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message']
_UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append(
_UGV_MESSAGE.fields_by_name['command_ack'])
_UGV_MESSAGE.fields_by_name['command_ack'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message']
_GROUNDCOMMAND.fields_by_name['type'].enum_type = _GROUNDCOMMANDTYPE
_GROUNDMESSAGE.fields_by_name['command'].message_type = _GROUNDCOMMAND
_GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append(
_GROUNDMESSAGE.fields_by_name['command'])
_GROUNDMESSAGE.fields_by_name['command'].containing_oneof = _GROUNDMESSAGE.oneofs_by_name['ground_message']
DESCRIPTOR.message_types_by_name['Location'] = _LOCATION
DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS
DESCRIPTOR.message_types_by_name['UGV_Message'] = _UGV_MESSAGE
DESCRIPTOR.message_types_by_name['GroundCommand'] = _GROUNDCOMMAND
DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE
DESCRIPTOR.enum_types_by_name['UGV_State'] = _UGV_STATE
DESCRIPTOR.enum_types_by_name['GroundCommandType'] = _GROUNDCOMMANDTYPE
_sym_db.RegisterFileDescriptor(DESCRIPTOR)
Location = _reflection.GeneratedProtocolMessageType('Location', (_message.Message,), dict(
DESCRIPTOR = _LOCATION,
__module__ = 'messages_pb2'
# @@protoc_insertion_point(class_scope:uas.ugv.messages.Location)
))
_sym_db.RegisterMessage(Location)
UGV_Status = _reflection.GeneratedProtocolMessageType('UGV_Status', (_message.Message,), dict(
DESCRIPTOR = _UGV_STATUS,
__module__ = 'messages_pb2'
# @@protoc_insertion_point(class_scope:uas.ugv.messages.UGV_Status)
))
_sym_db.RegisterMessage(UGV_Status)
UGV_Message = _reflection.GeneratedProtocolMessageType('UGV_Message', (_message.Message,), dict(
DESCRIPTOR = _UGV_MESSAGE,
__module__ = 'messages_pb2'
# @@protoc_insertion_point(class_scope:uas.ugv.messages.UGV_Message)
))
_sym_db.RegisterMessage(UGV_Message)
GroundCommand = _reflection.GeneratedProtocolMessageType('GroundCommand', (_message.Message,), dict(
DESCRIPTOR = _GROUNDCOMMAND,
__module__ = 'messages_pb2'
# @@protoc_insertion_point(class_scope:uas.ugv.messages.GroundCommand)
))
_sym_db.RegisterMessage(GroundCommand)
GroundMessage = _reflection.GeneratedProtocolMessageType('GroundMessage', (_message.Message,), dict(
DESCRIPTOR = _GROUNDMESSAGE,
__module__ = 'messages_pb2'
# @@protoc_insertion_point(class_scope:uas.ugv.messages.GroundMessage)
))
_sym_db.RegisterMessage(GroundMessage)
DESCRIPTOR._options = None
# @@protoc_insertion_point(module_scope)

66
e32_client/ugv.py
Unescape View File

@ -0,0 +1,66 @@
#!/usr/bin/env python3
import serial
from threading import Thread
import time
from e32 import E32
import messages_pb2 as messages
from google.protobuf.message import Message
class UGVComms(E32):
def __init__(self, serial_port: serial.Serial):
E32.__init__(self, serial_port)
def write_len_delimited(self, data: bytes):
len_data = (len(data)).to_bytes(
1, byteorder='big') # TODO: check byte order
self.ser.write(len_data)
self.ser.write(data)
def write_message(self, msg: Message):
data = msg.SerializeToString()
self.write_len_delimited(data)
def read_message(self):
len_data = self.ser.read(size=1)
msg_len = int.from_bytes(len_data, byteorder='big')
data = self.ser.read(size=msg_len)
msg = messages.UGV_Message()
msg.ParseFromString(data)
return msg
def __rx_thread_entry(ugv: UGVComms):
while True:
try:
msg = ugv.read_message()
print("received UGV message: ", msg)
except Exception as e:
print("error reading message: ", e)
continue
if __name__ == "__main__":
ser = serial.serial_for_url("loop://", baudrate=9600, parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
timeout=2.0)
ugv = UGVComms(ser)
rx_thread = Thread(target=__rx_thread_entry, args=(ugv, ))
rx_thread.start()
# print("resetting")
# ugv.reset()
cmd_id = 1
time.sleep(0.2)
while True:
gmsg = messages.GroundMessage()
gmsg.command.id = cmd_id
gmsg.command.type = messages.DISABLE
cmd_id += 1
print("writing message: ", gmsg)
ugv.write_message(gmsg)
time.sleep(2.)
rx_thread.join()
Write Preview
Loading…
Cancel
Save