Adds is_armable check.

dronekit · Nov 10, 2015 · 5f3f223 · 5f3f223
1 parent 83bbf3d
commit 5f3f223
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 47 deletions.
diff --git a/dronekit/lib/__init__.py b/dronekit/lib/__init__.py
@@ -1055,6 +1055,13 @@ def armed(self, value):
         else:
             self._master.arducopter_disarm()
 
+    @property
+    def is_armable(self):
+        # check that mode is not INITIALSING
+        # check that we have a GPS fix
+        # check that EKF pre-arm is complete
+        return self.mode != 'INITIALISING' and self.gps_0.fix_type > 1 and self._ekf_predposhorizabs
+
     @property
     def system_status(self):
         """

diff --git a/dronekit/test/sitl/test_goto.py b/dronekit/test/sitl/test_goto.py
@@ -27,43 +27,33 @@ def arm_and_takeoff(aTargetAltitude):
         Arms vehicle and fly to aTargetAltitude.
         """
 
-        # print "Basic pre-arm checks"
         # Don't let the user try to fly autopilot is booting
-        if vehicle.mode.name == "INITIALISING":
-            # print "Waiting for vehicle to initialise"
-            time.sleep(1)
-        while vehicle.gps_0.fix_type < 2:
-            # print "Waiting for GPS...:", vehicle.gps_0.fix_type
+        i = 60
+        while not vehicle.is_armable and i > 0:
             time.sleep(1)
-
-        # print "Arming motors"
-        # Copter should arm in GUIDED mode
-        vehicle.mode    = VehicleMode("GUIDED")
+            i = i - 1
+        assert_equals(vehicle.is_armable, True)
 
+        # Copter should arm in GUIDED mode
+        vehicle.mode = VehicleMode("GUIDED")
         i = 60
         while vehicle.mode.name != 'GUIDED' and i > 0:
             # print " Waiting for guided %s seconds..." % (i,)
             time.sleep(1)
             i = i - 1
+        assert_equals(vehicle.mode.name, 'GUIDED')
 
-        # EKF warmup. Await that EKF's predicted horizontal position (absolute) estimate is good
-        while not vehicle._ekf_predposhorizabs:
-            time.sleep(1)
-
+        # Arm copter.
         vehicle.armed = True
-
         i = 60
         while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0:
             # print " Waiting for arming %s seconds..." % (i,)
             time.sleep(1)
             i = i - 1
+        assert_equals(vehicle.armed, True)
 
-        # Failure will result in arming but immediately landing
-        assert vehicle.armed
-        assert_equals(vehicle.mode.name, 'GUIDED')
-
-        # print "Taking off!"
-        vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
+        # Take off to target altitude
+        vehicle.commands.takeoff(aTargetAltitude) 
 
         # Wait until the vehicle reaches a safe height before
         # processing the goto (otherwise the command after

diff --git a/dronekit/test/sitl/test_localposition.py b/dronekit/test/sitl/test_localposition.py
@@ -18,43 +18,33 @@ def arm_and_takeoff(aTargetAltitude):
         Arms vehicle and fly to aTargetAltitude.
         """
 
-        # print "Basic pre-arm checks"
         # Don't let the user try to fly autopilot is booting
-        if vehicle.mode.name == "INITIALISING":
-            # print "Waiting for vehicle to initialise"
-            time.sleep(1)
-        while vehicle.gps_0.fix_type < 2:
-            # print "Waiting for GPS...:", vehicle.gps_0.fix_type
+        i = 60
+        while not vehicle.is_armable and i > 0:
             time.sleep(1)
-
-        # print "Arming motors"
-        # Copter should arm in GUIDED mode
-        vehicle.mode    = VehicleMode("GUIDED")
+            i = i - 1
+        assert_equals(vehicle.is_armable, True)
 
+        # Copter should arm in GUIDED mode
+        vehicle.mode = VehicleMode("GUIDED")
         i = 60
         while vehicle.mode.name != 'GUIDED' and i > 0:
             # print " Waiting for guided %s seconds..." % (i,)
             time.sleep(1)
             i = i - 1
+        assert_equals(vehicle.mode.name, 'GUIDED')
 
-        # EKF warmup. Await that EKF's predicted horizontal position (absolute) estimate is good
-        while not vehicle._ekf_predposhorizabs:
-            time.sleep(1)
-
+        # Arm copter.
         vehicle.armed = True
-
         i = 60
         while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0:
             # print " Waiting for arming %s seconds..." % (i,)
             time.sleep(1)
             i = i - 1
+        assert_equals(vehicle.armed, True)
 
-        # Failure will result in arming but immediately landing
-        assert vehicle.armed
-        assert_equals(vehicle.mode.name, 'GUIDED')
-
-        # print "Taking off!"
-        vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
+        # Take off to target altitude
+        vehicle.commands.takeoff(aTargetAltitude) 
 
         # Wait until the vehicle reaches a safe height before
         # processing the goto (otherwise the command after

diff --git a/examples/simple_goto/simple_goto.py b/examples/simple_goto/simple_goto.py
@@ -33,11 +33,8 @@ def arm_and_takeoff(aTargetAltitude):
 
     print "Basic pre-arm checks"
     # Don't let the user try to fly autopilot is booting
-    if vehicle.mode.name == "INITIALISING":
-        print "Waiting for vehicle to initialise"
-        time.sleep(1)
-    while vehicle.gps_0.fix_type < 2:
-        print "Waiting for GPS...:", vehicle.gps_0.fix_type
+    while not vehicle.is_armable:
+        print " Waiting for vehicle to initialise..."
         time.sleep(1)