Add SDO write functionality, not tested..

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp

@@ -17,6 +17,8 @@
 #include <sstream>
 #include "ecmcCANOpenSDO.h"
 
+#define ECMC_SDO_TRANSFER_MAX_BYTES 7
+
 /** 
  * ecmc ecmcCANOpenSDO class
 */
@@ -48,8 +50,9 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   exeCounter_         = 0;
   busy_               = 0;
   recivedBytes_       = 0;
-  readStates_         = IDLE;
+  writtenBytes_       = 0; 
-  writeStates_        = IDLE;
+  readStates_         = READ_IDLE;
+  writeStates_        = WRITE_IDLE;
   useTg1Frame_        = 1;
   dataBuffer_         = new uint8_t(ODSize_);
 
@@ -117,16 +120,52 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeReqTransferFrame_.data[7] = ODLengthBytes_.byte3;
 
   // Slave write confirm frame
-  writeSlaveConfFrame_.can_id  = cobIdTx;
+  writeSlaveConfCmdFrame_.can_id  = cobIdRx;
-  writeSlaveConfFrame_.can_dlc = 8;     // data length
+  writeSlaveConfCmdFrame_.can_dlc = 8;     // data length
-  writeSlaveConfFrame_.data[0] = 0x60;  // confirm frame for write
+  writeSlaveConfCmdFrame_.data[0] = 0x60;  // confirm frame for write
-  writeSlaveConfFrame_.data[1] = ODIndexBytes_.byte0;
+  writeSlaveConfCmdFrame_.data[1] = ODIndexBytes_.byte0;
-  writeSlaveConfFrame_.data[2] = ODIndexBytes_.byte1;
+  writeSlaveConfCmdFrame_.data[2] = ODIndexBytes_.byte1;
-  writeSlaveConfFrame_.data[3] = ODSubIndex_;
+  writeSlaveConfCmdFrame_.data[3] = ODSubIndex_;
-  writeSlaveConfFrame_.data[4] = 0;
+  writeSlaveConfCmdFrame_.data[4] = 0;
-  writeSlaveConfFrame_.data[5] = 0;
+  writeSlaveConfCmdFrame_.data[5] = 0;
-  writeSlaveConfFrame_.data[6] = 0;
+  writeSlaveConfCmdFrame_.data[6] = 0;
-  writeSlaveConfFrame_.data[7] = 0;
+  writeSlaveConfCmdFrame_.data[7] = 0;
+
+  // Data frame base
+  writeDataFrame_.can_id  = cobIdTx;
+  writeDataFrame_.can_dlc = 8;     // data length
+  writeDataFrame_.data[0] = 0;     // need to toggle here
+  writeDataFrame_.data[1] = 0;
+  writeDataFrame_.data[2] = 0;
+  writeDataFrame_.data[3] = 0;
+  writeDataFrame_.data[4] = 0;
+  writeDataFrame_.data[5] = 0;
+  writeDataFrame_.data[6] = 0;
+  writeDataFrame_.data[7] = 0;
+
+  // Slave write confirm frame TG0
+  writeConfReqFrameTg0_.can_id  = cobIdRx;
+  writeConfReqFrameTg0_.can_dlc = 8;     // data length
+  writeConfReqFrameTg0_.data[0] = 0x20;  // Toggle 0
+  writeConfReqFrameTg0_.data[1] = 0;
+  writeConfReqFrameTg0_.data[2] = 0;
+  writeConfReqFrameTg0_.data[3] = 0;
+  writeConfReqFrameTg0_.data[4] = 0;
+  writeConfReqFrameTg0_.data[5] = 0;
+  writeConfReqFrameTg0_.data[6] = 0;
+  writeConfReqFrameTg0_.data[7] = 0;
+
+  // Slave write confirm frame TG1
+  writeConfReqFrameTg1_.can_id  = cobIdRx;
+  writeConfReqFrameTg1_.can_dlc = 8;     // data length
+  writeConfReqFrameTg1_.data[0] = 0x30;  // Toggle 1
+  writeConfReqFrameTg1_.data[1] = 0;
+  writeConfReqFrameTg1_.data[2] = 0;
+  writeConfReqFrameTg1_.data[3] = 0;
+  writeConfReqFrameTg1_.data[4] = 0;
+  writeConfReqFrameTg1_.data[5] = 0;
+  writeConfReqFrameTg1_.data[6] = 0;
+  writeConfReqFrameTg1_.data[7] = 0;  
 }
 
 ecmcCANOpenSDO::~ecmcCANOpenSDO() {
@@ -162,120 +201,172 @@ void ecmcCANOpenSDO::execute() {
 void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
   // Wait for:
   // # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00
+  int errorCode = 0;
   if(!busy_) {
-    // Not waiting for any data
+    // Not waiting for any data..
     return;
   }
 
+  // Esnure that frame is from slave
+  if(frame->can_id != cobIdTx_) {
+    return; // not correct frame NEED MORE CHECKS HERE!! Ensure correct and not error frame
+  }
+
   if(rw_ == DIR_READ) {
-    switch(readStates_) {
+    errorCode = readDataStateMachine(frame);
-     case READ_WAIT_FOR_CONF:
+  }  
-        // Compare to the conf frame.. might not always be correct
+  else { // Write
-        if ( !frameEqual(&readSlaveConfFrame_,frame)) {
+    errorCode = writeDataStateMachine(frame);
-          return;
+  }
+}
+
+int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
+  int bytesToRead = 0;
+  switch(readStates_) {
+    case READ_WAIT_FOR_CONF:
+      // Compare to the conf frame.. might not always be correct  NEED MORE CHECKS HERE!!
+      if (!frameEqual(&readSlaveConfFrame_,frame)) {
+        return 0;
+      }
+      readStates_ = READ_WAIT_FOR_DATA; //Next frame should be data!
+      if(dbgMode_) {
+        printf("STATE = READ_WAIT_FOR_DATA\n");
+      }
+      writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);  // Send tg0 frame and wait for data, also size must match to go ahead
+      useTg1Frame_ = 1;
+      break;
+    case READ_WAIT_FOR_DATA:
+      //Add data to buffer
+      bytesToRead = frame->can_dlc-1;
+      if( bytesToRead > ECMC_SDO_TRANSFER_MAX_BYTES) {
+        bytesToRead = ECMC_SDO_TRANSFER_MAX_BYTES;
+      }
+      
+      if(bytesToRead + recivedBytes_ <= ODSize_) {
+        memcpy(dataBuffer_ + recivedBytes_, &(frame->data[1]),bytesToRead);
+        recivedBytes_ += bytesToRead;
+      }
+      if(recivedBytes_ < ODSize_) {  // Ask for more data but must toggle so alternat the prepared frames
+        if(useTg1Frame_) {
+          writeBuffer_->addWriteCAN(&readConfReqFrameTg1_);
+          useTg1Frame_ = 0;
+        } else {
+          writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);
+          useTg1Frame_ = 1;
         }
-        readStates_ = READ_WAIT_FOR_DATA; //Next frame should be data!
+      }
+      if(dbgMode_) {
+        printf("recived bytes = %d\n",recivedBytes_);
+      }
+      
+      if (recivedBytes_ >= ODSize_) {
+        readStates_ =READ_IDLE;
+        busy_ = 0;
+        useTg1Frame_ = 0;
         if(dbgMode_) {
-          printf("STATE = READ_WAIT_FOR_DATA\n");
+          printf("STATE = READ_IDLE\n");
+          printf("All data read from slave SDO.\n");
+          printBuffer();            
         }
-        writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);  // Send tg0 frame and wait for data, also size must match to go ahead
+        return 0;
-        useTg1Frame_ = 1;
+      }
-        break;
+      break;
+    default:
+      return 0;
+      break;
+  }
+  return 0;
+}
 
