#Multithreading Program for Hover #This program is to test the use of the RPLidar for maintaining a hover #Version 4 #Import libraries for Dronekit from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative from pymavlink import mavutil # Needed for command message definitions #Import RPLidar library from rplidar import RPLidar from breezyslam.algorithms import RMHC_SLAM from breezyslam.sensors import RPLidarA1 as LaserModel #Import Threading library import threading #Import additional libraries import time import math import os os.system("sudo insmod cp210x.ko") #Global Variables x = 5000 y = 5000 theta = 0 MIN_SAMPLES = 40 mode = 0 prevMode = 0 rotation = 0 #Function to get the LiDAR measurements def lidarRun(): global x global y global theta global mode #Connect to the LiDAR lidar = RPLidar('/dev/ttyUSB0') #Change this for drone info = lidar.get_info() print(info) health = lidar.get_health() print(health) # Create an RMHC SLAM object with a laser model and optional robot model slam = RMHC_SLAM(LaserModel(), 500, 10) # Initialize an empty trajectory trajectory = [] time.sleep(2) # Create an iterator to collect scan data from the RPLidar iterator = lidar.iter_scans() # We will use these to store previous scan in case current scan is inadequate previous_distances = None previous_angles = None # First scan is crap, so ignore it next(iterator) try: while True: angles = [] # Extract (quality, angle, distance) triples from current scan items = [item for item in next(iterator)] # Extract distances and angles from triples distances = [item[2] for item in items] for item in items: ang = item[1] ang = 270-ang if ang < 0: ang = 360 + ang angles.append(ang) # angles = [(item[1]) for item in items] # print(angles) # Update SLAM with current Lidar scan and scan angles if adequate if len(distances) > MIN_SAMPLES: slam.update(distances, scan_angles_degrees=angles) previous_distances = distances*1 previous_angles = angles*1 # If not adequate, use previous elif previous_distances is not None: slam.update(previous_distances, scan_angles_degrees=previous_angles) # Get current robot position x, y, theta = slam.getpos() #Break if the mode gets set to 1 if mode == 1: break finally: # Shut down the lidar connection lidar.stop() lidar.stop_motor() lidar.disconnect() #Function to send mavlink messages to the flight controller def set_attitude(roll_angle = 0.0, pitch_angle = 0.0, yaw_rate = 0.0, thrust = 0.5, duration = 0): """ Note that from AC3.3 the message should be re-sent every second (after about 3 seconds with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified velocity persists until it is canceled. The code below should work on either version (sending the message multiple times does not cause problems). """ """ The roll and pitch rate cannot be controlled with rate in radian in AC3.4.4 or earlier, so you must use quaternion to control the pitch and roll for those vehicles. """ # Thrust > 0.5: Ascend # Thrust == 0.5: Hold the altitude # Thrust < 0.5: Descend msg = vehicle.message_factory.set_attitude_target_encode( 0, # time_boot_ms 1, # Target system 1, # Target component 0b00000000, # Type mask: bit 1 is LSB to_quaternion(roll_angle, pitch_angle), # Quaternion 0, # Body roll rate in radian 0, # Body pitch rate in radian math.radians(yaw_rate), # Body yaw rate in radian thrust # Thrust ) vehicle.send_mavlink(msg) #Function to convert the roll, pitch, and yaw to quaternion def to_quaternion(roll = 0.0, pitch = 0.0, yaw = 0.0): """ Convert degrees to quaternions """ t0 = math.cos(math.radians(yaw * 0.5)) t1 = math.sin(math.radians(yaw * 0.5)) t2 = math.cos(math.radians(roll * 0.5)) t3 = math.sin(math.radians(roll * 0.5)) t4 = math.cos(math.radians(pitch * 0.5)) t5 = math.sin(math.radians(pitch * 0.5)) w = t0 * t2 * t4 + t1 * t3 * t5 x = t0 * t3 * t4 - t1 * t2 * t5 y = t0 * t2 * t5 + t1 * t3 * t4 z = t1 * t2 * t4 - t0 * t3 * t5 return [w, x, y, z] #Function for control system def control(height,duration,desiredTheta,desiredY): startTime = time.time() prevAltitude = 0 positionLastXP = 0 positionLastYP = 0 rotationLastP = 0 heightLastP = 0 count = 0 #PID constants kp1 = -0.01 #Roll P term kd1 = -0.015 #Roll D term kp2 = 0.01 #Pitch P term kd2 = 0.015 #Pitch D term kp3 = 0.4 #Yaw P term kd3 = 0.8 #Yaw D term kp4 = -0.04 kd4 = -0.05 #Continuously updates the roll, pitch, and yaw commands while time.time()-startTime < duration: try: currentAltitude = vehicle.location.global_relative_frame.alt #Gets current altitude #Roll PID positionXP = x - 5000 positionXD = positionXP - positionLastXP positionLastXP = positionXP*1 #Pitch PID positionYP = y - desiredY positionYD = positionYP - positionLastYP positionLastYP = positionYP*1 #Yaw PID rotationP = theta - desiredTheta rotationD = rotationP-rotationLastP rotationLastP = rotationP*1 #Height PID heightP = currentAltitude - height heightD = heightP - heightLastP heightLastP = heightP*1 #Calculations for attitude oldRoll = kp1 * positionXP + kd1 * positionXD oldPitch = kp2 * positionYP + kd2 * positionYD yaw = kp3 * rotationP + kd3 * rotationD #Calculates the roll, pitch, and thrust roll = oldRoll * math.cos(math.radians(theta)) - oldPitch * math.sin(math.radians(theta)) pitch = oldPitch * math.cos(math.radians(theta)) + oldRoll * math.sin(math.radians(theta)) thrust_value = kp4*heightP + kd4*heightD + 0.5 #Setting limits for the attitude if roll > 4: roll = 4 elif roll < -4: roll = -4 if pitch > 4: pitch = 4 elif pitch < -4: pitch = -4 if yaw > 8: yaw = 8 elif yaw < -8: yaw = -8 if thrust_value > 0.55: thrust_value = 0.55 elif thrust_value < 0.45: thrust_value = 0.45 #Print the height print(" Altitude: %f " %(currentAltitude)) #Send attitude command print("X: %.2f Y: %.2f theta: %.2f" %(positionXP, positionYP, rotationP)) print("roll: %.2f pitch: %.2f yaw: %.2f thrust: %.2f" %(roll, pitch, yaw, thrust_value)) currentTime = time.time()-startTime print('Time = %d' %(currentTime)) print('') set_attitude(roll_angle = roll, pitch_angle = pitch, yaw_rate = yaw, thrust = thrust_value) time.sleep(0.2) except KeyboardInterrupt: print('Emergency Landing requested') break except: print('Runtime Error: Landing') mode = 1 break if __name__ == "__main__": #Initial Setup # Connect to the Vehicle connection_string = 'tcp:127.0.0.1:5760' print('Connecting to vehicle on: %s' % connection_string) vehicle = connect(connection_string, wait_ready=True) #Start taking measurements with the lidar t1 = threading.Thread(target=lidarRun,args=()) t1.start() time.sleep(2) #First few measurements are not accurate #Arm the motors print("Arming motors") # Copter should arm in GUIDED_NOGPS mode vehicle.mode = VehicleMode("GUIDED_NOGPS") vehicle.armed = True #Wait for vehicle to be armed try: while not vehicle.armed: print(" Waiting for arming...") time.sleep(1) except KeyboardInterrupt: mode = 1 print("Could not arm vehicle") #Takeoff and Hover if mode != 1: print("Taking off!") control(0.5,10,0,5000) print("Turning!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") control(0.5,15,-45,5000) # print("Return to original position") # control(0.5,10,0,5000) # print("Moving Forward!!!!") # control(0.5,5,0,5500) # print("Moving Backwards!!!") # control(0.5,5,0,4500) print("Setting LAND mode...") vehicle.mode = VehicleMode("LAND") time.sleep(1) print("Close vehicle object") vehicle.close() print("Stopping Lidar...") mode = 1 t1.join() print("Completed")