ecmc_plugin_daq/src/ecmcDAQDataArray.cpp

/*************************************************************************\
* Copyright (c) 2024 PSI
* ecmc is distributed subject to a Software License Agreement found
* in file LICENSE that is included with this distribution. 
*
*  ecmcDAQDataArray.cpp
*
*  Created on: March 1, 2024
*      Author: anders sandstrom
*      Credits to  https://github.com/sgreg/dynamic-loading 
*
\*************************************************************************/

// Needed to get headers in ecmc right...
#define ECMC_IS_PLUGIN

#define ECMC_PLUGIN_ASYN_PREFIX      "plugin.daq"
#define ECMC_PLUGIN_ASYN_ENABLE      "enable"
#define ECMC_PLUGIN_ASYN_RAWDATA     "data"

#include <sstream>
#include "ecmcDAQDataArray.h"
#include "ecmcPluginClient.h"


ecmcDAQDataArray::ecmcDAQDataArray(const char* name, const char* portName)
                 : asynPortDriver(portName,
                   1, /* maxAddr */
                   asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
                   asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
                   asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
                   asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */
                   asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
                   asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
                   asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
                   asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */
                   ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/
                   1, /* Autoconnect */
                   0, /* Default priority */
                   0) /* Default stack size */
                   {
  channelCounter_        = 0;
  dataElementCount_      = 0;
  totalElementCount_     = 0;
  firstDataElementIndex_ = 0;
  asynEnableId_          = 1;
  asynRawDataId_         = -1;
  enablePlugin_          = -1;
  dataSourcesLinked_     = 0;
  name_ = name;
  master_ = (ecmcEc*)getEcMaster();
  printf("Created an array with name = %s, asynPortName = %s\n",name, portName);

}

ecmcDAQDataArray::~ecmcDAQDataArray() {

}

void  ecmcDAQDataArray::addChannel(int type) {
  dataChannels_.push_back(new ecmcDAQDataChannel(type));
  channelCounter_++;
}

void ecmcDAQDataArray::addDataItemToChannel(const char* name, int format) {
  // Always add to last added channel
  dataChannels_.back()->addDataItem(name, format);
}

void ecmcDAQDataArray::connectToDataSources() {
  if( dataSourcesLinked_ ) {
    return;
  }

  dataElementCount_ = 0;

  for(std::vector<ecmcDAQDataChannel*>::iterator pDataCh = dataChannels_.begin(); pDataCh != dataChannels_.end(); ++pDataCh) {
    if(!(*pDataCh)) {
      throw std::runtime_error( "Channel empty..");
    }
    (*pDataCh)->connectToDataSources();
    dataElementCount_ = dataElementCount_ + (*pDataCh)->getDataElementCount();
  }
  
  // Header: First element is channel count then time stamp then 4 elements per channel
  totalElementCount_ = dataElementCount_ + channelCounter_ * 4 + 2;

  // Now we we can finally allocate teh buffer (ecmc is still not in realtime, enterRT)
  buffer_ = new double [totalElementCount_];
  memset(buffer_,0,totalElementCount_*sizeof(double));
  
  // Build header
  buildArrayHeader();

  // Register asyn parameters
  initAsyn();

  dataSourcesLinked_ = 1;
}