-      case READ_WAIT_FOR_DATA:
+int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
-        if(frame->can_id != cobIdTx_) {
+  int bytes = 0;
-          return; // not correct frame
+  switch(writeStates_) {
-        }
+    case WRITE_WAIT_FOR_CONF:
-        //Add data to buffer
+      
-        if(frame->can_dlc-1 + recivedBytes_ <= ODSize_) {
+      writtenBytes_ = 0;
-          memcpy(dataBuffer_ + recivedBytes_, &(frame->data[1]),frame->can_dlc-1);
+      useTg1Frame_ = 0;
-          recivedBytes_ += frame->can_dlc-1;
+      // Compare to the conf frame.. might not always be correct MORE TESTS NEEDED HERE!!!!
-        }
+      if ( !frameEqual(&writeSlaveConfCmdFrame_,frame)) {
-        if(recivedBytes_ < ODSize_) {  // Ask for more data but must toggle so alternat the prepared frames
+        return 0;
-          if(useTg1Frame_) {
+      }
-            writeBuffer_->addWriteCAN(&readConfReqFrameTg1_);
+
-            useTg1Frame_ = 0;
+      writeNextDataToSlave(useTg1Frame_);
-          } else {
+      writeStates_ = WRITE_DATA; //Next frame should be data!
-            writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);
+      if(dbgMode_) {
-            useTg1Frame_ = 1;
+        printf("STATE = WRITE_DATA\n");
-          }
+      }
-        }
+
+      break;
+
+    case WRITE_DATA:
+
+      // Wait for writeConfReqFrameTgX_ from from slave (toggle X = 1 or 0)
+      if(!writeWaitForDataConfFrame(useTg1Frame_, frame)) {
+          return 0;
+      }
+
+      // next frame use the other toggle option
+      useTg1Frame_ = !useTg1Frame_;
+      bytes = writeNextDataToSlave(useTg1Frame_);
+
+      if(writtenBytes_ >= ODSize_) {
+        writeStates_ = WRITE_IDLE;
+        useTg1Frame_ = 0;
+        busy_ = 0;
         if(dbgMode_) {
-          printf("recived bytes = %d\n",recivedBytes_);
+          printf("STATE = WRITE_IDLE\n");
+          printf("All data written to slave SDO.\n");
+          printBuffer();          
         }
+        return 0;
+      }
+      break;
+    default:
+      return 0;
+      break;
+  }
+  return 0;
+}
 
