def __init__(self):
## 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
# IO device
self.iodev = None
self.server_address = None
def __init__(self):
self.disabled = False
self.log = getLogger('sonar')
self.log.trace('Sonar sensor initialization started')
# set address
self.address = ("192.168.2.2", 9090)
# Configure Socket
self.__sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.__sock.settimeout(1.0)
# create pingmessage parser
self.__parser = pingmessage.PingParser()
self.log.trace('Sonar sensor initialization completed')
self.log.trace(
'Note that this does not guarantee a working sonar sensor is attached'
)
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
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 exception:
raise Exception(
"Failed to open the given serial port: {0}".format(exception))
## 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
def main():
""" Main function
"""
## The time that this script was started
tboot = time.time()
## Parser to decode incoming PingMessage
ping_parser = pingmessage.PingParser()
## Messages that have the current distance measurement in the payload
distance_messages = [
pingmessage.PING1D_DISTANCE, pingmessage.PING1D_DISTANCE_SIMPLE,
pingmessage.PING1D_PROFILE
]
## The minimum interval time for distance updates to the autopilot
ping_interval_ms = 0.1
## 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.PingMessage()
data.request_id = pingmessage.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(
(time.time() - tboot) * 1000, # time_boot_ms
min_distance, # min_distance
max_distance, # max_distance
distance / 10, # distance
sensor_type, # type
deviceid, # device id
orientation,
covarience)
while True:
time.sleep(0.001)
tnow = time.time()
# Declare our variables
last_distance_measurement_time = 0
distance = 0
deviceid = 0
confidence = 0
# 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)
print(data)
except socket.error as exception:
# check if it's waiting for data
if exception.errno != errno.EAGAIN:
raise exception
# decode data from ping device, forward to autopilot
for byte in data:
if ping_parser.parseByte(
byte) == pingmessage.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
print(distance)
deviceid = ping_parser.rx_msg.src_device_id
confidence = ping_parser.rx_msg.confidence
send_distance_data(distance, deviceid, confidence)
#incl socket lib
import socket
# Include bluerobotics-ping
from brping import pingmessage
# Set address
#ADDRESS = ("192.168.2.2", 9090)
ADDRESS = ("192.168.2.2", 5600)
# Set socket configuration
SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
SOCK.settimeout(1.0)
# Create pingmessage parser
PARSER = pingmessage.PingParser()
# Send a request for new messages from id
def request(id: int):
msg = pingmessage.PingMessage()
msg.request_id = id
msg.pack_msg_data()
SOCK.sendto(msg.msg_data, ADDRESS)
# Parse data to create new ping messages
def parse(data):
for b in bytearray(data):
if PARSER.parse_byte(b) == PARSER.NEW_MESSAGE:
return PARSER.rx_msg