From 3aba0a728b64908ee9caeaf751ca37fc0defa443 Mon Sep 17 00:00:00 2001 From: Tero K Date: Sat, 6 Feb 2016 22:59:27 +0200 Subject: Started development of buffered motion API. Compiles but not tested. --- bufferedmotion.c | 191 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100644 bufferedmotion.c (limited to 'bufferedmotion.c') 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;ibushandle,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()<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); +} + -- cgit v1.2.3 From 81a6bba3d855f77b7185b99f77250d8572127d75 Mon Sep 17 00:00:00 2001 From: Tero K Date: Mon, 8 Feb 2016 13:23:55 +0200 Subject: Corrections made to buffered motion code. Still work in progress. --- bufferedmotion.c | 65 +++++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 46 insertions(+), 19 deletions(-) (limited to 'bufferedmotion.c') 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;ibushandle,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); } -- cgit v1.2.3