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.
