I’ve now completed my hardware upgrades to the ROV and am now working on the software side of things, mostly focused on incorporating the Ping into my system. Before I dive into the nitty gritty of the code, I want to make sure I’ve got the big picture implementation correct in my head.
I have multiple programs that need to access the Ping data as outlined below.
MavProxy - I’d like to send the data out to QGC to allow terrain following. I’ve done this in the past with my own DIY depth sounder/altimeter but due to other unrelated technical issues I never really got to play with it in the field and ensured it worked correctly. My understanding is that if QGC has a height value, it should turn Depth Hold mode to Terrain Hold. For my old system, I have a switch on my topside control station to stop sending height data to QGC so I can use Depth Hold Mode and then switch it to provide data to QGC and allow Terrain Follow mode. Again, never fully tested that this worked. (sort of hard to do in a pool or in the ocean on a gently sloping sandy bottom).
Data Logger - Located on a secondary raspberry pi on the same network. I need to grab the current distance value and log it to an XML file whenever I want to based on whatever my mission is. I used to grab this from MavProxy but in my configuration above, I wouldn’t be able to grab that data if I had the switch set to Depth Mode rather than Terrain Follow mode. That means I want to grab data straight from the ping program, rather than MavProxy.
PingViewer - I’d also like to have the option to run PingViewer on my topside computer while the other two are running.
With all of this in mind, my strategy was to create a script on the companion computer that connected to the Ping, grabbed the data and then served it up to my data logger, PingViewer, and sent it to QGC via Mavproxy. As I was poking around the companion software, I found the ping1d_mavlink_driver.py script. This looks like it is already sending the Ping Data to QGC via Mavlink but I can’t seem to find that data anywhere in QGC. The rangefinder field is blank. Is this script currently running? I don’t have PingViewer or anything else running. However, if I do launch PingViewer, it does work giving me a 1.97m height with 100% confidence (it is sitting on a 30" high desk pointing at the ground but I’m guessing that has to do with the speed of sound in air vs water calculations). Should I be able to see this value in QGC somewhere?
Is there an easy way to also access this data myself from my own software at the same time? Do I need to run my own instance of ping-proxy or is that already running and I just need to connect to it? I haven’t poked around in companion all that much so I’m not exactly sure what is already running. Thanks!
EDIT: I also realize that the Ping software is in Beta and I’m pushing it beyond where it is already supported. I understand I will probably have to write a lot of this implementation myself, I just am trying to grasp what is already in place! Thanks
After messing around with this some more with zero progress, is there a python example of connecting to the ping proxy script over UDP from another computer like PingViewer does?
I’ve been trying to run the ping1d_mavlink_driver.py script but am getting the following error. Any suggestions or guidance would be greatly appreciated.
Traceback (most recent call last):
File "./ping1d_mavlink_driver.py", line 137, in <module>
main()
File "./ping1d_mavlink_driver.py", line 115, in main
send_ping1d_request()
File "./ping1d_mavlink_driver.py", line 78, in send_ping1d_request
data = pingmessage.pingmessage()
AttributeError: module 'brping.pingmessage' has no attribute 'pingmessage'
I’ll make some examples when I have the chance. Sorry I’ve been sick/busy lately.
PingProxy already runs on companion at boot you can connect to UDP port 9090 as a UDP client to communicate with the ping.
ping1d mavlink driver probably needs to be updated to work. This program will send the ping depth to the autopilot, and the autopilot will attempt to use it for depth control. This is not stable in ardusub yet, so it’s disabled.
So I have fixed the above error by changing line 78 to read “data = pingmessage.PingMessage()” but now I have a new error. According to the system page of the web GUI for the companion, PingProxy is running but I can’t get any of my scripts to receive any data from it. Here’s the error.
Traceback (most recent call last):
File "./ping1d_mavlink_driver.py", line 138, in <module>
main()
File "./ping1d_mavlink_driver.py", line 128, in main
for byte in data:
UnboundLocalError: local variable 'data' referenced before assignment
From what I can figure out, I’m not getting any info when I request it from PingProxy in this chunk of code (straight from ping1d mavlink driver which I have been using as an example to build from):
# read data in from ping device
try:
data, _ = ping1d_io.recvfrom(4096)
except socket.error as exception:
# check if it's waiting for data
print("Error")
if exception.errno != errno.EAGAIN:
raise exception
Next step for me is to see if I can figure out why. Thanks!
Yes, the request is called just prior to this. Entire script below. Straight from the companion software with a few edits to fix errors and a few print statements added for debugging.
#!/usr/bin/python -u
""" Request distance measurements from a Blue Robotics Ping1D device over udp (PingProxy)
Send results to autopilot via mavproxy over udp for use as mavlink rangefinder
Don't request if we are already getting data from device (ex. there is another client
(pingviewer gui) making requests to the proxy)
"""
import argparse
import errno
import socket
import time
from pymavlink import mavutil
from brping import pingmessage
PARSER = argparse.ArgumentParser(description="Ping1D to mavlink bridge.")
PARSER.add_argument('--ping',
action="store",
type=str,
default="0.0.0.0:9090",
help="Ping device udp address and port. ex \"0.0.0.0:9090\""
)
PARSER.add_argument('--mavlink',
action="store",
type=str,
default="0.0.0.0:9000",
help="Mavlink udp address and port. ex \"0.0.0.0:9000\""
)
PARSER.add_argument('--min-confidence',
action="store",
type=int,
default=50,
help="Minimum acceptable confidence percentage for depth measurements.\""
)
ARGS = PARSER.parse_args()
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)
print("Sent Request")
# 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()
# request data from ping device
print("Starting Send Message")
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
# 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
deviceid = ping_parser.rx_msg.src_device_id
confidence = ping_parser.rx_msg.confidence
print("Depth: "+str(distance))
# send_distance_data(distance, deviceid, confidence)
if __name__ == '__main__':
main()
I’ve tried it on two different machines. One is via ssh straight from the companion computer and the other is from my topside computer where I have changed the IP address to “192.168.2.2:9090”. Both result in the same error. I even just tried “192.168.2.2:9090” on the companion pi and that didn’t work either.
You may wait for this to be fixed, or use the version of brping that ships on companion 0.0.16. Probably your easiest option is to do a fresh companion install.
Please, install the last version of ping-python via pip with: pip install bluerobotics-ping --user
You should be able to run this example.
# include socket library
import socket
# include bluerobotics-ping
from brping import pingmessage
# set address
ADDRESS = ("192.168.2.2", 9090)
# set socket configuration
SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
SOCK.settimeout(1.0)
# create pingmessage parser
PARSER = pingmessage.PingParser()
# request 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 ping messages
def parse(data):
for b in bytearray(data):
if PARSER.parse_byte(b) == PARSER.NEW_MESSAGE:
return PARSER.rx_msg
return {}
# Request and process data
while True:
request(pingmessage.PING1D_DISTANCE_SIMPLE)
try:
data, addr = SOCK.recvfrom(1024)
print(parse(data))
except socket.timeout:
print('REQUEST TIMED OUT')
Thanks Patrick! I’ll take a look at it later this morning. I’m in the middle of some unrelated hardware design at this very moment but I look forward to working on this again later today.
Ok, I couldn’t wait… This works great for a single connection. With this script running and feeding data I’m happy. However if I open PingViewer at the same time then I get an error and the script won’t run again, even if I close PingViewer. The error I get is below.
{}
Traceback (most recent call last):
File "./pingtest.py", line 40, in <module>
print(parse(data))
File "/home/topside/.local/lib/python3.5/site-packages/brping/pingmessage.py", line 570, in __repr__
payload_string += "\n - " + attr + ": " + str([hex(ord(item)) for item in getattr(self, attr)])
File "/home/topside/.local/lib/python3.5/site-packages/brping/pingmessage.py", line 570, in <listcomp>
payload_string += "\n - " + attr + ": " + str([hex(ord(item)) for item in getattr(self, attr)])
TypeError: ord() expected string of length 1, but int found
I think in order for me to have multiple script connections to ping proxy, I need to set up my own version of ping proxy with multiple open UDP ports. Is that right?
This is weird, I have this script running with Ping-Viewer at the same time without problems here.
Also, you should not have this error if you are running my example script in the way that is right now. This error should appears only with PROFILE messages or anything that returns “array” types. Right now this is a know issue, we have a fix for that already, but it needs to be approved and merged to be available for everybody.
EDIT: the fix was merged and should be available in 0.0.8, but you can get it also with pip install git+https://github.com/bluerobotics/ping-python.git.
Did you change the request message id from the example ?
All I did was copy your script, add a shebang at the top to python 3 and chmod it +x and then run it. When I ran the pip install, it said I was already running version 0.0.7. I’m assuming install will install a new version if it is available since pip upgrade doesn’t seem to work like it does with apt-get.
Ok, that’s a bit weird. But you should be able to install the library via pip install git+ as said in my previous post. Probably I was not quickly enough in my edit.
Doing that now. Just to verify, I am doing this on my topside computer where the script resides. Do I need to install it on the companion computer as well?