def test_is_equals_lat(self):
loc = Location()
self.assertTrue(loc.is_equals_lat(0))
self.assertFalse(loc.is_equals_lat(1))
self.assertFalse(loc.is_equals_lat(0.05))
self.assertFalse(loc.is_equals_lat(-0.05))
self.assertFalse(loc.is_equals_lat(-0.06))
self.assertTrue(loc.is_equals_lat(-0.04))
self.assertTrue(loc.is_equals_lat(-0.03))
self.assertTrue(loc.is_equals_lat(-0.02))
self.assertTrue(loc.is_equals_lat(-0.01))
self.assertTrue(loc.is_equals_lat(+0.04))
self.assertTrue(loc.is_equals_lat(+0.03))
self.assertTrue(loc.is_equals_lat(+0.02))
self.assertTrue(loc.is_equals_lat(+0.01))
self.assertFalse(loc.is_right_alt(CHARACTER))
self.assertFalse(loc.is_right_alt(CHAR_NUMBER))
loc.set_latitude(LIMIT_NUMBER_LATITUDE)
self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+1))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.05))
self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.05))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.06))
self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.04))
self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.03))
self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.02))
self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.01))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.04))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.03))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.02))
self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.01))
loc.set_latitude(-LIMIT_NUMBER_LATITUDE)
self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+1))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.05))
self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.05))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.06))
self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.04))
self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.03))
self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.02))
self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.01))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.04))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.03))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.02))
self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.01))
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 test_is_right_alt(self):
loc = Location()
self.assertFalse(loc.is_right_alt(CHARACTER))
self.assertFalse(loc.is_right_alt(CHAR_NUMBER))
loc.set_altitude(CHAR_NUMBER)
self.assertTrue(loc.is_right_alt(CHAR_NUMBER))
self.assertTrue(loc.is_right_alt(float(CHAR_NUMBER)-0.04))
self.assertTrue(loc.is_right_alt(float(CHAR_NUMBER)+0.04))
self.assertFalse(loc.is_right_alt(float(CHAR_NUMBER)-0.06))
self.assertFalse(loc.is_right_alt(float(CHAR_NUMBER)+0.06))
self.assertTrue(loc.is_right_alt(float(CHAR_NUMBER)-0.05))
self.assertTrue(loc.is_right_alt(float(CHAR_NUMBER)+0.05))
self.assertFalse(loc.is_right_alt(float(CHAR_NUMBER)-0.051))
self.assertFalse(loc.is_right_alt(float(CHAR_NUMBER)+0.051))
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 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"
