Corrections made to buffered motion code. Still work in progress.

2 files changed, 61 insertions, 22 deletions

diff --git a/bufferedmotion.c b/bufferedmotion.c
index 992ccca..a593069 100644
--- a/bufferedmotion.c
+++ b/bufferedmotion.c
@@ -1,9 +1,10 @@
 #include "simplemotion.h"
 #include "simplemotion_private.h"
 #include "bufferedmotion.h"
+#include "sm485.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 )
+SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength )
 {
     //value out of range
     if(sampleRate<1 || sampleRate>2500)
@@ -55,7 +56,7 @@ LIB SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr d
 }
 
 /** 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 )
+SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis )
 {
     smBufferedAbort(axis);
 
@@ -79,32 +80,52 @@ SM_STATUS smBufferedRunAndSyncClocks(BufferedMotionAxis *axis)
     return smGetBufferClock( axis->bushandle, axis->deviceAddress, &axis->driveClock );
 }
 
-SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numPoints )
+SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree )
 {
     smint32 freebytes;
 
     if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK)
     {
-        *numPoints=0;//read has failed, assume 0
+        *numBytesFree=0;//read has failed, assume 0
         return getCumulativeStatus(axis->bushandle);
     }
 
     axis->bufferFreeBytes=freebytes;
-    axis->bufferFill=100*freebytes/axis->bufferLength;//calc buffer fill 0-100%
+    axis->bufferFill=100*(axis->bufferLength-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)
+    *numBytesFree=freebytes;
+
+    return getCumulativeStatus(axis->bushandle);
+}
+
+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;
+
+    //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)
         //*numPoints=(freebytes-2-4-2)/3+1;
-        *numPoints=(freebytes-2-4-2)/4+1;
+        return numBytesFree/4;
     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 (numBytesFree-2-3-2-3-2)/4;//if read data uninitialized, it takes extra n bytes to init on next fill, so reduce it here
+}
 
-    return getCumulativeStatus(axis->bushandle);
+smint32 smBufferedGetBytesConsumed(BufferedMotionAxis *axis, smint32 numFillPoints )
+{
+    //calculate number of bytes that the number of fill points will consume from buffer
+    if(axis->readParamInitialized==smtrue)
+        return numFillPoints*4;
+    else
+        return numFillPoints*4 +2+3+2+3+2;//if read data uninitialized, it takes extra n bytes to init on next fill, so reduce it here
 }
 
-SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints )
+
+SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled )
 {
+    smint32 bytesUsed=0;
 
     //if(freeBytesInDeviceBuffer>=cmdBufferSizeBytes)
 //        emit message(Warning,"Buffer underrun on axis "+QString::number(ax));
@@ -118,32 +139,37 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
     {
         //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,SM_WRITE_VALUE_24B,axis->readParamAddr);
         smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_LEN);
-        smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,axis->readParamLength);
+        smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B,axis->readParamLength);
+        smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT);
+        bytesUsed+=2+3+2+3+2;
 
         //next time we read return data, we discard first 4 return packets to avoid unexpected read data to user
-        axis->numberOfDiscardableReturnDataPackets+=4;
+        axis->numberOfDiscardableReturnDataPackets+=5;
         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++;
+        bytesUsed+=4;
+        axis->numberOfPendingReadPackets+=1;
     }
 
     //send rest of data as increments
     if(numFillPoints>=2)
     {
         int i;
-        smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_INCREMENTAL_SETPOINT);
+        //smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT);
+        //smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_INCREMENTAL_SETPOINT);
+        //axis->numberOfPendingReadPackets++;//FIXME ei toimi ihan oikein tää koska skippaa äskeisen writevaluen
 
         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] );
+            //smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B, fillPoints[i]-fillPoints[i-1] );
+            smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[i]);
+            bytesUsed+=4;
             axis->numberOfPendingReadPackets++;
         }
     }
@@ -151,7 +177,6 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
     //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)
     {
@@ -175,11 +200,13 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
             {
                 receivedPoints[n]=readval;
                 n++;
+                axis->numberOfPendingReadPackets--;
             }
         }
         *numReceivedPoints=n;
     }
 
+    *bytesFilled=bytesUsed;
     return getCumulativeStatus(axis->bushandle);
 }
 
diff --git a/bufferedmotion.h b/bufferedmotion.h
index 5de1a4a..bbd646c 100644
--- a/bufferedmotion.h
+++ b/bufferedmotion.h
@@ -1,6 +1,9 @@
 #ifndef BUFFEREDMOTION_H
 #define BUFFEREDMOTION_H
 
+#ifdef __cplusplus
+extern "C"{
+#endif
 
 #include "simplemotion.h"
 
@@ -47,9 +50,18 @@ 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 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 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).
+*/
 LIB SM_STATUS smBufferedAbort(BufferedMotionAxis *axis);
 
+
+#ifdef __cplusplus
+}
+#endif
 #endif // BUFFEREDMOTION_H