def takeoffLand(connect_str):
print("connect to: " + connect_str)
vehicle = connect('udp:172.29.113.100:14550', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
sleep(15)
takeSample(lidar, camera)
takeSample(lidar, camera)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
def takeoffLand():
print("connect...")
vehicle = connect('/dev/ttyUSB0', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
takeSample(lidar, camera)
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
def main():
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Not Connected")
return
else:
print("Connected")
for i in range(100):
distance = lidar.getDistance()
print("Distance to target = %s" % (distance))
if int(distance) < 300:
commandimage = 'raspistill -n --timeout 1 -h 183 -w 275 -o imgtmp.jpg'
os.system(commandimage)
device = open_ncs_device()
graph = load_graph(device)
img_draw = skimage.io.imread(ARGS.image)
img = pre_process_image(img_draw)
infer_image(graph, img, distance)
close_ncs_device(device, graph)
def setup_lidar(self, threshold):
from lidar_lite import Lidar_Lite as lidar
lidar = lidar()
connect = lidar.connect(1)
if connect <-1:
raise Exception("No LiDAR found")
lidar.setThreshold(threshold)
def range_dist():
sonar = False
lidar_sens = False # lidar sensing True, else EZ4 sonar or VL53L1X
if sonar:
# does not work well on grass
from smbus import SMBus
## i2cbus = SMBus(1)
while True:
try:
i2cbus = SMBus(1)
i2cbus.write_byte(0x70, 0x51)
time.sleep(0.12)
val = i2cbus.read_word_data(0x70, 0xE1)
distance_in_cm = val >> 8 | (val & 0x0F) << 8
## print distance_in_cm, 'cm'
print(distance_in_cm, 'cm', file=f)
msg_sensor(distance_in_cm, 25, 400), #sonar facing down
except IOError as err:
print(err)
time.sleep(0.1)
if lidar_sens:
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < 0:
print("\nlidar not connected")
else:
print("\nlidar connected")
while True:
dist = lidar.getDistance()
print(dist, 'cm')
print(dist, 'cm', file=f)
msg_sensor(dist, 25, 700), #lidar facing down
time.sleep(0.2)
else:
# does not work in sunlight
import VL53L1X
tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
tof.open() # Initialise the i2c bus and configure the sensor
tof.start_ranging(
2
) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
# Short range max:1.3m, Medium range max:3m, Long range max:4m
while True:
distance_in_cm = tof.get_distance(
) / 10 # Grab the range in cm (mm)
time.sleep(0.2) # timeout mavlink rangefinder = 500 ms
print(distance_in_cm, 'cm', file=f)
msg_sensor(distance_in_cm, 25, 250), #sensor facing down
tof.stop_ranging() # Stop ranging
def four_points(connect_str):
print("connect to: " + connect_str)
vehicle = connect(connect_str, wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("tern on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
time.sleep(15)
goto(10, 0, vehicle.simple_goto)
point_handler(1, camera, lidar)
goto(0, 10, vehicle.simple_goto)
point_handler(2, camera, lidar)
goto(-10, 0, vehicle.simple_goto)
point_handler(3, camera, lidar)
goto(0, -10, vehicle.simple_goto)
point_handler(4, camera, lidar)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
raw_input("Press any key to turn off the drone")
vehicle.close()
with open('4pts_dump/logs.json', 'w') as fp:
json.dump(l_samples, fp)
def takeoffLand():
print("connect...")
vehicle = connect('/dev/ttyUSB0', wait_ready=True)
vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
sleep(5)
if not vehicle.armed:
print("Vehicle not armed")
return
print("turn on devices...")
camera = PiCamera()
camera.resolution = (1024, 768)
lidar = Lidar_Lite()
connected = lidar.connect(1)
if connected < -1:
print("Lidar not Connected")
return
print("Taking off")
vehicle.simple_takeoff(10)
sleep(15)
takeSample(lidar, camera)
takeSample(lidar, camera)
print("Landing")
vehicle.mode = VehicleMode('LAND')
sleep(15)
raw_input("Press any key to turn off the drone")
vehicle.close()
with open('takeoff_dump/takeoff_land_logs.json', 'w') as fp:
json.dump(l_samples, fp)
from lidar_lite import Lidar_Lite
lidar = Lidar_Lite()
connected = lidar.connect(1)
tooClose = False
def scan_li(c):
for i in range(c):
distance = lidar.getDistance()
print ("Distance to target = ", distance)
if (int(distance) < 20):
tooClose = True
print ("\nToo Close!!! Back Off!!\n")
if (int(distance) > 35):
tooClose = False
print("\nProceed\n")
if (__name__) == "__main__":
if connected < -1:
print ("Not Connected")
else:
print ("Connected")
while (connected >= -1):
import time
import pigpio
import sys
sys.path.append('/root/lidar/Lidar-Lite/python')
from lidar_lite import Lidar_Lite
servos = 18
cur_pulse = 0
cur_angle = 0
pi = pigpio.pi()
lidar = Lidar_Lite()
distance = -1
velocity = -1
l_connected = lidar.connect(1)
def debug_print():
print("Current angle: {}, current pulse: {}".format(cur_angle, cur_pulse))
print("Lidar distance: {}, Lidar velocity: {}".format(distance, velocity))
def set_servo(pulse):
pi.set_servo_pulsewidth(servos, pulse)
distance = lidar.getDistance()
# velocity = lidar.getVelocity()
# time.sleep(.02)
# debug_print()
degree_change = 21.16 #1 degree = 10.588
center_pulse = 1500
angle_movement = 25
GPIO.setup(midButton, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(backButton, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.output(segmentClock,GPIO.LOW)
GPIO.output(segmentData,GPIO.LOW)
GPIO.output(segmentLatch,GPIO.LOW)
firebase= firebase.FirebaseApplication('https://lidarspeedometer.firebaseio.com/') #init database
seconds = 1545925769.9618232
today = time.ctime(seconds) #for database (date & time)
number=0
connected = lidar.connect(1) #connect lidar
#Takes a number and displays 2 numbers. Display absolute value (no negatives)
#look here maybe bug between value+number
def showNumber(value):
number = abs(value) #Remove negative signs and any decimals
x=0
while(x<2):
remainder=number % 10
postNumber(remainder)
number /= 10
x += 1
GPIO.output(segmentLatch,GPIO.LOW)
GPIO.output(segmentLatch,GPIO.HIGH)
class Drone:
def __init__(self):
self.alt = None
self.cam = None
self.lidar = None
self.logs = {}
self.h_logs = {}
self.vehicle = self._connect_drone()
self.vehicle.mode = VehicleMode('GUIDED')
self.f = open("dump/logs.txt", "w")
self.num_samples = 0
self.num_rows = 0
self.row_size = 0
def _connect_drone(self):
'''
Connect to the drone's control unit (Pixhawk4)
:return: dronekit lib object.
'''
print("connecting...")
print("Mode: ", MODE)
if MODE == DEBUG:
str = DEBUG_CONNECTION_STR
print(str)
else: # MODE == LIVE:
str = LIVE_CONNECTION_STR
vehicle = connect(str, wait_ready=True)
print("connected successfully")
return vehicle
def _arming(self):
self.vehicle.armed = True
self.alt = 0
time.sleep(5)
if not self.vehicle.armed:
print("Vehicle not armed")
return False
if MODE == LIVE:
print("Activating devices...")
self.cam = PiCamera()
self.cam.resolution = CAM_RES
self.lidar = Lidar_Lite()
connected = self.lidar.connect(1)
if connected < -1: #TODO: check this cond
print("Lidar not Connected")
return False
print("Armed successfully")
return True
def take_off(self, height):
'''
Arming the drone and taking off to the given height.
:param height: in meters
'''
if not self._arming():
return
print("Taking off...")
self.vehicle.simple_takeoff(height)
self.alt = height
time.sleep(height * 1.4)
self.goto(-14.14, 14.14) #TODO: Delete this line
def land(self):
'''
Landing.
'''
print("Landing...")
self.vehicle.mode = VehicleMode('LAND')
time.sleep(20)
def goto(self, dNorth, dEast):
'''
Go x meters to North and y meters to East.
'''
currentLocation = self.vehicle.location.global_relative_frame
targetLocation = get_location_metres(currentLocation, dNorth, dEast,
self.alt)
targetDistance = get_distance_metres(currentLocation, targetLocation)
self.vehicle.simple_goto(targetLocation)
while self.vehicle.mode.name == "GUIDED": # Stop action if we are no longer in guided mode.
remainingDistance = get_distance_metres(self.vehicle.location.global_frame, targetLocation)
print("Distance to target: ", remainingDistance, ", Heading: ",
self.vehicle.heading)
# if remainingDistance <= targetDistance * 0.05: # Just below target, in case of undershoot.
if remainingDistance <= 0.4:
print("Reached target")
break
time.sleep(2)
def take_sample(self):
'''
Capture image and takes height sample with the Lidar sensor.
'''
print("taking sample...")
if MODE == DEBUG:
sea_level_alt = self.vehicle.location.global_frame.alt
print("sea level alt:" + str(sea_level_alt))
return
# *** LiDAR ****
self.num_samples += 1
s = []
for j in range(5):
s.append(self.lidar.getDistance())
t = datetime.now().time()
sea_level_alt = self.vehicle.location.global_frame.alt
height_median = np.median(s) / 100
relative_alt = sea_level_alt - height_median
# self.h_logs[str(t)] = (height_median, sea_level_alt, relative_alt)
self.h_logs["s_" + str(self.num_samples)] = {"Time" : str(t), "Height" : str(relative_alt), "Hading" : str(self.vehicle.heading)}
# ==== write to file - DEBUG ====
self.f.write(str(self.num_samples) + "- LiD height:" + str(height_median) + "| Sea level:" + str(
sea_level_alt) + "| Relative: " + str(relative_alt) + "| Heading: " +
str(self.vehicle.heading) +'\n')
# *** Camera ****
self.cam.capture('dump/{}.jpg'.format("im_"+str(self.num_samples)))
time.sleep(2)
def close_connection(self):
'''
Close the connection of the dronekit object and save the flight logs.
'''
if MODE == LIVE:
self.logs["Logs"] = self.h_logs
self.logs["num_imgs"] = self.num_samples
self.logs["num_rows"] = self.num_rows
self.logs["row_size"] = self.row_size
with open('dump/logs.json', 'w') as fp:
json.dump(self.logs, fp)
self.vehicle.close()
self.f.close()
time.sleep(2)
print("drone disconnected")
def _scan_straight(self, height, length, width, adv_size):
raw_input("Place the drone in the South-East corner of the scan "
+ "area, and press any key to start the scan")
self.take_off(height)
for i in range(int(length // (2 * adv_size))):
self.take_sample()
for j in range(int(width // adv_size)):
self.goto(0, -adv_size)
self.take_sample()
self.goto(adv_size, 0)
self.take_sample()
for j in range(int(width // adv_size)):
self.goto(0, adv_size)
self.take_sample()
self.goto(adv_size, 0)
takeoff_return = int(length // adv_size) * adv_size + adv_size
self.goto(int(-takeoff_return), 0)
self.land()
raw_input("Press any key to turn off the drone")
self.close_connection()
def _scan_diagonal(self, height, length, width, north, east, adv_size):
fly_size = math.sqrt(adv_size ** 2 / 2)
self.take_off(height)
for i in range(int(length // (2 * adv_size))):
self.take_sample()
for j in range(int(width // adv_size)):
self.goto(north * fly_size, -fly_size)
self.take_sample()
self.goto(fly_size, east * fly_size)
self.take_sample()
time.sleep(7)
for j in range(int(width // adv_size)):
self.goto(-north * fly_size, fly_size)
self.take_sample()
self.goto(fly_size, east * fly_size)
takeoff_return = int(length // adv_size) * adv_size
fly_return = math.sqrt(takeoff_return ** 2 / 2)
self.goto(-fly_return, -east * fly_return)
self.land()
raw_input("Press any key to turn off the drone")
self.close_connection()
def scan(self, height, width, length):
pic_size = get_pic_size(height)
adv_size = 0.6 * pic_size
self.num_rows = int(length // (2 * adv_size))
self.row_size = int(width // adv_size)
found = False
while not found:
print("Select the orientation of the scan area: \n" +
"1. North \n" +
"2. North - East \n" +
"3. North - West")
direction = int(input())
if direction == 1:
found = True
self._scan_straight(height, length, width, adv_size)
elif direction == 2:
found = True
raw_input("Place the drone in the Southern corner of the scan "
+ "area, and press any key to start the scan")
self._scan_diagonal(height, length, width, 1, 1, adv_size)
elif direction == 3:
found = True
raw_input("Place the drone in the Eastern corner of the scan "
+ "area, and press any key to start the scan")
self._scan_diagonal(height, length, width, -1, -1, adv_size)
else:
print("Bad direction")
