Add Omniscan3D support and fix message definition lookup

brping/__init__.py

@@ -6,4 +6,5 @@
 from brping.ping360 import Ping360
 from brping.surveyor240 import Surveyor240
 from brping.s500 import S500
-from brping.omniscan450 import Omniscan450
+from brping.omniscan450 import Omniscan450
+from brping.omniscan3d import Omniscan3D

brping/pingmessage.py

@@ -14,7 +14,9 @@
     definitions.SURVEYOR240_ATOF_POINT_DATA,
     definitions.SURVEYOR240_YZ_POINT_DATA,
     definitions.S500_PROFILE6_T,
-    definitions.OMNISCAN450_OS_MONO_PROFILE
+    definitions.OMNISCAN450_OS_MONO_PROFILE,
+    definitions.OMNISCAN3D_JSON_WRAPPER,
+    definitions.OMNISCAN3D_OS3D_POINT_SET
 ]
 
 
@@ -75,7 +77,12 @@ class PingMessage(object):
     #     start_mm = m.start_mm
     #     length_mm = m.length_mm
     # @endcode
-    def __init__(self, msg_id=0, msg_data=None):
+    def __init__(self, msg_id=0, msg_data=None, payload_dict=None):
+        if payload_dict is None:
+            payload_dict = definitions.payload_dict_all
+
+        self.payload_dict = payload_dict
+
         ## The message id
         self.message_id = msg_id
 
@@ -106,10 +113,10 @@ def __init__(self, msg_id=0, msg_data=None):
 
             try:
                 ## The name of this message
-                self.name = payload_dict[self.message_id]["name"]
+                self.name = self.payload_dict[self.message_id]["name"]
 
                 ## The field names of this message
-                self.payload_field_names = payload_dict[self.message_id]["field_names"]
+                self.payload_field_names = self.payload_dict[self.message_id]["field_names"]
 
                 # initialize payload field members
                 for attr in self.payload_field_names:
@@ -141,7 +148,7 @@ def pack_msg_data(self):
         msg_format = PingMessage.endianess + PingMessage.header_format + self.get_payload_format()
 
         # Prepare complete list of field names (header + payload)
-        attrs = PingMessage.header_field_names + payload_dict[self.message_id]["field_names"]
+        attrs = PingMessage.header_field_names + self.payload_dict[self.message_id]["field_names"]
 
         # Prepare iterable ordered list of values to pack
         values = []
@@ -173,13 +180,13 @@ def unpack_msg_data(self, msg_data):
 
         ## The name of this message
         try:
-            self.name = payload_dict[self.message_id]["name"]
+            self.name = self.payload_dict[self.message_id]["name"]
         except KeyError:
             print("Unknown message: ", self.message_id)
             return False
 
         ## The field names of this message
-        self.payload_field_names = payload_dict[self.message_id]["field_names"]
+        self.payload_field_names = self.payload_dict[self.message_id]["field_names"]
 
         if self.payload_length > 0:
             ## The struct formatting string for the message payload
@@ -225,22 +232,22 @@ def verify_checksum(self):
     def update_payload_length(self):
         if self.message_id in variable_msgs or self.message_id in asciiMsgs:
             # The last field self.payload_field_names[-1] is always the single dynamic-length field
-            self.payload_length = payload_dict[self.message_id]["payload_length"] + len(getattr(self, self.payload_field_names[-1]))
+            self.payload_length = self.payload_dict[self.message_id]["payload_length"] + len(getattr(self, self.payload_field_names[-1]))
         else:
-            self.payload_length = payload_dict[self.message_id]["payload_length"]
+            self.payload_length = self.payload_dict[self.message_id]["payload_length"]
 
     ## Get the python struct formatting string for the message payload
     # @return the payload struct format string
     def get_payload_format(self):
         # messages with variable length fields
         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
+            var_length = self.payload_length - self.payload_dict[self.message_id]["payload_length"]  # Subtract static length portion from payload length
             if var_length <= 0:
