Add pdo write functionality.

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/CANopenSocket"]
 	path = ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/CANopenSocket
 	url = https://github.com/anderssandstrom/CANopenSocket
+[submodule "ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen"]
+	path = ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen
+	url = https://github.com/marel-keytech/openCANopen

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp

@@ -24,14 +24,21 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
                                uint32_t cobId,  // 0x580 + CobId
                                ecmc_can_direction rw,
                                uint32_t ODSize,
-                               int readTimeoutMs, 
+                               int readTimeoutMs,
+                               int writeCycleMs, 
                                int exeSampleTimeMs,
                                int dbgMode) {
 
   writeBuffer_        = writeBuffer;
   cobId_              = cobId;
-  ODSize_             = ODSize; 
+  ODSize_             = ODSize;
+  
+  if(ODSize_ > 8)  {
+    ODSize_ = 8;
+  }
+
   readTimeoutMs_      = readTimeoutMs;
+  writeCycleMs_       = writeCycleMs;
   exeSampleTimeMs_    = exeSampleTimeMs;
   rw_                 = rw;
   exeCounter_         = 0;
@@ -39,6 +46,18 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
   errorCode_          = 0;
   dataBuffer_         = new uint8_t(ODSize_);
   dbgMode_            = dbgMode;
+
+  writeFrame_.can_id  = cobId_;
+  writeFrame_.can_dlc = ODSize;     // data length
+  writeFrame_.data[0] = 0;  // request read cmd
+  writeFrame_.data[1] = 0;
+  writeFrame_.data[2] = 0;
+  writeFrame_.data[3] = 0;
+  writeFrame_.data[4] = 0;
+  writeFrame_.data[5] = 0;
+  writeFrame_.data[6] = 0;
+  writeFrame_.data[7] = 0;
+
 }
 
 ecmcCANOpenPDO::~ecmcCANOpenPDO() {
@@ -46,13 +65,27 @@ ecmcCANOpenPDO::~ecmcCANOpenPDO() {
 }
 
 void ecmcCANOpenPDO::execute() {
+  
   exeCounter_++;
-  if(exeCounter_* exeSampleTimeMs_ >= readTimeoutMs_) {
-    errorCode_ = ECMC_CAN_ERROR_PDO_TIMEOUT;
-    if(dbgMode_) {
-      printf("ECMC_CAN_ERROR_PDO_TIMEOUT (0x%x)\n",errorCode_);
+  
+  if(rw_ == DIR_READ) {
+    if(exeCounter_* exeSampleTimeMs_ >= readTimeoutMs_) {
+      errorCode_ = ECMC_CAN_ERROR_PDO_TIMEOUT;
+      if(dbgMode_) {
+        printf("ECMC_CAN_ERROR_PDO_TIMEOUT (0x%x)\n",errorCode_);
+      }
+      exeCounter_ = 0;
+    }
+  }
+  else {  //DIR_WRITE
+    if(writeCycleMs_<=0) {  // Only write on demand if cycle is less than 0
+      exeCounter_ = 0;
+      return;
+    }
+    if(exeCounter_* exeSampleTimeMs_ >= writeCycleMs_) {      
+      writePdoValue();  // write in defined cycle
+      exeCounter_ = 0;
     }
-    exeCounter_ = 0;
   }
   return;
 }
@@ -80,7 +113,7 @@ void ecmcCANOpenPDO::printBuffer() {
   for(uint32_t i = 0; i < ODSize_; i = i + 2) {
     uint16_t test;
     memcpy(&test,&dataBuffer_[i],2);
-    printf("data[%d]: %u\n",i/2,test);
+    printf("data[%02d]: %u\n",i/2,test);
   }
 }
 
@@ -94,3 +127,14 @@ int ecmcCANOpenPDO::validateFrame(can_frame *frame) {
   }
   return 1;
 }
+
+void ecmcCANOpenPDO::setPdoValue(uint64_t data) {
+  memcpy(dataBuffer_, &data, ODSize_);
+}
+
+void ecmcCANOpenPDO::writePdoValue() {
+  if(writeFrame_.can_dlc > 0) {
+    memcpy(&(writeFrame_.data[0]), dataBuffer_ ,writeFrame_.can_dlc);
+  }
+  writeBuffer_->addWriteCAN(&writeFrame_);
+}

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h

@@ -32,18 +32,22 @@ class ecmcCANOpenPDO {
                  uint32_t cobId,
                  ecmc_can_direction rw,
                  uint32_t ODSize,
-                 int readTimeoutMs, 
+                 int readTimeoutMs,
+                 int writeCycleMs,   //if <0 the write on demand..
                  int exeSampleTimeMs,
                  int dbgMode);
   ~ecmcCANOpenPDO();
   void execute();
   void newRxFrame(can_frame *frame);
+  void setPdoValue(uint64_t data);
+  void writePdoValue();
   
  private:
   int validateFrame(can_frame *frame);
   ecmcSocketCANWriteBuffer *writeBuffer_;
   uint32_t cobId_;   // with cobid
   int readTimeoutMs_;
+  int writeCycleMs_;
   int exeSampleTimeMs_;
   ecmc_can_direction rw_;
   uint32_t ODSize_;
@@ -53,6 +57,7 @@ class ecmcCANOpenPDO {
   int errorCode_;
   void printBuffer();
   int dbgMode_;
+  can_frame writeFrame_;
 };
 
 #endif  /* ECMC_CANOPEN_PDO_H_ */

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp

@@ -218,7 +218,7 @@ void ecmcCANOpenSDO::printBuffer() {
   for(uint32_t i = 0; i < ODSize_; i = i + 2) {
     uint16_t test;
     memcpy(&test,&dataBuffer_[i],2);
-    printf("data[%d]: %u\n",i/2,test);
+    printf("data[%02d]: %u\n",i/2,test);
   }
 }
 

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp

@@ -79,6 +79,10 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
   writeBuffer_ = NULL;
   testSdo_     = NULL;
   testPdo_     = NULL;
+  lssPdo_      = NULL;
+  syncPdo_     = NULL;
+  heartPdo_    = NULL;
+
   exeSampleTimeMs_ = exeSampleTimeMs;
   
   memset(&ifr_,0,sizeof(struct ifreq));
@@ -108,7 +112,24 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
   }
   writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
   testSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_READ,0x2640,0,56,5000,exeSampleTimeMs_, cfgDbgMode_);
