Object avoidance and GUIDED mode with bendy ruler

hello everyone
I could find proximity data in the mission planner but the drone does not get effected by the obstacle ahead in (front)

below is the code which i tried


from pymavlink import mavutil
import time
import RPi.GPIO as GPIO 
from threading import Thread
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink.dialects.v20 import ardupilotmega as mavlink2
import math


latt = "17.XXXXXX"
long = "78.XXXXXX"
alt = 1.5

vehicle = connect("/dev/ttyAMA0", wait_ready=True, baud=921600)
vehicle.wait_ready(True, raise_exception=False)

vehicle.parameters['LAND_SPEED'] = 25 ##Descent speed of 30cm/s
vehicle.parameters["WPNAV_SPEED"]=80
# vehicle.parameters["PRX_TYPE"]=2
# vehicle.parameters["OA_TYPE"]=2
# vehicle.parameters["OA_LOOKAHEAD"]=2
# vehicle.parameters["OA_BR_TYPE"]=1
# vehicle.parameters["OA_DB_EXPIRE"]=10
# vehicle.parameters["OA_DB_QUEUE_SIZE"]=100
# vehicle.parameters["OA_DB_OUTPUT"]=1
print("connected")
GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM) 

obstacledist = 70
vehicleheading = 0

distance = 1
distancespeed = 1

FRONTTRIG = 4
FRONTECHO = 17

sonardist ={}

front = 300
UNIT16_MAX = 251


GPIO.setup(FRONTTRIG,GPIO.OUT) 
GPIO.setup(FRONTECHO,GPIO.IN)



def send_distance_message():
   
    abc = [int(front),  UNIT16_MAX,UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX,
     UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX]

    msg = mavlink2.MAVLink_obstacle_distance_message(
        0, #time
        1, #sensor type
        abc, # abc is usually the array of 72 elements
        15, #angular width
        10, #min distance
        250, #max distance
        10,#https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE
        -40,
        12
        )
    time.sleep(0.3)
    if vehicle.location.global_relative_frame.alt  >= 1.0:
        vehicle.send_mavlink(msg)
        print(msg)
        vehicle.flush()




def frontsonar():
    global front
    while True:
        dist1=251
        GPIO.output(FRONTTRIG, True) 
        time.sleep(0.00001) 
        GPIO.output(FRONTTRIG, False) 
        echo_state=0 
        
        while echo_state == 0: 
            echo_state = GPIO.input(FRONTECHO) 
            pulse_start = time.time() 
        while GPIO.input(FRONTECHO)==1: 
            pulse_end = time.time() 
        pulse_duration = pulse_end - pulse_start 
        distance = pulse_duration * 17150 
        distance = round(distance, 2) 
        
        if(distance<250 and distance>5): #To filter out junk values 
            front=distance
                            
        else:
            front = dist1
        #print("front",front)
        time.sleep(0.1)

        
Thread(target = frontsonar).start()

def get_distance_meters(targetLocation,currentLocation):
    dLat=targetLocation.lat - currentLocation.lat
    dLon=targetLocation.lon - currentLocation.lon

    return math.sqrt((dLon*dLon)+(dLat*dLat))*1.113195e5



def arm_and_takeoff(aTargetAltitude):
        
        print("Arming motors")
        # Copter should arm in GUIDED mode
        vehicle.mode = VehicleMode("GUIDED")
        vehicle.armed = True

        # Confirm vehicle armed before attempting to take off
        while not vehicle.armed:
            print(" Waiting for arming...")
            time.sleep(1)

        print("Taking off!")
        vehicle.simple_takeoff(aTargetAltitude)  # Take off to target altitude
        
        while True:
            print(" Altitude: ", vehicle.location.global_relative_frame.alt)
            # Break and return from function just below target altitude.
            if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
                print("Reached target altitude")
                break
            time.sleep(1)
        
print(vehicle.battery.voltage)
arm_and_takeoff(alt)
vehicle.airspeed = 5
print("Take off complete")

# Hover for 10 seconds
time.sleep(3)
print("Vehicle going to the location")
point1 = LocationGlobalRelative(float(latt),float(long), alt)
distanceToTargetLocation = get_distance_meters(point1,vehicle.location.global_relative_frame)
vehicle.simple_goto(point1)
while True:
    send_distance_message()
    currentDistance = get_distance_meters(point1,vehicle.location.global_relative_frame)
    print("current distance: ", currentDistance,distanceToTargetLocation*.02,currentDistance<distanceToTargetLocation*.02)
    if currentDistance<distanceToTargetLocation*.02:
        print("Reached target location.")
        time.sleep(2)
        break
    time.sleep(1)
vehicle.mode = VehicleMode("LAND")
while True:
    print(" Altitude: ", vehicle.location.global_relative_frame.alt)
    # Break and return from function just below target altitude.
    if vehicle.location.global_relative_frame.alt <=0:
        print("Reached ground")
        vehicle.close() 
        break
    time.sleep(1)

i am using ultrasonic sensor at front , am i missing anything in the above code
and my aim is to perform bendyruler obstacle avoidance with px4(version: 4.1),raspberrypi,ultrasonic sensors

Thanks in advance.

Hi @suryaprakash_lokula … A few pointers:

  1. Why use Obstacle_Distance message when you only have one 1-D sensor. Use DISTANCE_SENSOR
    Messages (common) · MAVLink Developer Guide, you’ll have an easier time
  2. You need to send the correct timestamp in the message, don’t leave it zero. You also need to specify which frame of reference is the measurement in
  3. Can you send me your param file so I can see all the parameters are correct?
  4. Can you see the distances from your sensor in the proximity viewer in the mission planner?

