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)