-        if (recivedBytes_ == ODSize_) {
+// Return Number of bytes written if 0 then we are done!
-          readStates_ =IDLE;
+int ecmcCANOpenSDO::writeNextDataToSlave(int useToggle) {
-          busy_ = 0;
-          if(dbgMode_) {
-            printf("All data transfered for SDO.\n");
-            printBuffer();
-          }
-        }
 
-        break;
+  // How many bytes should we write
-      default:
+  int bytesToWrite = ODSize_-writtenBytes_;
-        return;
+  if(bytesToWrite>ECMC_SDO_TRANSFER_MAX_BYTES) {
-        break;
+    bytesToWrite = ECMC_SDO_TRANSFER_MAX_BYTES;          
-    }
+  }
+  if (bytesToWrite<=0) {
+    return 0;
+  }
+  //Copy data to frame (start at element 1 since toggle is at data 0)
+  memcpy(&(writeDataFrame_.data[1]),dataBuffer_ + writtenBytes_,bytesToWrite);
+  writeDataFrame_.can_dlc = bytesToWrite + 1; // need to include the toggle byte
+  // add toggle byte (start with toggle 0)
+  if(useToggle) {
+    writeDataFrame_.data[0] = 0x30; // toggle 1
+  } else {
+    writeDataFrame_.data[0] = 0x20; // toggle 0
   }
 
