Fast buffered motion protocol under development (work in progress, not working at the moment)feature/FastBufferedMotionProtocol

5 files changed, 159 insertions, 21 deletions

diff --git a/bufferedmotion.c b/bufferedmotion.c
index a593069..f1bd41d 100644
--- a/bufferedmotion.c
+++ b/bufferedmotion.c
@@ -10,6 +10,7 @@ SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr devic
     if(sampleRate<1 || sampleRate>2500)
         return recordStatus(handle,SM_ERR_PARAMETER);
 
+    newAxis->mode=Standard;
     newAxis->initialized=smfalse;
     newAxis->bushandle=handle;
     newAxis->samplerate=sampleRate;
@@ -21,6 +22,7 @@ SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr devic
     newAxis->driveClock=0;
     newAxis->bufferFill=0;
     newAxis->numberOfPendingReadPackets=0;
+    newAxis->maxSingleFillSizeInBytes=SM485_MAX_PAYLOAD_BYTES;
 
     //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);
@@ -55,6 +57,51 @@ SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr devic
     return getCumulativeStatus(handle);
 }
 
+/*if other than Standard mode is used, then call this right after smBufferedInit to set the mode.
+Do not change mode on the fly.*/
+LIB SM_STATUS smBufferedSetMode( BufferedMotionAxis *newAxis, smBufferedMode mode, smaddr masterDeviceAddress, smint32 numOfBufferedAxis )
+{
+    smbus handle=newAxis->bushandle;
+    newAxis->mode=mode;
+    newAxis->masterDeviceAddress=masterDeviceAddress;//master device address (must be the lowest number of buffered devices, i.e. if buffered axis are 4, 5, 6, 7 then 4 must be set master)
+    newAxis->numOfBufferedAxis=numOfBufferedAxis;//total number of axis used in buffered mode
+
+    if(mode==Standard)
+    {
+        newAxis->maxSingleFillSizeInBytes=SM485_MAX_PAYLOAD_BYTES;
+    }
+    else if (mode==Fast)
+    {
+        newAxis->maxSingleFillSizeInBytes=(SM485_MAX_PAYLOAD_BYTES-4)/numOfBufferedAxis;
+
+        //INIT INSTANT COMMAND
+        //setup return data. this sets what will be returned from smBufferedFillAndReceiveFast
+        smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN ); //2b
+        smAppendSMCommandToQueue( handle, SMPCMD_24B, newAxis->readParamLength );//3b
+        smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );//2b
+        smAppendSMCommandToQueue( handle, SMPCMD_24B, newAxis->readParamAddr );//3b
+
+        //INIT BUFFERED COMMAND
+        //set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive
+        smAppendSMCommandToQueue( handle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_ADDR);
+        smAppendSMCommandToQueue( handle,SM_WRITE_VALUE_24B,newAxis->readParamAddr);
+        smAppendSMCommandToQueue( handle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_LEN);
+        smAppendSMCommandToQueue( handle,SM_WRITE_VALUE_24B,newAxis->readParamLength);
+        smAppendSMCommandToQueue( handle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT);
+        newAxis->readParamInitialized=smtrue;
+        //send the commands that were added with smAppendSMCommandToQueue. this also reads all return packets that are available (executed already)
+        smUploadCommandQueueToDeviceBuffer(newAxis->bushandle,newAxis->deviceAddress);
+        //don't care to handle any return data as it's just non important return values of previous commands
+    }
+    else//unkown mode
+    {
+        recordStatus( newAxis->bushandle, SM_ERR_MODE );
+    }
+
+    return getCumulativeStatus(newAxis->bushandle);
+}
+
+
 /** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/
 SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis )
 {
@@ -78,31 +125,42 @@ SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis )
 SM_STATUS smBufferedRunAndSyncClocks(BufferedMotionAxis *axis)
 {
     return smGetBufferClock( axis->bushandle, axis->deviceAddress, &axis->driveClock );
+    //TODO fast mode does not need this called repeatedly
 }
 
+/*Ooperation depeonds on mode. In normal mode, it sends question to actual device to read available memory, but in fast mode it just returns value that has been received during the last smBufferedFillAndReceive*/
 SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree )
 {
     smint32 freebytes;
 
-    if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK)
+    if(axis->mode==Standard)
     {
-        *numBytesFree=0;//read has failed, assume 0
-        return getCumulativeStatus(axis->bushandle);
-    }
+        if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK)
+        {
+            *numBytesFree=0;//read has failed, assume 0
+            return getCumulativeStatus(axis->bushandle);
+        }
 
