[issue #684] MAVLink Server

src/drivers/MAVLinkServer/CMDVel.py → src/drivers/MAVLinkServer/MAVProxy/CMDVel.py

@@ -15,11 +15,11 @@ def __init__(self,lx,ly,lz,ax,ay,az):
         self.angularY = ay
         self.angularZ = az
 
-        print ("cmdvel start")
+        #print ("cmdvel start")
 
-    def __del__(self):
+    #def __del__(self):
 
-        print ("cmdvel end")
+        #print ("cmdvel end")
 
 
     def setCMDVelData(self, data, current=None):
@@ -39,7 +39,7 @@ def setCMDVelData(self, data, current=None):
 
     def getCMDVelData(self, current=None):
 
-        time.sleep(0.05)  # 50 ms rate to rx CMDVel
+        time.sleep(0.05)  # 20Hz (50ms) rate to rx CMDVel
 
         lock.acquire()
 
@@ -50,7 +50,6 @@ def getCMDVelData(self, current=None):
         data.angularX = self.angularX
         data.angularY = self.angularY
         data.angularZ = self.angularZ
-
         lock.release()
 
         return data

src/drivers/MAVLinkServer/MAVProxy/Extra.py

@@ -0,0 +1,52 @@
+__author__ = 'AeroCano'
+
+import jderobot, time, threading
+
+lockLand = threading.Lock()
+lockTakeOff = threading.Lock()
+
+class ExtraI(jderobot.ArDroneExtra):
+
+    def __init__(self):
+
+        print ("Extra start")
+        self.landDecision = False
+        self.takeOffDecision = False
+
+    def land(self,xxx):
+        self.setLand(True)
+        lockLand.acquire()
+        landDecision = self.landDecision
+        lockLand.release()
+
+        return landDecision
+
+    def takeoff(self,xxx):
+        self.setTakeOff(True)
+        lockTakeOff.acquire()
+        takeOffDecision = self.takeOffDecision
+        lockTakeOff.release()
+
+        return takeOffDecision
+
+    def setLand(self,decision):
+        lockLand.acquire()
+        self.landDecision = decision
+        lockLand.release()
+
+    def setTakeOff(self, decision):
+        lockTakeOff.acquire()
+        self.takeOffDecision = decision
+        lockTakeOff.release()
+
+    def setExtraData(self, data, current=None):
+
+        lockLand.acquire()
+        self.landDecision = data.landDecision
+        lockLand.release()
+
+        lockTakeOff.acquire()
+        self.takeOffDecision = data.takeOffDecision
+        lockTakeOff.release()
+
+        return 0

src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh

@@ -0,0 +1,5 @@
+cd ..
+
+python3.5 setup.py build install --user
+#python3.5 ./MAVProxy/mavproxy.py --master=10.1.1.191:14550 --console
+python3.5 ./MAVProxy/mavproxy.py --master=0.0.0.0:14550 --console

src/drivers/MAVLinkServer/Pose3D.py → src/drivers/MAVLinkServer/MAVProxy/Pose3D.py

@@ -17,33 +17,30 @@ def __init__(self,_x,_y,_z,_h,_q0,_q1,_q2,_q3):
         self.q2 = _q2
         self.q3 = _q3
 
-    def setPose3DData(self,_x,_y,_z,_h,_q0,_q1,_q2,_q3):
+        print ("Pose3D start")
+
+    def setPose3DData(self, data, current=None):
 
         lock.acquire()
-        #print ('adquired S')
 
-        self.x = _x
-        self.y = _y
-        self.z = _z
-        self.h = _h
-        self.q0 = _q0
-        self.q1 = _q1
-        self.q2 = _q2
-        self.q3 = _q3
+        self.x = data.x
+        self.y = data.y
+        self.z = data.z
+        self.h = data.h
+        self.q0 = data.q0
+        self.q1 = data.q1
+        self.q2 = data.q2
+        self.q3 = data.q3
 
-        #print ('released S')
         lock.release()
 
         return 0
 
-
-
     def getPose3DData(self, current=None):
 
-        time.sleep(0.05) #50 ms rate to tx Pose3D
+        time.sleep(0.05) # 20Hz (50ms) rate to tx Pose3D
 
         lock.acquire()
-        #print ('adquired G')
 
         data = jderobot.Pose3DData()
         data.x = self.x
@@ -55,7 +52,6 @@ def getPose3DData(self, current=None):
         data.q2 = self.q2
         data.q3 = self.q3
 
-        #print ('released G')
         lock.release()
 
-        return data
+        return data