Started development of buffered motion API. Compiles but not tested.

4 files changed, 252 insertions, 2 deletions

diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri
index 0885991..9eeef0d 100644
--- a/SimpleMotionV2.pri
+++ b/SimpleMotionV2.pri
@@ -5,8 +5,10 @@ DEPENDPATH += $$PWD
 

 DEFINES += SIMPLEMOTIONV2_LIBRARY

 

-SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/rs232.c

+SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/rs232.c \

+    $$PWD/bufferedmotion.c

 

 HEADERS += $$PWD/simplemotion_private.h\

-    $$PWD/rs232.h $$PWD/busdevice.h  $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h

+    $$PWD/rs232.h $$PWD/busdevice.h  $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h \

+    ../SimpleMotionV2Library/bufferedmotion.h

 

diff --git a/bufferedmotion.c b/bufferedmotion.c
new file mode 100644
index 0000000..992ccca
--- /dev/null
+++ b/bufferedmotion.c
@@ -0,0 +1,191 @@
+#include "simplemotion.h"
+#include "simplemotion_private.h"
+#include "bufferedmotion.h"
+
+/** initialize buffered motion for one axis with address and samplerate (Hz) */
+LIB SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength )
+{
+    //value out of range
+    if(sampleRate<1 || sampleRate>2500)
+        return recordStatus(handle,SM_ERR_PARAMETER);
+
+    newAxis->initialized=smfalse;
+    newAxis->bushandle=handle;
+    newAxis->samplerate=sampleRate;
+    newAxis->deviceAddress=deviceAddress;
+    newAxis->readParamAddr=readParamAddr;
+    newAxis->readParamLength=readDataLength;
+    newAxis->readParamInitialized=smfalse;
+    newAxis->numberOfDiscardableReturnDataPackets=0;
+    newAxis->driveClock=0;
+    newAxis->bufferFill=0;
+    newAxis->numberOfPendingReadPackets=0;
+
+    //discard any existing data in buffer, and to get correct reading of device buffer size
+    smSetParameter( newAxis->bushandle, newAxis->deviceAddress, SMP_SYSTEM_CONTROL,SMP_SYSTEM_CONTROL_ABORTBUFFERED);
+
+    //after abort, we can read the maximum size of data in device buffer
+    smRead1Parameter(newAxis->bushandle,newAxis->deviceAddress,SMP_BUFFER_FREE_BYTES,&newAxis->bufferLength);
+    newAxis->bufferFreeBytes=newAxis->bufferLength;
+
+    //set input smoothing filter on [CIS] if samplerate is not maximum. with filter samplerates 250,500,750,1000,1250 etc run smooth.
+    if(sampleRate<2500)
+    {
+        if(smRead1Parameter(handle,deviceAddress,SMP_DRIVE_FLAGS,&newAxis->driveFlagsBeforeInit)!=SM_OK)
+            return getCumulativeStatus(handle);//if error happens in read, dont set the flags
+
+        smSetParameter(handle,deviceAddress,SMP_DRIVE_FLAGS,newAxis->driveFlagsBeforeInit|FLAG_USE_INPUT_LP_FILTER);
+    }
+
+    //set buffer execution rate to max so init commands go fast
+    smSetParameter(handle,deviceAddress,SMP_BUFFERED_CMD_PERIOD,10000/2500);
+
+    //set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive
+    smRead1Parameter(handle,deviceAddress,SMP_TRAJ_PLANNER_ACCEL,&newAxis->driveAccelerationBeforeInit);//store original for restoration
+    smSetParameter(handle,deviceAddress,SMP_TRAJ_PLANNER_ACCEL,32767);
+
+    //set buffer execution rate
+    smSetParameter(handle,deviceAddress,SMP_BUFFERED_CMD_PERIOD,10000/sampleRate);
+
+    //FIXME can cause unnecessary initialized=false status if there was error flags in cumulative status before calling this func
+    if(getCumulativeStatus(handle)==SM_OK)
+        newAxis->initialized=smtrue;
+
+    return getCumulativeStatus(handle);
+}
+
+/** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/
+LIB SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis )
+{
+    smBufferedAbort(axis);
+
+    //restore drive in pre-init state
+    if(axis->initialized==smtrue)
+    {
+        smSetParameter(axis->bushandle,axis->deviceAddress,SMP_TRAJ_PLANNER_ACCEL,axis->driveAccelerationBeforeInit);
+        smSetParameter(axis->bushandle,axis->deviceAddress,SMP_DRIVE_FLAGS,axis->driveFlagsBeforeInit);
+    }
+
+    axis->initialized=smfalse;
+    axis->readParamInitialized=smfalse;
+
+    return getCumulativeStatus(axis->bushandle);
+}
+
+
+/* this also starts buffered motion when it's not running*/
+SM_STATUS smBufferedRunAndSyncClocks(BufferedMotionAxis *axis)
+{
+    return smGetBufferClock( axis->bushandle, axis->deviceAddress, &axis->driveClock );
+}
+
+SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numPoints )
+{
+    smint32 freebytes;
+
+    if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK)
+    {
+        *numPoints=0;//read has failed, assume 0
+        return getCumulativeStatus(axis->bushandle);
+    }
+
+    axis->bufferFreeBytes=freebytes;
+    axis->bufferFill=100*freebytes/axis->bufferLength;//calc buffer fill 0-100%
+
+    //calculate number of points that can be uploaded to buffer (max size 120 bytes and fill consumption is 2+4+2+3*(n-1) bytes)
+    if(axis->readParamInitialized==smtrue)
+        //*numPoints=(freebytes-2-4-2)/3+1;
+        *numPoints=(freebytes-2-4-2)/4+1;
+    else
+        //*numPoints=(freebytes-2-4-2 -2-2-2-2)/3+1;//if read data uninitialized, it takes extra 8 bytes to init on next fill, so reduce it here
+        *numPoints=(freebytes-2-4-2 -2-2-2-2)/4+1;//if read data uninitialized, it takes extra 8 bytes to init on next fill, so reduce it here
+
+    return getCumulativeStatus(axis->bushandle);
+}
+
+SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints )
+{
+
+    //if(freeBytesInDeviceBuffer>=cmdBufferSizeBytes)
+//        emit message(Warning,"Buffer underrun on axis "+QString::number(ax));
+
+    //freeBytesInDeviceBuffer-=8;
+//      if(drives[ax].bufferedStreamInitialized==false)
+//            cmdBufferSizeBytes=freeBytesInDeviceBuffer;//get empty buffer size
+
+    //first initialize the stream if not done yet
+    if(axis->readParamInitialized==smfalse)
+    {
+        //set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive
+        smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_ADDR);
+        smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,axis->readParamAddr);
+        smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_LEN);
+        smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,axis->readParamLength);
+
+        //next time we read return data, we discard first 4 return packets to avoid unexpected read data to user
+        axis->numberOfDiscardableReturnDataPackets+=4;
+        axis->readParamInitialized=smtrue;
+    }
+
+    if(numFillPoints>=1)//send first fill data
+    {
+        smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT);
+        smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[0]);
+        axis->numberOfPendingReadPackets++;
+    }
+
+    //send rest of data as increments
+    if(numFillPoints>=2)
+    {
+        int i;
+        smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_INCREMENTAL_SETPOINT);
+
+        for(i=1;i<numFillPoints;i++)
+        {
+            smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_INCREMENTAL_SETPOINT);
+            smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B, fillPoints[i]-fillPoints[i-1] );
+            axis->numberOfPendingReadPackets++;
+        }
+    }
+
+    //send the commands that were added with smAppendSMCommandToQueue. this also reads all return packets that are available (executed already)
+    smUploadCommandQueueToDeviceBuffer(axis->bushandle,axis->deviceAddress);
+
+
+    //read all available return data from stream (commands that have been axecuted in drive so far)
+    //return data works like FIFO for all sent commands (each sent stream command will produce return data packet that we fetch here)
+    {
+        smint32 bufferedReturnBytesReceived,readval;
+        int n=0;
+
+        //read return data buffer
+        smBytesReceived(axis->bushandle,&bufferedReturnBytesReceived);//get amount of data available
+        //qDebug()<<axis<<"return data bytes"<<bufferedReturnBytesReceived;
+        while(bufferedReturnBytesReceived>1)//loop until we have read it all
+        {
+            smGetQueuedSMCommandReturnValue(axis->bushandle, &readval);
+            smBytesReceived(axis->bushandle,&bufferedReturnBytesReceived);
+
+            if(axis->numberOfDiscardableReturnDataPackets>0)
+            {
+                //discard this return data as it's intialization return packets
+                axis->numberOfDiscardableReturnDataPackets--;
+            }
+            else//its read data that user expects
+            {
+                receivedPoints[n]=readval;
+                n++;
+            }
+        }
+        *numReceivedPoints=n;
+    }
+
+    return getCumulativeStatus(axis->bushandle);
+}
+
+/** this will stop executing buffered motion immediately and discard rest of already filled buffer on a given axis. May cause drive fault state such as tracking error if done at high speed because stop happens without deceleration.*/
+SM_STATUS smBufferedAbort(BufferedMotionAxis *axis)
+{
+    return smSetParameter( axis->bushandle, axis->deviceAddress, SMP_SYSTEM_CONTROL,SMP_SYSTEM_CONTROL_ABORTBUFFERED);
+}
+
diff --git a/bufferedmotion.h b/bufferedmotion.h
new file mode 100644
index 0000000..5de1a4a
--- /dev/null
+++ b/bufferedmotion.h
@@ -0,0 +1,55 @@
+#ifndef BUFFEREDMOTION_H
+#define BUFFEREDMOTION_H
+
+
+#include "simplemotion.h"
+
+//typedef enum _smBufferedState {BufferedStop=0,BufferedRun=1} smBufferedState;
+
+typedef struct _BufferedMotionAxis {
+    smbool initialized;
+    smbool readParamInitialized;
+    smint32 numberOfDiscardableReturnDataPackets;
+    smint32 numberOfPendingReadPackets;//number of read data packets that should be arriving from device (to read rest of pending data, use smBufferedFillAndReceive(numFillPoints=0) until this variable this goes to zero)
+    smbus bushandle;
+    smaddr deviceAddress;
+    smint32 samplerate;
+    smint16 readParamAddr;
+    smuint8 readParamLength;
+    smint32 driveFlagsBeforeInit;
+    smint32 driveAccelerationBeforeInit;
+    smuint16 driveClock;//clock counter is updated at smBufferedRunAndSyncClocks only for the one axis that is used with that func. clock is running up at 10kHz count rate, meaning that it rolls over every 6.5536 secs
+    smint32 bufferLength;//buffer lenght in bytes of the device. note this may be different in different devices types. so call smBufferedGetFree on the device that has the smallest buffer. however as of 2.2016 all GD drives have 2048 bytes buffers.
+    smint32 bufferFreeBytes;//number of bytes free in buffer, updated at smBufferedGetFree
+    smint32 bufferFill;//percentage of buffer fill, updated at smBufferedGetFree. this should stay above 50% to ensure gapless motion. if gaps occur, check SMV2USB adpater COM port latency setting (set to 1ms) or try lower samplerate.
+} BufferedMotionAxis;
+
+/** initialize buffered motion for one axis with address and samplerate (Hz) */
+/* returnDataLenght must be one of following:
+#define SM_RETURN_STATUS 3
+//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
+//consumes 2 byte from payload buffer. can contain value up to 14 bits. value greater than 14 bits is clipped (padded with 0s)
+#define SM_RETURN_VALUE_16B 2
+//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
+//consumes 3 byte from payload buffer. can contain value up to 14 bits. value greater than 22 bits is clipped (padded with 0s)
+#define SM_RETURN_VALUE_24B 1
+//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
+//consumes 4 byte from payload buffer. can contain value up to 30 bits. value greater than 14 bits is clipped (padded with 0s)
+#define SM_RETURN_VALUE_32B 0
+
+Note return data per one FillAndReceive must not exceed 120 bytes. So max allowed numFillPoints will depend on returnDataLength.
+numFillPoints must be equal or below 30 for 32B, 40 for 24B and 60 for 16B.
+*/
+LIB SM_STATUS smBufferedInit( BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength  );
+
+/** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/
+LIB SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis );
+
+/* this also starts buffered motion when it's not running*/
+LIB SM_STATUS smBufferedRunAndSyncClocks( BufferedMotionAxis *axis );
+LIB SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numPoints );
+LIB SM_STATUS smBufferedFillAndReceive( BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints );
+/** this will stop executing buffered motion immediately and discard rest of already filled buffer on a given axis. May cause drive fault state such as tracking error if done at high speed because stop happens without deceleration.*/
+LIB SM_STATUS smBufferedAbort(BufferedMotionAxis *axis);
+
+#endif // BUFFEREDMOTION_H
diff --git a/simplemotion_private.h b/simplemotion_private.h
index 1c80780..c56363e 100644
--- a/simplemotion_private.h
+++ b/simplemotion_private.h
@@ -43,6 +43,8 @@ extern FILE *smDebugOut; //such as stderr or file handle. if NULL, debug info di
 extern smuint16 readTimeoutMs;

 

 void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...);

+//accumulates status to internal variable by ORing the bits. returns same value that is fed as paramter

+SM_STATUS recordStatus( const smbus handle, const SM_STATUS stat );

 

 SM_STATUS smRawCmd( const char *axisname, smuint8 cmd, smuint16 val, smuint32 *retdata );