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)