Diff: XBusServo.cpp

Revision:

17:3ffb2e3e3bec

Parent:

16:e283810b53c3

Child:

18:75ddf12d93b6

--- a/XBusServo.cpp	Wed Oct 08 07:39:19 2014 +0000
+++ b/XBusServo.cpp	Thu Oct 30 05:11:46 2014 +0000
@@ -16,7 +16,10 @@
 
 #define kXBusBaudrate           250000          // bps
 
+#define kPacketHeaderFooterSize 3
+#define kPacketPreDataSize      3
 #define kStartOffsetOfCHData    4
+
 #define kCHDataSize             4
 #define kCHDataPacketCommand    0
 #define kCHDataPacketLength     1
@@ -43,7 +46,7 @@
 } XBusCmd;
 
 
-// XBUS device mode
+// XBus device mode
 typedef enum {
     kXBusMode_Operate =         0x01,
     kXBusMode_IDSet =           0x02
@@ -63,27 +66,44 @@
 
 /****************************************************************************
     XBusServo::XBusServo
-    
+
     @param tx           pin name for tx
     @param rx           pin name for rx
     @param maxServoNum  max number of servo that you want to connect.
                         (limit 50)
-                        this does just to resetve the buffer.  you need to
+                        this does just to reserve the buffer.  you need to
                         add XBus servo at the beginning of your sketch
 
     @bref               Constructor
 
     @note               2014/09/02 : move from Arduino lib by Sawa
 *****************************************************************************/
-XBusServo::XBusServo(PinName tx, PinName rx, uint8_t maxServoNum)
-    : XBusPort(tx, rx)
+XBusServo::XBusServo(PinName tx, PinName rx, PinName sw, uint8_t maxServoNum)
+    : XBusPort(tx, rx), TxSwitch(sw, 0)
 {
     DBG("XBusServo::XBusServo\n");
 
     // initialize serial
     txPin = tx;
-    txOnly = (rx == NC);
+    txOnly = false;
+    if (rx == NC)
+        txOnly = true;
     maxServo = maxServoNum;
+
+    // initialise vars
+    numOfServo = 0;
+    if (maxServo > kXBusMaxServoNum)
+        maxServo = kXBusMaxServoNum;
+    else if (maxServo == 0)
+        maxServo = 1;
+    dirty = false;
+    serialCommandBusy = false;
+    recieveBufferPointer = 0;
+    modifyServosNow = false;
+    sendLength = 0;
+    need2ReadData = false;
+    chPacketBuffer = NULL;
+    sendBuffer = NULL;
 }
 
 
@@ -101,23 +121,22 @@
 
     DBG("XBusServo::start\n");
 
-    // initialise vars
-    numOfServo = 0;
-    if (maxServo > kXBusMaxServoNum)
-        maxServo = kXBusMaxServoNum;
-    else if (maxServo == 0)
-        maxServo = 1;
-    dirty = 0;
-    serialWriteBusy = 0;
-    recieveBufferPointer = 0;
-    modifyServosNow = 0;
-    sendLength = 0;
+    // allocate my buffer
     bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1;     // add 1 for CRC
     if (bufferSize < kCmdDataPacketSize)
         bufferSize = kCmdDataPacketSize;
 
-    chPacketBuffer = (uint8_t*)malloc(bufferSize);
-    sendBuffer = (uint8_t*)malloc(bufferSize);
+    chPacketBuffer = new uint8_t[bufferSize];
+    if (chPacketBuffer == NULL) {
+        stop();
+        return kXBusError_MemoryFull;
+    }
+
+    sendBuffer = new uint8_t[bufferSize];
+    if (sendBuffer == NULL) {
+        stop();
+        return kXBusError_MemoryFull;
+    }
 
     // initialize channel packer buffer
     chPacketBuffer[kCHDataPacketCommand]    = kXBusCmd_ModeA;
@@ -127,7 +146,7 @@
 
     // initialize serial
     XBusPort.baud(kXBusBaudrate);
-    XBusPort.format(8, RawSerial::None, 1);
+    XBusPort.format(8, Serial::None, 1);
 #if DEVICE_SERIAL_FC
     XBusPort.set_flow_control(RawSerial::Disabled, NC, NC);
 #endif
@@ -137,13 +156,13 @@
 #else
     XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
 #endif
-    XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);
 
     serial_pinout_tx(txPin);
 
     return kXBusError_NoError;
 }
 
+
 //****************************************************************************
 //  XBusServo::stop
 //    return :    none
@@ -156,10 +175,14 @@
 {
     DBG("XBusServo::stop\n");
 
-    free(chPacketBuffer);
-    free(sendBuffer);
+    delete[] chPacketBuffer;
+    chPacketBuffer = NULL;
+
+    delete[] sendBuffer;
+    sendBuffer = NULL;
 }
 
+
 //****************************************************************************
 //  XBusServo::write
 //    return :    none
