Get "rangefinder1" and "rangefinder2" values of MissionPlanner with dronekit-python

ArduCopter
1

Hi, everyone.

I want to get “rangefinder2” value on dronekit-python. I would like to clarify that “rangefinder2” is different from “sonarrange”. (“rangefinder2” equals to “rangefinderX” (X=1,2,3,…).)

My CubeOrange+ has 2 LiDARs. One facing the ground(RNGFND1_ORIENT=25), the other facing the front(RNGFND2_ORIENT=0). Both LiDARs are working and their values are shown on “rangefinder1” “rangefinder2” in [status] tab of MissionPlanner(the unit is [cm], I got value of height ≒15). Additionally, the height value obtained from RNGFND1 is displayed in “sonarrange” in meters(I got value ≒0.15).

Let’s go to dronekit-python. I wrote code like this:

import dronekit
vehicle=dronekit.connect("COM9,57600", wait_ready=True)
while True:
    range=vehicle.rangefinder.distance
    print(range)

I got ≒0.15 is as same as “sonarrange” value.
I want to get “rangefinder2” value to detect obstacle.
How can I get “rangefinderX” value instead of “sonarrange”?

I apologize for my poor English and thank you in advance to everyone who watched this.

2

I solved it myself.
This is simple code to get rangefinder1 and rangefinder2 value with dronekit-python.

import dronekit
import time

sonars={}

def listener(self, name, message):
    #CHECK YOUR DEVICE ID!!
    #print(message)
    #This is my output sample.
    # -> DISTANCE_SENSOR {time_boot_ms : 435245, min_distance : 4, max_distance : 2500, current_distance : 40, type : 0, id : 10, orientation : 0, covariance : 0, horizontal_fov : 0.0, vertical_fov : 0.0, quaternion : [0.0, 0.0, 0.0, 0.0], signal_quality : 0}
    #My "id" is 0 for rangefinder1 and 10 for rangefinder2.

    id=message.id #0or10
    #print(id)
    sonars[str(id)]={}
    sonars[str(id)]["current_distance"]=message.current_distance
    #These are useful for checking the correspondence between id and rangefinderX.
    sonars[str(id)]["min2max"]=f"{message.min_distance}-{message.max_distance}"
    sonars[str(id)]["orientation"]=message.orientation
    #Some LiDARs stop working when they fail to retrieve a value.
    sonars[str(id)]["time_boot_ms"]=f"{message.time_boot_ms}"

# Connect to the vehicle
vehicle = dronekit.connect('com9,57600', wait_ready=True)

# Add the message listener
vehicle.add_message_listener('DISTANCE_SENSOR',listener)

while True:
    try:
        rngfnd1=sonars['0']['current_distance']
    except:
        rngfnd1="N/A"
    try:
        rngfnd2=sonars['10']['current_distance']
    except:
        rngfnd2="N/A"
    print(f"rangefinder1:{rngfnd1},rangefinder2:{rngfnd2}")

    #Detect LiDAR not working
    sonars={}
    time.sleep(1)

you can use

vehicle.add_message_listener('DISTANCE_SENSOR',listener)

and

distance=message.current_distance

You will get the values of rangefinder1 and rangefinder2 at the same time, so you need to separate by id.

1 Like