Merged in f5soh/librepilot/LP-564_python_manualControl (pull request #481)

LP-564 Add ManualControlCommand example to python example script

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>

Author: f5soh

python/examples/example.py

@@ -159,15 +159,105 @@ def driveServo(self):
             time.sleep(1)
 
 
+    def driveInput(self):
+        print "Taking control of self.ManualControl"
+        self.objMan.ManualControlCommand.metadata.access = UAVMetaDataObject.Access.READONLY
+        self.objMan.ManualControlCommand.metadata.updated()
+        self.objMan.ManualControlCommand.Connected.value = True
+        self.objMan.ManualControlCommand.updated()
+
+        print "Arming board using Yaw right"
+        # FIXME: Seems there is a issue with ArmedField.ARMED, 2 equals to the ARMED state
+        while (self.objMan.FlightStatus.Armed.value != 2):
+            self.objMan.ManualControlCommand.Yaw.value = 1
+            self.objMan.ManualControlCommand.updated()
+            self.objMan.ManualControlCommand.Throttle.value = -1
+            self.objMan.ManualControlCommand.updated()
+            time.sleep(1)
+
+        if (self.objMan.FlightStatus.Armed.value == 2):
+            print "Board is armed !"
+            self.objMan.ManualControlCommand.Yaw.value = 0
+            self.objMan.ManualControlCommand.updated()        
+
+        print "Applying Throttle"
+        self.objMan.ManualControlCommand.Throttle.value = 0.01 # very small value for safety
+        # Assuming board do not control a helicopter, Thrust value will be equal to Throttle value.
+        # Because a 'high' value can be read from latest real RC input value, 
+        # initial value is set now to zero for safety reasons.
+        self.objMan.ManualControlCommand.Thrust.value = 0
+        # self.objMan.ManualControlCommand.Throttle.value = self.objMan.ManualControlCommand.Thrust.value
+        self.objMan.ManualControlCommand.updated()
+        time.sleep(0.3)
+
+        count = 60
+        print "Moving Pitch input"
+        while (count > 0):
+            count-=1
+            if self.objMan.ManualControlCommand.Pitch.value < 1:
+                 self.objMan.ManualControlCommand.Pitch.value += 0.05
+                 self.objMan.ManualControlCommand.updated()
+                 time.sleep(0.1)
+            if self.objMan.ManualControlCommand.Pitch.value > 1:
+                self.objMan.ManualControlCommand.Pitch.value = 0
+                self.objMan.ManualControlCommand.updated()
+                time.sleep(0.1)
+
+        self.objMan.ManualControlCommand.Pitch.value = 0
+        self.objMan.ManualControlCommand.updated()
+        time.sleep(0.5)
+
+        count = 60
+        print "Moving Roll input"
+        while (count > 0):
+            count-=1
+            if self.objMan.ManualControlCommand.Roll.value < 1:
+                 self.objMan.ManualControlCommand.Roll.value += 0.05
+                 self.objMan.ManualControlCommand.updated()
+                 time.sleep(0.1)
+
+            if self.objMan.ManualControlCommand.Roll.value > 1:
+                self.objMan.ManualControlCommand.Roll.value = 0
+                self.objMan.ManualControlCommand.updated()
+                time.sleep(0.1)
+
+        self.objMan.ManualControlCommand.Roll.value = 0
+        self.objMan.ManualControlCommand.updated()
+        time.sleep(0.5)
+
+        print "Setting Throttle to minimum"
+        self.objMan.ManualControlCommand.Throttle.value = -1
+        self.objMan.ManualControlCommand.updated()
+        time.sleep(1)
+
+        print "Disarming board using Yaw left"
+        # FIXME: Seems there is a issue with ArmedField.DISARMED, 0 equals to the DISARMED state
+        while (self.objMan.FlightStatus.Armed.value != 0):
+             self.objMan.ManualControlCommand.Yaw.value = -1
+             self.objMan.ManualControlCommand.updated()
+             time.sleep(0.3)
+
+        self.objMan.ManualControlCommand.Yaw.value = 0
+        self.objMan.ManualControlCommand.updated()
+        time.sleep(1)
+
+        print "Back to self.ManualControl, controlled by RC radio"
+        self.objMan.ManualControlCommand.metadata.access = UAVMetaDataObject.Access.READWRITE
+        self.objMan.ManualControlCommand.metadata.updated()    
+        self.objMan.ManualControlCommand.Connected.value = False
+        self.objMan.ManualControlCommand.updated()
+
+
 def printUsage():
     appName = os.path.basename(sys.argv[0])
     print
     print "usage:"
-    print "  %s port o|w|g|s" % appName
+    print "  %s port o|w|g|s|i" % appName
     print "  o: Show Attitude using an \"observer\""
     print "  w: Show Attitude waiting for updates from flight"
     print "  g: Show Attitude performing get operations"
     print "  s: Drive Servo"
+    print "  i: Take control over RC input"
     print
     print "  for example: %s COM30 o" % appName
     print
@@ -182,7 +272,7 @@ def printUsage():
 
     port, option = sys.argv[1:]
 
-    if option not in ["o", "w", "g", "s"]:
+    if option not in ["o", "w", "g", "s", "i"]:
         print "ERROR: Invalid option"
         printUsage()
         sys.exit(2)
@@ -202,6 +292,8 @@ def printUsage():
             demo.showAttitudeViaGet()  # will not return
         if option == "s":
             demo.driveServo()  # will not return
+        if option == "i":
+            demo.driveInput()  # will not return
 
     except KeyboardInterrupt:
         pass