-
Notifications
You must be signed in to change notification settings - Fork 0
/
colAvoid.py
106 lines (81 loc) · 2.56 KB
/
colAvoid.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
import time
import socket
import exceptions
import math
import argparse
import serial
from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, LocationGlobal
from pymavlink import mavutil
ser = serial.Serial('/dev/ttyACM1', 9600)
gnd_speed = 5
def connectMyCopter():
parser = argparse.ArgumentParser(description='commands')
parser.add_argument('--connect')
args = parser.parse_args()
connection_string = args.connect
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
vehicle = connect(connection_string,wait_ready=True)
return vehicle
#-- Get distance from arduino
def getDist():
b = ser.readline()
try:
return int(b,10)
except:
return 0
#-- Define arm and takeoff
def arm_and_takeoff(altitude):
while not vehicle.is_armable:
print("waiting to be armable")
time.sleep(1)
print("Arming motors")
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
while not vehicle.armed: time.sleep(1)
print("Taking Off")
vehicle.simple_takeoff(altitude)
while True:
v_alt = vehicle.location.global_relative_frame.alt
print(">> Altitude = %.1f m"%v_alt)
if v_alt >= altitude - 1.0:
print("Target altitude reached")
break
time.sleep(1)
#-- Define the function for sending mavlink velocity command in body frame
def set_velocity_body(vehicle, vx, vy, vz):
""" Remember: vz is positive downward!!!
http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html
Bitmask to indicate which dimensions should be ignored by the vehicle
(a value of 0b0000000000000000 or 0b0000001000000000 indicates that
none of the setpoint dimensions should be ignored). Mapping:
bit 1: x, bit 2: y, bit 3: z,
bit 4: vx, bit 5: vy, bit 6: vz,
bit 7: ax, bit 8: ay, bit 9:
"""
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0,
0, 0,
mavutil.mavlink.MAV_FRAME_BODY_NED,
0b0000111111000111, #-- BITMASK -> Consider only the velocities
0, 0, 0, #-- POSITION
vx, vy, vz, #-- VELOCITY
0, 0, 0, #-- ACCELERATIONS
0, 0)
vehicle.send_mavlink(msg)
vehicle.flush()
#-- Collsion detection
def collision():
dist = getDist()
print(dist)
if dist <= 10:
set_velocity_body(vehicle,-gnd_speed, 0, 0)
#---- MAIN FUNCTION
#- Takeoff
vehicle = connectMyCopter()
arm_and_takeoff(10)
#-- collsion detection ON
while True:
collision()