def take_off(self,init_alt_in_meter): while not self.vehicle.is_armable: time.sleep(DELAY_HALF_SEC) print "Waiting for arming" self.vehicle.mode = VehicleMode("GUIDED") self.vehicle.armed = True while not self.vehicle.armed: time.sleep(DELAY_HALF_SEC) meter = init_alt_in_meter self.vehicle.simple_takeoff(meter) loc = Location() loc.setFromVehicleLocation(self.vehicle.location.global_relative_frame) while not loc.is_right_alt(meter): loc.setFromVehicleLocation(self.vehicle.location.global_relative_frame) time.sleep(DELAY_HALF_SEC)
def take_off(self,init_alt_in_meter): while not self.vehicle.is_armable: time.sleep(DELAY) print "Waiting for arming" #here the drone is armable and move to the next step self.vehicle.mode = VehicleMode("GUIDED") self.vehicle.armed = True while not self.vehicle.armed: time.sleep(DELAY) #after all the atriubute is set the simple take off can be done self.vehicle.simple_takeoff(init_alt_in_meter) loc = Location() loc.setFromVehicleLocation(self.vehicle.location.global_relative_frame) while not loc.is_right_alt(init_alt_in_meter): loc.setFromVehicleLocation(self.vehicle.location.global_relative_frame) time.sleep(DELAY)
def right(self,move_in_meter): loc = Location() loc1 = Location() head = self.vehicle.heading loc.setFromVehicleLocation(self.vehicle.location.global_frame) loc1.setFromVehicleLocation(self.vehicle.location.global_frame) loc.set_longitude(round(loc.get_longitude()+move_in_meter/100000.0*math.cos(self.vehicle.heading+90),7)) ############maybe 90 or 45 because of the angle loc.set_latitude(round(loc.get_latitude()+move_in_meter/100000.0*math.sin(self.vehicle.heading+90),7)) ############maybe 90 or 45 because of the angle location_global = LocationGlobal(loc.latitude,loc.longitude,loc.altitude) self.vehicle.simple_goto(location_global) while True: time.sleep(DELAY) if abs(self.vehicle.location.global_relative_frame.lat-location_global.lat) < 0.0000001 and abs(self.vehicle.location.global_relative_frame.lon-location_global.lon) < 0.000001: break; self.condition_yaw(head) if move_in_meter > 0: print "complete function right" else: print "complete function left"
def forward(self,move_forward_in_meter): dest_location = Location() loc1 = Location() head = self.vehicle.heading loc1.setFromVehicleLocation(self.vehicle.location.global_relative_frame) dest_location.setFromVehicleLocation(self.vehicle.location.global_relative_frame) add_lat = math.sin(self.vehicle.heading)*(move_forward_in_meter/100000.0) add_lon = math.cos(self.vehicle.heading)*(move_forward_in_meter/100000.0) dest_location.set_latitude(round(dest_location.get_latitude()+ add_lat,7)) dest_location.set_longitude(round(dest_location.get_longitude()+add_lon,7)) location_global = LocationGlobalRelative(dest_location.get_latitude(),dest_location.get_longitude(),dest_location.get_altitude()) self.vehicle.simple_goto(location_global,groundspeed=100) while True: time.sleep(DELAY) if abs(self.vehicle.location.global_relative_frame.lat-location_global.lat) < 0.0000001 and abs(self.vehicle.location.global_relative_frame.lon-location_global.lon) < 0.000001 : break; self.condition_yaw(head) if move_forward_in_meter > 0: print "complete function forward\n" else : print "complete function backwards\n"
def up(self,init_alt_in_meter): if(self.is_first_function): head = self.vehicle.heading/ANGLE_FIX self.is_first_function = False else: head= self.vehicle.heading loc = Location() loc1 = Location() loc.setFromVehicleLocation(self.vehicle.location.global_frame) loc1.setFromVehicleLocation(self.vehicle.location.global_frame) loc.set_altitude(loc.get_altitude()+init_alt_in_meter) location_global = LocationGlobal(loc.latitude,loc.longitude,loc.altitude) self.vehicle.simple_goto(location_global) while not loc1.is_right_alt(loc.altitude): loc1.setFromVehicleLocation(self.vehicle.location.global_frame) self.condition_yaw(head) time.sleep(DELAY) if init_alt_in_meter > 0: print "complete function up" else: print "complete function down"
def test_setFromVehicleLocation(self): #test Vehicle and after that check the location vec= Vec(0,0,0) loc = Location() loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and vec.alt == loc.altitude and vec.lat == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE,0,0) loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and vec.alt == loc.altitude and vec.lat == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE,LIMIT_NUMBER_LONGITUDE,0) loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and vec.alt == loc.altitude and vec.lat == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE,LIMIT_NUMBER_LONGITUDE,POSITIVE_NUMBER) loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and vec.alt == loc.altitude and vec.lat == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE+1,LIMIT_NUMBER_LONGITUDE,POSITIVE_NUMBER) loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and vec.alt == loc.altitude and LIMIT_NUMBER_LATITUDE == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE,LIMIT_NUMBER_LONGITUDE+1,POSITIVE_NUMBER) loc.setFromVehicleLocation(vec) self.assertTrue(LIMIT_NUMBER_LONGITUDE == loc.longitude and vec.alt == loc.altitude and vec.lat == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE,LIMIT_NUMBER_LONGITUDE,CHAR_NUMBER) loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and float(vec.alt) == loc.altitude and vec.lat == loc.latitude) vec= Vec(LIMIT_NUMBER_LATITUDE,LIMIT_NUMBER_LONGITUDE,CHARACTER) loc.setFromVehicleLocation(vec) self.assertTrue(vec.lon == loc.longitude and 0 == loc.altitude and vec.lat == loc.latitude)