@rishabsingh3003

  1. Actually I need to use 4 ultrasonic sensors in 360 degrees

2.ok I will try them

3.ok I will send you soon

4.yes I can see the proximity sensor message data in proximity window

hello @rishabsingh3003

thank you for your patience and sorry for late reply
answers are given for the questions asked above

1. Why use Obstacle_Distance message when you only have one 1-D sensor. Use DISTANCE_SENSOR
[Messages (common) · MAVLink Developer Guide](https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE), you’ll have an easier time

actually i would like to use 4 distance sensor at north,east,west,south of drone…

2. You need to send the correct timestamp in the message, don’t leave it zero. You also need to specify which frame of reference is the measurement in
ok i will try it…

3. Can you send me your param file so I can see all the parameters are correct?

myparams.param (16.6 KB)

4. 1. Can you see the distances from your sensor in the proximity viewer in the mission planner?



finally my python code is below
test_obj_avoid.txt (8.0 KB)

please help me…

.

Can I get logs of the flight, with the same parameters as you shared above?

@rishabsingh3003

all my flight logs with autonomous drive with obstacle avoidance
https://drive.google.com/file/d/1L-VF4wIrMoh2kkvIE51gI2Go45yhjVqd/view?usp=sharing

logs analyzer

Log File C:\Users\91970\AppData\Local\Temp\tmpBB72.tmp.log
Size (kb) 3317.09375
No of lines 37599
Duration 0:01:27
Vehicletype ArduCopter
Firmware Version V4.1.1
Firmware Hash 01d1aa1e
Hardware Type 
Free Mem 0
Skipped Lines 0
Test: Autotune = UNKNOWN - No ATUN log data
Test: Brownout = GOOD - 
Test: Compass = WARN - Moderate change in mag_field (32.32%)

Test: Dupe Log Data = GOOD - 
Test: Empty = GOOD - 
Test: Event/Failsafe = GOOD - 
Test: GPS = GOOD - 
Test: IMU Mismatch = NA - 
Test: Motor Balance = UNKNOWN - 'QUAD/X'
Test: NaNs = FAIL - Found NaN in CTUN.DSAlt

Test: OpticalFlow = FAIL - 'FLOW_FXSCALER' not found
Test: Parameters = FAIL - 'MAG_ENABLE' not found
Test: PM = GOOD - 
Test: Pitch/Roll = GOOD - 
Test: Thrust = GOOD - 
Test: VCC = UNKNOWN - No CURR log data

@suryaprakash_lokula this is a telemetry log. I need an inflight log (which is a .bin file). You need to fly in Auto mode, with BendyRuler enabled and ideally have some obstacles approach the vehicle. I need the .bin logs of that mission to guess what might be the problem.

@rishabsingh3003
i am driving in guided mode using raspberry pi

as long as you are giving WP position command, that is okay. Please send me the logs

@rishabsingh3003
https://drive.google.com/file/d/1McEb3iYb4NtIDm-S7ZaHB1mFei3Ru1zt/view?usp=sharing

this is .bin file of today’s obstacle avoidance test

@suryaprakash_lokula I think what the problem maybe is that since Copter 4.1, guided mode does not use the WP controller as default. To use BendyRuler, you will need to set param GUID_OPTIONS: Complete Parameter List — Copter documentation

Set this param to 32 (6th bit), and try again. Please note that this will use the Scurve WP nav method, which means you can’t have a very high update rate for guided mode locations.

Thank you for spending your time, i will try it and update by tomorrow

@rishabsingh3003
wow thank you so much it worked but with some inconvenience due to ultrasonic sensor

hello @rishabsingh3003

i had a small dought with the mavlink message parameters, i couldn’t understand from docs properly.

Below is my code of mavlink message

   abc = [200  UNIT16_MAX,UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, 
    UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX,
     UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX, UNIT16_MAX]
    
    ts = datetime.datetime.now().timestamp()
    print(ts)
    msg = mavlink2.MAVLink_obstacle_distance_message(
        int(ts), #time
        1, #sensor type
        abc, # abc is usually the array of 72 elements
        15, #angular width
        10, #min distance
        250, #max distance
        90,#increment_f
        -40,#angle_offset
        12
        )
  1. what is

UNIT16_MAX

and it is not defined in python, how to define it

  1. what are proper values for

increment_f

and

angle_offset

in the above message for a drone which has 4 ultrasonic sensors(HC-SR04) front,back,left,right placed at 90 degrees to each other

@suryaprakash_lokula the array named “abc” in your message, each element corresponds to a distance. You can pass 72 distance’s in each message. You only have 4 distances, therefore you shouldn’t use this message, but instead, use DISTANCE_SENSOR.

But, to answer your question: UINT16_MAX = 65535. This number is used to portray that the index contains invalid distance in the distance array (abc).

  1. Not sure how increment_f and angle_offset would play out when you only have 4 distances. That means 68 elements will need to be set to 65535. You can set increment = 90 degrees. Then just fill in the first 4 elements of the array. But again, that seems like a really bad thing to do. Use DISTANCE_SENSOR as advised

@rishabsingh3003
If i would like to use camera and object detection and measurred individual object distance in view how can i pass the values into the array

Hello.

I’m trying to perform object avoidance using 3 HC-SR04 sensors connected to a raspberry pi 4 in my quadcopter and I have set up all the parameters as indicated in the documentation at Object Avoidance with Bendy Ruler — Copter documentation , but when I upload a mission and switch to AUTO mode, the drone takes off as indicated in the mission, but then stops in place and does not execute the rest of the commands in the mission. Did you have any similar issues?

Here’s the code i’m testing with:

object_avoidance.txt (2.4 KB)

Thank you in advance.