-    axis->bufferFreeBytes=freebytes;
-    axis->bufferFill=100*(axis->bufferLength-freebytes)/axis->bufferLength;//calc buffer fill 0-100%
+        axis->bufferFreeBytes=freebytes;
+        axis->bufferFill=100*(axis->bufferLength-freebytes)/axis->bufferLength;//calc buffer fill 0-100%
 
-    *numBytesFree=freebytes;
+        *numBytesFree=freebytes;
 
-    return getCumulativeStatus(axis->bushandle);
+        return getCumulativeStatus(axis->bushandle);
+    }
+    else if(axis->mode==Fast)
+    {
+        //in fast mode, just return last value that was obtained from SM fast buffered command
+        *numBytesFree=axis->bufferFreeBytes;
+        axis->bufferFill=100*(axis->bufferLength-axis->bufferFreeBytes)/axis->bufferLength;//calc buffer fill 0-100%
+    }
 }
 
 smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 numBytesFree )
 {
-    //even if we have lots of free space in buffer, we can only send up to SM485_MAX_PAYLOAD_BYTES bytes at once in one SM transmission
-    if(numBytesFree>SM485_MAX_PAYLOAD_BYTES)
-        numBytesFree=SM485_MAX_PAYLOAD_BYTES;
+    //even if we have lots of free space in buffer, we can only send up to limited amount of bytes at once in one SM transmission
+    if(numBytesFree > axis->maxSingleFillSizeInBytes)
+        numBytesFree=axis->maxSingleFillSizeInBytes;
 
     //calculate number of points that can be uploaded to buffer (max size SM485_MAX_PAYLOAD_BYTES bytes and fill consumption is 2+4+2+3*(n-1) bytes)
     if(axis->readParamInitialized==smtrue)
@@ -127,6 +185,12 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
 {
     smint32 bytesUsed=0;
 
+    if(axis->mode!=Standard)
+    {
+        recordStatus( axis->bushandle, SM_ERR_MODE );
+        return getCumulativeStatus(axis->bushandle);
+    }
+
     //if(freeBytesInDeviceBuffer>=cmdBufferSizeBytes)
 //        emit message(Warning,"Buffer underrun on axis "+QString::number(ax));
 
@@ -157,7 +221,7 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
         axis->numberOfPendingReadPackets+=1;
     }
 
-    //send rest of data as increments
+    //send rest of data as increments. actually not in this code, we just send more 32 bit absolute setpoints to during development
     if(numFillPoints>=2)
     {
         int i;
@@ -210,6 +274,49 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
     return getCumulativeStatus(axis->bushandle);
 }
 
+/*this uploads setpoints of multimple axes at single call. drive that that this command is targetter must be cycled between ann devices if one
+ *wishes to read any data from it (because this call will send command to one device address and that address only is able to talk back. i.e. send return data to host)*/
+LIB SM_STATUS smBufferedFillAndReceiveFast( BufferedMotionAxis *masterAxis, BufferedMotionAxis *axis, smint32 numFillPointsPerAxis, smint32 **fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled )
+{
+    smint32 bytesUsed=0;
+
+    if(axis->mode!=Fast)
+    {
+        recordStatus( axis->bushandle, SM_ERR_MODE );
+        return getCumulativeStatus(axis->bushandle);
+    }
+
+    int i,a;
+    for(a=0;a<masterAxis->numOfBufferedAxis;a++)
+    {
+        for(i=0;i<numFillPointsPerAxis;i++)
+        {
+            smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[i]);
+            bytesUsed+=4;
+            axis->numberOfPendingReadPackets++;
+        }
+    }
+
+    smuint16 clock, buffree;
+
+    //send the commands that were added with smAppendSMCommandToQueue. this also reads all return packets that are available (executed already)
+    smTransmitAndReceiveFastBufferedCommand(axis->bushandle,axis->deviceAddress, &clock, &buffree);
+
+    axis->bufferFreeBytes=buffree;
+    masterAxis->bufferFreeBytes=buffree;
+    axis->driveClock=clock;
+
+    //grab the return data from the previous SM command
+    smint32 readval;
+    smGetQueuedSMCommandReturnValue(axis->bushandle, &readval);
+    receivedPoints[0]=readval;
+    *numReceivedPoints=1;
+
+    *bytesFilled=bytesUsed;
+    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)
 {
diff --git a/bufferedmotion.h b/bufferedmotion.h
index bbd646c..9961d93 100644
--- a/bufferedmotion.h
+++ b/bufferedmotion.h
@@ -6,11 +6,15 @@ extern "C"{
 #endif
 
 #include "simplemotion.h"
+#include "simplemotion_private.h"
 
 //typedef enum _smBufferedState {BufferedStop=0,BufferedRun=1} smBufferedState;
 
+typedef enum _smBufferedMode {Standard=0,Fast=1} smBufferedMode;
+
 typedef struct _BufferedMotionAxis {
-    smbool initialized;
+    smBufferedMode mode;
+    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)
@@ -25,6 +29,10 @@ typedef struct _BufferedMotionAxis {
     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.
+    smint32 maxSingleFillSizeInBytes;//depends on mode (how many bytes we can send to device at one cycle)
+    //params that are needed in Fast mode only
+    smint32 masterDeviceAddress;//master device address (must be the lowest number of buffered devices, i.e. if buffered axis are 4, 5, 6, 7 then 4 must be set master)
+    smint32 numOfBufferedAxis;//total number of axis used in buffered mode
 } BufferedMotionAxis;
 
 /** initialize buffered motion for one axis with address and samplerate (Hz) */
@@ -43,7 +51,11 @@ typedef struct _BufferedMotionAxis {
 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  );
+LIB SM_STATUS smBufferedInit( BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength );
+
+/*if other than Standard mode is used, then call this right after smBufferedInit to set the mode.
+Do not change mode on the fly.*/
+LIB SM_STATUS smBufferedSetMode( BufferedMotionAxis *newAxis, smBufferedMode mode, smaddr masterDeviceAddress, smint32 numOfBufferedAxis );
 
 /** 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 );
@@ -54,6 +66,11 @@ LIB SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree
 LIB smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 numBytesFree );
 LIB smint32 smBufferedGetBytesConsumed(BufferedMotionAxis *axis, smint32 numFillPoints );
 LIB SM_STATUS smBufferedFillAndReceive( BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled );
+
+/*this uploads setpoints of multimple axes at single call. drive that that this command is targetter must be cycled between ann devices if one
+ *wishes to read any data from it (because this call will send command to one device address and that address only is able to talk back. i.e. send return data to host)*/
+LIB SM_STATUS smBufferedFillAndReceiveFast( BufferedMotionAxis *masterAxis, BufferedMotionAxis *axis, smint32 numFillPointsPerAxis, smint32 **fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled );
+
 /** 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.
 Note: this will not stop motion, but just stop executing the sent buffered commands. The last executed motion point will be still followed by drive. So this is bad function
 for quick stopping stopping, for stop to the actual place consider using disable drive instead (prefferably phsyical input disable).
diff --git a/simplemotion.c b/simplemotion.c
index cea0238..3d12322 100644
--- a/simplemotion.c
+++ b/simplemotion.c
@@ -256,6 +256,9 @@ char *cmdidToStr(smuint8 cmdid )
     case SMCMD_GET_CLOCK_RET :str="SMCMD_GET_CLOCK_RET";break;

     case SMCMD_FAST_UPDATE_CYCLE:str="SMCMD_FAST_UPDATE_CYCLE";break;

     case SMCMD_FAST_UPDATE_CYCLE_RET:str="SMCMD_FAST_UPDATE_CYCLE_RET";break;

+    case SMCMD_FAST_BUFFERED_CMD:str="SMCMD_FAST_BUFFERED_CMD";break;

+    case SMCMD_FAST_BUFFERED_CMD_RET:str="SMCMD_FAST_BUFFERED_CMD_RET";break;

+    case SMCMD_FAST_BUFFERED_CMD_RET_SHORT:str="SMCMD_FAST_BUFFERED_CMD_RET_SHORT";break;

     default: str="unknown cmdid";break;

     }

     //puts(str);

@@ -418,7 +421,6 @@ SM_STATUS smReceiveErrorHandler( smbus handle, smbool flushrx )
 

 SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType,smint32 paramvalue  )

 {

-    smuint8 txbyte;

     int cmdlength;

 

     //check if bus handle is valid & opened

@@ -452,12 +454,6 @@ SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType,smint32 paramva
         SMPayloadCommand16 newcmd;

         newcmd.ID=SMPCMD_SETPARAMADDR;

         newcmd.param=paramvalue;

-

-        //          bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes, 5);

-        //            bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes, 6);

-        /*

-FIXME

-            ei toimi, menee vaa nollaa*/

         bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);

         bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);

     }

