The following code in python is designed so that it will keep a drone hovering safely indoors at 3ft. Using distance data from a lidar lite v3 sending to a rasspbery pi and using a pixhawk 4 as the flight controller. Please help change or restructure this code so the drone safely hovers stable at 3ft. Please explain the changes you made and comment the changes 

import time from lidar_lite import Lidar_Lite from dronekit import connect, VehicleMode lidar = Lidar_Lite() connected = lidar.connect(1) distance = 0 #varable that holds the distance value in centimeters distavg=0# holds the distance waiting to be averaged connection_string = '/dev/ttyS0' baud_rate= 921600 c5 = 0 c6=0 c7=0 c8=0 A=1034 #the override rc channel for throttle pwm value 8.67 = 1% throttle q=0 #this sets is the trigger variable for hovering tk=0 #set to high after take off command def dist():#takes the average of 3 distance measurments global distance global distavg for i in range(2): distavg = distavg + lidar.getDistance() distance = distavg/2 distavg=0 print ("distance %s" % distance) def takeoff(): global A global q global tk global distance global c7 print ("takeoff") vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) dist() # gets distance from lidar lite if c7>=1800: #if the drone is less than 1 foot off the ground A=A+9 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) #time.sleep(1) while c7>1800: print("release button") c7 = vehicle.channels['7'] elif distance <=61 and distance >= 25:# if the drone is less than 2 feet high then increase throttle A=A+1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(0.25) #elif distance <=177: # if dicstance is less than 4 cm less than 6 ft #i_height()#this function increases the height if distance >= 62: # if the drone is above 4 cm less than 6 ft then set hover and take off variable high tk = 1 q = 1 def hover():#this function keeps the drone at 6 ft with a tolerance of 4cm global distance vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) dist()# reads the drones current distance from the ground if distance >=107:# if the drone is above 6ft 6cm then decrease throttle d_height() elif distance <=77:# if the drone is below 6cm less than 6ft then increase throttle i_height() else : vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) def land():# this function will land the drone global q global A global tk global distance q=0 #sets the hover variable low so code doesnt run hover function dist()# gets the drones # if the drone is 5 cm off the ground or the throttle is less than 28% then disarm the drone vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) if distance <= 30: vehicle.channels.overrides = {'3' : 1000} vehicle.armed =False #disarms the drone #time.sleep(4)# waits for the drone to register the disarm print (" Armed: %s" % vehicle.armed) if vehicle.armed == False: tk = 0 #resets the take off trigger variable because the drone is disarmed elif distance <=40:# if the drone is less than 1 feet from the ground then drop the drone for i in range(15):#this ramps down the throttle instead of instant throttle A=A-3 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(0.2) elif distance <= 92:#if the drone is 3ft off the ground A=A-1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.5)#time delay so the drone desends slowly elif distance <= 153:#if the drone is 5 feet off the ground A=A-8 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(1) else: #if the drone is above 5ft A=A-5#8.67 is 1 percent throttle vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(1) def i_height(): #increase height global distance global A if distance <=30:#if the drone is 2 ft from the ground big increase A=A+1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.1) elif distance <= 50:#if the drone is 3 off the ground medium increase A=A+1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.1) elif distance <= 77:#if the drone is 5 ft off the ground small increase A=A+1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.1) def d_height(): #decrease height of drone global distance global A if distance >= 152:#if the drone is 8ft above the ground big decrease A=A-1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.05) elif distance >= 120:# if the drone is 7ft off the ground medium decrease A=A-1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.05) elif distance >= 107: #if the drone is 6ft 7cm above the ground then small decrease A=A-1 vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(.05) print("Connecting to vehicle on: %s" %connection_string) vehicle = connect(connection_string, baud=baud_rate, wait_ready=True) """print (" Get some vehicle attribute values:") print (" Last Heartbeat: %s" %vehicle.last_heartbeat) print (" Is Armable?: %s" % vehicle.is_armable) print (" System Status: %s" % vehicle.system_status.state) print (" Mode: %s" % vehicle.mode.name) print (" Attitude: %s" % vehicle.attitude) vehicle.armed =True time.sleep(2) print (" Armed: %s" % vehicle.armed) time.sleep(2) print (" Armed: %s" % vehicle.armed) for i in range(16): vehicle.channels.overrides = {'3' : A} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(0.2) A=A+50 vehicle.channels.overrides = {'3' : 1400} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(1) vehicle.channels.overrides = {'3' : 1500} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(1) vehicle.channels.overrides = {'3' : 1900} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(3) vehicle.channels.overrides = {'3' : 1500} print ("Channel Overrides: %s" %vehicle.channels.overrides) time.sleep(6) print (" Armed: %s" % vehicle.armed) vehicle.armed =False time.sleep(1) print (" Armed: %s" % vehicle.armed) if connected < -1: print ('Not Connected') else: print ('Connected') for i in range(5): distance = lidar.getDistance() print('Distance = {} cm' .format(distance))""" while True: #time.sleep(.5) c5 = vehicle.channels['5'] c6 = vehicle.channels['6'] c7 = vehicle.channels['7'] c8 = vehicle.channels['8'] print ("Battery: %s)" % vehicle.battery) print ("Channel value: %s)" % vehicle.channels['3']) if c8 >= 1800: vehicle.channels.overrides = {'3' : None} while c8 >= 1800: A = 1034 #c8 = vehicle.channels['8'] #vehicle.channels.overrides = {'3' : 1000} #print ("Channel Overrides: %s" %vehicle.channels.overrides) #vehicle.armed = False #time.sleep(2) #vehicle.channels.overrides = {'3' : None} print ("Channel Overrides: %s" %vehicle.channels.overrides) c8 = vehicle.channels['8'] if c5 >= 1800 and vehicle.armed == True and tk == 0 : #switch is high and armed liftoff set q=1 after finish liftoff and have lift off end at 6ft print ("in takeoff of main loop") takeoff() if q == 1: #q=1 then run hover print ("in hover of main loop") hover() if c6 >= 1800 and tk == 1: #2 switchs are high then run land print ("in land of main loop") land() elif tk == 1 and (c6 <= 1800): #1 switch is low then hover and tk is 1 q = 1 #if decrease height was been called within last x seconds, then don't do it again??? #add a counter, so sleep only occurs 1 out of every x loops #whatever ht im at, this is the throttle value I should be at