-                return payload_dict[self.message_id]["format"]  # variable data portion is empty
+                return self.payload_dict[self.message_id]["format"]  # variable data portion is empty
 
-            return payload_dict[self.message_id]["format"] + str(var_length) + "s"
+            return self.payload_dict[self.message_id]["format"] + str(var_length) + "s"
         else: # messages with a static (constant) length
-            return payload_dict[self.message_id]["format"]
+            return self.payload_dict[self.message_id]["format"]
 
     ## Dump object into string representation
     # @return string representation of the object
@@ -258,17 +265,17 @@ def __repr__(self):
             if self.message_id in variable_msgs:
 
                 # static fields are handled as usual
-                for attr in payload_dict[self.message_id]["field_names"][:-1]:
+                for attr in self.payload_dict[self.message_id]["field_names"][:-1]:
                     payload_string += "\n  - " + attr + ": " + str(getattr(self, attr))
 
                 # the variable length field is always the last field
-                attr = payload_dict[self.message_id]["field_names"][-1:][0]
+                attr = self.payload_dict[self.message_id]["field_names"][-1:][0]
 
                 # format this field as a list of hex values (rather than a string if we did not perform this handling)
                 payload_string += "\n  - " + attr + ": " + str([hex(item) for item in getattr(self, attr)])
 
             else:  # handling of static length messages and text messages
-                for attr in payload_dict[self.message_id]["field_names"]:
+                for attr in self.payload_dict[self.message_id]["field_names"]:
                     payload_string += "\n  - " + attr + ": " + str(getattr(self, attr))
 
         representation = (
@@ -293,6 +300,7 @@ class PingParser(object):
         "errors",
         "parsed",
         "rx_msg",
+        "payload_dict",
     )
 
     NEW_MESSAGE       = 0    # Just got a complete checksum-verified message
@@ -309,7 +317,8 @@ class PingParser(object):
     WAIT_CHECKSUM_H   = 11   # Waiting for the checksum high byte
     ERROR             = 12   # Checksum didn't check out
 
-    def __init__(self):
+    def __init__(self, payload_dict=None):
+        self.payload_dict = payload_dict or definitions.payload_dict_all
         self.buf = bytearray()
         self.state = self.WAIT_START
         self.payload_length = 0 # remaining for the message currently being parsed
@@ -377,7 +386,10 @@ def wait_checksum_h(self, msg_byte):
         self.message_id = 0
 
         self.buf.append(msg_byte)
-        self.rx_msg = PingMessage(msg_data=self.buf)
+        self.rx_msg = PingMessage(
+            msg_data=self.buf,
+            payload_dict=self.payload_dict
+        )
 
         if self.rx_msg.verify_checksum():
             self.parsed += 1

ci/deploy-whitelist

@@ -6,6 +6,7 @@ brping/ping360.py
 brping/surveyor240.py
 brping/s500.py
 brping/omniscan450.py
+brping/omniscan3d.py
 brping/pingmessage.py
 examples
 tools

examples/omniscan3dExample.py

