Diff: XBusServo.cpp

Revision:

18:75ddf12d93b6

Parent:

17:3ffb2e3e3bec

--- a/XBusServo.cpp	Thu Oct 30 05:11:46 2014 +0000
+++ b/XBusServo.cpp	Wed Nov 05 03:12:27 2014 +0000
@@ -10,8 +10,6 @@
 
 #include "XBusServo.h"
 #include "pinmap.h"
-#include "gpio_api.h"
-
 
 
 #define kXBusBaudrate           250000          // bps
@@ -258,7 +256,6 @@
 //    2014/10/24 : add to setup reading for command data packet
 //****************************************************************************
 void XBusServo::TxIrqHandler(void)
-//void XBusServo::TxIrqHandler(MODSERIAL_IRQ_INFO *q)
 {
     int             result = 0;
 //    DBG("XBusServo::TxIrqHandler\n");
@@ -306,7 +303,6 @@
 //    2014/10/24 : modify to get the data from 1st byte of buffer
 //****************************************************************************
 void XBusServo::RxIrqHandler(void)
-//void XBusServo::RxIrqHandler(MODSERIAL_IRQ_INFO *q)
 {
 //    DBG("XBusServo::RxIrqHandler\n");
 
@@ -362,8 +358,8 @@
 XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
 {
     int                 sendSize;
-    int                 prevData = -1;
     uint8_t             dummy;
+    volatile uint8_t*   recieveDataPtr;
 
     DBG("XBusServo::sendCommandDataPacket\n");
 
@@ -394,22 +390,22 @@
     // send command
     sendSize = sendBuffer[kCmdDataPacketLength] + kPacketHeaderFooterSize;
     need2ReadData = ((! txOnly) && (channelID != 0));
-    recieveBuffer[sendSize + kCmdDataPacketLength] = 5;            // dummy to get real packet size
+    recieveDataPtr = &(recieveBuffer[sendSize]);
+    recieveDataPtr[kCmdDataPacketLength] = 5;            // dummy to get real packet size
     write(sendBuffer, sendSize);
 //    DBG("XBusServo::sendCommandDataPacket 2\n");
 
-    // if it's tx only mode or ID=0, it done
+    // if it's tx only mode or ID=0, it's done
     if (! need2ReadData) {
         flush();
         return kXBusError_NoError;
     }
 
 // wait to read all packet
-    while(recieveBufferPointer < (recieveBuffer[sendSize + kCmdDataPacketLength] + kPacketHeaderFooterSize + sendSize)) {
-        if (prevData != recieveBufferPointer) {
-            prevData = recieveBufferPointer;
-//           DBGF("XBusServo::sendCommandDataPacket 3 %02d %02X \n", recieveBufferPointer, recieveBuffer[recieveBufferPointer - 1]);
-        }
+    while(recieveBufferPointer < (recieveDataPtr[kCmdDataPacketLength] + kPacketHeaderFooterSize + sendSize)) {
+        if (recieveBufferPointer > 16)
+            break;                                  // some trouble happen
+            
         //                                           need to add time out
     }
 
@@ -421,26 +417,26 @@
     serialCommandBusy = false;
 
     // check CRC
-    if (crc8(recieveBuffer, recieveBuffer[kCHDataPacketLength] + 3) != 0) {
+    if (crc8((uint8_t*)recieveDataPtr, recieveDataPtr[kCHDataPacketLength] + 3) != 0) {
         DBG("XBusServo::sendCommandDataPacket kXBusError_CRCError\n");
         return kXBusError_CRCError;
     }
 
     // check unsupported
-    if (recieveBuffer[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported) {
+    if (recieveDataPtr[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported) {
         DBG("XBusServo::sendCommandDataPacket kXBusError_Unsupported\n");
         return kXBusError_Unsupported;
     }
 
     // send back the value
     if (valueSize == 1) {                   // 1 byte value
-        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
+        *value = recieveDataPtr[kCmdDataPacketData1];
         if (*value & 0x0080)
             *value |= 0xFF00;
     } else {
-        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
+        *value = recieveDataPtr[kCmdDataPacketData1];
         *value <<= 8;
-        *value |= recieveBuffer[sendSize + kCmdDataPacketData2];
+        *value |= recieveDataPtr[kCmdDataPacketData2];
     }
 
     return kXBusError_NoError;
@@ -652,7 +648,7 @@
 
     DBG("XBusServo::setChannelID\n");
 
-    if (txOnly)
+    if (! txOnly)
         return kXBusError_OnlyForTxOnlyMode;
 
     value = kXBusMode_IDSet;
@@ -677,9 +673,7 @@
 //****************************************************************************
 uint8_t XBusServo::getDataSize(uint8_t order)
 {
-    uint8_t        dataSize = 1;
-
-    DBG("XBusServo::getDataSize\n");
+ //   DBG("XBusServo::getDataSize\n");
 
     switch(order) {
         case kXBusOrder_2_Version:      // only for get
@@ -696,10 +690,10 @@
         case kXBusOrder_2_AlarmDelay:
         case kXBusOrder_2_CurrentPos:   // only for get
         case kXBusOrder_2_MaxInteger:
-            dataSize = 2;
+            return 2;
+        default:
+            return 1;
     }
-
-    return dataSize;
 }
 
 
@@ -753,7 +747,7 @@
 {
     DBG("XBusServo::setCommand\n");
 
-    if (txOnly)
+    if (! txOnly)
         return kXBusError_OnlyForTxOnlyMode;
 
     return sendCommandDataPacket(kXBusCmd_Set, 0, order, value, getDataSize(order));