@@ -945,3 +941,11 @@ void resetCumulativeStatus( const smbus handle )
     smBus[handle].cumulativeSmStatus=0;

 }

 

+//fast method for buffered motion stream, this provides limited return data and feeds all drives simultaneously

+SM_STATUS smTransmitAndReceiveFastBufferedCommand( const smbus handle, const smaddr nodeAddress, smint16 &clockOut, smint16 &bufferfreeOut )

+{

+

+

+

+}

+

diff --git a/simplemotion.h b/simplemotion.h
index 056c745..7f5b64a 100644
--- a/simplemotion.h
+++ b/simplemotion.h
@@ -43,6 +43,7 @@ extern "C"{
 #define SM_ERR_COMMUNICATION 8

 #define SM_ERR_PARAMETER 16

 #define SM_ERR_LENGTH 32

+#define SM_ERR_MODE 64

 

 ///////////////////////////////////////////////////////////////////////////////////////

 //TYPES & VALUES //////////////////////////////////////////////////////////////////////

@@ -131,6 +132,9 @@ LIB SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, c
 LIB SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 );

 LIB SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal );

 

+//fast method for buffered motion stream, this provides limited return data and feeds all drives simultaneously

+LIB SM_STATUS smTransmitAndReceiveFastBufferedCommand( const smbus handle, const smaddr nodeAddress, smint16 &clockOut, smint16 &bufferfreeOut );

