diff options
Diffstat (limited to 'bufferedmotion.c')
| -rw-r--r-- | bufferedmotion.c | 191 |
1 files changed, 191 insertions, 0 deletions
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); +} + |