-  // Write
+  writeBuffer_->addWriteCAN(&writeDataFrame_);  // Send first data frame
+  writtenBytes_ += bytesToWrite;
+  return bytesToWrite;
+}
+
+int ecmcCANOpenSDO::writeWaitForDataConfFrame(int useToggle, can_frame *frame) { 
+  // Wait for writeConfReqFrameTg0_ from from slave (toggle 1 or 0)
+  if (useToggle) {
+    if (frameEqual(&writeConfReqFrameTg1_,frame)) {          
+      return 1;
+    }
+  }
   else {
-    switch(writeStates_) {
+    if (frameEqual(&writeConfReqFrameTg0_,frame)) {          
-     case WRITE_WAIT_FOR_CONF:
+      return 1;
-        // Compare to the conf frame.. might not always be correct
-        if ( !frameEqual(&writeSlaveConfFrame_,frame)) {
-          return;
-        }
-
-        readStates_ = WRITE_DATA; //Next frame should be data!
-        if(dbgMode_) {
-          printf("STATE = READ_WAIT_FOR_DATA\n");
-        }
-        writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);  // Send first data frame
-        
-        useTg1Frame_ = 1;
-        break;
-
-      case READ_WAIT_FOR_DATA:
-        if(frame->can_id != cobIdTx_) {
-          return; // not correct frame
-        }
-        //Add data to buffer
-        if(frame->can_dlc-1 + recivedBytes_ <= ODSize_) {
-          memcpy(dataBuffer_ + recivedBytes_, &(frame->data[1]),frame->can_dlc-1);
-          recivedBytes_ += frame->can_dlc-1;
-        }
-        if(recivedBytes_ < ODSize_) {  // Ask for more data but must toggle so alternat the prepared frames
-          if(useTg1Frame_) {
-            writeBuffer_->addWriteCAN(&readConfReqFrameTg1_);
-            useTg1Frame_ = 0;
-          } else {
-            writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);
-            useTg1Frame_ = 1;
-          }
-        }
-        if(dbgMode_) {
-          printf("recived bytes = %d\n",recivedBytes_);
-        }
-        
-        if (recivedBytes_ == ODSize_) {
-          readStates_ =IDLE;
-          busy_ = 0;
-          if(dbgMode_) {
-            printf("All data transfered for SDO.\n");
-            printBuffer();
-          }
-        }
-
-        break;
-      default:
-        return;
-        break;
     }
   }
-
+  return 0;
 }
 
 int ecmcCANOpenSDO::frameEqual(can_frame *frame1,can_frame *frame2) {
@@ -320,7 +411,7 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
 
 int ecmcCANOpenSDO::writeValue() {
   // Busy right now!
-  if(busy_ || writeStates_ != IDLE ) {
+  if(busy_ || writeStates_ != WRITE_IDLE ) {
     return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
   }
   busy_ = 1;

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h

@@ -47,6 +47,10 @@ class ecmcCANOpenSDO {
 
  private:
   int frameEqual(can_frame *frame1,can_frame *frame2);
+  int readDataStateMachine(can_frame *frame);
+  int writeDataStateMachine(can_frame *frame);
+  int writeNextDataToSlave(int useToggle);
+  int writeWaitForDataConfFrame(int useToggle, can_frame *frame);
 
   ecmcSocketCANWriteBuffer *writeBuffer_;
   uint32_t cobIdRx_;   // with cobid
@@ -66,6 +70,11 @@ class ecmcCANOpenSDO {
   can_frame readSlaveConfFrame_;
 
   can_frame writeReqTransferFrame_;
+  can_frame writeSlaveConfCmdFrame_;
+  can_frame writeDataFrame_;
+  can_frame writeConfReqFrameTg0_;
+  can_frame writeConfReqFrameTg1_;
+  
   int dbgMode_;
   int busy_;
   uint8_t *dataBuffer_;
@@ -74,6 +83,7 @@ class ecmcCANOpenSDO {
   ecmc_read_states readStates_;
   ecmc_write_states writeStates_;
   void printBuffer();
+  uint32_t writtenBytes_;
 };
 
 #endif  /* ECMC_CANOPEN_SDO_H_ */

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp

@@ -128,7 +128,7 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
   // can0  0x701   [1]  05
   //can_add_write(1793,1,5,0,0,0,0,0,0,0);
   heartPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x701,DIR_WRITE,1,0,1000,exeSampleTimeMs_, cfgDbgMode_);
-  heartPdo_->setPdoValue(5);
+  heartPdo_->setValue(5);
 
   initAsyn();
 }

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h

@@ -24,13 +24,13 @@ enum ecmc_can_direction {
     DIR_WRITE };
 
 enum ecmc_read_states {
-    IDLE,
+    READ_IDLE,
     READ_REQ_TRANSFER,
     READ_WAIT_FOR_CONF,
     READ_WAIT_FOR_DATA};
 
 enum ecmc_write_states {
-    IDLE,
+    WRITE_IDLE,
     WRITE_REQ_TRANSFER,
     WRITE_WAIT_FOR_CONF,
     WRITE_DATA,