+

 

 LIB SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock );

 

diff --git a/sm485.h b/sm485.h
index 2d79815..93b7bbe 100644
--- a/sm485.h
+++ b/sm485.h
@@ -46,6 +46,11 @@
 #define SMCMD_BUFFERED_RETURN_DATA SMCMD_ID(7,SMCMD_MASK_0_PARAMS)
 #define SMCMD_BUFFERED_RETURN_DATA_RET SMCMD_ID(7,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
 
+//fast buffered commands
+#define SMCMD_FAST_BUFFERED_CMD SMCMD_ID(8,SMCMD_MASK_N_PARAMS)
+#define SMCMD_FAST_BUFFERED_CMD_RET SMCMD_ID(8,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
+#define SMCMD_FAST_BUFFERED_CMD_RET_SHORT SMCMD_ID(9,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
+
 //cmd 20,u8 toaddr, u8 crc
 //ret 21, u8=2, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n
 //oldret 21, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n
@@ -60,6 +65,7 @@
 #define SMCMD_FAST_UPDATE_CYCLE_RET SMCMD_ID(2,SMCMD_MASK_0_PARAMS|SMCMD_MASK_RETURN)
 
 
+
 #ifdef PROCESS_IMAGE_SUPPORT
 
 //PROCESS_IMAGE communication not supported by SM V2.0.0. placeholders here