def __init__(self): self.client = None # (ip address, port) of connected client (if any) self.parser = PingParser() # used to parse incoming client comunications # Socket to serve on self.sockit = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.sockit.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.sockit.setblocking(False) self.sockit.bind(('0.0.0.0', 6676)) self._profile_data_length = 200 # number of data points in profile messages (constant for now) self._pulse_duration = 100 # length of acoustic pulse (constant for now) 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
def __init__(self, port='COM1', baudrate=115200, timeout=1, bytesize=8, parity='N', stopbits=1, xonxoff=0, rtscts=0): self.name = port self.port = port self.timeout = timeout self.parity = parity self.baudrate = baudrate self.bytesize = bytesize self.stopbits = stopbits self.xonxoff = xonxoff self.rtscts = rtscts self._isOpen = True self._receivedData = "" self.in_waiting = 1 self.parser = PingParser( ) # used to parse incoming client comunications self._gain_setting = 0 self._mode = 0 self._angle = 0 self._transmit_duration = 0 self._sample_period = 0 self._transmit_frequency = 100 self._number_of_samples = 10 self._data = "".join([chr(0) for _ in xrange(self._number_of_samples)]) self._data_length = 10 self._noise = perlin.noise(400, 50, 50)
class PingClient(object): def __init__(self): ## Queued messages received from client self.rx_msgs = deque([]) ## Parser to verify client comms self.parser = PingParser() ## Digest incoming client data # @return None def parse(self, data): for b in bytearray(data): if self.parser.parse_byte(b) == PingParser.NEW_MESSAGE: self.rx_msgs.append(self.parser.rx_msg) ## Dequeue a message received from client # @return None: if there are no comms in the queue # @return PingMessage: the next ping message in the queue def dequeue(self): if len(self.rx_msgs) == 0: return None return self.rx_msgs.popleft()
class Ping1DSimulation(object): def __init__(self): self.client = None # (ip address, port) of connected client (if any) self.parser = PingParser() # used to parse incoming client comunications # Socket to serve on self.sockit = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.sockit.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.sockit.setblocking(False) self.sockit.bind(('0.0.0.0', 6676)) self._profile_data_length = 200 # number of data points in profile messages (constant for now) self._pulse_duration = 100 # length of acoustic pulse (constant for now) 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 # read incoming client data def read(self): try: data, self.client = self.sockit.recvfrom(4096) # digest data coming in from client for byte in data: if self.parser.parse_byte(byte) == PingParser.NEW_MESSAGE: # we decoded a message from the client self.handleMessage(self.parser.rx_msg) except Exception as e: if e.errno == errno.EAGAIN: pass # waiting for data else: print("Error reading data", e) # write data to client def write(self, data): if self.client is not None: print("sending data to client") self.sockit.sendto(data, self.client) # Send a message to the client, the message fields are populated by the # attributes of this object (either variable or method) with names matching # the message field names def sendMessage(self, message_id): msg = PingMessage(message_id) # pull attributes of this class into the message fields (they are named the same) for attr in payload_dict[message_id]["field_names"]: try: # see if we have a function for this attribute (dynamic data) # if so, call it and put the result in the message field setattr(msg, attr, getattr(self, attr)()) except AttributeError as e: try: # if we don't have a function for this attribute, check for a _<field_name> member # these are static values (or no function implemented yet) setattr(msg, attr, getattr(self, "_" + attr)) except AttributeError as e: # anything else we haven't implemented yet, just send a sine wave setattr(msg, attr, self.periodicFnInt(20, 120)) # send the message to the client msg.pack_msg_data() self.write(msg.msg_data) # handle an incoming client message def handleMessage(self, message): if message.payload_length == 0: # the client is requesting a message from us self.sendMessage(message.message_id) else: # the client is controlling some parameter of the device self.setParameters(message) # Extract message fields into attribute values # This should only be performed with the 'set' category of messages # TODO: mechanism to filter by "set" def setParameters(self, message): for attr in payload_dict[message.message_id]["field_names"]: setattr(self, "_" + attr, getattr(message, attr)) ########### # Helpers for generating periodic data ########### def periodicFn(self, amplitude = 0, offset = 0, frequency = 1.0, shift = 0): return amplitude * math.sin(frequency * time.time() + shift) + offset def periodicFnInt(self, amplitude = 0, offset = 0, frequency = 1.0, shift = 0): return int(self.periodicFn(amplitude, offset, frequency, shift)) ########### # Device properties/state ########### def distance(self): return self.periodicFnInt(self.scan_length() / 2, self.scan_start() + self.scan_length() / 2, 5) def confidence(self): return self.periodicFnInt(40, 50) def scan_start(self): if self._mode_auto: return 0 return self._scan_start def scan_length(self): if self._mode_auto: self._scan_length = self.periodicFnInt(2000, 3000, 0.2) return self._scan_length def profile_data(self): stops = 20 len = int(200/stops) data = [] for x in range(stops): data = data + [int(x*255/stops)]*len return bytearray(data) # stepwise change in signal strength #return bytearray(range(0,200)) # linear change in signal strength def pcb_temperature(self): return self.periodicFnInt(250, 3870, 3) def processor_temperature(self): return self.periodicFnInt(340, 3400) def voltage_5(self): return self.periodicFnInt(100, 3500) def profile_data_length(self): return self._profile_data_length def gain_index(self): return 2
def main(): """ Main function """ ## The time that this script was started tboot = time.time() ## Parser to decode incoming PingMessage ping_parser = PingParser() ## Messages that have the current distance measurement in the payload distance_messages = [ PING1D_DISTANCE, PING1D_DISTANCE_SIMPLE, PING1D_PROFILE ] ## The minimum interval time for distance updates to the autopilot ping_interval_ms = 0.075 ## The last time a distance measurement was received last_distance_measurement_time = 0 ## The last time a distance measurement was requested last_ping_request_time = 0 pingargs = ARGS.ping.split(':') pingserver = (pingargs[0], int(pingargs[1])) autopilot_io = mavutil.mavlink_connection("udpout:" + ARGS.mavlink, source_system=1, source_component=192) ping1d_io = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) ping1d_io.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) ping1d_io.setblocking(False) ## Send a request for distance_simple message to ping device def send_ping1d_request(): data = PingMessage() data.request_id = PING1D_DISTANCE_SIMPLE data.src_device_id = 0 data.pack_msg_data() ping1d_io.sendto(data.msg_data, pingserver) # some extra information for the DISTANCE_SENSOR mavlink message fields min_distance = 20 max_distance = 5000 sensor_type = 2 orientation = 25 covarience = 0 ## Send distance_sensor message to autopilot def send_distance_data(distance, deviceid, confidence): print("sending distance %d confidence %d" % (distance, confidence)) if confidence < ARGS.min_confidence: distance = 0 autopilot_io.mav.distance_sensor_send( int((time.time() - tboot) * 1000), # time_boot_ms min_distance, # min_distance max_distance, # max_distance int(distance / 10), # distance sensor_type, # type deviceid, # device id orientation, covarience) while True: time.sleep(0.001) tnow = time.time() # request data from ping device if tnow > last_distance_measurement_time + ping_interval_ms: if tnow > last_ping_request_time + ping_interval_ms: last_ping_request_time = time.time() send_ping1d_request() # read data in from ping device try: data, _ = ping1d_io.recvfrom(4096) except socket.error as exception: # check if it's waiting for data if exception.errno != errno.EAGAIN: raise exception else: continue # decode data from ping device, forward to autopilot for byte in data: if ping_parser.parse_byte(byte) == PingParser.NEW_MESSAGE: if ping_parser.rx_msg.message_id in distance_messages: last_distance_measurement_time = time.time() distance = ping_parser.rx_msg.distance deviceid = ping_parser.rx_msg.src_device_id confidence = ping_parser.rx_msg.confidence send_distance_data(distance, deviceid, confidence)
parser = ArgumentParser(description=__doc__) parser.add_argument("-f", dest="file", required=True, help="File that contains PingViewer sensor log file.") args = parser.parse_args() # Open log and process it log = Log(args.file) log.process() # Get information from log print(log.header) input("Press Enter to continue and decode received messages...") ping_parser = PingParser() for (timestamp, message) in log.messages: print("timestamp: %s" % timestamp) # Parse each byte of the message for byte in message: # Check if the parser has a new message if ping_parser.parse_byte(byte) is PingParser.NEW_MESSAGE: # Get decoded message decoded_message = ping_parser.rx_msg # Filter for the desired ID # 1300 for Ping1D profile message and 2300 for Ping360 if decoded_message.message_id in [1300, 2300]: # Print message print(decoded_message)
def __init__(self): ## Queued messages received from client self.rx_msgs = deque([]) ## Parser to verify client comms self.parser = PingParser()
def main(): """ Main function """ ## The time that this script was started tboot = time.time() ## Parser to decode incoming PingMessage ping_parser = PingParser() ## Messages that have the current distance measurement in the payload distance_messages = [ PING1D_DISTANCE, PING1D_DISTANCE_SIMPLE, PING1D_PROFILE ] ## The minimum interval time for distance updates to the autopilot ping_interval_ms = 0.075 ## The last time a distance measurement was received last_distance_measurement_time = 0 ## The last time a distance measurement was requested last_ping_request_time = 0 pingargs = ARGS.ping.split(':') pingserver = (pingargs[0], int(pingargs[1])) autopilot_io = mavutil.mavlink_connection("udpout:" + ARGS.mavlink, source_system=1, source_component=192 ) ping1d_io = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) ping1d_io.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) ping1d_io.setblocking(False) ## Send a request for distance_simple message to ping device def send_ping1d_request(): data = PingMessage() data.request_id = PING1D_DISTANCE_SIMPLE data.src_device_id = 0 data.pack_msg_data() ping1d_io.sendto(data.msg_data, pingserver) # some extra information for the DISTANCE_SENSOR mavlink message fields min_distance = 20 max_distance = 5000 sensor_type = 2 orientation = 25 covarience = 0 ## Send distance_sensor message to autopilot def send_distance_data(distance, deviceid, confidence): print("sending distance %d confidence %d" % (distance, confidence)) if confidence < ARGS.min_confidence: distance = 0 autopilot_io.mav.distance_sensor_send( int((time.time() - tboot) * 1000), # time_boot_ms min_distance, # min_distance max_distance, # max_distance int(distance/10), # distance sensor_type, # type deviceid, # device id orientation, covarience) while True: time.sleep(0.001) tnow = time.time() # request data from ping device if tnow > last_distance_measurement_time + ping_interval_ms: if tnow > last_ping_request_time + ping_interval_ms: last_ping_request_time = time.time() send_ping1d_request() # read data in from ping device try: data, _ = ping1d_io.recvfrom(4096) except socket.error as exception: # check if it's waiting for data if exception.errno != errno.EAGAIN: raise exception else: continue # decode data from ping device, forward to autopilot for byte in data: if ping_parser.parse_byte(byte) == PingParser.NEW_MESSAGE: if ping_parser.rx_msg.message_id in distance_messages: last_distance_measurement_time = time.time() distance = ping_parser.rx_msg.distance deviceid = ping_parser.rx_msg.src_device_id confidence = ping_parser.rx_msg.confidence send_distance_data(distance, deviceid, confidence)
class Serial: # init(): the constructor. Many of the arguments have default values # and can be skipped when calling the constructor. def __init__(self, port='COM1', baudrate=115200, timeout=1, bytesize=8, parity='N', stopbits=1, xonxoff=0, rtscts=0): self.name = port self.port = port self.timeout = timeout self.parity = parity self.baudrate = baudrate self.bytesize = bytesize self.stopbits = stopbits self.xonxoff = xonxoff self.rtscts = rtscts self._isOpen = True self._receivedData = "" self.in_waiting = 1 self.parser = PingParser( ) # used to parse incoming client comunications self._gain_setting = 0 self._mode = 0 self._angle = 0 self._transmit_duration = 0 self._sample_period = 0 self._transmit_frequency = 100 self._number_of_samples = 10 self._data = "".join([chr(0) for _ in xrange(self._number_of_samples)]) self._data_length = 10 self._noise = perlin.noise(400, 50, 50) # isOpen() # returns True if the port to the Arduino is open. False otherwise def isOpen(self): return self._isOpen def send_break(self): pass # open() # opens the port def open(self): self._isOpen = True # close() # closes the port def close(self): self._isOpen = False # write() # writes a string of characters to the Arduino def write(self, data): try: # digest data coming in from client for byte in data: if self.parser.parse_byte(byte) == PingParser.NEW_MESSAGE: # we decoded a message from the client self.handleMessage(self.parser.rx_msg) except EnvironmentError as e: if e.errno == errno.EAGAIN: pass # waiting for data else: if verbose: print("Error reading data", e) except KeyError as e: if verbose: print("skipping unrecognized message id: %d" % self.parser.rx_msg.message_id) print("contents: %s" % self.parser.rx_msg.msg_data) pass # read() # reads n characters from the fake Arduino. Actually n characters # are read from the string _data and returned to the caller. def read(self, n=1): s = self._read_data[0:n] self._read_data = self._read_data[n:] return s # readline() # reads characters from the fake Arduino until a \n is found. def readline(self): returnIndex = self._read_data.index("\n") if returnIndex != -1: s = self._read_data[0:returnIndex + 1] self._read_data = self._read_data[returnIndex + 1:] return s else: return "" # __str__() # returns a string representation of the serial class def __str__(self): return "Serial<id=0xa81c10, open=%s>( port='%s', baudrate=%d," \ % (str(self.isOpen), self.port, self.baudrate) \ + " bytesize=%d, parity='%s', stopbits=%d, xonxoff=%d, rtscts=%d)" \ % (self.bytesize, self.parity, self.stopbits, self.xonxoff, self.rtscts) # Send a message to the client, the message fields are populated by the # attributes of this object (either variable or method) with names matching # the message field names def sendMessage(self, message_id): msg = PingMessage(message_id) if verbose: print("sending message %d\t(%s)" % (msg.message_id, msg.name)) # pull attributes of this class into the message fields (they are named the same) for attr in payload_dict[message_id]["field_names"]: try: # see if we have a function for this attribute (dynamic data) # if so, call it and put the result in the message field setattr(msg, attr, getattr(self, attr)()) except AttributeError as e: try: # if we don't have a function for this attribute, check for a _<field_name> member # these are static values (or no function implemented yet) setattr(msg, attr, getattr(self, "_" + attr)) except AttributeError as e: # anything else we haven't implemented yet, just send a sine wave setattr(msg, attr, self.periodicFnInt(20, 120)) # send the message to the client msg.pack_msg_data() self._read_data = msg.msg_data # handle an incoming client message def handleMessage(self, message): if verbose: print("receive message %d\t(%s)" % (message.message_id, message.name)) if message.message_id == definitions.COMMON_GENERAL_REQUEST: # the client is requesting a message from us self.sendMessage(message.requested_id) # hack for legacy requests elif message.payload_length == 0: self.sendMessage(message.message_id) elif message.message_id == definitions.PING360_TRANSDUCER: # the client is controlling some parameter of the device self.setParameters(message) else: if verbose: print("Unknown msg type") # Extract message fields into attribute values # This should only be performed with the 'set' category of messages # TODO: mechanism to filter by "set" def setParameters(self, message): for attr in payload_dict[message.message_id]["field_names"]: setattr(self, "_" + attr, getattr(message, attr)) self.sendDataResponse(message) def sendDataResponse(self, message): # Send a response self.generateRandomData() msg = PingMessage(definitions.PING360_DEVICE_DATA) if verbose: print("sending a reply %d\t(%s)" % (msg.message_id, msg.name)) # pull attributes of this class into the message fields (they are named the same) for attr in payload_dict[ definitions.PING360_DEVICE_DATA]["field_names"]: setattr(msg, attr, getattr(self, "_" + attr)) # send the message to the client msg.pack_msg_data() self._read_data = msg.msg_data def generateRandomData(self): sigma = 10 if (self._angle == 0): self._noise = perlin.noise(400, 50, 50) mu = 100 + int(self._noise[self._angle]) + random.randint(-1, 1) self._data = "".join([ chr( int(255 * np.exp(-np.power(x - mu, 2.) / (2 * np.power(sigma, 2.))))) for x in range(self._number_of_samples) ]) # # Helpers for generating periodic data # def periodicFn(self, amplitude=0, offset=0, frequency=1.0, shift=0): return amplitude * math.sin(frequency * time.time() + shift) + offset def periodicFnInt(self, amplitude=0, offset=0, frequency=1.0, shift=0): return int(self.periodicFn(amplitude, offset, frequency, shift))
def main(): """ Main function """ autopilot_io = mavutil.mavlink_connection("udpout:" + ARGS.mavlink, source_system=1, source_component=192) if not is_compatible_ardusub_version(autopilot_io): print("Uncompatible ardusub version, aborting...") exit(-1) ## The time that this script was started tboot = time.time() ## Parser to decode incoming PingMessage ping_parser = PingParser() ## Messages that have the current distance measurement in the payload distance_messages = [ PING1D_DISTANCE, PING1D_DISTANCE_SIMPLE, PING1D_PROFILE ] ## The minimum interval time for distance updates to the autopilot ping_interval_ms = 0.075 ## The last time a distance measurement was received last_distance_measurement_time = 0 ## The last time a distance measurement was requested last_ping_request_time = 0 pingargs = ARGS.ping.split(':') pingserver = (pingargs[0], int(pingargs[1])) ## Set RNGFND1_TYPE to MavLink ## This file will only run if ping1d is detected, so we don't need to check for its presence again autopilot_io.mav.param_set_send(autopilot_io.target_system, autopilot_io.target_component, b"RNGFND1_TYPE", 10, mavutil.mavlink.MAV_PARAM_TYPE_INT8) ping1d_io = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) ping1d_io.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) ping1d_io.setblocking(False) ## Send a request for distance_simple message to ping device def send_ping1d_request(): data = PingMessage() data.request_id = PING1D_DISTANCE_SIMPLE data.src_device_id = 0 data.pack_msg_data() ping1d_io.sendto(data.msg_data, pingserver) # some extra information for the DISTANCE_SENSOR mavlink message fields min_distance = 20 max_distance = 5000 sensor_type = 2 orientation = 25 covarience = 0 ## Send distance_sensor message to autopilot def send_distance_data(distance, deviceid, confidence): print("sending distance %d confidence %d" % (distance, confidence)) if confidence < ARGS.min_confidence: distance = 0 autopilot_io.mav.distance_sensor_send( int((time.time() - tboot) * 1000), # time_boot_ms min_distance, # min_distance max_distance, # max_distance int(distance / 10), # distance sensor_type, # type deviceid, # device id orientation, covarience) # set the ping interval once at startup # the ping interval may change if another client to the pingproxy requests it data = PingMessage() data.request_id = PING1D_SET_PING_INTERVAL data.src_device_id = 0 data.ping_interval = int(ping_interval_ms * 1000) data.pack_msg_data() ping1d_io.sendto(data.msg_data, pingserver) while True: time.sleep(0.001) tnow = time.time() # request data from ping device if tnow > last_distance_measurement_time + ping_interval_ms: if tnow > last_ping_request_time + ping_interval_ms: last_ping_request_time = time.time() send_ping1d_request() # read data in from ping device try: data, _ = ping1d_io.recvfrom(4096) except socket.error as exception: # check if it's waiting for data if exception.errno != errno.EAGAIN: raise exception else: continue # decode data from ping device, forward to autopilot for byte in data: if ping_parser.parse_byte(byte) == PingParser.NEW_MESSAGE: if ping_parser.rx_msg.message_id in distance_messages: last_distance_measurement_time = time.time() distance = ping_parser.rx_msg.distance deviceid = ping_parser.rx_msg.src_device_id confidence = ping_parser.rx_msg.confidence send_distance_data(distance, deviceid, confidence)