WebSVN - Projects - Diff - Rev 653 and 696 - /VibrationTest/branches/ParameterTest/VibrationTest/MkProtocol.py

       return "Invalid Arguments"
 class InvalidMsgType(MkException):
     def __str__(self):
+      return "Invalid Message type"
+class InvalidMsgLen(MkException):
+    def __str__(self):
-      return "Invalid Message type"
+      return "Invalid Message Length"
 class NoResponse(MkException):
         int = self.data[index]+self.data[index+1]*256
         if (int > 0xFFFF/2):
             int -= 0xFFFF
         return int
+class SettingsMsg:
+    DATENREVISION   = 80
+    IDX_INDEX       = 0
+    IDX_STICK_P     = 1 + 19
+    IDX_STICK_D     = 1 + 20
+    IDX_NAME        = 1 + 90
+    def __init__(self, msg):
+        if (msg.cmd != 'Q'):
+            raise InvalidMsgType
+        if len(msg.data) != 105:
+            raise InvalidMsgLen
+        self.msg = msg
+    def getSettings(self):
+        return self.msg.data
+    def getIndex(self):
+        return self.getSetting(SettingsMsg.IDX_INDEX)
+    def getSetting(self, settingIndex):
+        return self.msg.data[settingIndex]
+    def getName(self):
+        name = ""
+        for i in self.msg.data[SettingsMsg.IDX_NAME:]:
+            if i==0:
+                break
+            name += chr(i)
+        return name
+    def setSetting(self, settingIndex, value):
+        self.msg.data[settingIndex] = value
 class DebugDataMsg:
     IDX_ANALOG_ACCNICK  =   2+2*2
         if (msg.cmd != 'D'):
             raise InvalidMsgType
         self.msg = msg
     def getAccNick(self):
-        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK);
+        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK)
     def getAccRoll(self):
-        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL);
+        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL)
     def getCompassHeading(self):
-        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS);
+        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS)
     def getVoltage(self):
-        return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10;
+        return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10
     def sendNCRedirectUartFromFC(self):
         self.serPort.flushInput()
         msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
         self.sendLn(msg.generateMsg())
         time.sleep(.5)
         # No reply expected...
+    def sendSettings(self, settings):
+        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)
+        self.sendLn(msg.generateMsg())
+        time.sleep(1)
+        msg = self.waitForMsg('S')
     def getDebugMsg(self):
         self.serPort.flushInput()
         self.sendLn(self.getDebugMsgLn)
         msg = self.waitForMsg('D')
         msg = DebugDataMsg(msg)
         return msg
+    def getSettingsMsg(self, index=0xFF):
+        self.serPort.flushInput()
+        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])
+        self.sendLn(msg.generateMsg())
+        msg = self.waitForMsg('Q')
+        msg = SettingsMsg(msg)
+        return msg
     def getVersionMsg(self):
         self.sendLn(self.getVersionMsgLn)
         msg = self.waitForMsg('V')
-if __name__ == '__main__':
-    try:
-##        file = open('mklog.txt', "rbU")
-##
-##        for line in file:
-##            try:
-##                msg = MkMsg(msg=line)
-##                #print "Rec Addr:%02d cmd:%c data:" % (msg.address, msg.cmd), msg.data
-##                if (msg.cmd == 'D'):
-##                    #print len(msg.data)
-##                    msg = DebugDataMsg(msg)
-##
-##                    #print "%d %d" % (msg.getAccNick(), msg.getAccRoll())
-##
-##                    #val = abs(msg.getAccNick())
-##                    #print "*************************************************************"[:val/10]
-##
-##                    print msg.getCompassHeading()
-##
-##            except InvalidMsg:
-##                pass
-##
-##        file.close()
-##        print "OK"
-##        msg = MkMsg(address=1, cmd='a', data=[255, 254, 10, 11, 12])
-##        print msg.generateMsg()
 ##        msg2 = MkMsg(msg.generateMsg())
-##        print msg2.address, msg2.cmd, msg2.data
+if __name__ == '__main__':
     try:
         comm = MkComm()
         comm.open(comPort="COM5")
-        msg = comm.getVersionMsg()
-        print "Version: %d.%d" % msg.getVersion()
+        msg = comm.getVersionMsg()
-        comm.doVibrationTest()
-#         msg = comm.getVersionMsg()
-#         print "Version: %d.%d" % msg.getVersion()
-#
-#         comm.setMotorTest([0,0,80,0])
-#         time.sleep(10)
-#
-#         minN = 0
-#         maxN = 0
-#         minR = 0
-#         maxR = 0
-#
-#         time.clock()
+        print "Version: %d.%d" % msg.getVersion()
-#         for i in range(0,1000):
-#             try:
-#                 msg = comm.getDebugMsg()
-#                 n = msg.getAccNick()
-#                 r = msg.getAccRoll()
+        msg = comm.getSettingsMsg()
-#                 print "%d,%d" % (n, r)
-#                 minN = min(minN, n)
-#                 maxN = max(maxN, n)
-#                 minR = min(minR, r)
+        print "Index=",msg.getIndex()
-#                 maxR = max(maxR, r)
+        print "Name=",msg.getName()
-#             except Exception:
-#                 pass
+        print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)
-#
-#         comm.setMotorTest([0,0,0,0])
-#         print time.clock()
+        print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)
 #         print maxN-minN,maxR-minR
         msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)

Subversion Repositories Projects

(root)/VibrationTest/branches/ParameterTest/VibrationTest/MkProtocol.py @ 708 - Rev 653 → 696