/* Prepare header first in array, 4 elements per channel:
*  array[0] = Ec timestamp, updated each cycle, the reset of header is static
* Channel 1:
*  array[1] = Channel type
*  array[2] = Data start index
*  array[3] = Data element count
*  array[4] = spare..
* Channel 2:
*  array[5] = Channel type
*  array[6] = Data start index
*  array[7] = Data element count
*  array[8] = spare..
*....
*/
void ecmcDAQDataArray::buildArrayHeader(){
  // 4 elements plus channel count + timestamp  to first data element, only first element will change in realtime

  size_t dataStartOffset = channelCounter_* 4 + 2;
  firstDataElementIndex_ = dataStartOffset;

  if( totalElementCount_ < dataStartOffset) {
    throw std::runtime_error( "Array to small, header will not fit (array size must be bigger than total data size plus 4*data_channel_count+1)..");
  }

  size_t index = 0;
  buffer_[index] = channelCounter_; // Channel count
  index++;
  buffer_[index] = 0; // Timestamp, will be set in each loop in execute
  index++;

  // Each Data channel takes 4 doubles
  for(std::vector<ecmcDAQDataChannel*>::iterator pDataCh = dataChannels_.begin(); pDataCh != dataChannels_.end(); ++pDataCh) {
    if(!(*pDataCh)) {
      throw std::runtime_error( "Channel empty..");
    }
    // First element: Type
    buffer_[index] = (double)(*pDataCh)->getType();
    index++;
    // Second element: Data start index in array
    buffer_[index] = (double)dataStartOffset;
    index++;
    // Third element: Data element count (size)
    buffer_[index] = (double)(*pDataCh)->getDataElementCount();
    // Prepare data start index for next data item
    dataStartOffset = dataStartOffset + (*pDataCh)->getDataElementCount(); 
    index++;
    // Forth index spare...
    buffer_[index] = -1;
    index++;
  }
}

void ecmcDAQDataArray::setEnable(int enable) {
  enablePlugin_ = enable;
}

void ecmcDAQDataArray::execute() {
  if(!enablePlugin_) {
    return;
  }

  //convert to micro secs 32bit and double
  buffer_[1] =(double)((master_->getTimeNs()/1000)& 0xFFFFFFFF);

  int first = 1;
  size_t index =  firstDataElementIndex_ ;
  for(std::vector<ecmcDAQDataChannel*>::iterator pDataCh = dataChannels_.begin(); pDataCh != dataChannels_.end(); ++pDataCh) {
    first = 1;
    for(size_t i = 0 ; i < (*pDataCh)->getDataElementCount(); i++) {
      buffer_[index]=(*pDataCh)->getData(first);
      index++;
      first = 0;
    }
  }
  updateAsyn();
}

void ecmcDAQDataArray::updateAsyn() {
  doCallbacksFloat64Array(buffer_, totalElementCount_, asynRawDataId_,0);
  setIntegerParam(asynEnableId_, enablePlugin_);
  // Update integers
  callParamCallbacks();
}

void ecmcDAQDataArray::initAsyn() {

  // Add enable "plugin.daq.<name>.enable"
  std::string paramName =ECMC_PLUGIN_ASYN_PREFIX ".";
  paramName += name_;
  paramName += ".";
  paramName += ECMC_PLUGIN_ASYN_ENABLE;
  
  if( createParam(0, paramName.c_str(), asynParamInt32, &asynEnableId_) != asynSuccess ) {
    throw std::runtime_error("Failed create asyn parameter enable");
  }
  setIntegerParam(asynEnableId_, enablePlugin_);

  // Add data "plugin.daq.<name>.data"
  paramName =ECMC_PLUGIN_ASYN_PREFIX ".";
  paramName += name_;
  paramName += ".";
  paramName += ECMC_PLUGIN_ASYN_RAWDATA;
  
  if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynRawDataId_ ) != asynSuccess ) {
    throw std::runtime_error("Failed create asyn parameter data array");
  }
  doCallbacksFloat64Array(buffer_, totalElementCount_, asynRawDataId_,0);

  // Update integers
  callParamCallbacks();
  return;
}

int ecmcDAQDataArray::validate() {
  if(dataElementCount_==0) {
    throw std::runtime_error("Error: DAQ-Array element count 0");
  }
  if(channelCounter_==0) {
    throw std::runtime_error("Error: DAQ-Array channel count 0");
  }
  for(std::vector<ecmcDAQDataChannel*>::iterator pDataCh = dataChannels_.begin(); pDataCh != dataChannels_.end(); ++pDataCh) {
    if(!(*pDataCh)) {
      throw std::runtime_error( "Channel empty..");
    }
    (*pDataCh)->validate();
  }
  return 0;
}

size_t ecmcDAQDataArray::getArraySize() {
  return totalElementCount_;
}

std::string ecmcDAQDataArray::getName() {
  return name_;
}