@@ -173,22 +196,24 @@
 {
 //  DBG("XBusServo::write\n");
 
-    if (serialWriteBusy)
+    if (serialCommandBusy)
         return;
 
-    serialWriteBusy = 1;
+    serialCommandBusy = true;
     XBusServo::sendLength = length;
     sendBufferPointer = buffer;
 
     if (XBusPort.putc(*sendBufferPointer++) < 0) {
-        serialWriteBusy = 0;
+        serialCommandBusy = false;
         XBusServo::sendLength = 0;
     } else {
         XBusServo::sendLength--;
+    }
+
 #ifdef TARGET_KL25Z
-        XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
+    XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
 #endif
-    }
+
 }
 
 
@@ -197,48 +222,76 @@
 //    return :    none
 //    parameter : none
 //
-//    wait to send all packet data
+//    wait to send all packet data.  never call from interrupt.
 //    2014/09/30 : first write by Sawa
 //****************************************************************************
 void XBusServo::flush(void)
 {
 //    DBG("XBusServo::flush\n");
 
-    while(serialWriteBusy)
+    while(serialCommandBusy)
         ;
 }
 
 
 //****************************************************************************
+//  XBusServo::TxSwitchHandler
+//    return :    none
+//    parameter : none
+//
+//    handler for Tx switch
+//    2014/10/29 : first write by Sawa
+//****************************************************************************
+void XBusServo::TxSwitchHandler(void)
+{
+    TxSwitch.write(1);
+}
+
+
+//****************************************************************************
 //  XBusServo::TxIrqHandler
 //    return :    none
 //    parameter : none
 //
 //    handler for Tx buffer empty
 //    2014/09/30 : first write by Sawa
+//    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");
 
-    if (! serialWriteBusy)
+    if (! serialCommandBusy)
+        return;                                       // Is it noise ?
+
+    if (XBusServo::sendLength > 0) {
+        result = XBusPort.putc(*sendBufferPointer++);
+        if (result < 0) {
+            // error occured
+            XBusServo::sendLength = 0;
+            serialCommandBusy = false;
+        } else {
+            XBusServo::sendLength--;
+            return;
+        }
+    }
+
+#ifdef TARGET_KL25Z
+    XBusPort.attach(NULL, RawSerial::TxIrq);
+#endif
+
+    if (result < 0)
         return;
 
-    if (XBusServo::sendLength <= 0) {
-        serialWriteBusy = 0;
+    if (need2ReadData) {
 #ifdef TARGET_KL25Z
-        XBusPort.attach(NULL, RawSerial::TxIrq);
+        TxSwitchTimer.attach_us(this, &XBusServo::TxSwitchHandler, 27);
 #endif
-    } else {
-        if (XBusPort.putc(*sendBufferPointer++) < 0) {
-            serialWriteBusy = 0;
-            XBusServo::sendLength = 0;
-#ifdef TARGET_KL25Z
-            XBusPort.attach(NULL, RawSerial::TxIrq);
-#endif
-        } else
-            XBusServo::sendLength--;
     }
+
+    serialCommandBusy = false;
 }
 
 
@@ -249,18 +302,22 @@
 //
 //    handler for Rx buffer full
 //    2014/09/30 : first write by Sawa
+//    2014/10/21 : add to ignore the data myself
+//    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");
 
-    recieveBuffer[recieveBufferPointer++] = XBusPort.getc();
-    if (recieveBufferPointer >= kRecieveBufferSize)
-        recieveBufferPointer = 0;
+    while (XBusPort.readable()) {
+        recieveBuffer[recieveBufferPointer++] = XBusPort.getc();
+        if (recieveBufferPointer >= kRecieveBufferSize)
+            recieveBufferPointer = 0;
+    }
 }
 
 
-
 //****************************************************************************
 //  XBusServo::sendChannelDataPacket
 //    return :    none
@@ -279,11 +336,12 @@
 
     if (numOfServo > 0) {
         if (dirty) {
-            memcpy(sendBuffer, chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 3);
-            dirty = 0;
+            memcpy(sendBuffer, chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + kPacketHeaderFooterSize);
+            dirty = false;
         }
 
-        write(sendBuffer, sendBuffer[kCHDataPacketLength] + 3);
+        need2ReadData = false;
+        write(sendBuffer, sendBuffer[kCHDataPacketLength] + kPacketHeaderFooterSize);
     }
 }
 
@@ -297,20 +355,21 @@
 //                value       The value that you want to set / get
 //                valueSize   The value size.  1 byte(char) or 2 byte(int)
 //
-//    This should NOT be called on the timer handler like MsTimer2 when you
+//    This should NOT be called on interrupt handler when you
 //    setup the XBus servo.
 //    2014/09/02 : move from Arduino lib by Sawa
 //****************************************************************************
 XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
 {
     int                 sendSize;
-    uint8_t*            recievedPacket;
+    int                 prevData = -1;
+    uint8_t             dummy;
 
     DBG("XBusServo::sendCommandDataPacket\n");
 
     // setup command
     sendBuffer[kCmdDataPacketCommand] = command;
-    sendBuffer[kCmdDataPacketLength] = valueSize + 3;
+    sendBuffer[kCmdDataPacketLength] = valueSize + kPacketPreDataSize;
     sendBuffer[kCmdDataPacketKey] = 0x00;
     sendBuffer[kCmdDataPacketCH_ID] = channelID;
     sendBuffer[kCmdDataPacketOrder] = order;
@@ -324,49 +383,64 @@
     }
 
     // to recover channel data packet when you call sendChannelDataPacket after this
