Home        Store        Docs        Blog

Ping Implementation Overview


(Drew Fulton) #1

Good morning,

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.

  1. 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).

  2. 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.

  3. 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


(Drew Fulton) #2

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'

(Jacob) #3

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.


(Drew Fulton) #4

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!


(Jacob) #5

The ping will not send any data until you request data.


(Drew Fulton) #6

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()

(Jacob) #7

Where are you running the script? What is the ip address of the server? You are using 0.0.0.0.


(Drew Fulton) #8

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.


(Jacob) #9

There’s a bug.

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.


(Drew Fulton) #10

Ok, so in the meantime, is there no way to connect to ping proxy over UDP from another machine if I am installing brping with pip?


(Jacob) #11

You may connect to pingproxy with the stable version of ping-python, but we don’t have any example for you at the moment.


(Drew Fulton) #12

Ok, thank you.


(Patrick José Pereira) #13

Hi Drew,

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')

(Drew Fulton) #14

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.


(Drew Fulton) #15

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?


(Patrick José Pereira) #16

Hi Drew,

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 ?


(Drew Fulton) #17

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.


(Patrick José Pereira) #18

Hi Drew,

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.


(Drew Fulton) #19

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?


(Patrick José Pereira) #20

Hi Drew,

You can run the script in companion or in your surface computer, since it connects with companion IP, it will work in both cases.