-  testPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x183,DIR_READ,8,10000,exeSampleTimeMs_, cfgDbgMode_);
+  testPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x183,DIR_READ,8,10000,0,exeSampleTimeMs_, cfgDbgMode_);
+
+
+  // Test LSS heartbeat "master" signal. This makes the led on pmu905 to go to "Normal Communication"
+  // can0  0x7E5   [0]
+  lssPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x7E5,DIR_WRITE,0,0,1000,exeSampleTimeMs_, cfgDbgMode_);
+
+  
+  // Test sync signal
+  // can0  0x80   [0]
+  syncPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x80,DIR_WRITE,0,0,1000,exeSampleTimeMs_, cfgDbgMode_);
+
+  // Test heartbeat signal
+  // 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);
+
   initAsyn();
 }
 
@@ -213,6 +234,16 @@ void ecmcSocketCAN::doReadWorker() {
     if(testPdo_) {
       testPdo_->newRxFrame(&rxmsg_);
     }
+    if(lssPdo_) {
+      lssPdo_->newRxFrame(&rxmsg_);
+    }
+    if(syncPdo_) {
+      syncPdo_->newRxFrame(&rxmsg_);
+    }    
+
+    if(heartPdo_) {
+      heartPdo_->newRxFrame(&rxmsg_);
+    }    
 
     if(cfgDbgMode_) {
       // Simulate candump printout
@@ -287,6 +318,18 @@ void  ecmcSocketCAN::execute() {
     testPdo_->execute();
   }
 
+  if(lssPdo_) {
+    lssPdo_->execute();
+  }
+
+  if(syncPdo_) {
+    syncPdo_->execute();
+  }
+
+  if(heartPdo_) {
+    heartPdo_->execute();
+  }
+
   return;
 }
 

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h

@@ -107,6 +107,9 @@ class ecmcSocketCAN : public asynPortDriver {
   ecmcSocketCANWriteBuffer *writeBuffer_;
   ecmcCANOpenSDO *testSdo_;
   ecmcCANOpenPDO *testPdo_;
+  ecmcCANOpenPDO *lssPdo_;
+  ecmcCANOpenPDO *syncPdo_;
+  ecmcCANOpenPDO *heartPdo_;
 };
 
 #endif  /* ECMC_SOCKETCAN_H_ */