@@ -0,0 +1,224 @@
+#!/usr/bin/env python
+
+#omniscan3dExample.py
+from brping import definitions
+from brping import Omniscan3D
+from brping import PingMessage
+import time
+import argparse
+
+from builtins import input
+
+import signal
+import sys
+import math
+from datetime import datetime
+from pathlib import Path
+
+##Parse Command line options
+############################
+
+parser = argparse.ArgumentParser(description="Ping python library example.")
+parser.add_argument('--device', action="store", required=False, type=str, help="Ping device port. E.g: /dev/ttyUSB0")
+parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate. E.g: 115200")
+parser.add_argument('--tcp', action="store", required=False, type=str, help="Omniscan3D IP:Port. E.g: 192.168.2.86:62312")
+parser.add_argument('--range', action="store", required=False, type=str, help="Set range. E.g: 5000 or 0:5000")
+parser.add_argument('--log', action="store", nargs='?', const=True, type=str, help="Log filename and/or directory path. Will create new log if blank or directory is specified. Will replay if file is specified and exists.")
+args = parser.parse_args()
+if args.device is None and args.tcp is None and args.log is None:
+    parser.print_help()
+    exit(1)
+
+# Signal handler to stop pinging on the Omniscan3D
+def signal_handler(sig, frame):
+    print("Stopping pinging on Omniscan3D...")
+    myOmniscan3D.control_os3d_set_ping_params(ping_enable = False)
+    # Close socket if open
+    if myOmniscan3D.iodev:
+        try:
+            myOmniscan3D.iodev.close()
+        except Exception as e:
+            print(f"Failed to close socket: {e}")
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, signal_handler)
+
+# Check for log argument and make new Omniscan3D
+# If no .svlog is specified, create one using default directory
+# If directory specified, .svlog be created in specified directory
+# If a .svlog is specified, existing log will be opened
+new_log = False
+log_path = ""
+replay_path = None
+default_dir = Path("logs/omniscan3d").resolve()
+if args.log is not None:
+    if args.log is True:
+        # Logging to default directory
+        default_dir.mkdir(parents=True, exist_ok=True)
+        myOmniscan3D = Omniscan3D(logging=True, log_directory=default_dir)
+        new_log = True
+    elif isinstance(args.log, str):
+        log_path = Path(args.log).expanduser()
+
+        if log_path.suffix == ".svlog" and log_path.parent == Path("."):
+            log_path = default_dir / log_path.name
+
+        log_path = log_path.resolve()
+
+        if log_path.suffix == ".svlog":
+            if log_path.exists() and log_path.is_file():
+                # File exists, replaying
+                new_log = False
+                myOmniscan3D = Omniscan3D(logging=False)
+                replay_path = log_path
+                print(f"Replaying from: {replay_path}")
+            else:
+                raise FileNotFoundError(f"Log file not found: {log_path}")
+
+        elif log_path.is_dir() or log_path.suffix == "":
+            # Path is directory, logging to that directory
+            myOmniscan3D = Omniscan3D(logging=True, log_directory=log_path)
+            new_log = True
+
+        else:
+            raise ValueError(f"Invalid log argument: {args.log}")
+else:
+    myOmniscan3D = Omniscan3D()
+
+if args.log is None or new_log:
+    if args.device is not None:
+        myOmniscan3D.connect_serial(args.device, args.baudrate)
+    elif args.tcp is not None:
+        (host, port) = args.tcp.split(':')
+        try:
+            myOmniscan3D.connect_tcp(host, int(port))
+        except Exception as e:
+            print(f"Could not connect to Omniscan3D at {host}:{port}")
+            print(f"Reason: {e}")
+            sys.exit(1)
+
+    if myOmniscan3D.initialize() is False:
+        print("Failed to initialize Omniscan3D!")
+        exit(1)
+
+print("------------------------------------")
+print("Starting Omniscan3D..")
+print("Press CTRL+C to exit")
+print("------------------------------------")
+
+input("Press Enter to continue...")
+
+# Running omniscan3d.py from existing log file
+if args.log is not None and not new_log:
+    with open(log_path, 'rb') as f:
+        while True:
+            data = Omniscan3D.read_packet(f)
+
+            if data is None:
+                break   # EOF or bad packet
+
+            # print(f"ID: {data.message_id}\tName: {data.name}")
+
+            if data.message_id == definitions.OMNISCAN3D_ATTITUDE_REPORT:
+                # Print pitch and roll data
+                vector = (data.up_vec_x, data.up_vec_y, data.up_vec_z)
+                pitch = math.asin(vector[0])
+                roll = math.atan2(vector[1], vector[2])
+                print(f"Pitch: {pitch}\tRoll: {roll}")
+            elif data.message_id == definitions.OMNISCAN3D_OS3D_POINT_SET:
+                # print out data
+                print(f"ping_number        : {data.ping_number}")
+                print(f"sos_mps            : {data.sos_mps}")
+                print(f"num_points         : {data.num_points}")
+                print(f"utc_msec           : {data.utc_msec}")
+                print(f"pwr_up_msec        : {data.pwr_up_msec}")
+                print(f"version            : {data.version}")
+                print(f"device_number      : {data.device_number}")
+                print(f"pwr_threshold_high : {data.pwr_threshold_high}")
+                print(f"pwr_threshold_med  : {data.pwr_threshold_med}")
+                print(f"pwr_threshold_low  : {data.pwr_threshold_low}")
+            elif data.message_id == definitions.OMNISCAN3D_END_PING_INFO:
+                # print out data
+                print("got OS3D_END_PING_INFO packet")
+
+# Connected to physical Omniscan3D
+else:
+    if args.range is not None:
+        parts = args.range.split(':')
+
+        if len(parts) == 2:
+            myOmniscan3D.control_os3d_set_ping_params(
+                start_m=int(parts[0]),
+                end_m=int(parts[1]),
+                ping_enable=True,
+                enable_atof_data=True
+            )
+        elif len(parts) == 1:
+            myOmniscan3D.control_os3d_set_ping_params(
+                start_m=0,
+                end_m=int(parts[0]),
+                ping_enable=True,
+                enable_atof_data=True
+            )
+        else:
+            print("Invalid range input, using default range")
+            myOmniscan3D.control_os3d_set_ping_params(
+                ping_enable=True,
+                enable_atof_data=True
+            )
+    else:
+        print("start_m=0, end_m-=7, msec_per_ping=100, sos_mps=1500, ping_enable = True, enable_atof_data = True")
+        myOmniscan3D.control_os3d_set_ping_params(
+            start_m=0,
+            end_m=7,
+            msec_per_ping=200,
+            diagnostic_injected_signal = 1,
+            sos_mps=1500,
+            ping_enable = True,
+            enable_atof_data = True
+        )
+
+    if new_log:
+        print("Logging...\nCTRL+C to stop logging")
+    else:
+        print("CTRL-C to end program...")
+    try:
+        while True:
+            # Set multiple packets to listen for
+            data = myOmniscan3D.wait_message([definitions.OMNISCAN3D_ATTITUDE_REPORT,
+                                               definitions.OMNISCAN3D_OS3D_POINT_SET,
+                                               definitions.OMNISCAN3D_END_PING_INFO])
+
+            if data:
+                ## To watch pitch and roll data in real time while recording, uncomment this block
+                if data.message_id == definitions.OMNISCAN3D_ATTITUDE_REPORT:
+                    # Print pitch and roll data
+                    vector = (data.up_vec_x, data.up_vec_y, data.up_vec_z)
+                    pitch = math.asin(vector[0])
+                    roll = math.atan2(vector[1], vector[2])
+                    print(f"Pitch: {pitch}\tRoll: {roll}")
+                elif data.message_id == definitions.OMNISCAN3D_OS3D_POINT_SET:
+                    # print out data
+                    print(f"ping_number        : {data.ping_number}")
+                    print(f"sos_mps            : {data.sos_mps}")
+                    print(f"num_points         : {data.num_points}")
+                    print(f"utc_msec           : {data.utc_msec}")
+                    print(f"pwr_up_msec        : {data.pwr_up_msec}")
+                    print(f"version            : {data.version}")
+                    print(f"device_number      : {data.device_number}")
+                    print(f"pwr_threshold_high : {data.pwr_threshold_high}")
+                    print(f"pwr_threshold_med  : {data.pwr_threshold_med}")
+                    print(f"pwr_threshold_low  : {data.pwr_threshold_low}")
+
+    except KeyboardInterrupt:
+        if new_log:
+            print("Stopping logging...")
+
+
+    # Stop pinging from Omniscan3D
+    myOmniscan3D.control_os3d_set_ping_params(ping_enable = False)
+    if myOmniscan3D.iodev:
+        try:
+            myOmniscan3D.iodev.close()
+        except Exception as e:
+            print(f"Failed to close socket: {e}")