From 001990484a2f6a922a5b93897ec509788be94535 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:36:03 -0400 Subject: [PATCH 01/12] tools: make executables executable --- tools/ping1d-simulation.py | 0 tools/pingproxy.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 tools/ping1d-simulation.py mode change 100644 => 100755 tools/pingproxy.py diff --git a/tools/ping1d-simulation.py b/tools/ping1d-simulation.py old mode 100644 new mode 100755 diff --git a/tools/pingproxy.py b/tools/pingproxy.py old mode 100644 new mode 100755 From b74c01532ea713b2fb0ac1388805388459c69892 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:36:24 -0400 Subject: [PATCH 02/12] tools: use python3 --- tools/ping1d-simulation.py | 2 +- tools/pingproxy.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/ping1d-simulation.py b/tools/ping1d-simulation.py index 1099a7c..ac72064 100755 --- a/tools/ping1d-simulation.py +++ b/tools/ping1d-simulation.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # This script simulates a Blue Robotics Ping Echosounder device # A client may connect to the device simulation on local UDP port 6676 diff --git a/tools/pingproxy.py b/tools/pingproxy.py index 527c070..b932171 100755 --- a/tools/pingproxy.py +++ b/tools/pingproxy.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # PingProxy.py # Connect multiple udp clients to a single serial device From df2012a7a5057140d2317de2b0ee40e4d61bd83a Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:36:45 -0400 Subject: [PATCH 03/12] Ping1DSimulation: default continuous mode to false, like real device --- tools/ping1d-simulation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/ping1d-simulation.py b/tools/ping1d-simulation.py index ac72064..7ec526a 100755 --- a/tools/ping1d-simulation.py +++ b/tools/ping1d-simulation.py @@ -26,7 +26,7 @@ def __init__(self): self._ping_number = 0 # count the total measurements taken since boot self._ping_interval = 100 # milliseconds between measurements self._mode_auto = True # automatic gain and range selection - self._mode_continuous = True # automatic continuous output of profile messages + self._mode_continuous = False # automatic continuous output of profile messages # read incoming client data def read(self): From 6b6c353c1297e22404ef644c91c199645d80d3ed Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:44:10 -0400 Subject: [PATCH 04/12] pingmessage: various bugfixes and improve stdout --- brping/pingmessage.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/brping/pingmessage.py b/brping/pingmessage.py index 5a119c5..82a418d 100644 --- a/brping/pingmessage.py +++ b/brping/pingmessage.py @@ -95,6 +95,7 @@ def __init__(self, msg_id=0, msg_data=None): ## The field names of this message self.payload_field_names = payload_dict[self.message_id]["field_names"] + self.update_payload_length() ## Number of bytes in the message payload self.update_payload_length() @@ -151,9 +152,15 @@ def unpack_msg_data(self, msg_data): print("error unpacking payload: %s" % e) print("msg_data: %s, header: %s" % (msg_data, header)) print("format: %s, buf: %s" % (PingMessage.endianess + self.get_payload_format(), self.msg_data[PingMessage.headerLength:PingMessage.headerLength + self.payload_length])) + print(self.get_payload_format()) else: # only use payload if didn't raise exception for i, attr in enumerate(payload_dict[self.message_id]["field_names"]): - setattr(self, attr, payload[i]) + try: + setattr(self, attr, payload[i]) + except IndexError as e: + if self.message_id in variable_msgs: + setattr(self, attr, bytearray()) + pass # Extract checksum self.checksum = struct.unpack(PingMessage.endianess + PingMessage.checksum_format, self.msg_data[PingMessage.headerLength + self.payload_length: PingMessage.headerLength + self.payload_length + PingMessage.checksumLength])[0] @@ -190,7 +197,7 @@ def get_payload_format(self): if self.message_id in variable_msgs or self.message_id in asciiMsgs: var_length = self.payload_length - payload_dict[self.message_id]["payload_length"] # Subtract static length portion from payload length if var_length <= 0: - return "" # variable data portion is empty + return payload_dict[self.message_id]["format"] # variable data portion is empty return payload_dict[self.message_id]["format"] + str(var_length) + "s" else: # messages with a static (constant) length @@ -325,6 +332,8 @@ def parse_byte(self, msg_byte): self.parsed += 1 return PingParser.NEW_MESSAGE else: + # TODO add/return error state + print("parse error") self.errors += 1 return self.state From 6c34d935d8d35a9dd7f0993305887038a89d23c1 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:45:29 -0400 Subject: [PATCH 05/12] pingmessage: tag PING360_DEVICE_DATA for variable length handling --- brping/pingmessage.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/brping/pingmessage.py b/brping/pingmessage.py index 82a418d..6a595cc 100644 --- a/brping/pingmessage.py +++ b/brping/pingmessage.py @@ -7,7 +7,8 @@ from brping import definitions payload_dict = definitions.payload_dict_all asciiMsgs = [definitions.COMMON_NACK, definitions.COMMON_ASCII_TEXT] -variable_msgs = [definitions.PING1D_PROFILE, ] +variable_msgs = [definitions.PING1D_PROFILE, definitions.PING360_DEVICE_DATA, ] + class PingMessage(object): ## header start byte 1 From 9edd830d5ff9a2fb60fa36ee1920aff7758d41f9 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:47:24 -0400 Subject: [PATCH 06/12] generate: generate device classes --- generate/generate-python.py | 18 +++ generate/templates/device.py.in | 187 +++++++++++++++++++++++++++++++ generate/templates/ping1d.py.in | 125 +++++++++++++++++++++ generate/templates/ping360.py.in | 158 ++++++++++++++++++++++++++ 4 files changed, 488 insertions(+) create mode 100644 generate/templates/device.py.in create mode 100644 generate/templates/ping1d.py.in create mode 100644 generate/templates/ping360.py.in diff --git a/generate/generate-python.py b/generate/generate-python.py index b24180f..d465e42 100755 --- a/generate/generate-python.py +++ b/generate/generate-python.py @@ -65,3 +65,21 @@ f.write(")\n") f.close() + +definitionFile = "%s/common.json" % definitionPath +templateFile = "%s/device.py.in" % templatePath +f = open("%s/device.py" % args.output_directory, "w") +f.write(g.generate(definitionFile, templateFile, {"structToken": struct_token})) +f.close() + +definitionFile = "%s/ping1d.json" % definitionPath +templateFile = "%s/ping1d.py.in" % templatePath +f = open("%s/ping1d.py" % args.output_directory, "w") +f.write(g.generate(definitionFile, templateFile, {"structToken": struct_token})) +f.close() + +definitionFile = "%s/ping360.json" % definitionPath +templateFile = "%s/ping360.py.in" % templatePath +f = open("%s/ping360.py" % args.output_directory, "w") +f.write(g.generate(definitionFile, templateFile, {"structToken": struct_token})) +f.close() diff --git a/generate/templates/device.py.in b/generate/templates/device.py.in new file mode 100644 index 0000000..e8b6482 --- /dev/null +++ b/generate/templates/device.py.in @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 + +# device.py +# A device API for devices implementing Blue Robotics ping-protocol + +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! +# THIS IS AN AUTOGENERATED FILE +# DO NOT EDIT +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! + +from brping import definitions +from brping import pingmessage +import serial +import time + +class PingDevice(object): +{% for field in all_fields|sort %} + _{{field}} = None +{% endfor%} + + def __init__(self, device_name, baudrate=115200): + if device_name is None: + print("Device name is required") + return + + try: + print("Opening %s at %d bps" % (device_name, baudrate)) + + ## Serial object for device communication + self.iodev = serial.Serial(device_name, baudrate) + self.iodev.send_break() + self.iodev.write("U".encode("utf-8")) + + except Exception as e: + print("Failed to open the given serial port") + print("\t", e) + exit(1) + + ## A helper class to take care of decoding the input stream + self.parser = pingmessage.PingParser() + + ## device id of this Ping1D object, used for dst_device_id in outgoing messages + self.my_id = 255 + + ## + # @brief Consume rx buffer data until a new message is successfully decoded + # + # @return A new PingMessage: as soon as a message is parsed (there may be data remaining in the buffer to be parsed, thus requiring subsequent calls to read()) + # @return None: if the buffer is empty and no message has been parsed + def read(self): + while self.iodev.in_waiting: + b = self.iodev.read() + + if self.parser.parse_byte(ord(b)) == pingmessage.PingParser.NEW_MESSAGE: + self.handle_message(self.parser.rx_msg) + return self.parser.rx_msg + return None + + ## + # @brief Write data to device + # + # @param data: bytearray to write to device + # + # @return Number of bytes written + def write(self, data): + return self.iodev.write(data) + + ## + # @brief Make sure there is a device on and read some initial data + # + # @return True if the device replies with expected data, False otherwise + def initialize(self): + if (self.request(definitions.COMMON_PROTOCOL_VERSION) is None): + return False + return True + + ## + # @brief Request the given message ID + # + # @param m_id: The message ID to request from the device + # @param timeout: The time in seconds to wait for the device to send + # the requested message before timing out and returning + # + # @return PingMessage: the device reply if it is received within timeout period, None otherwise + # + # @todo handle nack to exit without blocking + def request(self, m_id, timeout=0.5): + msg = pingmessage.PingMessage(definitions.COMMON_GENERAL_REQUEST) + msg.requested_id = m_id + msg.pack_msg_data() + self.write(msg.msg_data) + + # uncomment to return nacks in addition to m_id + # return self.wait_message([m_id, definitions.COMMON_NACK], timeout) + + return self.wait_message([m_id], timeout) + + ## + # @brief Wait until we receive a message from the device with the desired message_id for timeout seconds + # + # @param message_id: The message id to wait to receive from the device + # @param timeout: The timeout period in seconds to wait + # + # @return PingMessage: the message from the device if it is received within timeout period, None otherwise + def wait_message(self, message_ids, timeout=0.5): + tstart = time.time() + while time.time() < tstart + timeout: + msg = self.read() + if msg is not None: + if msg.message_id in message_ids: + return msg + return None + + ## + # @brief Handle an incoming message from the device. + # Extract message fields into self attributes. + # + # @param msg: the PingMessage to handle. + def handle_message(self, msg): + if msg.message_id in pingmessage.payload_dict: + for attr in pingmessage.payload_dict[msg.message_id]["field_names"]: + setattr(self, "_" + attr, getattr(msg, attr)) + else: + print("Unrecognized message: %d", msg) + + ## + # @brief Dump object into string representation. + # + # @return string: a string representation of the object + def __repr__(self): + representation = "---------------------------------------------------------\n~Ping1D Object~" + + attrs = vars(self) + for attr in sorted(attrs): + try: + if attr != 'iodev': + representation += "\n - " + attr + "(hex): " + str([hex(item) for item in getattr(self, attr)]) + if attr != 'data': + representation += "\n - " + attr + "(string): " + str(getattr(self, attr)) + # TODO: Better filter this exception + except: + representation += "\n - " + attr + ": " + str(getattr(self, attr)) + return representation + +{% for msg in messages["get"]|sort %} + ## + # @brief Get a {{msg|replace("get_", "")}} message from the device\n + # Message description:\n + # {{messages["get"][msg].description}} + # + # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n +{% for field in messages["get"][msg].payload %} + # {{field.name}}: {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}}\n +{% endfor%} + def get_{{msg}}(self): + if self.request(definitions.COMMON_{{msg|upper}}) is None: + return None + data = ({ +{% for field in messages["get"][msg].payload %} + "{{field.name}}": self._{{field.name}}, # {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}} +{% endfor %} + }) + return data + +{% endfor %} + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(description="Ping python library example.") + parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") + parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate.") + args = parser.parse_args() + + p = PingDevice(args.device, args.baudrate) + + print("Initialized: %s" % p.initialize()) + +{% for msg in messages["get"]|sort %} + print("\ntesting get_{{msg}}") + result = p.get_{{msg}}() + print(" " + str(result)) + print(" > > pass: %s < <" % (result is not None)) + +{% endfor %} + + print(p) diff --git a/generate/templates/ping1d.py.in b/generate/templates/ping1d.py.in new file mode 100644 index 0000000..39cbeab --- /dev/null +++ b/generate/templates/ping1d.py.in @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 + +# ping1d.py +# A device API for the Blue Robotics Ping1D echosounder + +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! +# THIS IS AN AUTOGENERATED FILE +# DO NOT EDIT +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! + +from brping import definitions +from brping import PingDevice +from brping import pingmessage +import serial +import time + +class Ping1D(PingDevice): + + def legacyRequest(self, m_id, timeout=0.5): + msg = pingmessage.PingMessage() + # legacy hack logic is in PingMessage + # TODO: remove that logic and construct/assemble an arbitrary PingMessage + msg.request_id = m_id + msg.pack_msg_data() + self.write(msg.msg_data) + + # uncomment to return nacks in addition to m_id + # return self.wait_message([m_id, definitions.COMMON_NACK], timeout) + + return self.wait_message([m_id], timeout) + + def initialize(self): + if not PingDevice.initialize(self): + return False + if self.legacyRequest(definitions.PING1D_GENERAL_INFO) is None: + return False + return True + +{% for msg in messages["get"]|sort %} + ## + # @brief Get a {{msg|replace("get_", "")}} message from the device\n + # Message description:\n + # {{messages["get"][msg].description}} + # + # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n +{% for field in messages["get"][msg].payload %} + # {{field.name}}: {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}}\n +{% endfor%} + def get_{{msg}}(self): + if self.legacyRequest(definitions.PING1D_{{msg|upper}}) is None: + return None + data = ({ +{% for field in messages["get"][msg].payload %} + "{{field.name}}": self._{{field.name}}, # {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}} +{% endfor %} + }) + return data + +{% endfor %} +{% for msg in messages["set"]|sort %} + ## + # @brief Send a {{msg}} message to the device\n + # Message description:\n + # {{messages["set"][msg].description}}\n + # Send the message to write the device parameters, then read the values back from the device\n + # +{% for field in messages["set"][msg].payload %} + # @param {{field.name}} - {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}} +{% endfor %} + # + # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) + def {{msg}}(self{% for field in messages["set"][msg].payload %}, {{field.name}}{% endfor %}, verify=True): + m = pingmessage.PingMessage(definitions.PING1D_{{msg|upper}}) +{% for field in messages["set"][msg].payload %} + m.{{field.name}} = {{field.name}} +{% endfor %} + m.pack_msg_data() + self.write(m.msg_data) + if self.legacyRequest(definitions.PING1D_{{msg|replace("set_", "")|upper}}) is None: + return False + # Read back the data and check that changes have been applied + if (verify +{% if messages["set"][msg].payload %} + and ({% for field in messages["set"][msg].payload %}self._{{field.name}} != {{field.name}}{{ " or " if not loop.last }}{% endfor %})): +{% endif %} + return False + return True # success + +{% endfor %} + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(description="Ping python library example.") + parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") + parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate.") + args = parser.parse_args() + + p = Ping1D(args.device, args.baudrate) + + print("Initialized: %s" % p.initialize()) + +{% for msg in messages["get"]|sort %} + print("\ntesting get_{{msg}}") + result = p.get_{{msg}}() + print(" " + str(result)) + print(" > > pass: %s < <" % (result is not None)) + +{% endfor %} + print("\ntesting set_device_id") + print(" > > pass: %s < <" % p.set_device_id(43)) + print("\ntesting set_mode_auto") + print(" > > pass: %s < <" % p.set_mode_auto(False)) + print("\ntesting set_range") + print(" > > pass: %s < <" % p.set_range(1000, 2000)) + print("\ntesting set_speed_of_sound") + print(" > > pass: %s < <" % p.set_speed_of_sound(1444000)) + print("\ntesting set_ping_interval") + print(" > > pass: %s < <" % p.set_ping_interval(36)) + print("\ntesting set_gain_setting") + print(" > > pass: %s < <" % p.set_gain_setting(3)) + print("\ntesting set_ping_enable") + print(" > > pass: %s < <" % p.set_ping_enable(False)) + + print(p) diff --git a/generate/templates/ping360.py.in b/generate/templates/ping360.py.in new file mode 100644 index 0000000..cf243d0 --- /dev/null +++ b/generate/templates/ping360.py.in @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 + +# ping360.py +# A device API for the Blue Robotics Ping360 scanning sonar + +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! +# THIS IS AN AUTOGENERATED FILE +# DO NOT EDIT +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! + +from brping import definitions +from brping import PingDevice +from brping import pingmessage +import serial +import time + +class Ping360(PingDevice): + def initialize(self): + if not PingDevice.initialize(self): + return False + if (self.readDeviceInformation() is None): + return False + return True + +{% for msg in messages["get"]|sort %} + ## + # @brief Get a {{msg|replace("get_", "")}} message from the device\n + # Message description:\n + # {{messages["get"][msg].description}} + # + # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n +{% for field in messages["get"][msg].payload %} + # {{field.name}}: {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}}\n +{% endfor%} + def get_{{msg}}(self): + if self.request(definitions.PING360_{{msg|upper}}) is None: + return None + data = ({ +{% for field in messages["get"][msg].payload %} + "{{field.name}}": self._{{field.name}}, # {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}} +{% endfor %} + }) + return data + +{% endfor %} +{% for msg in messages["set"]|sort %} + ## + # @brief Send a {{msg}} message to the device\n + # Message description:\n + # {{messages["set"][msg].description}}\n + # Send the message to write the device parameters, then read the values back from the device\n + # +{% for field in messages["set"][msg].payload %} + # @param {{field.name}} - {% if field.units %}Units: {{field.units}}; {% endif %}{{field.description}} +{% endfor %} + # + # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) + def {{msg}}(self{% for field in messages["set"][msg].payload %}, {{field.name}}{% endfor %}, verify=True): + m = pingmessage.PingMessage(definitions.PING360_{{msg|upper}}) +{% for field in messages["set"][msg].payload %} + m.{{field.name}} = {{field.name}} +{% endfor %} + m.pack_msg_data() + self.write(m.msg_data) + if self.request(definitions.PING360_{{msg|replace("set_", "")|upper}}) is None: + return False + # Read back the data and check that changes have been applied + if (verify +{% if messages["set"][msg].payload %} + and ({% for field in messages["set"][msg].payload %}self._{{field.name}} != {{field.name}}{{ " or " if not loop.last }}{% endfor %})): +{% endif %} + return False + return True # success{% for field in messages["set"][msg].payload %} + m.{{field.name}} = {{field.name}} +{% endfor %} + m.pack_msg_data() + self.write(m.msg_data) + +{% endfor %} + +{% for msg in messages["control"]|sort %} + def control_{{msg}}(self{% for field in messages["control"][msg].payload %}, {{field.name}}{% endfor %}): + m = pingmessage.PingMessage(definitions.PING360_{{msg|upper}}) +{% for field in messages["control"][msg].payload %} + m.{{field.name}} = {{field.name}} +{% endfor %} + m.pack_msg_data() + self.write(m.msg_data) + +{% endfor %} + +{% for field in messages["control"]["transducer"].payload %} +{% if field.name != "transmit" and field.name != "reserved" %} + def set_{{field.name}}(self, {{field.name}}): + self.control_transducer( +{% for field2 in messages["control"]["transducer"].payload %} +{% if field2.name != "transmit" and field2.name != "reserved" %} +{% if field == field2 %} + {{field2.name}}, +{% else %} + self._{{field2.name}}, +{% endif %} +{% endif %} +{% endfor %} + 0, + 0 + ) + return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0) + +{% endif %} +{% endfor %} + + def readDeviceInformation(self): + return self.request(definitions.PING360_DEVICE_DATA) + + def transmitAngle(self, angle): + self.control_transducer( + 0, # reserved + self._gain_setting, + angle, + self._transmit_duration, + self._sample_period, + self._transmit_frequency, + self._number_of_samples, + 1, + 0 + ) + return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0) + + def transmit(self): + return self.transmitAngle(self._angle) + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(description="Ping python library example.") + parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") + parser.add_argument('--baudrate', action="store", type=int, default=2000000, help="Ping device baudrate.") + args = parser.parse_args() + + p = Ping360(args.device, args.baudrate) + + print("Initialized: %s" % p.initialize()) + + print(p.set_transmit_frequency(800)) + print(p.set_sample_period(80)) + print(p.set_number_of_samples(200)) + + tstart_s = time.time() + for x in range(400): + p.transmitAngle(x) + tend_s = time.time() + + print("full scan in %dms, %dHz" % (1000*(tend_s - tstart_s), 400/(tend_s - tstart_s))) + + print(p) + + p.control_reset(0, 0) From 5599bee9608f9084fed502cdd33e6e8d91d05a6b Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 27 Aug 2019 13:48:07 -0400 Subject: [PATCH 07/12] ping1d.py: remove autogenerated file from version control --- brping/ping1d.py | 777 ----------------------------------------------- 1 file changed, 777 deletions(-) delete mode 100644 brping/ping1d.py diff --git a/brping/ping1d.py b/brping/ping1d.py deleted file mode 100644 index 605749d..0000000 --- a/brping/ping1d.py +++ /dev/null @@ -1,777 +0,0 @@ -#!/usr/bin/env python - -# Ping1D.py -# A device API for the Blue Robotics Ping1D echosounder - -# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! -# THIS IS AN AUTOGENERATED FILE -# DO NOT EDIT -# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! - -from brping import pingmessage -from brping import definitions -import serial -import time - - -class Ping1D(object): - _hello = None - _acked_id = None - _ascii_message = None - _confidence = None - _device_id = None - _device_model = None - _device_revision = None - _device_type = None - _distance = None - _firmware_version_major = None - _firmware_version_minor = None - _firmware_version_patch = None - _gain_setting = None - _id = None - _mode_auto = None - _nack_message = None - _nacked_id = None - _pcb_temperature = None - _ping_enabled = None - _ping_interval = None - _ping_number = None - _processor_temperature = None - _profile_data = None - _requested_id = None - _reserved = None - _scan_length = None - _scan_start = None - _speed_of_sound = None - _transmit_duration = None - _version_major = None - _version_minor = None - _version_patch = None - _voltage_5 = None - - def __init__(self, device_name, baudrate=115200): - if device_name is None: - print("Device name is required") - return - - try: - print("Opening %s at %d bps" % (device_name, baudrate)) - - ## Serial object for device communication - self.iodev = serial.Serial(device_name, baudrate) - self.iodev.timeout = 1 - - except Exception as e: - print("Failed to open the given serial port") - print("\t", e) - exit(1) - - ## A helper class to take care of decoding the input stream - self.parser = pingmessage.PingParser() - - ## device id of this Ping1D object, used for dst_device_id in outgoing messages - self.my_id = 255 - - ## - # @brief Consume rx buffer data until a new message is successfully decoded - # - # @return A new PingMessage: as soon as a message is parsed (there may be data remaining in the buffer to be parsed, thus requiring subsequent calls to read()) - # @return None: if the buffer is empty and no message has been parsed - def read(self): - while self.iodev.in_waiting: - b = self.iodev.read() - - if self.parser.parse_byte(ord(b)) == pingmessage.PingParser.NEW_MESSAGE: - self.handle_message(self.parser.rx_msg) - return self.parser.rx_msg - return None - - ## - # @brief Write data to device - # - # @param data: bytearray to write to device - # - # @return Number of bytes written - def write(self, data): - return self.iodev.write(data) - - ## - # @brief Make sure there is a device on and read some initial data - # - # @return True if the device replies with expected data, False otherwise - def initialize(self): - if self.request(definitions.PING1D_VOLTAGE_5) is None: - return False - return True - - ## - # @brief Request the given message ID - # - # @param m_id: The message ID to request from the device - # @param timeout: The time in seconds to wait for the device to send - # the requested message before timing out and returning - # - # @return PingMessage: the device reply if it is received within timeout period, None otherwise - # - # @todo handle nack to exit without blocking - def request(self, m_id, timeout=0.5): - msg = pingmessage.PingMessage() - msg.request_id = m_id - msg.pack_msg_data() - self.write(msg.msg_data) - return self.wait_message(m_id, timeout) - - ## - # @brief Wait until we receive a message from the device with the desired message_id for timeout seconds - # - # @param message_id: The message id to wait to receive from the device - # @param timeout: The timeout period in seconds to wait - # - # @return PingMessage: the message from the device if it is received within timeout period, None otherwise - def wait_message(self, message_id, timeout=0.5): - tstart = time.time() - while time.time() < tstart + timeout: - msg = self.read() - if msg is not None: - if msg.message_id == message_id: - return msg - return None - - ## - # @brief Handle an incoming messge from the device. - # Extract message fields into self attributes. - # - # @param msg: the PingMessage to handle. - def handle_message(self, msg): - if msg.message_id in definitions.payload_dict_all: - for attr in pingmessage.payload_dict[msg.message_id]["field_names"]: - setattr(self, "_" + attr, getattr(msg, attr)) - else: - print("Unrecognized message: %d", msg) - - ## - # @brief Dump object into string representation. - # - # @return string: a string representation of the object - def __repr__(self): - representation = "---------------------------------------------------------\n~Ping1D Object~" - - attrs = vars(self) - for attr in sorted(attrs): - try: - if attr != 'iodev': - representation += "\n - " + attr + "(hex): " + str([hex(item) for item in getattr(self, attr)]) - if attr != 'data': - representation += "\n - " + attr + "(string): " + str(getattr(self, attr)) - # TODO: Better filter this exception - except: - representation += "\n - " + attr + ": " + str(getattr(self, attr)) - return representation - - ## - # @brief Get a device_id message from the device\n - # Message description:\n - # The device ID. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # device_id: The device ID (0-254). 255 is reserved for broadcast messages.\n - def get_device_id(self): - if self.request(definitions.PING1D_DEVICE_ID) is None: - return None - data = ({ - "device_id": self._device_id, # The device ID (0-254). 255 is reserved for broadcast messages. - }) - return data - - ## - # @brief Get a device_information message from the device\n - # Message description:\n - # Device information - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # device_type: Device type. 0: Unknown; 1: Ping Echosounder; 2: Ping360\n - # device_revision: device-specific hardware revision\n - # firmware_version_major: Firmware version major number.\n - # firmware_version_minor: Firmware version minor number.\n - # firmware_version_patch: Firmware version patch number.\n - # reserved: reserved\n - def get_device_information(self): - if self.request(definitions.COMMON_DEVICE_INFORMATION) is None: - return None - data = ({ - "device_type": self._device_type, # Device type. 0: Unknown; 1: Ping Echosounder; 2: Ping360 - "device_revision": self._device_revision, # device-specific hardware revision - "firmware_version_major": self._firmware_version_major, # Firmware version major number. - "firmware_version_minor": self._firmware_version_minor, # Firmware version minor number. - "firmware_version_patch": self._firmware_version_patch, # Firmware version patch number. - "reserved": self._reserved, # reserved - }) - return data - - ## - # @brief Get a distance message from the device\n - # Message description:\n - # The distance to target with confidence estimate. Relevant device parameters during the measurement are also provided. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # distance: Units: mm; The current return distance determined for the most recent acoustic measurement.\n - # confidence: Units: %; Confidence in the most recent range measurement.\n - # transmit_duration: Units: us; The acoustic pulse length during acoustic transmission/activation.\n - # ping_number: The pulse/measurement count since boot.\n - # scan_start: Units: mm; The beginning of the scan region in mm from the transducer.\n - # scan_length: Units: mm; The length of the scan region.\n - # gain_setting: The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144\n - def get_distance(self): - if self.request(definitions.PING1D_DISTANCE) is None: - return None - data = ({ - "distance": self._distance, # Units: mm; The current return distance determined for the most recent acoustic measurement. - "confidence": self._confidence, # Units: %; Confidence in the most recent range measurement. - "transmit_duration": self._transmit_duration, # Units: us; The acoustic pulse length during acoustic transmission/activation. - "ping_number": self._ping_number, # The pulse/measurement count since boot. - "scan_start": self._scan_start, # Units: mm; The beginning of the scan region in mm from the transducer. - "scan_length": self._scan_length, # Units: mm; The length of the scan region. - "gain_setting": self._gain_setting, # The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144 - }) - return data - - ## - # @brief Get a distance_simple message from the device\n - # Message description:\n - # The distance to target with confidence estimate. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # distance: Units: mm; Distance to the target.\n - # confidence: Units: %; Confidence in the distance measurement.\n - def get_distance_simple(self): - if self.request(definitions.PING1D_DISTANCE_SIMPLE) is None: - return None - data = ({ - "distance": self._distance, # Units: mm; Distance to the target. - "confidence": self._confidence, # Units: %; Confidence in the distance measurement. - }) - return data - - ## - # @brief Get a firmware_version message from the device\n - # Message description:\n - # Device information - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # device_type: Device type. 0: Unknown; 1: Echosounder\n - # device_model: Device model. 0: Unknown; 1: Ping1D\n - # firmware_version_major: Firmware version major number.\n - # firmware_version_minor: Firmware version minor number.\n - def get_firmware_version(self): - if self.request(definitions.PING1D_FIRMWARE_VERSION) is None: - return None - data = ({ - "device_type": self._device_type, # Device type. 0: Unknown; 1: Echosounder - "device_model": self._device_model, # Device model. 0: Unknown; 1: Ping1D - "firmware_version_major": self._firmware_version_major, # Firmware version major number. - "firmware_version_minor": self._firmware_version_minor, # Firmware version minor number. - }) - return data - - ## - # @brief Get a gain_setting message from the device\n - # Message description:\n - # The current gain setting. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # gain_setting: The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144\n - def get_gain_setting(self): - if self.request(definitions.PING1D_GAIN_SETTING) is None: - return None - data = ({ - "gain_setting": self._gain_setting, # The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144 - }) - return data - - ## - # @brief Get a general_info message from the device\n - # Message description:\n - # General information. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # firmware_version_major: Firmware major version.\n - # firmware_version_minor: Firmware minor version.\n - # voltage_5: Units: mV; Device supply voltage.\n - # ping_interval: Units: ms; The interval between acoustic measurements.\n - # gain_setting: The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144\n - # mode_auto: The current operating mode of the device. 0: manual mode, 1: auto mode\n - def get_general_info(self): - if self.request(definitions.PING1D_GENERAL_INFO) is None: - return None - data = ({ - "firmware_version_major": self._firmware_version_major, # Firmware major version. - "firmware_version_minor": self._firmware_version_minor, # Firmware minor version. - "voltage_5": self._voltage_5, # Units: mV; Device supply voltage. - "ping_interval": self._ping_interval, # Units: ms; The interval between acoustic measurements. - "gain_setting": self._gain_setting, # The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144 - "mode_auto": self._mode_auto, # The current operating mode of the device. 0: manual mode, 1: auto mode - }) - return data - - ## - # @brief Get a mode_auto message from the device\n - # Message description:\n - # The current operating mode of the device. Manual mode allows for manual selection of the gain and scan range. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # mode_auto: 0: manual mode, 1: auto mode\n - def get_mode_auto(self): - if self.request(definitions.PING1D_MODE_AUTO) is None: - return None - data = ({ - "mode_auto": self._mode_auto, # 0: manual mode, 1: auto mode - }) - return data - - ## - # @brief Get a pcb_temperature message from the device\n - # Message description:\n - # Temperature of the on-board thermistor. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # pcb_temperature: Units: cC; The temperature in centi-degrees Centigrade (100 * degrees C).\n - def get_pcb_temperature(self): - if self.request(definitions.PING1D_PCB_TEMPERATURE) is None: - return None - data = ({ - "pcb_temperature": self._pcb_temperature, # Units: cC; The temperature in centi-degrees Centigrade (100 * degrees C). - }) - return data - - ## - # @brief Get a ping_enable message from the device\n - # Message description:\n - # Acoustic output enabled state. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # ping_enabled: The state of the acoustic output. 0: disabled, 1:enabled\n - def get_ping_enable(self): - if self.request(definitions.PING1D_PING_ENABLE) is None: - return None - data = ({ - "ping_enabled": self._ping_enabled, # The state of the acoustic output. 0: disabled, 1:enabled - }) - return data - - ## - # @brief Get a ping_interval message from the device\n - # Message description:\n - # The interval between acoustic measurements. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # ping_interval: Units: ms; The minimum interval between acoustic measurements. The actual interval may be longer.\n - def get_ping_interval(self): - if self.request(definitions.PING1D_PING_INTERVAL) is None: - return None - data = ({ - "ping_interval": self._ping_interval, # Units: ms; The minimum interval between acoustic measurements. The actual interval may be longer. - }) - return data - - ## - # @brief Get a processor_temperature message from the device\n - # Message description:\n - # Temperature of the device cpu. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # processor_temperature: Units: cC; The temperature in centi-degrees Centigrade (100 * degrees C).\n - def get_processor_temperature(self): - if self.request(definitions.PING1D_PROCESSOR_TEMPERATURE) is None: - return None - data = ({ - "processor_temperature": self._processor_temperature, # Units: cC; The temperature in centi-degrees Centigrade (100 * degrees C). - }) - return data - - ## - # @brief Get a profile message from the device\n - # Message description:\n - # A profile produced from a single acoustic measurement. The data returned is an array of response strength at even intervals across the scan region. The scan region is defined as the region between and millimeters away from the transducer. A distance measurement to the target is also provided. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # distance: Units: mm; The current return distance determined for the most recent acoustic measurement.\n - # confidence: Units: %; Confidence in the most recent range measurement.\n - # transmit_duration: Units: us; The acoustic pulse length during acoustic transmission/activation.\n - # ping_number: The pulse/measurement count since boot.\n - # scan_start: Units: mm; The beginning of the scan region in mm from the transducer.\n - # scan_length: Units: mm; The length of the scan region.\n - # gain_setting: The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144\n - # profile_data: An array of return strength measurements taken at regular intervals across the scan region.\n - def get_profile(self): - if self.request(definitions.PING1D_PROFILE) is None: - return None - data = ({ - "distance": self._distance, # Units: mm; The current return distance determined for the most recent acoustic measurement. - "confidence": self._confidence, # Units: %; Confidence in the most recent range measurement. - "transmit_duration": self._transmit_duration, # Units: us; The acoustic pulse length during acoustic transmission/activation. - "ping_number": self._ping_number, # The pulse/measurement count since boot. - "scan_start": self._scan_start, # Units: mm; The beginning of the scan region in mm from the transducer. - "scan_length": self._scan_length, # Units: mm; The length of the scan region. - "gain_setting": self._gain_setting, # The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144 - "profile_data": self._profile_data, # An array of return strength measurements taken at regular intervals across the scan region. - }) - return data - - ## - # @brief Get a protocol_version message from the device\n - # Message description:\n - # The protocol version - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # version_major: Protocol version major number.\n - # version_minor: Protocol version minor number.\n - # version_patch: Protocol version patch number.\n - # reserved: reserved\n - def get_protocol_version(self): - if self.request(definitions.COMMON_PROTOCOL_VERSION) is None: - return None - data = ({ - "version_major": self._version_major, # Protocol version major number. - "version_minor": self._version_minor, # Protocol version minor number. - "version_patch": self._version_patch, # Protocol version patch number. - "reserved": self._reserved, # reserved - }) - return data - - ## - # @brief Get a range message from the device\n - # Message description:\n - # The scan range for acoustic measurements. Measurements returned by the device will lie in the range (scan_start, scan_start + scan_length). - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # scan_start: Units: mm; The beginning of the scan range in mm from the transducer.\n - # scan_length: Units: mm; The length of the scan range.\n - def get_range(self): - if self.request(definitions.PING1D_RANGE) is None: - return None - data = ({ - "scan_start": self._scan_start, # Units: mm; The beginning of the scan range in mm from the transducer. - "scan_length": self._scan_length, # Units: mm; The length of the scan range. - }) - return data - - ## - # @brief Get a speed_of_sound message from the device\n - # Message description:\n - # The speed of sound used for distance calculations. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # speed_of_sound: Units: mm/s; The speed of sound in the measurement medium. ~1,500,000 mm/s for water.\n - def get_speed_of_sound(self): - if self.request(definitions.PING1D_SPEED_OF_SOUND) is None: - return None - data = ({ - "speed_of_sound": self._speed_of_sound, # Units: mm/s; The speed of sound in the measurement medium. ~1,500,000 mm/s for water. - }) - return data - - ## - # @brief Get a transmit_duration message from the device\n - # Message description:\n - # The duration of the acoustic activation/transmission. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # transmit_duration: Units: microseconds; Acoustic pulse duration.\n - def get_transmit_duration(self): - if self.request(definitions.PING1D_TRANSMIT_DURATION) is None: - return None - data = ({ - "transmit_duration": self._transmit_duration, # Units: microseconds; Acoustic pulse duration. - }) - return data - - ## - # @brief Get a voltage_5 message from the device\n - # Message description:\n - # The 5V rail voltage. - # - # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n - # voltage_5: Units: mV; The 5V rail voltage.\n - def get_voltage_5(self): - if self.request(definitions.PING1D_VOLTAGE_5) is None: - return None - data = ({ - "voltage_5": self._voltage_5, # Units: mV; The 5V rail voltage. - }) - return data - - ## - # @brief Send a set_device_id message to the device\n - # Message description:\n - # Set the device ID.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param device_id - Device ID (0-254). 255 is reserved for broadcast messages. - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_device_id(self, device_id, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_DEVICE_ID) - m.device_id = device_id - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_DEVICE_ID) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._device_id != device_id)): - return False - return True # success - - ## - # @brief Send a set_gain_setting message to the device\n - # Message description:\n - # Set the current gain setting.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param gain_setting - The current gain setting. 0: 0.6, 1: 1.8, 2: 5.5, 3: 12.9, 4: 30.2, 5: 66.1, 6: 144 - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_gain_setting(self, gain_setting, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_GAIN_SETTING) - m.gain_setting = gain_setting - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_GAIN_SETTING) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._gain_setting != gain_setting)): - return False - return True # success - - ## - # @brief Send a set_mode_auto message to the device\n - # Message description:\n - # Set automatic or manual mode. Manual mode allows for manual selection of the gain and scan range.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param mode_auto - 0: manual mode. 1: auto mode. - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_mode_auto(self, mode_auto, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_MODE_AUTO) - m.mode_auto = mode_auto - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_MODE_AUTO) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._mode_auto != mode_auto)): - return False - return True # success - - ## - # @brief Send a set_ping_enable message to the device\n - # Message description:\n - # Enable or disable acoustic measurements.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param ping_enabled - 0: Disable, 1: Enable. - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_ping_enable(self, ping_enabled, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_PING_ENABLE) - m.ping_enabled = ping_enabled - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_PING_ENABLE) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._ping_enabled != ping_enabled)): - return False - return True # success - - ## - # @brief Send a set_ping_interval message to the device\n - # Message description:\n - # The interval between acoustic measurements.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param ping_interval - Units: ms; The interval between acoustic measurements. - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_ping_interval(self, ping_interval, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_PING_INTERVAL) - m.ping_interval = ping_interval - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_PING_INTERVAL) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._ping_interval != ping_interval)): - return False - return True # success - - ## - # @brief Send a set_range message to the device\n - # Message description:\n - # Set the scan range for acoustic measurements.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param scan_start - Units: mm; - # @param scan_length - Units: mm; The length of the scan range. - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_range(self, scan_start, scan_length, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_RANGE) - m.scan_start = scan_start - m.scan_length = scan_length - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_RANGE) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._scan_start != scan_start or self._scan_length != scan_length)): - return False - return True # success - - ## - # @brief Send a set_speed_of_sound message to the device\n - # Message description:\n - # Set the speed of sound used for distance calculations.\n - # Send the message to write the device parameters, then read the values back from the device\n - # - # @param speed_of_sound - Units: mm/s; The speed of sound in the measurement medium. ~1,500,000 mm/s for water. - # - # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure) - def set_speed_of_sound(self, speed_of_sound, verify=True): - m = pingmessage.PingMessage(definitions.PING1D_SET_SPEED_OF_SOUND) - m.speed_of_sound = speed_of_sound - m.pack_msg_data() - self.write(m.msg_data) - if self.request(definitions.PING1D_SPEED_OF_SOUND) is None: - return False - # Read back the data and check that changes have been applied - if (verify - and (self._speed_of_sound != speed_of_sound)): - return False - return True # success - - -if __name__ == "__main__": - import argparse - - parser = argparse.ArgumentParser(description="Ping python library example.") - parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") - parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate.") - args = parser.parse_args() - - p = Ping1D(args.device, args.baudrate) - - print("Initialized: %s" % p.initialize()) - - print("\ntesting get_device_id") - result = p.get_device_id() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_device_information") - result = p.get_device_information() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_distance") - result = p.get_distance() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_distance_simple") - result = p.get_distance_simple() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_firmware_version") - result = p.get_firmware_version() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_gain_setting") - result = p.get_gain_setting() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_general_info") - result = p.get_general_info() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_mode_auto") - result = p.get_mode_auto() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_pcb_temperature") - result = p.get_pcb_temperature() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_ping_enable") - result = p.get_ping_enable() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_ping_interval") - result = p.get_ping_interval() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_processor_temperature") - result = p.get_processor_temperature() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_profile") - result = p.get_profile() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_protocol_version") - result = p.get_protocol_version() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_range") - result = p.get_range() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_speed_of_sound") - result = p.get_speed_of_sound() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_transmit_duration") - result = p.get_transmit_duration() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting get_voltage_5") - result = p.get_voltage_5() - print(" " + str(result)) - print(" > > pass: %s < <" % (result is not None)) - - print("\ntesting set_device_id") - print(" > > pass: %s < <" % p.set_device_id(43)) - print("\ntesting set_mode_auto") - print(" > > pass: %s < <" % p.set_mode_auto(False)) - print("\ntesting set_range") - print(" > > pass: %s < <" % p.set_range(1000, 2000)) - print("\ntesting set_speed_of_sound") - print(" > > pass: %s < <" % p.set_speed_of_sound(1444000)) - print("\ntesting set_ping_interval") - print(" > > pass: %s < <" % p.set_ping_interval(36)) - print("\ntesting set_gain_setting") - print(" > > pass: %s < <" % p.set_gain_setting(3)) - print("\ntesting set_ping_enable") - print(" > > pass: %s < <" % p.set_ping_enable(False)) - - print(p) \ No newline at end of file From a6e5b35f6bba759e487172d59474720e0c9a3a1a Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 28 Aug 2019 14:08:03 -0400 Subject: [PATCH 08/12] brping: update __init__.py to export device classes --- brping/__init__.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/brping/__init__.py b/brping/__init__.py index f33fe95..50559a5 100644 --- a/brping/__init__.py +++ b/brping/__init__.py @@ -1,4 +1,6 @@ #'Ping python package' from brping.definitions import * from brping.pingmessage import * +from brping.device import PingDevice from brping.ping1d import Ping1D +from brping.ping360 import Ping360 From dfd29fbcaeed1fad0379334a05fc8e3535fecc87 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 28 Aug 2019 14:08:15 -0400 Subject: [PATCH 09/12] ci: deploy device classes --- ci/deploy-whitelist | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ci/deploy-whitelist b/ci/deploy-whitelist index 3d5497d..ee658b1 100644 --- a/ci/deploy-whitelist +++ b/ci/deploy-whitelist @@ -1,6 +1,8 @@ brping/__init__.py brping/definitions.py +brping/device.py brping/ping1d.py +brping/ping360.py brping/pingmessage.py examples tools From 28e225c66110d0256db5bce99c0ce0106dfa9939 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 28 Aug 2019 14:19:15 -0400 Subject: [PATCH 10/12] device: add time.sleep to wait message to prevent spinlock --- generate/templates/device.py.in | 1 + 1 file changed, 1 insertion(+) diff --git a/generate/templates/device.py.in b/generate/templates/device.py.in index e8b6482..4de66eb 100644 --- a/generate/templates/device.py.in +++ b/generate/templates/device.py.in @@ -109,6 +109,7 @@ class PingDevice(object): if msg is not None: if msg.message_id in message_ids: return msg + time.sleep(0.005) return None ## From 51b8411bbe7aba280b3f73009c78bf95f96637df Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 28 Aug 2019 14:42:15 -0400 Subject: [PATCH 11/12] device: use deque buffer to optimize, thanks willian --- generate/templates/device.py.in | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/generate/templates/device.py.in b/generate/templates/device.py.in index 4de66eb..d9c456b 100644 --- a/generate/templates/device.py.in +++ b/generate/templates/device.py.in @@ -10,6 +10,7 @@ from brping import definitions from brping import pingmessage +from collections import deque import serial import time @@ -18,6 +19,7 @@ class PingDevice(object): _{{field}} = None {% endfor%} + _input_buffer = deque() def __init__(self, device_name, baudrate=115200): if device_name is None: print("Device name is required") @@ -48,10 +50,13 @@ class PingDevice(object): # @return A new PingMessage: as soon as a message is parsed (there may be data remaining in the buffer to be parsed, thus requiring subsequent calls to read()) # @return None: if the buffer is empty and no message has been parsed def read(self): - while self.iodev.in_waiting: - b = self.iodev.read() + bytes = self.iodev.read(self.iodev.in_waiting) + self._input_buffer.extendleft(bytes) - if self.parser.parse_byte(ord(b)) == pingmessage.PingParser.NEW_MESSAGE: + while len(self._input_buffer): + b = self._input_buffer.pop() + + if self.parser.parse_byte(b) == pingmessage.PingParser.NEW_MESSAGE: self.handle_message(self.parser.rx_msg) return self.parser.rx_msg return None From 4cf58f484611dccef5f541eb0e45557d16a1a53b Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 29 Aug 2019 12:54:48 -0400 Subject: [PATCH 12/12] device: extract source and destination device ids from message headers --- generate/templates/device.py.in | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/generate/templates/device.py.in b/generate/templates/device.py.in index d9c456b..ae93a72 100644 --- a/generate/templates/device.py.in +++ b/generate/templates/device.py.in @@ -123,6 +123,10 @@ class PingDevice(object): # # @param msg: the PingMessage to handle. def handle_message(self, msg): + # TODO is this message for us? + setattr(self, "_src_device_id", msg.src_device_id) + setattr(self, "_dst_device_id", msg.dst_device_id) + if msg.message_id in pingmessage.payload_dict: for attr in pingmessage.payload_dict[msg.message_id]["field_names"]: setattr(self, "_" + attr, getattr(msg, attr))