-    dirty = 1;
+    dirty = true;
+
+    // prepair recieve data
+    XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);
+    while (XBusPort.readable())
+        dummy = XBusPort.getc();
+    recieveBufferPointer = 0;                                   // reset recieve buffer
 
     // send command
-    recieveBufferPointer = 0;                                   // reset recieve buffer
-    sendSize = sendBuffer[kCmdDataPacketLength] + 3;
+    sendSize = sendBuffer[kCmdDataPacketLength] + kPacketHeaderFooterSize;
+    need2ReadData = ((! txOnly) && (channelID != 0));
+    recieveBuffer[sendSize + kCmdDataPacketLength] = 5;            // dummy to get real packet size
     write(sendBuffer, sendSize);
-    flush();                                                    // wait to send all bytes
-
-    // if it's tx only mode, it done
-    if (txOnly)
-        return kXBusError_NoError;
+//    DBG("XBusServo::sendCommandDataPacket 2\n");
 
-    if (channelID != 0) {
-        // change bus direction to Rx mode
-//       DigitalInOut::input();
+    // if it's tx only mode or ID=0, it done
+    if (! need2ReadData) {
+        flush();
+        return kXBusError_NoError;
+    }
 
-        recievedPacket = &(recieveBuffer[sendSize]);
-
-        // wait to read all packet
-        while(recieveBufferPointer < (sendSize + kCmdDataPacketSize))
-            ;
+// 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]);
+        }
+        //                                           need to add time out
+    }
 
-        // change bus direction to Tx mode
-        serial_pinout_tx(txPin);
+    TxSwitch.write(0);
+//    DBG("XBusServo::sendCommandDataPacket 4\n");
+    // change bus direction to Tx mode
+    XBusPort.attach(NULL, RawSerial::RxIrq);
+    need2ReadData = false;
+    serialCommandBusy = false;
 
-        // check CRC
-        if (crc8(recievedPacket, recievedPacket[kCHDataPacketLength] + 3) != 0)
-            return kXBusError_CRCError;
-
-        // check unsupported
-        if (recievedPacket[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported)
-            return kXBusError_Unsupported;
+    // check CRC
+    if (crc8(recieveBuffer, recieveBuffer[kCHDataPacketLength] + 3) != 0) {
+        DBG("XBusServo::sendCommandDataPacket kXBusError_CRCError\n");
+        return kXBusError_CRCError;
+    }
 
-        // send back the value
-        if (valueSize == 1) {                   // 1 byte value
-            *value = recievedPacket[kCmdDataPacketData1];
-            if (*value & 0x0080)
-                *value |= 0xFF00;
-        } else {
-            *value = recievedPacket[kCmdDataPacketData1];
-            *value <<= 8;
-            *value |= recievedPacket[kCmdDataPacketData2];
-        }
+    // check unsupported
+    if (recieveBuffer[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];
+        if (*value & 0x0080)
+            *value |= 0xFF00;
+    } else {
+        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
+        *value <<= 8;
+        *value |= recieveBuffer[sendSize + kCmdDataPacketData2];
     }
 
     return kXBusError_NoError;
@@ -406,7 +480,7 @@
     }
 
     // atomic flag on
-    modifyServosNow = 1;
+    modifyServosNow = true;
 
     // add new servo
     dataOffset = kStartOffsetOfCHData + kCHDataSize * numOfServo;
@@ -420,10 +494,10 @@
 
     // calc CRC
     chPacketBuffer[dataOffset + 4] = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
-    dirty = 1;
+    dirty = true;
 
     // atomic flag off
-    modifyServosNow = 0;
+    modifyServosNow = false;
 
     return kXBusError_NoError;
 }
@@ -455,7 +529,7 @@
         for (servoNo = 0; servoNo < numOfServo; servoNo++)
             if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID) {
                 // atomic flag on
-                modifyServosNow = 1;
+                modifyServosNow = true;
 
                 // copy data after that
                 if (servoNo < (numOfServo - 1))
@@ -470,10 +544,10 @@
                 // calc CRC
                 chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
                 = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
-                dirty = 1;
+                dirty = true;
 
                 // atomic flag off
-                modifyServosNow = 0;
+                modifyServosNow = false;
 
                 return kXBusError_NoError;
             }
@@ -511,7 +585,7 @@
 
             if (chPacketBuffer[dataOffset] == channelID) {
                 // atomic flag on
-                modifyServosNow = 1;
+                modifyServosNow = true;
 
                 // set value
                 chPacketBuffer[dataOffset + 2] = (value >> 8) & 0x00FF;
@@ -523,7 +597,7 @@
                 dirty = 1;
 
                 // atomic flag off
-                modifyServosNow = 0;
+                modifyServosNow = false;
 
                 return kXBusError_NoError;
             }