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. --- SimpleMotionV2.pri | 6 +- bufferedmotion.c | 191 +++++++++++++++++++++++++++++++++++++++++++++++++ bufferedmotion.h | 55 ++++++++++++++ simplemotion_private.h | 2 + 4 files changed, 252 insertions(+), 2 deletions(-) create mode 100644 bufferedmotion.c create mode 100644 bufferedmotion.h 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;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); +} + 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 ); -- 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 +++++++++++++++++++++++++++++++++++++++----------------- bufferedmotion.h | 18 +++++++++++++--- 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;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); } 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 -- cgit v1.2.3 From f5eec0c446ebe7952667143dc98f3be94c2a6186 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Sun, 29 Jan 2017 17:00:51 +0200 Subject: New commutation sensor parameters --- simplemotion_defs.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index ec3e2f7..026e538 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -352,6 +352,8 @@ #define SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA 1024 //stores encoder index position in SMP_DEBUGPARAM_1. while busy (index not found) SMP_DEBUGPARAM_2 will be 100, after found it is 200. #define SMP_SYSTEM_CONTROL_CAPTURE_INDEX_POSITION 2048 + //start a procedure to automatically configure hall sensors direction & offset, or other absolute sensor capable of commutation + #define SMP_SYSTEM_CONTROL_START_COMMUTATION_SENSOR_AUTOSETUP 4096 //write SM bus SM_CRCINIT constant modifier. special purposes only, don't use if unsure because //it is one time programmable variable (permanently irreversible operation, can't be ever reset to default by provided methods) #define SMP_SYSTEM_CONTROL_MODIFY_CRCINIT 262144 @@ -429,7 +431,7 @@ #define FLAG_ALLOW_VOLTAGE_CLIPPING BV(10) #define FLAG_USE_INPUT_LP_FILTER BV(11) #define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0/*obsolete*/ - #define FLAG_INVERTED_HALLS BV(13) + #define FLAG_INVERT_COMMUTATION_SENSOR_DIRECTION BV(13) /*inverts direction of commutation sensor instead, used together with SMP_COMMUTATION_SENSOR_OFFSET*//*originally was: invert hall sensors digital states*/ #define FLAG_USE_HALLS BV(14) #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) @@ -539,7 +541,9 @@ #define SMP_PHASESEARCH_VOLTAGE_SLOPE 480 //by default this is calculated from other motor params: #define SMP_PHASESEARCH_CURRENT 481 -//selector value 0-9 = 100 - 3300Hz (see Granity): +//commutation angle calibration, i.e. for hall sensors or absolute encoder. can be set with SMP_SYSTEM_CONTROL_START_COMMUTATION_SENSOR_AUTOSET +#define SMP_COMMUTATION_SENSOR_OFFSET 482 +//low pass filter selector, value 0=100Hz, 9=3300Hz, 10=4700Hz, 11=unlimited (see Granity for all options): #define SMP_TORQUE_LPF_BANDWIDTH 490 //motor rev to rotary/linear unit scale. like 5mm/rev or 0.1rev/rev. 30bit parameter & scale 100000=1.0 -- cgit v1.2.3 From ba6b2a32db48ccd6decfcdb3c42c92691915b32d Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Mon, 30 Jan 2017 01:01:52 +0200 Subject: Rename FLAG_USE_HALLS to FLAG_USE_COMMUTATION_SENSOR (backwards compatible change) --- simplemotion_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 026e538..0b1628b 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -432,7 +432,7 @@ #define FLAG_USE_INPUT_LP_FILTER BV(11) #define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0/*obsolete*/ #define FLAG_INVERT_COMMUTATION_SENSOR_DIRECTION BV(13) /*inverts direction of commutation sensor instead, used together with SMP_COMMUTATION_SENSOR_OFFSET*//*originally was: invert hall sensors digital states*/ - #define FLAG_USE_HALLS BV(14) + #define FLAG_USE_COMMUTATION_SENSOR BV(14) #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) #define SMP_MOTION_FAULT_THRESHOLD 568 -- cgit v1.2.3 From fdec7c0e027b3e9f4c7fd977ee846291bb86617d Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Tue, 31 Jan 2017 21:52:16 +0200 Subject: Revert "Rename FLAG_USE_HALLS to FLAG_USE_COMMUTATION_SENSOR (backwards compatible change)" This reverts commit ba6b2a32db48ccd6decfcdb3c42c92691915b32d. --- simplemotion_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 0b1628b..026e538 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -432,7 +432,7 @@ #define FLAG_USE_INPUT_LP_FILTER BV(11) #define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0/*obsolete*/ #define FLAG_INVERT_COMMUTATION_SENSOR_DIRECTION BV(13) /*inverts direction of commutation sensor instead, used together with SMP_COMMUTATION_SENSOR_OFFSET*//*originally was: invert hall sensors digital states*/ - #define FLAG_USE_COMMUTATION_SENSOR BV(14) + #define FLAG_USE_HALLS BV(14) #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) #define SMP_MOTION_FAULT_THRESHOLD 568 -- cgit v1.2.3 From 991845467391138bbb1200d76fc7a97f5a2f4cd5 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Tue, 31 Jan 2017 23:13:57 +0200 Subject: Reverse some changes: keep original halls sensor flags intact, and instead use SMP_COMMUTATION_SENSOR_CONFIG for new FW and ignore hall sensor flags (these flags becoming obsolete) --- simplemotion_defs.h | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 026e538..51e4123 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -431,8 +431,8 @@ #define FLAG_ALLOW_VOLTAGE_CLIPPING BV(10) #define FLAG_USE_INPUT_LP_FILTER BV(11) #define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0/*obsolete*/ - #define FLAG_INVERT_COMMUTATION_SENSOR_DIRECTION BV(13) /*inverts direction of commutation sensor instead, used together with SMP_COMMUTATION_SENSOR_OFFSET*//*originally was: invert hall sensors digital states*/ - #define FLAG_USE_HALLS BV(14) + //#define FLAG_INVERTED_HALLS BV(13) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ + //#define FLAG_USE_HALLS BV(14) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) #define SMP_MOTION_FAULT_THRESHOLD 568 @@ -541,8 +541,17 @@ #define SMP_PHASESEARCH_VOLTAGE_SLOPE 480 //by default this is calculated from other motor params: #define SMP_PHASESEARCH_CURRENT 481 -//commutation angle calibration, i.e. for hall sensors or absolute encoder. can be set with SMP_SYSTEM_CONTROL_START_COMMUTATION_SENSOR_AUTOSET -#define SMP_COMMUTATION_SENSOR_OFFSET 482 +/* Commutation angle congiuration, i.e. for hall sensors or absolute encoder. can be automatically set with SMP_SYSTEM_CONTROL_START_COMMUTATION_SENSOR_AUTOSET. + * Format: + * bits 0-15 LSB: commutation sensor offset 0-65535 represents commutation angle offset 0-360 electical degrees + * bit 16: invert sensor count direction + * bit 17: enable commutation sensor + * bits 18-31: reserved, always 0 + */ +#define SMP_COMMUTATION_SENSOR_CONFIG 482 + #define SMP_COMMUTATION_SENSOR_CONFIG_ANGLE_MASK 0xFFFF + #define SMP_COMMUTATION_SENSOR_CONFIG_INVERT_MASK 0x10000 + #define SMP_COMMUTATION_SENSOR_CONFIG_ENABLE_MASK 0x20000 //low pass filter selector, value 0=100Hz, 9=3300Hz, 10=4700Hz, 11=unlimited (see Granity for all options): #define SMP_TORQUE_LPF_BANDWIDTH 490 -- cgit v1.2.3 From ed98f0292d7510141fdb754c249719f11efba3d9 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Wed, 1 Feb 2017 14:03:08 +0200 Subject: Restore mistakenly commented-out definitions --- simplemotion_defs.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 51e4123..0a4e779 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -431,8 +431,8 @@ #define FLAG_ALLOW_VOLTAGE_CLIPPING BV(10) #define FLAG_USE_INPUT_LP_FILTER BV(11) #define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0/*obsolete*/ - //#define FLAG_INVERTED_HALLS BV(13) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ - //#define FLAG_USE_HALLS BV(14) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ + #define FLAG_INVERTED_HALLS BV(13) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ + #define FLAG_USE_HALLS BV(14) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) #define SMP_MOTION_FAULT_THRESHOLD 568 -- cgit v1.2.3 From 1e03af77d1e1a04de7084fa2f3bd90a86d150df9 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 2 Feb 2017 15:24:14 +0200 Subject: Fix smDebug calls (swapped bushandle & verbosity level arguments when incorrect) --- simplemotion.c | 44 ++++++++++++++++++++++---------------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/simplemotion.c b/simplemotion.c index cdee1d1..b9ef9c0 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -225,25 +225,25 @@ smbus smOpenBus( const char * devicename ) return handle; } -/** Change baudrate of SM communication port. This does not affect already opened ports but the next smOpenBus will be opened at the new speed. - Calling this is optional. By default SM bus and all slave devices operates at 460800 BPS speed. - Parameters: - -bps: bus speed in bits per second. for possible choices, see rs232.c (but note that all speeds are not necessarily supported by SM devices) - Typical usage is: - - first call smSetParameter(handle,0,SMP_BUS_SPEED,N) to change speed of all connected slaves to N PBS - - then close port with smCloseBus - - then call smSetBaudrate(N) - - then open bus again with smOpenBus - - Note that in upcoming SM device firmware versions, bitrate will be reset to default (460800) if device side SM bus watchdog timer has been enabled, and it timeouts. - This allows re-establishing connection at defautl speed if connection breaks up and SM bus watchdog timeout gets exceeded. To identify is device supports this, - read parameter SMP_SM_VERSION. Values above 25 support this feature. Value 25 and below will not reset baudrate. - - Note also that SMP_BUS_SPEED will not be saved in device flash memory - it will reset to default at every reset & power on. - */ +/** Change baudrate of SM communication port. This does not affect already opened ports but the next smOpenBus will be opened at the new speed. + Calling this is optional. By default SM bus and all slave devices operates at 460800 BPS speed. + Parameters: + -bps: bus speed in bits per second. for possible choices, see rs232.c (but note that all speeds are not necessarily supported by SM devices) + Typical usage is: + - first call smSetParameter(handle,0,SMP_BUS_SPEED,N) to change speed of all connected slaves to N PBS + - then close port with smCloseBus + - then call smSetBaudrate(N) + - then open bus again with smOpenBus + + Note that in upcoming SM device firmware versions, bitrate will be reset to default (460800) if device side SM bus watchdog timer has been enabled, and it timeouts. + This allows re-establishing connection at defautl speed if connection breaks up and SM bus watchdog timeout gets exceeded. To identify is device supports this, + read parameter SMP_SM_VERSION. Values above 25 support this feature. Value 25 and below will not reset baudrate. + + Note also that SMP_BUS_SPEED will not be saved in device flash memory - it will reset to default at every reset & power on. + */ LIB void smSetBaudrate( unsigned long pbs ) { - SMBusBaudrate=pbs; + SMBusBaudrate=pbs; } /** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening. @@ -582,7 +582,7 @@ SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retV if(smBus[bushandle].cmd_recv_queue_bytes>=smBus[bushandle].recv_payloadsize) { - smDebug(Trace,bushandle,"Packet receive error, return data coudn't be parsed\n"); + smDebug(bushandle,Trace, "Packet receive error, return data coudn't be parsed\n"); //return 0 if(retValue!=NULL) *retValue=0;//check every time if retValue is set NULL by caller -> don't store anything to it if its NULL @@ -603,7 +603,7 @@ SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retV smuint8 *readBuf=(smuint8*)&read; readBuf[1]=rxbyte; readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++); - smDebug(Trace,bushandle,"RET16B: %d\n",read.retData); + smDebug(bushandle,Trace,"RET16B: %d\n",read.retData); if(retValue!=NULL) *retValue=read.retData; return recordStatus(bushandle,SM_OK); @@ -616,7 +616,7 @@ SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retV readBuf[2]=rxbyte; readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++); readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++); - smDebug(Trace,bushandle,"RET24B: %d\n",read.retData); + smDebug(bushandle,Trace,"RET24B: %d\n",read.retData); if(retValue!=NULL) *retValue=read.retData; return recordStatus(bushandle,SM_OK); @@ -630,7 +630,7 @@ SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retV readBuf[2]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++); readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++); readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++); - smDebug(Trace,bushandle,"RET32B: %d\n",read.retData); + smDebug(bushandle,Trace,"RET32B: %d\n",read.retData); if(retValue!=NULL) *retValue=read.retData; return recordStatus(bushandle,SM_OK); @@ -641,7 +641,7 @@ SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retV SMPayloadCommandRet8 read; smuint8 *readBuf=(smuint8*)&read; readBuf[0]=rxbyte; - smDebug(Trace,bushandle,"RET_OTHER: %d\n",read.retData); + smDebug(bushandle,Trace,"RET_OTHER: %d\n",read.retData); if(retValue!=NULL) *retValue=read.retData; return recordStatus(bushandle,SM_OK); -- cgit v1.2.3 From 84e88b1de92b1d4edf3d093ca1a66054d7d85f1e Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Wed, 8 Feb 2017 14:12:19 +0200 Subject: Add SSI encoder definitions --- simplemotion_defs.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index ec3e2f7..2c7a10a 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -446,10 +446,11 @@ #define EL_MODE_SIMUCUBE 2 #define EL_MODE_IONIZER 3 -/*for BiSS encoder - * bits defined as: - * lowest 8 bits: single turn bits, value range 4-24 - * next 8 bits: multi turn bits, value range 0-16 +/*for BiSS/SSI encoder + * bits defined as (from LSB): + * bits 0-7: single turn bits, accepted value range 4-24 + * bits 8-15: multi turn bits, accepted value range 0-16 + * bits 16-17: SSI encoder mode: 00=SSI 01=SPI (SSI with CS) 10=AMS SSI (SSI with CS + with error bit monitoring & parity check) * rest: reserved for future use (always 0) */ #define SMP_SERIAL_ENC_BITS 574 -- cgit v1.2.3 From 7442165d48b012efcfd8301e805f0f17ad172276 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Wed, 8 Feb 2017 19:50:14 +0200 Subject: Changed license to Apache license 2.0 --- LICENSE | 542 ++++++++++++++++++------------------------------- simplemotion.c | 11 - simplemotion.h | 11 - simplemotion_private.h | 10 - 4 files changed, 203 insertions(+), 371 deletions(-) diff --git a/LICENSE b/LICENSE index ea5a3f2..f4f87bd 100644 --- a/LICENSE +++ b/LICENSE @@ -1,339 +1,203 @@ -GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Lesser General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - SimpleMotion V2 library - Copyright (C) 2013 GraniteDevices - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this program; if not, write to the Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) year name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - {signature of Ty Coon}, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + \ No newline at end of file diff --git a/simplemotion.c b/simplemotion.c index b9ef9c0..78059ab 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -1,16 +1,5 @@ //Copyright (c) Granite Devices Oy -/* - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; version 2 of the License. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. -*/ - #include #include diff --git a/simplemotion.h b/simplemotion.h index 913a595..965512f 100644 --- a/simplemotion.h +++ b/simplemotion.h @@ -1,17 +1,6 @@ //Global SimpleMotion functions & definitions //Copyright (c) Granite Devices Oy -/* - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; version 2 of the License. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. -*/ - #ifndef SIMPLEMOTION_H #define SIMPLEMOTION_H diff --git a/simplemotion_private.h b/simplemotion_private.h index e0f70f3..94cc8d1 100644 --- a/simplemotion_private.h +++ b/simplemotion_private.h @@ -1,16 +1,6 @@ //Internal functions & definitions, not for library user //Copyright (c) Granite Devices Oy -/* - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; version 2 of the License. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. -*/ #ifndef SIMPLEMOTION_PRIVATE_H #define SIMPLEMOTION_PRIVATE_H -- cgit v1.2.3 From 77972a62d6d04b3a8b1ad2def67fef06a48b4da0 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Mon, 20 Feb 2017 22:06:14 +0200 Subject: Redefined SMP_SERIAL_ENC_BITS specs --- simplemotion_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 2c7a10a..f85b40f 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -450,7 +450,7 @@ * bits defined as (from LSB): * bits 0-7: single turn bits, accepted value range 4-24 * bits 8-15: multi turn bits, accepted value range 0-16 - * bits 16-17: SSI encoder mode: 00=SSI 01=SPI (SSI with CS) 10=AMS SSI (SSI with CS + with error bit monitoring & parity check) + * bits 16-18: serial encoder mode: 000=BiSS, 001=SSI, 010=AMS SSI (SSI+CS+error monitoring), 011=SPI (SSI+CS) 100=GRAY SSI (i.e. SICK TTK70) * rest: reserved for future use (always 0) */ #define SMP_SERIAL_ENC_BITS 574 -- cgit v1.2.3 From d303b5967b4351af84e6b7faf46970b5c539ec01 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 23 Feb 2017 01:05:32 +0200 Subject: Got rid of various compiler warnings --- busdevice.h | 2 +- simplemotion.c | 6 +++--- simplemotion.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/busdevice.h b/busdevice.h index 580cda8..6a02a26 100644 --- a/busdevice.h +++ b/busdevice.h @@ -7,7 +7,7 @@ #include "simplemotion.h" #include "simplemotion_private.h" -typedef smint8 smbusdevicehandle; +typedef smint16 smbusdevicehandle; #define SM_BAUDRATE 460800 diff --git a/simplemotion.c b/simplemotion.c index 78059ab..4ef9b00 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -308,7 +308,6 @@ smbool smTransmitBuffer( const smbus handle ) SM_STATUS smSendSMCMD( smbus handle, smuint8 cmdid, smuint8 addr, smuint8 datalen, smuint8 *cmddata ) { int i; - smuint8 data; smuint16 sendcrc; //check if bus handle is valid & opened @@ -431,7 +430,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 @@ -951,10 +949,12 @@ SM_STATUS getCumulativeStatus( const smbus handle ) } /** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/ -void resetCumulativeStatus( const smbus handle ) +SM_STATUS resetCumulativeStatus( const smbus handle ) { if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE; smBus[handle].cumulativeSmStatus=0; + + return SM_OK; } diff --git a/simplemotion.h b/simplemotion.h index 965512f..725d3dc 100644 --- a/simplemotion.h +++ b/simplemotion.h @@ -112,7 +112,7 @@ LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream ); /** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/ LIB SM_STATUS getCumulativeStatus( const smbus handle ); /** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/ -LIB void resetCumulativeStatus( const smbus handle ); +LIB SM_STATUS resetCumulativeStatus( const smbus handle ); /** SMV2 Device communication functionss */ -- cgit v1.2.3 From 2be57946f0ad4f8d94d461fcfa9a36efd99d1258 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 23 Feb 2017 01:23:14 +0200 Subject: Purge port buffers on device open call --- pcserialport.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/pcserialport.c b/pcserialport.c index 792f079..ef33aa9 100644 --- a/pcserialport.c +++ b/pcserialport.c @@ -78,6 +78,12 @@ smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps) smDebug(-1, Low, "Serial port error: failed to set port parameters"); return -1; } + + //flush any stray bytes from device receive buffer that may reside in it + //note: according to following page, delay before this may be necessary http://stackoverflow.com/questions/13013387/clearing-the-serial-ports-buffer + usleep(100000); + tcflush(port_handle,TCIOFLUSH); + return port_handle; } @@ -167,6 +173,9 @@ smint32 serialPortOpen(const char *port_device_name, smint32 baudrate_bps) return(-1); } + //flush any stray bytes from device receive buffer that may reside in it + PurgeComm((HANDLE)port_handle,PURGE_RXABORT|PURGE_RXCLEAR|PURGE_TXABORT|PURGE_TXCLEAR); + return( (smint32)port_handle); } -- cgit v1.2.3 From 260d52898737bc52e602a56f694931fc9e5c8fa2 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 23 Feb 2017 01:40:58 +0200 Subject: Fix rest of the compiler warnings --- pcserialport.c | 1 + simplemotion.c | 9 ++------- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/pcserialport.c b/pcserialport.c index ef33aa9..2b44add 100644 --- a/pcserialport.c +++ b/pcserialport.c @@ -23,6 +23,7 @@ #include #include #include +#include smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps) { diff --git a/simplemotion.c b/simplemotion.c index 4ef9b00..9a0b3fb 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -1,3 +1,4 @@ + //Copyright (c) Granite Devices Oy #include @@ -463,12 +464,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]); } @@ -866,7 +861,7 @@ SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint1 if(stat!=SM_OK) return recordStatus(handle,stat); //maybe timeouted if(clock!=NULL) - *clock=bufget16bit(smBus[handle].recv_rsbuf,0); + memcpy(clock,smBus[handle].recv_rsbuf,sizeof(smuint16)); smBus[handle].recv_storepos=0; -- cgit v1.2.3 From 6728ffdb6f8253a23dc600a8dc810bbc94fd69ab Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 23 Feb 2017 02:29:09 +0200 Subject: Add parameter definitions (device capabilities & restore saved config command) --- simplemotion_defs.h | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index f85b40f..138c42b 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -352,6 +352,8 @@ #define SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA 1024 //stores encoder index position in SMP_DEBUGPARAM_1. while busy (index not found) SMP_DEBUGPARAM_2 will be 100, after found it is 200. #define SMP_SYSTEM_CONTROL_CAPTURE_INDEX_POSITION 2048 + //load settings that are saved in device flash memory. useful when changing parameters on the fly and want to restore originals, or when app is started and drive may have unknown parameter modifications active. + #define SMP_SYSTEM_CONTROL_RESTORE_SAVED_CONFIG 4096 //write SM bus SM_CRCINIT constant modifier. special purposes only, don't use if unsure because //it is one time programmable variable (permanently irreversible operation, can't be ever reset to default by provided methods) #define SMP_SYSTEM_CONTROL_MODIFY_CRCINIT 262144 @@ -719,7 +721,21 @@ //#define RUNTIME_FEATURES1 6000 #define SMP_SERIAL_NR 6002 #define SMP_UID_NR 6003 -#define SMP_DRIVE_CAPABILITIES 6006 + +//read only bit field that is can be used to identify device capabilities +//the list below is subject to extend +#define SMP_DEVICE_CAPABILITIES 6006 + #define DEVICE_CAPABILITY_HOMING BV(0) + #define DEVICE_CAPABILITY_SERIAL_FBD BV(1) + #define DEVICE_CAPABILITY_RESTORE_SAVED_CONFIG BV(2) + #define DEVICE_CAPABILITY_MEASURE_RL BV(3) + #define DEVICE_CAPABILITY_TORQUE_RIPPLE_COMPENSATION BV(4) + #define DEVICE_CAPABILITY_NOTCH_FILTER BV(5) + #define DEVICE_CAPABILITY_TORQUE_EFFECTS BV(6) + #define DEVICE_CAPABILITY_AUTOSETUP_COMMUTATION_SENSOR BV(7) + #define DEVICE_CAPABILITY_SENSORLESS_COMMUTATION BV(8) + #define DEVICE_CAPABILITY_ANALOG_OUTPUT BV(9) + #define SMP_FIRMWARE_VERSION 6010 #define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011 #define SMP_GC_FIRMWARE_VERSION 6014 -- cgit v1.2.3 From 72604ec957bd831a97ca2c402a0dd3ab381cb92e Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 6 Apr 2017 01:34:40 +0300 Subject: Add FW & config deployment library (WIP, Argon not supported yet) --- SimpleMotionV2.pri | 4 +- smdeploymenttool.c | 634 +++++++++++++++++++++++++++++++++++++++++++++++++++++ smdeploymenttool.h | 89 ++++++++ 3 files changed, 725 insertions(+), 2 deletions(-) create mode 100644 smdeploymenttool.c create mode 100644 smdeploymenttool.h diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri index b5619bc..7254e17 100644 --- a/SimpleMotionV2.pri +++ b/SimpleMotionV2.pri @@ -6,9 +6,9 @@ DEPENDPATH += $$PWD DEFINES += SIMPLEMOTIONV2_LIBRARY SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/pcserialport.c \ - $$PWD/bufferedmotion.c + $$PWD/bufferedmotion.c $$PWD/smdeploymenttool.c HEADERS += $$PWD/simplemotion_private.h\ $$PWD/pcserialport.h $$PWD/busdevice.h $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h \ - $$PWD/bufferedmotion.h + $$PWD/bufferedmotion.h $$PWD/smdeploymenttool.h diff --git a/smdeploymenttool.c b/smdeploymenttool.c new file mode 100644 index 0000000..f1f1931 --- /dev/null +++ b/smdeploymenttool.c @@ -0,0 +1,634 @@ +#include "smdeploymenttool.h" +#include +#include +#include +#include + +#ifdef __unix__ +#include +void sleep_ms(int millisecs) +{ + msleep(millisecs); +} +#else +#include +void sleep_ms(int millisecs) +{ + Sleep(millisecs); +} +#endif + +int globalErrorDetailCode=0; + +int smGetDeploymentToolErrroDetail() +{ + return globalErrorDetailCode; +} + +//return -1 if EOF +unsigned int readFileLine( FILE *f, int charlimit, char *output, smbool *eof) +{ + int len=0; + char c; + do + { + c=fgetc(f); + + if(feof(f)) + *eof=smtrue; + else + *eof=smfalse; + + //eol or eof + if( *eof==smtrue || c=='\n' || c=='\r' || len>=charlimit-1 ) + { + output[len]=0;//terminate str + return len; + } + + output[len]=c; + len++; + } while(1); + return len; +} + +smbool parseParameter(FILE *f, int idx, smint32 *paramaddr, smint32 *value, smbool *readonly) +{ + const int maxLineLen=100; + char line[maxLineLen]; + char scanline[maxLineLen]; + //search correct row + fseek(f,0,SEEK_SET); + smbool gotaddr=smfalse,gotvalue=smfalse, gotreadonly=smfalse, readRDonly; + int readAddr, readValue; + unsigned int readbytes; + smbool eof; + + do//loop trhu all lines of file + { + readbytes=readFileLine(f,maxLineLen,line,&eof);//read line + + //try read address + sprintf(scanline,"%d\\addr=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%d",&readAddr)==1)//parse number after the start of line + gotaddr=smtrue;//number parse success + + //try read value + sprintf(scanline,"%d\\value=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%d",&readValue)==1)//parse number after the start of line + gotvalue=smtrue;//number parse success + + //try read readonly status + sprintf(scanline,"%d\\readonly=true",idx);//check if readonly=true + if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match + { + readRDonly=smtrue; + gotreadonly=smtrue; + } + sprintf(scanline,"%d\\readonly=false",idx);//check if readonly=false + if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match + { + readRDonly=smfalse; + gotreadonly=smtrue; + } + } + while( (gotvalue==smfalse || gotaddr==smfalse || gotreadonly==smfalse) && eof==smfalse ); + + if(gotvalue==smtrue&&gotaddr==smtrue) + { + *paramaddr=readAddr; + *value=readValue; + *readonly=readRDonly; + return smtrue; + } + + return smfalse;//not found +} + +/** + * @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param filename .DRC file name + * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). + * @return Enum LoadConfigurationStatus + * + * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. + */ +LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smaddress, const char *filename, unsigned int mode , int *skippedCount, int *errorCount) +{ + //test connection + smint32 devicetype; + SM_STATUS stat; + FILE *f; + int ignoredCount=0; + int setErrors=0; + smint32 CB1Value; + int changed=0; + + *skippedCount=-1; + *errorCount=-1; + + //test connection + stat=smRead1Parameter(smhandle,smaddress,SMP_DEVICE_TYPE,&devicetype); + if(stat!=SM_OK) + return CFGCommunicationError; + + //smSetParameter( smhandle, smaddress, SMP_RETURN_PARAM_LEN, SMPRET_CMD_STATUS );//get command status as feedback from each executed SM command + + if(mode&CONFIGMODE_DISABLE_DURING_CONFIG) + { + smRead1Parameter( smhandle, smaddress, SMP_CONTROL_BITS1, &CB1Value ); + smSetParameter( smhandle, smaddress, SMP_CONTROL_BITS1, 0);//disable drive + } + + if(getCumulativeStatus( smhandle )!=SM_OK ) + return CFGCommunicationError; + + f=fopen(filename,"rb"); + if(f==NULL) + return CFGUnableToOpenFile; + + smDebug(smhandle,Low,"Setting parameters\n"); + + int i=1; + smbool readOk; + do + { + smint32 readAddr, readValue; + smbool isReadOnly; + readOk=parseParameter(f,i,&readAddr,&readValue,&isReadOnly); + + if(readOk==smtrue && isReadOnly==smfalse) + { + smint32 currentValue; + //set parameter to device + if(smRead1Parameter( smhandle, smaddress, readAddr, ¤tValue )==SM_OK) + { + if(currentValue!=readValue ) //set only if different + { + resetCumulativeStatus( smhandle ); + smint32 dummy; + smint32 cmdSetAddressStatus; + smint32 cmdSetValueStatus; + + //use low level SM commands so we can get execution status of each subpacet: + smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, SMP_RETURN_PARAM_LEN ); + smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_24B, SMPRET_CMD_STATUS ); + smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, readAddr ); + smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_32B, readValue ); + smExecuteCommandQueue( smhandle, smaddress ); + smGetQueuedSMCommandReturnValue( smhandle, &dummy ); + smGetQueuedSMCommandReturnValue( smhandle, &dummy ); + smGetQueuedSMCommandReturnValue( smhandle, &cmdSetAddressStatus ); + smGetQueuedSMCommandReturnValue( smhandle, &cmdSetValueStatus ); + + //check if above code succeed + if( getCumulativeStatus(smhandle)!=SM_OK || cmdSetAddressStatus!=SMP_CMD_STATUS_ACK || cmdSetValueStatus!=SMP_CMD_STATUS_ACK ) + { + SM_STATUS stat=getCumulativeStatus(smhandle); + setErrors++; + smDebug(smhandle,Low,"Failed to write parameter value %d to address %d (status: %d %d %d)\n",readValue,readAddr,(int)stat,cmdSetAddressStatus,cmdSetValueStatus); + } + + changed++; + } + } + else//device doesn't have such parameter. perhaps wrong model or fw version. + { + ignoredCount++; + smDebug(smhandle,Low,"Ignoring parameter parameter value %d to address %d\n",readValue,readAddr); + } + } + + i++; + } while(readOk==smtrue); + + *skippedCount=ignoredCount; + *errorCount=setErrors; + + resetCumulativeStatus( smhandle ); + + //save to flash if some value was changed + if(changed>0) + smSetParameter( smhandle, smaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG ); + + if(mode&CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG ) + { + smDebug(smhandle,Low,"Restting faults\n"); + smSetParameter( smhandle, smaddress, SMP_FAULTS, 0 );//reset faults + } + + //re-enable drive + if(mode&CONFIGMODE_DISABLE_DURING_CONFIG) + { + smDebug(smhandle,Low,"Restoring CONTROL_BITS1 to value 0x%x\n",CB1Value); + smSetParameter( smhandle, smaddress, SMP_CONTROL_BITS1, CB1Value );//restore controbits1 (enable if it was enabled before) + } + + smint32 statusbits; + smRead1Parameter( smhandle, smaddress, SMP_STATUS, &statusbits ); + + //restart drive if necessary or if forced + if( (statusbits&STAT_PERMANENT_STOP) || (mode&CONFIGMODE_ALWAYS_RESTART_TARGET) ) + { + smDebug(smhandle,Low,"Restarting device\n"); + smSetParameter( smhandle, smaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART ); + sleep_ms(2000);//wait power-on + } + + if(getCumulativeStatus(smhandle)!=SM_OK) + return CFGCommunicationError; + + return CFGComplete; +} + +/** + * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. + * @param UID result will be written to this pointer + * @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version) + */ +smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID )//FIXME gives questionable value if device is in DFU mode. Check FW side. +{ + smint32 fwBinaryChecksum; + resetCumulativeStatus(smhandle); + smSetParameter( smhandle, deviceaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA); + smRead1Parameter( smhandle, deviceaddress, SMP_DEBUGPARAM1, &fwBinaryChecksum ); + *UID=(smuint32) fwBinaryChecksum; + + if(getCumulativeStatus(smhandle)==SM_OK) + return smtrue; + else + return smfalse; +} + +FirmwareUploadStatus verifyFirmwareData(smuint8 *data, smuint32 numbytes, int connectedDeviceTypeId, + smuint32 *primaryMCUDataOffset, smuint32 *primaryMCUDataLenth, + smuint32 *secondaryMCUDataOffset,smuint32 *secondaryMCUDataLength ) +{ + //see http://granitedevices.com/wiki/Argon_firmware_file_format + + smuint32 filetype; + filetype=((smuint32*)data)[0]; + if(filetype!=0x57464447) //check header string "GDFW" + return FWInvalidFile; + + smuint16 filever, deviceid; + smuint32 primaryMCUSize, secondaryMCUSize; + + filever=((smuint16*)data)[2]; + deviceid=((smuint16*)data)[3]; + primaryMCUSize=((smuint32*)data)[2]; + secondaryMCUSize=((smuint32*)data)[3]; + if(secondaryMCUSize==0xffffffff) + secondaryMCUSize=0;//it is not present + + if(filever!=300) + return FWIncompatibleFW; + + if(deviceid/1000!=connectedDeviceTypeId/1000)//compare only device and model family. AABBCC so AAB is compared value, ARGON=004 IONI=011 + return FWIncompatibleFW; + + //get checksum and check it + smuint32 cksum,cksumcalc=0; + smuint32 i; + smuint32 cksumOffset=4+2+2+4+4+primaryMCUSize+secondaryMCUSize; + if(cksumOffset>numbytes-4) + return FWInvalidFile; + cksum=((smuint32*)((smuint32)data+cksumOffset))[0]; + + for(i=0;i< numbytes-4;i++) + { + cksumcalc+=data[i]; + } + + if(cksum!=cksumcalc) + return FWIncompatibleFW; + + //let caller know where the firmware data is located in buffer + *primaryMCUDataOffset=4+2+2+4+4; + *primaryMCUDataLenth=primaryMCUSize; + *secondaryMCUDataOffset=*primaryMCUDataOffset+*primaryMCUDataLenth; + *secondaryMCUDataLength=secondaryMCUSize; + + return FWComplete; +} + +smbool loadBinaryFile( const char *filename, smuint8 **data, int *numbytes ) +{ + FILE *f; + f=fopen(filename,"rb"); + if(f==NULL) + return smfalse; + + *numbytes=0; + + //get length + fseek(f,0,SEEK_END); + int length=ftell(f); + fseek(f,0,SEEK_SET); + + //allocate buffer + *data=malloc(length); + if(*data==NULL) + return smfalse; + + //read + *numbytes=fread(*data,1,length,f); + if(*numbytes!=length)//failed to read it all + { + free(*data); + *numbytes=0; + return smfalse; + } + + return smtrue;//successl +} + + + +//flashing STM32 (host side mcu) +smbool flashFirmwarePrimaryMCU( smbus smhandle, int deviceaddress, const smuint8 *data, smint32 size, int *progress ) +{ + smint32 ret; + static smint32 deviceType, fwVersion; + static int uploadIndex; + int c; + const int BL_CHUNK_LEN=32; + static enum {Init,Upload,Finish} state=Init; + + if(state==Init) + { + resetCumulativeStatus( smhandle ); + smRead2Parameters( smhandle, deviceaddress, SMP_FIRMWARE_VERSION, &fwVersion, SMP_DEVICE_TYPE,&deviceType ); + + if(getCumulativeStatus(smhandle)!=SM_OK) + { + state=Init; + return smfalse; + } + +/* kommentoitu pois koska ei haluta erasoida parskuja koska parametri SMO ei saisi nollautua mielellään + + if(deviceType!=4000)//argon does not support BL function 11 + smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,11);//BL func on ioni 11 = do mass erase on STM32, also confifuration + else//does not reset on ioni and drives that support preserving settings. but resets on argon +*/ + smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,1);//BL func 1 = do mass erase on STM32. On Non-Argon devices it doesn't reset confifuration + + sleep_ms(2000);//wait some time. 500ms too little 800ms barely enough + + //flash + smSetParameter(smhandle,deviceaddress,SMP_RETURN_PARAM_LEN, SMPRET_CMD_STATUS); + + if(getCumulativeStatus(smhandle)!=SM_OK) + { + state=Init; + return smfalse; + } + + state=Upload; + uploadIndex=0; + *progress=5; + } + else if(state==Upload) + { + size/=2;//bytes to 16 bit words + + //upload data in 32=BL_CHUNK_LEN word chunks + for(;uploadIndex=size) + upword=0xeeee; + else + upword=((smuint16*)data)[uploadIndex]; + smAppendSMCommandToQueue( smhandle, SMPCMD_24B, upword ); + uploadIndex++; + } + + smAppendSMCommandToQueue( smhandle, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION ); + smAppendSMCommandToQueue( smhandle, SMPCMD_24B, 2);//BL func 2 = do write on STM32 + smExecuteCommandQueue( smhandle, deviceaddress ); + + //read return packets + for(c=0;c=94)//95 will indicate that progress is complete. dont let it indicate that yet. + *progress=94; + + if(uploadIndex%256==0) + { + //printf("upload %d\n",uploadIndex); + return smtrue;//in progress. return often to make upload non-blocking + } + } + if(uploadIndex>=size)//finished + { + state=Finish; + } + } + else if(state==Finish) + { + //verify STM32 flash if supported by BL version + if(fwVersion>=1210) + { + smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,3);//BL func 3 = verify STM32 FW integrity + smint32 faults; + smRead1Parameter(smhandle,deviceaddress,SMP_FAULTS,&faults); + + if(getCumulativeStatus(smhandle)!=SM_OK) + { + state=Init; + *progress=0; + return smfalse; + } + + if(faults&FLT_FLASHING_COMMSIDE_FAIL) + { + //printf("verify failed\n"); + *progress=0; + state=Init; + return smfalse; + } + else + { + //printf("verify success\n"); + } + } + + *progress=95;//my job is complete + state=Init; + } + + return smtrue; +} + + +typedef enum { StatIdle=0, StatEnterDFU, StatFindDFUDevice, StatLoadFile, StatUpload, StatLaunch } UploadState;//state machine status + +//free buffer and return given status code +FirmwareUploadStatus abortFWUpload( FirmwareUploadStatus stat, smuint8 *fwData, UploadState *state, int errorDetailCode ) +{ + globalErrorDetailCode=errorDetailCode; + *state=StatIdle; + free(fwData); + return stat; +} + +/** + * @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error). + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. + * @param filename .gdf file name + * @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100. + */ +FirmwareUploadStatus smFirmwareUpload( const smbus smhandle, const int smaddress, const char *firmware_filename ) +{ + static smuint8 *fwData=NULL; + static int fwDataLength; + static smuint32 primaryMCUDataOffset, primaryMCUDataLenth; + static smuint32 secondaryMCUDataOffset,secondaryMCUDataLength; + static UploadState state=StatIdle;//state machine status + static smint32 deviceType=0; + static int DFUAddress; + static int progress=0; + + SM_STATUS stat; + + //state machine + if(state==StatIdle) + { + + //check if device is in DFU mode already + smint32 busMode; + stat=smRead2Parameters(smhandle,smaddress,SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); + if(stat==SM_OK && busMode==SMP_BUS_MODE_DFU) + { + state=StatLoadFile; + } + else if(stat==SM_OK && busMode!=0)//device not in bus mode + { + if(deviceType==4000)//argon does not support restarting in DFU mode by software + { + return abortFWUpload(FWConnectionError,fwData,&state,200); + } + + //restart device into DFU mode + state=StatEnterDFU; + + stat=smSetParameter(smhandle,smaddress,SMP_SYSTEM_CONTROL,64);//reset device to DFU command + if(stat!=SM_OK) + return abortFWUpload(FWConnectionError,fwData,&state,300); + } + else + state=StatFindDFUDevice;//search DFU device in brute force, fallback for older BL versions that don't preserve same smaddress than non-DFU mode + //return abortFWUpload(FWConnectionError,fwData,&state,301); + + progress=1; + DFUAddress=smaddress; + } + + else if(state==StatEnterDFU) + { + sleep_ms(2500);//wait device to reboot in DFU mode. probably shorter delay would do. + + //check if device is in DFU mode already + smint32 busMode; + stat=smRead2Parameters(smhandle,smaddress, SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); + if(stat==SM_OK && busMode==0)//busmode 0 is DFU mode + { + state=StatLoadFile; + } + else + state=StatFindDFUDevice;//search DFU device in brute force, fallback for older BL versions that don't preserve same smaddress than non-DFU mode + + progress=2; + } + + else if(state==StatFindDFUDevice) + { + int i; + for(i=245;i<=255;i++) + { + smint32 busMode; + stat=smRead2Parameters(smhandle,i, SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); + if(stat==SM_OK && busMode==0)//busmode 0 is DFU mode + { + state=StatLoadFile; + DFUAddress=i; + break;//DFU found, break out of for loop + } + } + + if(i==256)//DFU device not found + return abortFWUpload(CFGConnectingDFUModeFailed,fwData,&state,400);//setting DFU mode failed + + progress=3; + } + + else if(state==StatLoadFile) + { + if(loadBinaryFile(firmware_filename,&fwData,&fwDataLength)!=smtrue) + return FWFileNotReadable; + + FirmwareUploadStatus stat=verifyFirmwareData(fwData, fwDataLength, deviceType, + &primaryMCUDataOffset, &primaryMCUDataLenth, + &secondaryMCUDataOffset, &secondaryMCUDataLength); + if(stat!=FWComplete)//error in verify + { + return abortFWUpload(stat,fwData,&state,100); + } + + //all good, upload firmware + state=StatUpload; + + progress=4; + } + + else if(state==StatUpload) + { + smbool ret=flashFirmwarePrimaryMCU(smhandle,DFUAddress,fwData+primaryMCUDataOffset,primaryMCUDataLenth,&progress); + if(ret==smfalse)//failed + { + return abortFWUpload(FWConnectionError,fwData,&state,1000); + } + else + { + if(progress>=95) + state=StatLaunch; + } + } + + else if(state==StatLaunch) + { + smSetParameter(smhandle,DFUAddress,SMP_BOOTLOADER_FUNCTION,4);//BL func 4 = launch. + sleep_ms(2000); + progress=100; + state=StatIdle; + } + + return (FirmwareUploadStatus)progress; +} + + + + diff --git a/smdeploymenttool.h b/smdeploymenttool.h new file mode 100644 index 0000000..8838664 --- /dev/null +++ b/smdeploymenttool.h @@ -0,0 +1,89 @@ +#ifndef SMDEPLOYMENTTOOL_H +#define SMDEPLOYMENTTOOL_H + +#ifdef WIN32 +//dll specs +#ifdef BUILD_DLL + #define LIB __declspec(dllexport) +#else +// #define LIB __declspec(dllimport) +#define LIB +#endif +#else +#define LIB +#endif + + +#include "simplemotion.h" + + +#ifdef __cplusplus +extern "C"{ +#endif + + +typedef enum +{ + FWComplete=100, + FWInvalidFile=-1, + FWConnectionError=-2, + FWIncompatibleFW=-3, + FWConnectionLoss=-4, + FWUnsupportedTargetDevice=-5, + FWFileNotReadable=-6 +} FirmwareUploadStatus; + +/** + * @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error). + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param filename .gdf file name + * @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100. + */ +LIB FirmwareUploadStatus smFirmwareUpload(const smbus smhandle, const int smaddress, const char *firmware_filename ); + +typedef enum +{ + CFGComplete=100, + CFGInvalidFile=-1, + CFGCommunicationError=-2, + CFGConnectingDFUModeFailed=-3, + CFGIncompatibleFW=-4, + CFGUnsupportedTargetDevice=-5, + CFGUnableToOpenFile=-6 + +} LoadConfigurationStatus; + +//TODO implement: #define CONFIGMODE_REQUIRE_SAME_FW 1 //will return IncompatibleFW if firmware checksum does not match the one in .drc files. if this error is returned, perform smFirmwareUpload and perform smLoadConfiguration again. Requires DRC file version 111 or later (if not met, returns InvalidFile). +#define CONFIGMODE_ALWAYS_RESTART_TARGET 2 //will perform device restart after setup even when it's not required +#define CONFIGMODE_DISABLE_DURING_CONFIG 4 //will set device in disabled state during configuration +#define CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG 8 //will perform clear faults command after configuration + +/** + * @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param filename .DRC file name + * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). + * @return Enum LoadConfigurationStatus + * + * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. + */ +LIB LoadConfigurationStatus smLoadConfiguration( const smbus smhandle, const int smaddress, const char *filename, unsigned int mode, int *skippedCount, int *errorCount ); + + +/** + * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. + * @param UID result will be written to this pointer + * @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version) + */ +smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID ); + + + +#ifdef __cplusplus +} +#endif +#endif // SMDEPLOYMENTTOOL_H -- cgit v1.2.3 From a780e4b1123dbc416dd06af9ee2d7f99f281a9c7 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 6 Apr 2017 02:22:19 +0300 Subject: Add proper parameter value conversion --- smdeploymenttool.c | 64 +++++++++++++++++++++++++++++++++++------------------- 1 file changed, 42 insertions(+), 22 deletions(-) diff --git a/smdeploymenttool.c b/smdeploymenttool.c index f1f1931..2551128 100644 --- a/smdeploymenttool.c +++ b/smdeploymenttool.c @@ -3,6 +3,7 @@ #include #include #include +#include #ifdef __unix__ #include @@ -52,15 +53,23 @@ unsigned int readFileLine( FILE *f, int charlimit, char *output, smbool *eof) return len; } -smbool parseParameter(FILE *f, int idx, smint32 *paramaddr, smint32 *value, smbool *readonly) +typedef struct +{ + int address; + double value; + smbool readOnly; + double scale; + double offset; +} Parameter; + +smbool parseParameter(FILE *f, int idx, Parameter *param ) { const int maxLineLen=100; char line[maxLineLen]; char scanline[maxLineLen]; //search correct row fseek(f,0,SEEK_SET); - smbool gotaddr=smfalse,gotvalue=smfalse, gotreadonly=smfalse, readRDonly; - int readAddr, readValue; + smbool gotaddr=smfalse,gotvalue=smfalse, gotreadonly=smfalse, gotscale=smfalse,gotoffset=smfalse; unsigned int readbytes; smbool eof; @@ -71,36 +80,45 @@ smbool parseParameter(FILE *f, int idx, smint32 *paramaddr, smint32 *value, smbo //try read address sprintf(scanline,"%d\\addr=",idx); if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line - if(sscanf(line+strlen(scanline),"%d",&readAddr)==1)//parse number after the start of line + if(sscanf(line+strlen(scanline),"%d",¶m->address)==1)//parse number after the start of line gotaddr=smtrue;//number parse success //try read value sprintf(scanline,"%d\\value=",idx); if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line - if(sscanf(line+strlen(scanline),"%d",&readValue)==1)//parse number after the start of line + if(sscanf(line+strlen(scanline),"%lf",¶m->value)==1)//parse number after the start of line gotvalue=smtrue;//number parse success + //try read offset + sprintf(scanline,"%d\\offset=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%lf",¶m->offset)==1)//parse number after the start of line + gotoffset=smtrue;//number parse success + + //try read scale + sprintf(scanline,"%d\\scaling=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%lf",¶m->scale)==1)//parse number after the start of line + gotscale=smtrue;//number parse success + //try read readonly status sprintf(scanline,"%d\\readonly=true",idx);//check if readonly=true if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match { - readRDonly=smtrue; + param->readOnly=smtrue; gotreadonly=smtrue; } sprintf(scanline,"%d\\readonly=false",idx);//check if readonly=false if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match { - readRDonly=smfalse; + param->readOnly=smfalse; gotreadonly=smtrue; } } - while( (gotvalue==smfalse || gotaddr==smfalse || gotreadonly==smfalse) && eof==smfalse ); + while( (gotvalue==smfalse || gotaddr==smfalse || gotreadonly==smfalse || gotscale==smfalse || gotoffset==smfalse) && eof==smfalse ); - if(gotvalue==smtrue&&gotaddr==smtrue) + if(gotvalue==smtrue&&gotaddr==smtrue&&gotoffset==smtrue&&gotscale==smtrue&&gotreadonly==smtrue) { - *paramaddr=readAddr; - *value=readValue; - *readonly=readRDonly; return smtrue; } @@ -157,17 +175,19 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad smbool readOk; do { - smint32 readAddr, readValue; - smbool isReadOnly; - readOk=parseParameter(f,i,&readAddr,&readValue,&isReadOnly); + Parameter param; + readOk=parseParameter(f,i,¶m); - if(readOk==smtrue && isReadOnly==smfalse) + if(readOk==smtrue && param.readOnly==smfalse) { smint32 currentValue; + + int configFileValue=round(param.value*param.scale-param.offset); + //set parameter to device - if(smRead1Parameter( smhandle, smaddress, readAddr, ¤tValue )==SM_OK) + if(smRead1Parameter( smhandle, smaddress, param.address, ¤tValue )==SM_OK) { - if(currentValue!=readValue ) //set only if different + if(currentValue!=configFileValue ) //set only if different { resetCumulativeStatus( smhandle ); smint32 dummy; @@ -177,8 +197,8 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad //use low level SM commands so we can get execution status of each subpacet: smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, SMP_RETURN_PARAM_LEN ); smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_24B, SMPRET_CMD_STATUS ); - smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, readAddr ); - smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_32B, readValue ); + smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, param.address ); + smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_32B, configFileValue ); smExecuteCommandQueue( smhandle, smaddress ); smGetQueuedSMCommandReturnValue( smhandle, &dummy ); smGetQueuedSMCommandReturnValue( smhandle, &dummy ); @@ -190,7 +210,7 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad { SM_STATUS stat=getCumulativeStatus(smhandle); setErrors++; - smDebug(smhandle,Low,"Failed to write parameter value %d to address %d (status: %d %d %d)\n",readValue,readAddr,(int)stat,cmdSetAddressStatus,cmdSetValueStatus); + smDebug(smhandle,Low,"Failed to write parameter value %d to address %d (status: %d %d %d)\n",configFileValue,param.address,(int)stat,cmdSetAddressStatus,cmdSetValueStatus); } changed++; @@ -199,7 +219,7 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad else//device doesn't have such parameter. perhaps wrong model or fw version. { ignoredCount++; - smDebug(smhandle,Low,"Ignoring parameter parameter value %d to address %d\n",readValue,readAddr); + smDebug(smhandle,Low,"Ignoring parameter parameter value %d to address %d\n",configFileValue,param.address); } } -- cgit v1.2.3 From 1d6f69d1f056777401bcdfcde0844c9b9af1cbac Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 6 Apr 2017 17:06:50 +0300 Subject: Update readme.md & rename library files --- README.md | 36 ++- SimpleMotionV2.pri | 4 +- devicedeployment.c | 654 +++++++++++++++++++++++++++++++++++++++++++++++++++++ devicedeployment.h | 100 ++++++++ smdeploymenttool.c | 654 ----------------------------------------------------- smdeploymenttool.h | 89 -------- 6 files changed, 790 insertions(+), 747 deletions(-) create mode 100644 devicedeployment.c create mode 100644 devicedeployment.h delete mode 100644 smdeploymenttool.c delete mode 100644 smdeploymenttool.h diff --git a/README.md b/README.md index 88b0006..1b2e53a 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,39 @@ SimpleMotionV2 ============== -SimpleMotion V2 library +This is a SimpleMotion V2 library, which is an API to control motor controller from any programmable platform, such as PC, PLC or MCU. -For more information, see: +For main documentation, see: http://granitedevices.com/wiki/SimpleMotion_V2 + + +Files & usage +============= + +There are files that are needed for basic SimpleMotion usage, and files that are optional and needed only for certain features. Following may be useful info when porting library to different platforms (such as embedded MCU) to avoid unnecessary work on some files. + +Compulsory +---------- + +- simplemotion.c/.h +- sm485.h +- sm_consts.c +- simplemotion_defs.h +- simplemotion_private.h +- busdevice.c/.h + +Platform specific +----------------- +Following files probably need modification if ported to another platform + +- pcserialport.c/.h - Contains a driver for communication device interface. This driver controls serial/COM port on Unix & Windows. Also busdevice.c/.h need to be modified accordingly if this driver is removed or modified. + +A typical platform port would involve writing a communication driver that implements same functions as pcserialport.c and adding their relevant calls to busdevice.c/.h. + +Feature specific +---------------- + +- bufferedmotion.c/.h - Library used for buffered motion stream applications +- devicedeployment.c/.h - Library used for installing firmware and loading settings into target devices. Can be used to configure devices automatically in-system. + +For practical usage examples, refer to https://github.com/GraniteDevices/SimpleMotionV2Examples \ No newline at end of file diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri index 7254e17..9d2caf5 100644 --- a/SimpleMotionV2.pri +++ b/SimpleMotionV2.pri @@ -6,9 +6,9 @@ DEPENDPATH += $$PWD DEFINES += SIMPLEMOTIONV2_LIBRARY SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/pcserialport.c \ - $$PWD/bufferedmotion.c $$PWD/smdeploymenttool.c + $$PWD/bufferedmotion.c $$PWD/devicedeployment.c HEADERS += $$PWD/simplemotion_private.h\ $$PWD/pcserialport.h $$PWD/busdevice.h $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h \ - $$PWD/bufferedmotion.h $$PWD/smdeploymenttool.h + $$PWD/bufferedmotion.h $$PWD/devicedeployment.h diff --git a/devicedeployment.c b/devicedeployment.c new file mode 100644 index 0000000..bdb718d --- /dev/null +++ b/devicedeployment.c @@ -0,0 +1,654 @@ +#include "devicedeployment.h" +#include +#include +#include +#include +#include + +#ifdef __unix__ +#include +void sleep_ms(int millisecs) +{ + msleep(millisecs); +} +#else +#include +void sleep_ms(int millisecs) +{ + Sleep(millisecs); +} +#endif + +int globalErrorDetailCode=0; + +int smGetDeploymentToolErrroDetail() +{ + return globalErrorDetailCode; +} + +//return -1 if EOF +unsigned int readFileLine( FILE *f, int charlimit, char *output, smbool *eof) +{ + int len=0; + char c; + do + { + c=fgetc(f); + + if(feof(f)) + *eof=smtrue; + else + *eof=smfalse; + + //eol or eof + if( *eof==smtrue || c=='\n' || c=='\r' || len>=charlimit-1 ) + { + output[len]=0;//terminate str + return len; + } + + output[len]=c; + len++; + } while(1); + return len; +} + +typedef struct +{ + int address; + double value; + smbool readOnly; + double scale; + double offset; +} Parameter; + +smbool parseParameter(FILE *f, int idx, Parameter *param ) +{ + const int maxLineLen=100; + char line[maxLineLen]; + char scanline[maxLineLen]; + //search correct row + fseek(f,0,SEEK_SET); + smbool gotaddr=smfalse,gotvalue=smfalse, gotreadonly=smfalse, gotscale=smfalse,gotoffset=smfalse; + unsigned int readbytes; + smbool eof; + + do//loop trhu all lines of file + { + readbytes=readFileLine(f,maxLineLen,line,&eof);//read line + + //try read address + sprintf(scanline,"%d\\addr=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%d",¶m->address)==1)//parse number after the start of line + gotaddr=smtrue;//number parse success + + //try read value + sprintf(scanline,"%d\\value=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%lf",¶m->value)==1)//parse number after the start of line + gotvalue=smtrue;//number parse success + + //try read offset + sprintf(scanline,"%d\\offset=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%lf",¶m->offset)==1)//parse number after the start of line + gotoffset=smtrue;//number parse success + + //try read scale + sprintf(scanline,"%d\\scaling=",idx); + if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line + if(sscanf(line+strlen(scanline),"%lf",¶m->scale)==1)//parse number after the start of line + gotscale=smtrue;//number parse success + + //try read readonly status + sprintf(scanline,"%d\\readonly=true",idx);//check if readonly=true + if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match + { + param->readOnly=smtrue; + gotreadonly=smtrue; + } + sprintf(scanline,"%d\\readonly=false",idx);//check if readonly=false + if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match + { + param->readOnly=smfalse; + gotreadonly=smtrue; + } + } + while( (gotvalue==smfalse || gotaddr==smfalse || gotreadonly==smfalse || gotscale==smfalse || gotoffset==smfalse) && eof==smfalse ); + + if(gotvalue==smtrue&&gotaddr==smtrue&&gotoffset==smtrue&&gotscale==smtrue&&gotreadonly==smtrue) + { + return smtrue; + } + + return smfalse;//not found +} + +/** + * @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param filename .DRC file name + * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). + * @return Enum LoadConfigurationStatus + * + * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. + */ +LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smaddress, const char *filename, unsigned int mode , int *skippedCount, int *errorCount) +{ + //test connection + smint32 devicetype; + SM_STATUS stat; + FILE *f; + int ignoredCount=0; + int setErrors=0; + smint32 CB1Value; + int changed=0; + + *skippedCount=-1; + *errorCount=-1; + + //test connection + stat=smRead1Parameter(smhandle,smaddress,SMP_DEVICE_TYPE,&devicetype); + if(stat!=SM_OK) + return CFGCommunicationError; + + //smSetParameter( smhandle, smaddress, SMP_RETURN_PARAM_LEN, SMPRET_CMD_STATUS );//get command status as feedback from each executed SM command + + if(mode&CONFIGMODE_DISABLE_DURING_CONFIG) + { + smRead1Parameter( smhandle, smaddress, SMP_CONTROL_BITS1, &CB1Value ); + smSetParameter( smhandle, smaddress, SMP_CONTROL_BITS1, 0);//disable drive + } + + if(getCumulativeStatus( smhandle )!=SM_OK ) + return CFGCommunicationError; + + f=fopen(filename,"rb"); + if(f==NULL) + return CFGUnableToOpenFile; + + smDebug(smhandle,Low,"Setting parameters\n"); + + int i=1; + smbool readOk; + do + { + Parameter param; + readOk=parseParameter(f,i,¶m); + + if(readOk==smtrue && param.readOnly==smfalse) + { + smint32 currentValue; + + int configFileValue=round(param.value*param.scale-param.offset); + + //set parameter to device + if(smRead1Parameter( smhandle, smaddress, param.address, ¤tValue )==SM_OK) + { + if(currentValue!=configFileValue ) //set only if different + { + resetCumulativeStatus( smhandle ); + smint32 dummy; + smint32 cmdSetAddressStatus; + smint32 cmdSetValueStatus; + + //use low level SM commands so we can get execution status of each subpacet: + smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, SMP_RETURN_PARAM_LEN ); + smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_24B, SMPRET_CMD_STATUS ); + smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, param.address ); + smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_32B, configFileValue ); + smExecuteCommandQueue( smhandle, smaddress ); + smGetQueuedSMCommandReturnValue( smhandle, &dummy ); + smGetQueuedSMCommandReturnValue( smhandle, &dummy ); + smGetQueuedSMCommandReturnValue( smhandle, &cmdSetAddressStatus ); + smGetQueuedSMCommandReturnValue( smhandle, &cmdSetValueStatus ); + + //check if above code succeed + if( getCumulativeStatus(smhandle)!=SM_OK || cmdSetAddressStatus!=SMP_CMD_STATUS_ACK || cmdSetValueStatus!=SMP_CMD_STATUS_ACK ) + { + SM_STATUS stat=getCumulativeStatus(smhandle); + setErrors++; + smDebug(smhandle,Low,"Failed to write parameter value %d to address %d (status: %d %d %d)\n",configFileValue,param.address,(int)stat,cmdSetAddressStatus,cmdSetValueStatus); + } + + changed++; + } + } + else//device doesn't have such parameter. perhaps wrong model or fw version. + { + ignoredCount++; + smDebug(smhandle,Low,"Ignoring parameter parameter value %d to address %d\n",configFileValue,param.address); + } + } + + i++; + } while(readOk==smtrue); + + *skippedCount=ignoredCount; + *errorCount=setErrors; + + resetCumulativeStatus( smhandle ); + + //save to flash if some value was changed + if(changed>0) + smSetParameter( smhandle, smaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG ); + + if(mode&CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG ) + { + smDebug(smhandle,Low,"Restting faults\n"); + smSetParameter( smhandle, smaddress, SMP_FAULTS, 0 );//reset faults + } + + //re-enable drive + if(mode&CONFIGMODE_DISABLE_DURING_CONFIG) + { + smDebug(smhandle,Low,"Restoring CONTROL_BITS1 to value 0x%x\n",CB1Value); + smSetParameter( smhandle, smaddress, SMP_CONTROL_BITS1, CB1Value );//restore controbits1 (enable if it was enabled before) + } + + smint32 statusbits; + smRead1Parameter( smhandle, smaddress, SMP_STATUS, &statusbits ); + + //restart drive if necessary or if forced + if( (statusbits&STAT_PERMANENT_STOP) || (mode&CONFIGMODE_ALWAYS_RESTART_TARGET) ) + { + smDebug(smhandle,Low,"Restarting device\n"); + smSetParameter( smhandle, smaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART ); + sleep_ms(2000);//wait power-on + } + + if(getCumulativeStatus(smhandle)!=SM_OK) + return CFGCommunicationError; + + return CFGComplete; +} + +/** + * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. + * @param UID result will be written to this pointer + * @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version) + */ +smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID )//FIXME gives questionable value if device is in DFU mode. Check FW side. +{ + smint32 fwBinaryChecksum; + resetCumulativeStatus(smhandle); + smSetParameter( smhandle, deviceaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA); + smRead1Parameter( smhandle, deviceaddress, SMP_DEBUGPARAM1, &fwBinaryChecksum ); + *UID=(smuint32) fwBinaryChecksum; + + if(getCumulativeStatus(smhandle)==SM_OK) + return smtrue; + else + return smfalse; +} + +FirmwareUploadStatus verifyFirmwareData(smuint8 *data, smuint32 numbytes, int connectedDeviceTypeId, + smuint32 *primaryMCUDataOffset, smuint32 *primaryMCUDataLenth, + smuint32 *secondaryMCUDataOffset,smuint32 *secondaryMCUDataLength ) +{ + //see http://granitedevices.com/wiki/Argon_firmware_file_format + + smuint32 filetype; + filetype=((smuint32*)data)[0]; + if(filetype!=0x57464447) //check header string "GDFW" + return FWInvalidFile; + + smuint16 filever, deviceid; + smuint32 primaryMCUSize, secondaryMCUSize; + + filever=((smuint16*)data)[2]; + deviceid=((smuint16*)data)[3]; + primaryMCUSize=((smuint32*)data)[2]; + secondaryMCUSize=((smuint32*)data)[3]; + if(secondaryMCUSize==0xffffffff) + secondaryMCUSize=0;//it is not present + + if(filever!=300) + return FWIncompatibleFW; + + if(deviceid/1000!=connectedDeviceTypeId/1000)//compare only device and model family. AABBCC so AAB is compared value, ARGON=004 IONI=011 + return FWIncompatibleFW; + + //get checksum and check it + smuint32 cksum,cksumcalc=0; + smuint32 i; + smuint32 cksumOffset=4+2+2+4+4+primaryMCUSize+secondaryMCUSize; + if(cksumOffset>numbytes-4) + return FWInvalidFile; + cksum=((smuint32*)((smuint32)data+cksumOffset))[0]; + + for(i=0;i< numbytes-4;i++) + { + cksumcalc+=data[i]; + } + + if(cksum!=cksumcalc) + return FWIncompatibleFW; + + //let caller know where the firmware data is located in buffer + *primaryMCUDataOffset=4+2+2+4+4; + *primaryMCUDataLenth=primaryMCUSize; + *secondaryMCUDataOffset=*primaryMCUDataOffset+*primaryMCUDataLenth; + *secondaryMCUDataLength=secondaryMCUSize; + + return FWComplete; +} + +smbool loadBinaryFile( const char *filename, smuint8 **data, int *numbytes ) +{ + FILE *f; + f=fopen(filename,"rb"); + if(f==NULL) + return smfalse; + + *numbytes=0; + + //get length + fseek(f,0,SEEK_END); + int length=ftell(f); + fseek(f,0,SEEK_SET); + + //allocate buffer + *data=malloc(length); + if(*data==NULL) + return smfalse; + + //read + *numbytes=fread(*data,1,length,f); + if(*numbytes!=length)//failed to read it all + { + free(*data); + *numbytes=0; + return smfalse; + } + + return smtrue;//successl +} + + + +//flashing STM32 (host side mcu) +smbool flashFirmwarePrimaryMCU( smbus smhandle, int deviceaddress, const smuint8 *data, smint32 size, int *progress ) +{ + smint32 ret; + static smint32 deviceType, fwVersion; + static int uploadIndex; + int c; + const int BL_CHUNK_LEN=32; + static enum {Init,Upload,Finish} state=Init; + + if(state==Init) + { + resetCumulativeStatus( smhandle ); + smRead2Parameters( smhandle, deviceaddress, SMP_FIRMWARE_VERSION, &fwVersion, SMP_DEVICE_TYPE,&deviceType ); + + if(getCumulativeStatus(smhandle)!=SM_OK) + { + state=Init; + return smfalse; + } + +/* kommentoitu pois koska ei haluta erasoida parskuja koska parametri SMO ei saisi nollautua mielellään + + if(deviceType!=4000)//argon does not support BL function 11 + smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,11);//BL func on ioni 11 = do mass erase on STM32, also confifuration + else//does not reset on ioni and drives that support preserving settings. but resets on argon +*/ + smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,1);//BL func 1 = do mass erase on STM32. On Non-Argon devices it doesn't reset confifuration + + sleep_ms(2000);//wait some time. 500ms too little 800ms barely enough + + //flash + smSetParameter(smhandle,deviceaddress,SMP_RETURN_PARAM_LEN, SMPRET_CMD_STATUS); + + if(getCumulativeStatus(smhandle)!=SM_OK) + { + state=Init; + return smfalse; + } + + state=Upload; + uploadIndex=0; + *progress=5; + } + else if(state==Upload) + { + size/=2;//bytes to 16 bit words + + //upload data in 32=BL_CHUNK_LEN word chunks + for(;uploadIndex=size) + upword=0xeeee; + else + upword=((smuint16*)data)[uploadIndex]; + smAppendSMCommandToQueue( smhandle, SMPCMD_24B, upword ); + uploadIndex++; + } + + smAppendSMCommandToQueue( smhandle, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION ); + smAppendSMCommandToQueue( smhandle, SMPCMD_24B, 2);//BL func 2 = do write on STM32 + smExecuteCommandQueue( smhandle, deviceaddress ); + + //read return packets + for(c=0;c=94)//95 will indicate that progress is complete. dont let it indicate that yet. + *progress=94; + + if(uploadIndex%256==0) + { + //printf("upload %d\n",uploadIndex); + return smtrue;//in progress. return often to make upload non-blocking + } + } + if(uploadIndex>=size)//finished + { + state=Finish; + } + } + else if(state==Finish) + { + //verify STM32 flash if supported by BL version + if(fwVersion>=1210) + { + smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,3);//BL func 3 = verify STM32 FW integrity + smint32 faults; + smRead1Parameter(smhandle,deviceaddress,SMP_FAULTS,&faults); + + if(getCumulativeStatus(smhandle)!=SM_OK) + { + state=Init; + *progress=0; + return smfalse; + } + + if(faults&FLT_FLASHING_COMMSIDE_FAIL) + { + //printf("verify failed\n"); + *progress=0; + state=Init; + return smfalse; + } + else + { + //printf("verify success\n"); + } + } + + *progress=95;//my job is complete + state=Init; + } + + return smtrue; +} + + +typedef enum { StatIdle=0, StatEnterDFU, StatFindDFUDevice, StatLoadFile, StatUpload, StatLaunch } UploadState;//state machine status + +//free buffer and return given status code +FirmwareUploadStatus abortFWUpload( FirmwareUploadStatus stat, smuint8 *fwData, UploadState *state, int errorDetailCode ) +{ + globalErrorDetailCode=errorDetailCode; + *state=StatIdle; + free(fwData); + return stat; +} + +/** + * @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error). + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. + * @param filename .gdf file name + * @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100. + */ +FirmwareUploadStatus smFirmwareUpload( const smbus smhandle, const int smaddress, const char *firmware_filename ) +{ + static smuint8 *fwData=NULL; + static int fwDataLength; + static smuint32 primaryMCUDataOffset, primaryMCUDataLenth; + static smuint32 secondaryMCUDataOffset,secondaryMCUDataLength; + static UploadState state=StatIdle;//state machine status + static smint32 deviceType=0; + static int DFUAddress; + static int progress=0; + + SM_STATUS stat; + + //state machine + if(state==StatIdle) + { + + //check if device is in DFU mode already + smint32 busMode; + stat=smRead2Parameters(smhandle,smaddress,SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); + if(stat==SM_OK && busMode==SMP_BUS_MODE_DFU) + { + state=StatLoadFile; + } + else if(stat==SM_OK && busMode!=0)//device not in bus mode + { + if(deviceType==4000)//argon does not support restarting in DFU mode by software + { + return abortFWUpload(FWConnectionError,fwData,&state,200); + } + + //restart device into DFU mode + state=StatEnterDFU; + + stat=smSetParameter(smhandle,smaddress,SMP_SYSTEM_CONTROL,64);//reset device to DFU command + if(stat!=SM_OK) + return abortFWUpload(FWConnectionError,fwData,&state,300); + } + else + state=StatFindDFUDevice;//search DFU device in brute force, fallback for older BL versions that don't preserve same smaddress than non-DFU mode + //return abortFWUpload(FWConnectionError,fwData,&state,301); + + progress=1; + DFUAddress=smaddress; + } + + else if(state==StatEnterDFU) + { + sleep_ms(2500);//wait device to reboot in DFU mode. probably shorter delay would do. + + //check if device is in DFU mode already + smint32 busMode; + stat=smRead2Parameters(smhandle,smaddress, SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); + if(stat==SM_OK && busMode==0)//busmode 0 is DFU mode + { + state=StatLoadFile; + } + else + state=StatFindDFUDevice;//search DFU device in brute force, fallback for older BL versions that don't preserve same smaddress than non-DFU mode + + progress=2; + } + + else if(state==StatFindDFUDevice) + { + int i; + for(i=245;i<=255;i++) + { + smint32 busMode; + stat=smRead2Parameters(smhandle,i, SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); + if(stat==SM_OK && busMode==0)//busmode 0 is DFU mode + { + state=StatLoadFile; + DFUAddress=i; + break;//DFU found, break out of for loop + } + } + + if(i==256)//DFU device not found + return abortFWUpload(CFGConnectingDFUModeFailed,fwData,&state,400);//setting DFU mode failed + + progress=3; + } + + else if(state==StatLoadFile) + { + if(loadBinaryFile(firmware_filename,&fwData,&fwDataLength)!=smtrue) + return FWFileNotReadable; + + FirmwareUploadStatus stat=verifyFirmwareData(fwData, fwDataLength, deviceType, + &primaryMCUDataOffset, &primaryMCUDataLenth, + &secondaryMCUDataOffset, &secondaryMCUDataLength); + if(stat!=FWComplete)//error in verify + { + return abortFWUpload(stat,fwData,&state,100); + } + + //all good, upload firmware + state=StatUpload; + + progress=4; + } + + else if(state==StatUpload) + { + smbool ret=flashFirmwarePrimaryMCU(smhandle,DFUAddress,fwData+primaryMCUDataOffset,primaryMCUDataLenth,&progress); + if(ret==smfalse)//failed + { + return abortFWUpload(FWConnectionError,fwData,&state,1000); + } + else + { + if(progress>=95) + state=StatLaunch; + } + } + + else if(state==StatLaunch) + { + smSetParameter(smhandle,DFUAddress,SMP_BOOTLOADER_FUNCTION,4);//BL func 4 = launch. + sleep_ms(2000); + progress=100; + state=StatIdle; + } + + return (FirmwareUploadStatus)progress; +} + + + + diff --git a/devicedeployment.h b/devicedeployment.h new file mode 100644 index 0000000..f6fa08e --- /dev/null +++ b/devicedeployment.h @@ -0,0 +1,100 @@ +/* Device deployment library based on SimpleMotionV2 lib. Features: + * + * - install .gdf format firmware to drive + * - load .drc settings file to drive + * - read installed firmware binary checksum from device - this may be used to verify that exact correct FW build is installed in the drive + * + * TODO: + * - Support Argon. Currently tested only on IONI/ATOMI series drives. Currently FW upgrade on Argon will fail, but settings load may work. + * - Add some way of reading binary checksum from .gdf file or settings file. Now user must read it from device and store that number somewhere manually. + */ + +#ifndef SMDEPLOYMENTTOOL_H +#define SMDEPLOYMENTTOOL_H + +#ifdef WIN32 +//dll specs +#ifdef BUILD_DLL + #define LIB __declspec(dllexport) +#else +// #define LIB __declspec(dllimport) +#define LIB +#endif +#else +#define LIB +#endif + + +#include "simplemotion.h" + + +#ifdef __cplusplus +extern "C"{ +#endif + + +typedef enum +{ + FWComplete=100, + FWInvalidFile=-1, + FWConnectionError=-2, + FWIncompatibleFW=-3, + FWConnectionLoss=-4, + FWUnsupportedTargetDevice=-5, + FWFileNotReadable=-6 +} FirmwareUploadStatus; + +/** + * @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error). + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param filename .gdf file name + * @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100. + */ +LIB FirmwareUploadStatus smFirmwareUpload(const smbus smhandle, const int smaddress, const char *firmware_filename ); + +typedef enum +{ + CFGComplete=100, + CFGInvalidFile=-1, + CFGCommunicationError=-2, + CFGConnectingDFUModeFailed=-3, + CFGIncompatibleFW=-4, + CFGUnsupportedTargetDevice=-5, + CFGUnableToOpenFile=-6 + +} LoadConfigurationStatus; + +//TODO implement: #define CONFIGMODE_REQUIRE_SAME_FW 1 //will return IncompatibleFW if firmware checksum does not match the one in .drc files. if this error is returned, perform smFirmwareUpload and perform smLoadConfiguration again. Requires DRC file version 111 or later (if not met, returns InvalidFile). +#define CONFIGMODE_ALWAYS_RESTART_TARGET 2 //will perform device restart after setup even when it's not required +#define CONFIGMODE_DISABLE_DURING_CONFIG 4 //will set device in disabled state during configuration +#define CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG 8 //will perform clear faults command after configuration + +/** + * @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param filename .DRC file name + * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). + * @return Enum LoadConfigurationStatus + * + * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. + */ +LIB LoadConfigurationStatus smLoadConfiguration( const smbus smhandle, const int smaddress, const char *filename, unsigned int mode, int *skippedCount, int *errorCount ); + + +/** + * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. + * @param UID result will be written to this pointer + * @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version) + */ +smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID ); + + + +#ifdef __cplusplus +} +#endif +#endif // SMDEPLOYMENTTOOL_H diff --git a/smdeploymenttool.c b/smdeploymenttool.c deleted file mode 100644 index 2551128..0000000 --- a/smdeploymenttool.c +++ /dev/null @@ -1,654 +0,0 @@ -#include "smdeploymenttool.h" -#include -#include -#include -#include -#include - -#ifdef __unix__ -#include -void sleep_ms(int millisecs) -{ - msleep(millisecs); -} -#else -#include -void sleep_ms(int millisecs) -{ - Sleep(millisecs); -} -#endif - -int globalErrorDetailCode=0; - -int smGetDeploymentToolErrroDetail() -{ - return globalErrorDetailCode; -} - -//return -1 if EOF -unsigned int readFileLine( FILE *f, int charlimit, char *output, smbool *eof) -{ - int len=0; - char c; - do - { - c=fgetc(f); - - if(feof(f)) - *eof=smtrue; - else - *eof=smfalse; - - //eol or eof - if( *eof==smtrue || c=='\n' || c=='\r' || len>=charlimit-1 ) - { - output[len]=0;//terminate str - return len; - } - - output[len]=c; - len++; - } while(1); - return len; -} - -typedef struct -{ - int address; - double value; - smbool readOnly; - double scale; - double offset; -} Parameter; - -smbool parseParameter(FILE *f, int idx, Parameter *param ) -{ - const int maxLineLen=100; - char line[maxLineLen]; - char scanline[maxLineLen]; - //search correct row - fseek(f,0,SEEK_SET); - smbool gotaddr=smfalse,gotvalue=smfalse, gotreadonly=smfalse, gotscale=smfalse,gotoffset=smfalse; - unsigned int readbytes; - smbool eof; - - do//loop trhu all lines of file - { - readbytes=readFileLine(f,maxLineLen,line,&eof);//read line - - //try read address - sprintf(scanline,"%d\\addr=",idx); - if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line - if(sscanf(line+strlen(scanline),"%d",¶m->address)==1)//parse number after the start of line - gotaddr=smtrue;//number parse success - - //try read value - sprintf(scanline,"%d\\value=",idx); - if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line - if(sscanf(line+strlen(scanline),"%lf",¶m->value)==1)//parse number after the start of line - gotvalue=smtrue;//number parse success - - //try read offset - sprintf(scanline,"%d\\offset=",idx); - if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line - if(sscanf(line+strlen(scanline),"%lf",¶m->offset)==1)//parse number after the start of line - gotoffset=smtrue;//number parse success - - //try read scale - sprintf(scanline,"%d\\scaling=",idx); - if(!strncmp(line,scanline,strlen(scanline)) && readbytes > strlen(scanline))//string starts with correct line - if(sscanf(line+strlen(scanline),"%lf",¶m->scale)==1)//parse number after the start of line - gotscale=smtrue;//number parse success - - //try read readonly status - sprintf(scanline,"%d\\readonly=true",idx);//check if readonly=true - if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match - { - param->readOnly=smtrue; - gotreadonly=smtrue; - } - sprintf(scanline,"%d\\readonly=false",idx);//check if readonly=false - if(!strncmp(line,scanline,strlen(scanline)) && readbytes >= strlen(scanline))//line match - { - param->readOnly=smfalse; - gotreadonly=smtrue; - } - } - while( (gotvalue==smfalse || gotaddr==smfalse || gotreadonly==smfalse || gotscale==smfalse || gotoffset==smfalse) && eof==smfalse ); - - if(gotvalue==smtrue&&gotaddr==smtrue&&gotoffset==smtrue&&gotscale==smtrue&&gotreadonly==smtrue) - { - return smtrue; - } - - return smfalse;//not found -} - -/** - * @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. - * @param smhandle SM bus handle, must be opened before call - * @param smaddress Target SM device address - * @param filename .DRC file name - * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). - * @return Enum LoadConfigurationStatus - * - * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. - */ -LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smaddress, const char *filename, unsigned int mode , int *skippedCount, int *errorCount) -{ - //test connection - smint32 devicetype; - SM_STATUS stat; - FILE *f; - int ignoredCount=0; - int setErrors=0; - smint32 CB1Value; - int changed=0; - - *skippedCount=-1; - *errorCount=-1; - - //test connection - stat=smRead1Parameter(smhandle,smaddress,SMP_DEVICE_TYPE,&devicetype); - if(stat!=SM_OK) - return CFGCommunicationError; - - //smSetParameter( smhandle, smaddress, SMP_RETURN_PARAM_LEN, SMPRET_CMD_STATUS );//get command status as feedback from each executed SM command - - if(mode&CONFIGMODE_DISABLE_DURING_CONFIG) - { - smRead1Parameter( smhandle, smaddress, SMP_CONTROL_BITS1, &CB1Value ); - smSetParameter( smhandle, smaddress, SMP_CONTROL_BITS1, 0);//disable drive - } - - if(getCumulativeStatus( smhandle )!=SM_OK ) - return CFGCommunicationError; - - f=fopen(filename,"rb"); - if(f==NULL) - return CFGUnableToOpenFile; - - smDebug(smhandle,Low,"Setting parameters\n"); - - int i=1; - smbool readOk; - do - { - Parameter param; - readOk=parseParameter(f,i,¶m); - - if(readOk==smtrue && param.readOnly==smfalse) - { - smint32 currentValue; - - int configFileValue=round(param.value*param.scale-param.offset); - - //set parameter to device - if(smRead1Parameter( smhandle, smaddress, param.address, ¤tValue )==SM_OK) - { - if(currentValue!=configFileValue ) //set only if different - { - resetCumulativeStatus( smhandle ); - smint32 dummy; - smint32 cmdSetAddressStatus; - smint32 cmdSetValueStatus; - - //use low level SM commands so we can get execution status of each subpacet: - smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, SMP_RETURN_PARAM_LEN ); - smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_24B, SMPRET_CMD_STATUS ); - smAppendSMCommandToQueue( smhandle, SM_SET_WRITE_ADDRESS, param.address ); - smAppendSMCommandToQueue( smhandle, SM_WRITE_VALUE_32B, configFileValue ); - smExecuteCommandQueue( smhandle, smaddress ); - smGetQueuedSMCommandReturnValue( smhandle, &dummy ); - smGetQueuedSMCommandReturnValue( smhandle, &dummy ); - smGetQueuedSMCommandReturnValue( smhandle, &cmdSetAddressStatus ); - smGetQueuedSMCommandReturnValue( smhandle, &cmdSetValueStatus ); - - //check if above code succeed - if( getCumulativeStatus(smhandle)!=SM_OK || cmdSetAddressStatus!=SMP_CMD_STATUS_ACK || cmdSetValueStatus!=SMP_CMD_STATUS_ACK ) - { - SM_STATUS stat=getCumulativeStatus(smhandle); - setErrors++; - smDebug(smhandle,Low,"Failed to write parameter value %d to address %d (status: %d %d %d)\n",configFileValue,param.address,(int)stat,cmdSetAddressStatus,cmdSetValueStatus); - } - - changed++; - } - } - else//device doesn't have such parameter. perhaps wrong model or fw version. - { - ignoredCount++; - smDebug(smhandle,Low,"Ignoring parameter parameter value %d to address %d\n",configFileValue,param.address); - } - } - - i++; - } while(readOk==smtrue); - - *skippedCount=ignoredCount; - *errorCount=setErrors; - - resetCumulativeStatus( smhandle ); - - //save to flash if some value was changed - if(changed>0) - smSetParameter( smhandle, smaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG ); - - if(mode&CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG ) - { - smDebug(smhandle,Low,"Restting faults\n"); - smSetParameter( smhandle, smaddress, SMP_FAULTS, 0 );//reset faults - } - - //re-enable drive - if(mode&CONFIGMODE_DISABLE_DURING_CONFIG) - { - smDebug(smhandle,Low,"Restoring CONTROL_BITS1 to value 0x%x\n",CB1Value); - smSetParameter( smhandle, smaddress, SMP_CONTROL_BITS1, CB1Value );//restore controbits1 (enable if it was enabled before) - } - - smint32 statusbits; - smRead1Parameter( smhandle, smaddress, SMP_STATUS, &statusbits ); - - //restart drive if necessary or if forced - if( (statusbits&STAT_PERMANENT_STOP) || (mode&CONFIGMODE_ALWAYS_RESTART_TARGET) ) - { - smDebug(smhandle,Low,"Restarting device\n"); - smSetParameter( smhandle, smaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART ); - sleep_ms(2000);//wait power-on - } - - if(getCumulativeStatus(smhandle)!=SM_OK) - return CFGCommunicationError; - - return CFGComplete; -} - -/** - * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed - * @param smhandle SM bus handle, must be opened before call - * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. - * @param UID result will be written to this pointer - * @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version) - */ -smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID )//FIXME gives questionable value if device is in DFU mode. Check FW side. -{ - smint32 fwBinaryChecksum; - resetCumulativeStatus(smhandle); - smSetParameter( smhandle, deviceaddress, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA); - smRead1Parameter( smhandle, deviceaddress, SMP_DEBUGPARAM1, &fwBinaryChecksum ); - *UID=(smuint32) fwBinaryChecksum; - - if(getCumulativeStatus(smhandle)==SM_OK) - return smtrue; - else - return smfalse; -} - -FirmwareUploadStatus verifyFirmwareData(smuint8 *data, smuint32 numbytes, int connectedDeviceTypeId, - smuint32 *primaryMCUDataOffset, smuint32 *primaryMCUDataLenth, - smuint32 *secondaryMCUDataOffset,smuint32 *secondaryMCUDataLength ) -{ - //see http://granitedevices.com/wiki/Argon_firmware_file_format - - smuint32 filetype; - filetype=((smuint32*)data)[0]; - if(filetype!=0x57464447) //check header string "GDFW" - return FWInvalidFile; - - smuint16 filever, deviceid; - smuint32 primaryMCUSize, secondaryMCUSize; - - filever=((smuint16*)data)[2]; - deviceid=((smuint16*)data)[3]; - primaryMCUSize=((smuint32*)data)[2]; - secondaryMCUSize=((smuint32*)data)[3]; - if(secondaryMCUSize==0xffffffff) - secondaryMCUSize=0;//it is not present - - if(filever!=300) - return FWIncompatibleFW; - - if(deviceid/1000!=connectedDeviceTypeId/1000)//compare only device and model family. AABBCC so AAB is compared value, ARGON=004 IONI=011 - return FWIncompatibleFW; - - //get checksum and check it - smuint32 cksum,cksumcalc=0; - smuint32 i; - smuint32 cksumOffset=4+2+2+4+4+primaryMCUSize+secondaryMCUSize; - if(cksumOffset>numbytes-4) - return FWInvalidFile; - cksum=((smuint32*)((smuint32)data+cksumOffset))[0]; - - for(i=0;i< numbytes-4;i++) - { - cksumcalc+=data[i]; - } - - if(cksum!=cksumcalc) - return FWIncompatibleFW; - - //let caller know where the firmware data is located in buffer - *primaryMCUDataOffset=4+2+2+4+4; - *primaryMCUDataLenth=primaryMCUSize; - *secondaryMCUDataOffset=*primaryMCUDataOffset+*primaryMCUDataLenth; - *secondaryMCUDataLength=secondaryMCUSize; - - return FWComplete; -} - -smbool loadBinaryFile( const char *filename, smuint8 **data, int *numbytes ) -{ - FILE *f; - f=fopen(filename,"rb"); - if(f==NULL) - return smfalse; - - *numbytes=0; - - //get length - fseek(f,0,SEEK_END); - int length=ftell(f); - fseek(f,0,SEEK_SET); - - //allocate buffer - *data=malloc(length); - if(*data==NULL) - return smfalse; - - //read - *numbytes=fread(*data,1,length,f); - if(*numbytes!=length)//failed to read it all - { - free(*data); - *numbytes=0; - return smfalse; - } - - return smtrue;//successl -} - - - -//flashing STM32 (host side mcu) -smbool flashFirmwarePrimaryMCU( smbus smhandle, int deviceaddress, const smuint8 *data, smint32 size, int *progress ) -{ - smint32 ret; - static smint32 deviceType, fwVersion; - static int uploadIndex; - int c; - const int BL_CHUNK_LEN=32; - static enum {Init,Upload,Finish} state=Init; - - if(state==Init) - { - resetCumulativeStatus( smhandle ); - smRead2Parameters( smhandle, deviceaddress, SMP_FIRMWARE_VERSION, &fwVersion, SMP_DEVICE_TYPE,&deviceType ); - - if(getCumulativeStatus(smhandle)!=SM_OK) - { - state=Init; - return smfalse; - } - -/* kommentoitu pois koska ei haluta erasoida parskuja koska parametri SMO ei saisi nollautua mielellään - - if(deviceType!=4000)//argon does not support BL function 11 - smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,11);//BL func on ioni 11 = do mass erase on STM32, also confifuration - else//does not reset on ioni and drives that support preserving settings. but resets on argon -*/ - smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,1);//BL func 1 = do mass erase on STM32. On Non-Argon devices it doesn't reset confifuration - - sleep_ms(2000);//wait some time. 500ms too little 800ms barely enough - - //flash - smSetParameter(smhandle,deviceaddress,SMP_RETURN_PARAM_LEN, SMPRET_CMD_STATUS); - - if(getCumulativeStatus(smhandle)!=SM_OK) - { - state=Init; - return smfalse; - } - - state=Upload; - uploadIndex=0; - *progress=5; - } - else if(state==Upload) - { - size/=2;//bytes to 16 bit words - - //upload data in 32=BL_CHUNK_LEN word chunks - for(;uploadIndex=size) - upword=0xeeee; - else - upword=((smuint16*)data)[uploadIndex]; - smAppendSMCommandToQueue( smhandle, SMPCMD_24B, upword ); - uploadIndex++; - } - - smAppendSMCommandToQueue( smhandle, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION ); - smAppendSMCommandToQueue( smhandle, SMPCMD_24B, 2);//BL func 2 = do write on STM32 - smExecuteCommandQueue( smhandle, deviceaddress ); - - //read return packets - for(c=0;c=94)//95 will indicate that progress is complete. dont let it indicate that yet. - *progress=94; - - if(uploadIndex%256==0) - { - //printf("upload %d\n",uploadIndex); - return smtrue;//in progress. return often to make upload non-blocking - } - } - if(uploadIndex>=size)//finished - { - state=Finish; - } - } - else if(state==Finish) - { - //verify STM32 flash if supported by BL version - if(fwVersion>=1210) - { - smSetParameter(smhandle,deviceaddress,SMP_BOOTLOADER_FUNCTION,3);//BL func 3 = verify STM32 FW integrity - smint32 faults; - smRead1Parameter(smhandle,deviceaddress,SMP_FAULTS,&faults); - - if(getCumulativeStatus(smhandle)!=SM_OK) - { - state=Init; - *progress=0; - return smfalse; - } - - if(faults&FLT_FLASHING_COMMSIDE_FAIL) - { - //printf("verify failed\n"); - *progress=0; - state=Init; - return smfalse; - } - else - { - //printf("verify success\n"); - } - } - - *progress=95;//my job is complete - state=Init; - } - - return smtrue; -} - - -typedef enum { StatIdle=0, StatEnterDFU, StatFindDFUDevice, StatLoadFile, StatUpload, StatLaunch } UploadState;//state machine status - -//free buffer and return given status code -FirmwareUploadStatus abortFWUpload( FirmwareUploadStatus stat, smuint8 *fwData, UploadState *state, int errorDetailCode ) -{ - globalErrorDetailCode=errorDetailCode; - *state=StatIdle; - free(fwData); - return stat; -} - -/** - * @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error). - * @param smhandle SM bus handle, must be opened before call - * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. - * @param filename .gdf file name - * @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100. - */ -FirmwareUploadStatus smFirmwareUpload( const smbus smhandle, const int smaddress, const char *firmware_filename ) -{ - static smuint8 *fwData=NULL; - static int fwDataLength; - static smuint32 primaryMCUDataOffset, primaryMCUDataLenth; - static smuint32 secondaryMCUDataOffset,secondaryMCUDataLength; - static UploadState state=StatIdle;//state machine status - static smint32 deviceType=0; - static int DFUAddress; - static int progress=0; - - SM_STATUS stat; - - //state machine - if(state==StatIdle) - { - - //check if device is in DFU mode already - smint32 busMode; - stat=smRead2Parameters(smhandle,smaddress,SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); - if(stat==SM_OK && busMode==SMP_BUS_MODE_DFU) - { - state=StatLoadFile; - } - else if(stat==SM_OK && busMode!=0)//device not in bus mode - { - if(deviceType==4000)//argon does not support restarting in DFU mode by software - { - return abortFWUpload(FWConnectionError,fwData,&state,200); - } - - //restart device into DFU mode - state=StatEnterDFU; - - stat=smSetParameter(smhandle,smaddress,SMP_SYSTEM_CONTROL,64);//reset device to DFU command - if(stat!=SM_OK) - return abortFWUpload(FWConnectionError,fwData,&state,300); - } - else - state=StatFindDFUDevice;//search DFU device in brute force, fallback for older BL versions that don't preserve same smaddress than non-DFU mode - //return abortFWUpload(FWConnectionError,fwData,&state,301); - - progress=1; - DFUAddress=smaddress; - } - - else if(state==StatEnterDFU) - { - sleep_ms(2500);//wait device to reboot in DFU mode. probably shorter delay would do. - - //check if device is in DFU mode already - smint32 busMode; - stat=smRead2Parameters(smhandle,smaddress, SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); - if(stat==SM_OK && busMode==0)//busmode 0 is DFU mode - { - state=StatLoadFile; - } - else - state=StatFindDFUDevice;//search DFU device in brute force, fallback for older BL versions that don't preserve same smaddress than non-DFU mode - - progress=2; - } - - else if(state==StatFindDFUDevice) - { - int i; - for(i=245;i<=255;i++) - { - smint32 busMode; - stat=smRead2Parameters(smhandle,i, SMP_BUS_MODE,&busMode, SMP_DEVICE_TYPE, &deviceType); - if(stat==SM_OK && busMode==0)//busmode 0 is DFU mode - { - state=StatLoadFile; - DFUAddress=i; - break;//DFU found, break out of for loop - } - } - - if(i==256)//DFU device not found - return abortFWUpload(CFGConnectingDFUModeFailed,fwData,&state,400);//setting DFU mode failed - - progress=3; - } - - else if(state==StatLoadFile) - { - if(loadBinaryFile(firmware_filename,&fwData,&fwDataLength)!=smtrue) - return FWFileNotReadable; - - FirmwareUploadStatus stat=verifyFirmwareData(fwData, fwDataLength, deviceType, - &primaryMCUDataOffset, &primaryMCUDataLenth, - &secondaryMCUDataOffset, &secondaryMCUDataLength); - if(stat!=FWComplete)//error in verify - { - return abortFWUpload(stat,fwData,&state,100); - } - - //all good, upload firmware - state=StatUpload; - - progress=4; - } - - else if(state==StatUpload) - { - smbool ret=flashFirmwarePrimaryMCU(smhandle,DFUAddress,fwData+primaryMCUDataOffset,primaryMCUDataLenth,&progress); - if(ret==smfalse)//failed - { - return abortFWUpload(FWConnectionError,fwData,&state,1000); - } - else - { - if(progress>=95) - state=StatLaunch; - } - } - - else if(state==StatLaunch) - { - smSetParameter(smhandle,DFUAddress,SMP_BOOTLOADER_FUNCTION,4);//BL func 4 = launch. - sleep_ms(2000); - progress=100; - state=StatIdle; - } - - return (FirmwareUploadStatus)progress; -} - - - - diff --git a/smdeploymenttool.h b/smdeploymenttool.h deleted file mode 100644 index 8838664..0000000 --- a/smdeploymenttool.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef SMDEPLOYMENTTOOL_H -#define SMDEPLOYMENTTOOL_H - -#ifdef WIN32 -//dll specs -#ifdef BUILD_DLL - #define LIB __declspec(dllexport) -#else -// #define LIB __declspec(dllimport) -#define LIB -#endif -#else -#define LIB -#endif - - -#include "simplemotion.h" - - -#ifdef __cplusplus -extern "C"{ -#endif - - -typedef enum -{ - FWComplete=100, - FWInvalidFile=-1, - FWConnectionError=-2, - FWIncompatibleFW=-3, - FWConnectionLoss=-4, - FWUnsupportedTargetDevice=-5, - FWFileNotReadable=-6 -} FirmwareUploadStatus; - -/** - * @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error). - * @param smhandle SM bus handle, must be opened before call - * @param smaddress Target SM device address - * @param filename .gdf file name - * @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100. - */ -LIB FirmwareUploadStatus smFirmwareUpload(const smbus smhandle, const int smaddress, const char *firmware_filename ); - -typedef enum -{ - CFGComplete=100, - CFGInvalidFile=-1, - CFGCommunicationError=-2, - CFGConnectingDFUModeFailed=-3, - CFGIncompatibleFW=-4, - CFGUnsupportedTargetDevice=-5, - CFGUnableToOpenFile=-6 - -} LoadConfigurationStatus; - -//TODO implement: #define CONFIGMODE_REQUIRE_SAME_FW 1 //will return IncompatibleFW if firmware checksum does not match the one in .drc files. if this error is returned, perform smFirmwareUpload and perform smLoadConfiguration again. Requires DRC file version 111 or later (if not met, returns InvalidFile). -#define CONFIGMODE_ALWAYS_RESTART_TARGET 2 //will perform device restart after setup even when it's not required -#define CONFIGMODE_DISABLE_DURING_CONFIG 4 //will set device in disabled state during configuration -#define CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG 8 //will perform clear faults command after configuration - -/** - * @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. - * @param smhandle SM bus handle, must be opened before call - * @param smaddress Target SM device address - * @param filename .DRC file name - * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). - * @return Enum LoadConfigurationStatus - * - * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. - */ -LIB LoadConfigurationStatus smLoadConfiguration( const smbus smhandle, const int smaddress, const char *filename, unsigned int mode, int *skippedCount, int *errorCount ); - - -/** - * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed - * @param smhandle SM bus handle, must be opened before call - * @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255. - * @param UID result will be written to this pointer - * @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version) - */ -smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID ); - - - -#ifdef __cplusplus -} -#endif -#endif // SMDEPLOYMENTTOOL_H -- cgit v1.2.3 From 52bf3a6b791a381cf08a3ac167fac7375459aab9 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 6 Apr 2017 17:19:06 +0300 Subject: Add missing fclose calls --- devicedeployment.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/devicedeployment.c b/devicedeployment.c index bdb718d..54f07e0 100644 --- a/devicedeployment.c +++ b/devicedeployment.c @@ -226,6 +226,8 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad i++; } while(readOk==smtrue); + fclose(f); + *skippedCount=ignoredCount; *errorCount=setErrors; @@ -355,7 +357,10 @@ smbool loadBinaryFile( const char *filename, smuint8 **data, int *numbytes ) //allocate buffer *data=malloc(length); if(*data==NULL) + { + fclose(f); return smfalse; + } //read *numbytes=fread(*data,1,length,f); @@ -363,9 +368,11 @@ smbool loadBinaryFile( const char *filename, smuint8 **data, int *numbytes ) { free(*data); *numbytes=0; + fclose(f); return smfalse; } + fclose(f); return smtrue;//successl } -- cgit v1.2.3 From 7dc56301dd8388cdf60e0c3270ec99b403c6063f Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Fri, 7 Apr 2017 15:33:40 +0300 Subject: Switch using stdint.h types --- simplemotion.c | 2 +- simplemotion.h | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/simplemotion.c b/simplemotion.c index 9a0b3fb..d78e511 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -158,7 +158,7 @@ SM_STATUS smSetTimeout( smuint16 millsecs ) return SM_ERR_PARAMETER; } -unsigned long smGetVersion() +smuint32 smGetVersion() { return SM_VERSION; } diff --git a/simplemotion.h b/simplemotion.h index 725d3dc..dcf499d 100644 --- a/simplemotion.h +++ b/simplemotion.h @@ -17,6 +17,7 @@ #endif #include +#include #include "simplemotion_defs.h" @@ -38,13 +39,13 @@ extern "C"{ /////////////////////////////////////////////////////////////////////////////////////// //declare SM lib types typedef long smbus; -typedef unsigned long smuint32; -typedef unsigned short smuint16; -typedef unsigned char smuint8; -typedef long smint32; -typedef short smint16; -typedef char smint8; -typedef char smbool; +typedef uint32_t smuint32; +typedef uint16_t smuint16; +typedef uint8_t smuint8; +typedef int32_t smint32; +typedef int16_t smint16; +typedef int8_t smint8; +typedef int8_t smbool; #define smtrue 1 #define smfalse 0 typedef int SM_STATUS; -- cgit v1.2.3 From 6996814437efb94368aac8db4d5344d59ccb7b7e Mon Sep 17 00:00:00 2001 From: Oskari Timperi Date: Mon, 10 Apr 2017 14:35:15 +0300 Subject: Support serial port on OS X --- pcserialport.c | 102 ++++++++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 86 insertions(+), 16 deletions(-) diff --git a/pcserialport.c b/pcserialport.c index 2b44add..d9cd0f8 100644 --- a/pcserialport.c +++ b/pcserialport.c @@ -14,7 +14,7 @@ #include "pcserialport.h" #include "simplemotion_private.h" //needed for timeout variable -#ifdef __unix__ +#if defined(__unix__) || defined(__APPLE__) #include #include @@ -25,59 +25,129 @@ #include #include +#if defined(__APPLE__) +#include +#include +#include +#include +#include +#endif + smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps) { int port_handle; int err; int baudrateEnumValue; struct termios new_port_settings; + bool customBaudRate = false; + + port_handle = open(port_device_name, O_RDWR | O_NOCTTY); + + if(port_handle==-1) + { + smDebug(-1, Low, "Serial port error: port open failed"); + return(port_handle); + } switch(baudrate_bps) { +#if defined(B9600) case 9600 : baudrateEnumValue = B9600; break; +#endif +#if defined(B19200) case 19200 : baudrateEnumValue = B19200; break; +#endif +#if defined(B38400) case 38400 : baudrateEnumValue = B38400; break; +#endif +#if defined(B57600) case 57600 : baudrateEnumValue = B57600; break; +#endif +#if defined(B115200) case 115200 : baudrateEnumValue = B115200; break; +#endif +#if defined(B230400) case 230400 : baudrateEnumValue = B230400; break; +#endif +#if defined(B460800) case 460800 : baudrateEnumValue = B460800; break; +#endif +#if defined(B500000) case 500000 : baudrateEnumValue = B500000; break; +#endif +#if defined(B576000) case 576000 : baudrateEnumValue = B576000; break; +#endif +#if defined(B921600) case 921600 : baudrateEnumValue = B921600; break; +#endif +#if defined(B1000000) case 1000000 : baudrateEnumValue = B1000000; break; +#endif +#if defined(B1152000) case 1115200 : baudrateEnumValue = B1152000; break; +#endif +#if defined(B1500000) case 1500000 : baudrateEnumValue = B1500000; break; +#endif +#if defined(B2000000) case 2000000 : baudrateEnumValue = B2000000; break; +#endif +#if defined(B2500000) case 2500000 : baudrateEnumValue = B2500000; break; +#endif +#if defined(B3000000) case 3000000 : baudrateEnumValue = B3000000; break; +#endif +#if defined(B3500000) case 3500000 : baudrateEnumValue = B3500000; break; +#endif +#if defined(B4000000) case 4000000 : baudrateEnumValue = B4000000; break; - default : smDebug(-1,Low,"Serial port error: unsupported baudrate\n"); - return(1); - break; - } - - port_handle = open(port_device_name, O_RDWR | O_NOCTTY ); - if(port_handle==-1) - { - smDebug(-1, Low, "Serial port error: port open failed"); - return(port_handle); +#endif + default: + customBaudRate = true; +#if defined(__APPLE__) + if (ioctl(port_handle, IOSSIOSPEED, &baudrate_bps) == -1) + { + smDebug(-1, Low, "Serial port error: unsupported baudrate\n"); + close(port_handle); + return -1; + } +#else + smDebug(-1,Low,"Serial port error: unsupported baudrate\n"); + close(port_handle); + return(-1); + break; +#endif } memset(&new_port_settings, 0, sizeof(new_port_settings)); //reset struct - new_port_settings.c_cflag = baudrateEnumValue | CS8 | CLOCAL | CREAD; + new_port_settings.c_cflag = CS8 | CLOCAL | CREAD; new_port_settings.c_iflag = IGNPAR; new_port_settings.c_oflag = 0; new_port_settings.c_lflag = 0; new_port_settings.c_cc[VMIN] = 0; /* non blocking mode */ new_port_settings.c_cc[VTIME] = readTimeoutMs/100; /* timeout 100 ms steps */ - err = tcsetattr(port_handle, TCSANOW, &new_port_settings); + + if (!customBaudRate) + { +#if defined(_BSD_SOURCE) + cfsetspeed(&new_port_settings, baudrateEnumValue); +#else + cfsetispeed(&new_port_settings, baudrateEnumValue); + cfsetospeed(&new_port_settings, baudrateEnumValue); +#endif + } + + // Activate settings + err = tcsetattr(port_handle, TCSANOW, &new_port_settings); if(err==-1) { - close(port_handle); - smDebug(-1, Low, "Serial port error: failed to set port parameters"); - return -1; + close(port_handle); + smDebug(-1, Low, "Serial port error: failed to set port parameters"); + return -1; } //flush any stray bytes from device receive buffer that may reside in it -- cgit v1.2.3 From c576063923d927d1f8c8d5d3acf1b2eee18297db Mon Sep 17 00:00:00 2001 From: Oskari Timperi Date: Tue, 11 Apr 2017 09:38:28 +0300 Subject: Add TCP/IP support to busdevice.c --- SimpleMotionV2.pri | 4 +- busdevice.c | 168 +++++++++++++++++++++++++++++++++++++++++++++-- tcpclient.c | 189 +++++++++++++++++++++++++++++++++++++++++++++++++++++ tcpclient.h | 22 +++++++ 4 files changed, 376 insertions(+), 7 deletions(-) create mode 100644 tcpclient.c create mode 100644 tcpclient.h diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri index b5619bc..79bfc56 100644 --- a/SimpleMotionV2.pri +++ b/SimpleMotionV2.pri @@ -6,9 +6,9 @@ DEPENDPATH += $$PWD DEFINES += SIMPLEMOTIONV2_LIBRARY SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/pcserialport.c \ - $$PWD/bufferedmotion.c + $$PWD/bufferedmotion.c $$PWD/tcpclient.c HEADERS += $$PWD/simplemotion_private.h\ $$PWD/pcserialport.h $$PWD/busdevice.h $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h \ - $$PWD/bufferedmotion.h + $$PWD/bufferedmotion.h $$PWD/tcpclient.h diff --git a/busdevice.c b/busdevice.c index c4f89a1..6a89396 100644 --- a/busdevice.c +++ b/busdevice.c @@ -1,11 +1,17 @@ #include "busdevice.h" #include "pcserialport.h" +#include "tcpclient.h" + #include +#include +#include +#include #define BD_NONE 0 #define BD_RS 1 #define BD_FTDI 2 +#define BD_TCP 3 //how much bytes available in transmit buffer #define TANSMIT_BUFFER_LENGTH 128 @@ -46,6 +52,116 @@ void smBDinit() bdInitialized=smtrue; } +static int validateIpAddress(const char *s, const char **pip_end, + const char **pport_start) +{ + int octets = 0; + int ch = 0, prev = 0; + int len = 0; + const char *ip_end = NULL; + const char *port_start = NULL; + + while (*s) + { + ch = *s; + + if (isdigit(ch)) + { + ++len; + // Octet len must be 1-3 digits + if (len > 3) + { + return -1; + } + } + else if (ch == '.' && isdigit(prev)) + { + ++octets; + len = 0; + // No more than 4 octets please + if (octets > 4) + { + return -1; + } + } + else if (ch == ':' && isdigit(prev)) + { + ++octets; + // We want exactly 4 octets at this point + if (octets != 4) + { + return -1; + } + ip_end = s; + ++s; + port_start = s; + while (isdigit((ch = *s))) + ++s; + // After port we want the end of the string + if (ch != '\0') + return -1; + // This will skip over the ++s below + continue; + } + else + { + return -1; + } + + prev = ch; + ++s; + } + + // We reached the end of the string and did not encounter the port + if (*s == '\0' && ip_end == NULL) + { + ++octets; + ip_end = s; + } + + // Check that there are exactly 4 octets + if (octets != 4) + return -1; + + if (pip_end) + *pip_end = ip_end; + + if (pport_start) + *pport_start = port_start; + + return 0; +} + +static int parseIpAddress(const char *s, char *ip, size_t ipsize, short *port) +{ + const char *ip_end, *port_start; + + if (validateIpAddress(s, &ip_end, &port_start) == -1) + return -1; + + // If ip=NULL, we just report that the parsing was ok + if (!ip) + return 0; + + if (ipsize < ip_end - s + 1) + return -1; + + memcpy(ip, s, ip_end - s); + ip[ip_end - s] = '\0'; + + if (port_start) + { + *port = 0; + while (*port_start) + { + *port = *port * 10 + (*port_start - '0'); + ++port_start; + } + } + + return 0; +} + //ie "COM1" "VSD2USB" //return -1 if fails, otherwise handle number smbusdevicehandle smBDOpen( const char *devicename ) @@ -75,10 +191,24 @@ smbusdevicehandle smBDOpen( const char *devicename ) BusDevice[handle].bdType=BD_RS; BusDevice[handle].txBufferUsed=0; } - else//no other bus types supproted yet - { - return -1; - } + else if (validateIpAddress(devicename, NULL, NULL) == 0) + { + char ip[128]; + short port = 4001; + if (parseIpAddress(devicename, ip, sizeof(ip), &port) < 0) + return -1; + BusDevice[handle].comPort=OpenTCPPort( ip, port ); + if( BusDevice[handle].comPort == -1 ) + { + return -1; //failed to open + } + BusDevice[handle].bdType=BD_TCP; + BusDevice[handle].txBufferUsed=0; + } + else//no other bus types supproted yet + { + return -1; + } //success BusDevice[handle].cumulativeSmStatus=0; @@ -105,6 +235,12 @@ smbool smBDClose( const smbusdevicehandle handle ) BusDevice[handle].opened=smfalse; return smtrue; } + else if( BusDevice[handle].bdType==BD_TCP ) + { + CloseTCPport( BusDevice[handle].comPort ); + BusDevice[handle].opened=smfalse; + return smtrue; + } return smfalse; } @@ -119,7 +255,7 @@ smbool smBDWrite(const smbusdevicehandle handle, const smuint8 byte ) //check if handle valid & open if( smIsBDHandleOpen(handle)==smfalse ) return smfalse; - if( BusDevice[handle].bdType==BD_RS ) + if( BusDevice[handle].bdType==BD_RS || BusDevice[handle].bdType==BD_TCP ) { if(BusDevice[handle].txBufferUsed + +#if defined(_WIN32) +#if defined(CM_NONE) +#undef CM_NONE +#endif +#include +#include +#define close closesocket +#define read(SOCKET, BUF, LEN) recv((SOCKET), (BUF), (LEN), 0) +#define write(SOCKET, BUF, LEN) send((SOCKET), (BUF), (LEN), 0) +#ifdef errno +#undef errno +#endif +#define errno (WSAGetLastError()) +#ifdef EINPROGRESS +#undef EINPROGRESS +#endif +#define EINPROGRESS WSAEWOULDBLOCK +#else +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#if defined(_WIN32) +static int initwsa() +{ + WORD req; + WSADATA data; + req = MAKEWORD(2, 2); + int err = WSAStartup(req, &data); + if (err != 0) + { + printf("WSAStartup failed\n"); + return 0; + } + return 1; +} +#endif + +int OpenTCPPort(const char * ip_addr, int port) +{ + int sockfd; + struct sockaddr_in server; + struct timeval tv; + fd_set myset; + int res, valopt; + socklen_t lon; + long arg; + +#if defined(_WIN32) + initwsa(); +#endif + + //Create socket + sockfd = socket(AF_INET , SOCK_STREAM , IPPROTO_TCP); + if (sockfd == -1) + { + return -1; + } + + // Set OFF NAGLE algorithm to reduce latency with small packets + /* + int one = 1; + setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (void *)&one, sizeof(one)); + */ + + server.sin_addr.s_addr = inet_addr(ip_addr); + server.sin_family = AF_INET; + server.sin_port = htons(port); + + // Set non-blocking when trying to establish the connection +#if !defined(_WIN32) + arg = fcntl(sockfd, F_GETFL, NULL); + arg |= O_NONBLOCK; + fcntl(sockfd, F_SETFL, arg); +#else + arg = 1; + ioctlsocket(sockfd, FIONBIO, &arg); +#endif + + res = connect(sockfd, (struct sockaddr *)&server, sizeof(server)); + + if (res < 0) + { + if (errno == EINPROGRESS) + { + tv.tv_sec = 5; + tv.tv_usec = 0; + FD_ZERO(&myset); + FD_SET(sockfd, &myset); + if (select(sockfd+1, NULL, &myset, NULL, &tv) > 0) + { + lon = sizeof(int); + getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void*)(&valopt), &lon); + if (valopt) + { + return -1; + } + } + else + { + return -1; + } + } + else + { + return -1; + } + } + + // Set to blocking mode again +#if !defined(_WIN32) + arg = fcntl(sockfd, F_GETFL, NULL); + arg &= (~O_NONBLOCK); + fcntl(sockfd, F_SETFL, arg); +#else + arg = 0; + ioctlsocket(sockfd, FIONBIO, &arg); +#endif + + return sockfd; +} + +// Read bytes from socket +int PollTCPPort(int sockfd, unsigned char *buf, int size) +{ + int n; + fd_set input; + FD_ZERO(&input); + FD_SET(sockfd, &input); + struct timeval timeout; + timeout.tv_sec = 0; + timeout.tv_usec = readTimeoutMs * 1000; + + n = select(sockfd + 1, &input, NULL, NULL, &timeout); + + // Error or timeout + if (n < 1) + { + return(-1); + } + if(!FD_ISSET(sockfd, &input)) + { + return(-1); + } + + n = read(sockfd, buf, size); + return(n); +} + +int SendTCPByte(int sockfd, unsigned char byte) +{ + int n; + n = write(sockfd, &byte, 1); + if(n<0) + return(1); + return(0); +} + + +int SendTCPBuf(int sockfd, unsigned char *buf, int size) +{ + int sent = write(sockfd, buf, size); + if (sent != size) + { + return sent; + } + return sent; +} + + +void CloseTCPport(int sockfd) +{ + close(sockfd); +#if defined(_WIN32) + WSACleanup(); +#endif +} diff --git a/tcpclient.h b/tcpclient.h new file mode 100644 index 0000000..ea02e42 --- /dev/null +++ b/tcpclient.h @@ -0,0 +1,22 @@ +#ifndef tcpclient_INCLUDED +#define tcpclient_INCLUDED + +#ifdef __cplusplus +extern "C" { +#endif + +//return port handle or -1 if fails +int OpenTCPPort(const char * ip_addr, int port); +int PollTCPPort(int, unsigned char *, int); +int SendTCPByte(int, unsigned char); +int SendTCPBuf(int, unsigned char *, int); +void CloseTCPport(int); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif + + -- cgit v1.2.3 From 2760f444b7c1cec95753778ff16b33782421f483 Mon Sep 17 00:00:00 2001 From: Oskari Timperi Date: Tue, 11 Apr 2017 10:50:08 +0300 Subject: Turn a lonely bool variable into an int variable --- pcserialport.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pcserialport.c b/pcserialport.c index d9cd0f8..35a7668 100644 --- a/pcserialport.c +++ b/pcserialport.c @@ -39,7 +39,7 @@ smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps) int err; int baudrateEnumValue; struct termios new_port_settings; - bool customBaudRate = false; + int customBaudRate = 0; port_handle = open(port_device_name, O_RDWR | O_NOCTTY); @@ -106,7 +106,7 @@ smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps) case 4000000 : baudrateEnumValue = B4000000; break; #endif default: - customBaudRate = true; + customBaudRate = 1; #if defined(__APPLE__) if (ioctl(port_handle, IOSSIOSPEED, &baudrate_bps) == -1) { -- cgit v1.2.3 From 91e90823cb19d1490d5223d2835f9064ea566c6f Mon Sep 17 00:00:00 2001 From: Oskari Timperi Date: Tue, 11 Apr 2017 13:34:07 +0300 Subject: Fix wrong handles being passed to smBDRead in simplemotion.c smReceiveErrorHandler and smReceiveReturnPacket passed the smbus handle to smBDRead while smBDRead expects the smbusdevicehandle. This also corrects the type of SM_BUS.bdHandle from smint8 to smbusdevicehandle. --- simplemotion.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simplemotion.c b/simplemotion.c index 9a0b3fb..606eda0 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -34,7 +34,7 @@ SM_STATUS smReceiveReturnPacket( smbus bushandle ); typedef struct SM_BUS_ { - smint8 bdHandle; + smbusdevicehandle bdHandle; smbool opened; enum RecvState recv_state,recv_state_next; @@ -420,7 +420,7 @@ SM_STATUS smReceiveErrorHandler( smbus handle, smbool flushrx ) smbool success; do{ smuint8 rx; - success=smBDRead(handle,&rx); + success=smBDRead(smBus[handle].bdHandle,&rx); }while(success==smtrue); } smResetSM485variables(handle); @@ -644,7 +644,7 @@ SM_STATUS smReceiveReturnPacket( smbus bushandle ) smuint8 ret; SM_STATUS stat; - smbool succ=smBDRead(bushandle,&ret); + smbool succ=smBDRead(smBus[bushandle].bdHandle,&ret); if(succ==smfalse) { -- cgit v1.2.3 From 6ed02275c76150f78ce1d4f2571b99d903855cbd Mon Sep 17 00:00:00 2001 From: Oskari Timperi Date: Wed, 12 Apr 2017 12:32:18 +0300 Subject: Fix devicedeployment.c on 64-bit OS X --- devicedeployment.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/devicedeployment.c b/devicedeployment.c index 54f07e0..222992a 100644 --- a/devicedeployment.c +++ b/devicedeployment.c @@ -5,11 +5,11 @@ #include #include -#ifdef __unix__ +#if defined(__unix__) || defined(__APPLE__) #include void sleep_ms(int millisecs) { - msleep(millisecs); + usleep(millisecs*1000); } #else #include @@ -321,7 +321,7 @@ FirmwareUploadStatus verifyFirmwareData(smuint8 *data, smuint32 numbytes, int co smuint32 cksumOffset=4+2+2+4+4+primaryMCUSize+secondaryMCUSize; if(cksumOffset>numbytes-4) return FWInvalidFile; - cksum=((smuint32*)((smuint32)data+cksumOffset))[0]; + cksum=((smuint32*)(data+cksumOffset))[0]; for(i=0;i< numbytes-4;i++) { -- cgit v1.2.3 From 706a71c01664ad990c60f6187eb1886693a93153 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Mon, 17 Apr 2017 22:44:31 +0300 Subject: Fix FW uploader crashing on 64 bit OS X --- devicedeployment.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devicedeployment.c b/devicedeployment.c index 54f07e0..a5078b3 100644 --- a/devicedeployment.c +++ b/devicedeployment.c @@ -321,7 +321,7 @@ FirmwareUploadStatus verifyFirmwareData(smuint8 *data, smuint32 numbytes, int co smuint32 cksumOffset=4+2+2+4+4+primaryMCUSize+secondaryMCUSize; if(cksumOffset>numbytes-4) return FWInvalidFile; - cksum=((smuint32*)((smuint32)data+cksumOffset))[0]; + cksum=((smuint32*)(data+cksumOffset))[0]; for(i=0;i< numbytes-4;i++) { -- cgit v1.2.3 From 4843d7be1b8d65b5e2d97fca889bc1c7588f1f74 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Sat, 6 May 2017 14:32:56 +0300 Subject: Added SMP_CAPTURE_TRIGGER_DELAY that sets scope capture trigger delay. Value 0 is traditional, -n starts capture n sample cycles before trigger, +n after trigger. In negative delay, the minimum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected --- simplemotion_defs.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 138c42b..5f58564 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -714,6 +714,8 @@ #define SMP_CAPTURE_SAMPLERATE 5012 //rdonly #define SMP_CAPTURE_BUF_LENGHT 5013 +//SMP_CAPTURE_TRIGGER_DELAY sets scope capture trigger delay. Value 0 is traditional, -n starts capture n sample cycles before trigger, +n after trigger. In negative delay, the minimum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected. +#define SMP_CAPTURE_TRIGGER_DELAY 5014 //this is looped 0-n to make samples 0-n readable from SMP_CAPTURE_BUFFER_GET_VALUE #define SMP_CAPTURE_BUFFER_GET_ADDR 5333 #define SMP_CAPTURE_BUFFER_GET_VALUE 5334 -- cgit v1.2.3 From 71114c624dff4fe61d9ba5947bcd100505ca62b2 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Sat, 6 May 2017 14:34:51 +0300 Subject: added DEVICE_CAPABILITY_SCOPE_TRIGGER_DELAY --- simplemotion_defs.h | 1 + 1 file changed, 1 insertion(+) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 5f58564..2ad027b 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -737,6 +737,7 @@ #define DEVICE_CAPABILITY_AUTOSETUP_COMMUTATION_SENSOR BV(7) #define DEVICE_CAPABILITY_SENSORLESS_COMMUTATION BV(8) #define DEVICE_CAPABILITY_ANALOG_OUTPUT BV(9) + #define DEVICE_CAPABILITY_SCOPE_TRIGGER_DELAY BV(10) #define SMP_FIRMWARE_VERSION 6010 #define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011 -- cgit v1.2.3 From 7c59a7af9d80aa0d3e8189e419b8b116d868771d Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Sat, 6 May 2017 15:45:01 +0300 Subject: Change SMP_CAPTURE_TRIGGER_DELAY description --- simplemotion_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 2ad027b..65b8930 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -714,7 +714,7 @@ #define SMP_CAPTURE_SAMPLERATE 5012 //rdonly #define SMP_CAPTURE_BUF_LENGHT 5013 -//SMP_CAPTURE_TRIGGER_DELAY sets scope capture trigger delay. Value 0 is traditional, -n starts capture n sample cycles before trigger, +n after trigger. In negative delay, the minimum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected. +//SMP_CAPTURE_TRIGGER_DELAY sets scope capture trigger delay. Value 0 is traditional, +n starts capture n sample cycles before trigger, -n after trigger. In positive delay, the maximum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected. #define SMP_CAPTURE_TRIGGER_DELAY 5014 //this is looped 0-n to make samples 0-n readable from SMP_CAPTURE_BUFFER_GET_VALUE #define SMP_CAPTURE_BUFFER_GET_ADDR 5333 -- cgit v1.2.3 From f357575b3ac2dc9caaf5c9673f0c74c4b7f87d91 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Sun, 7 May 2017 21:51:57 +0300 Subject: - Renamed scope param 5014 as SMP_CAPTURE_BEFORE_TRIGGER_SAMPLE_COUNT - Added support for external trigger input for capture --- simplemotion_defs.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 65b8930..4d438ab 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -710,12 +710,13 @@ #define TRIG_TARGETCHANGE 3 #define TRIG_TARGETCHANGE_POS 4 #define TRIG_SERIALCMD 5 + #define TRIG_EXTERNAL_INPUT 6 #define SMP_CAPTURE_SAMPLERATE 5012 //rdonly #define SMP_CAPTURE_BUF_LENGHT 5013 -//SMP_CAPTURE_TRIGGER_DELAY sets scope capture trigger delay. Value 0 is traditional, +n starts capture n sample cycles before trigger, -n after trigger. In positive delay, the maximum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected. -#define SMP_CAPTURE_TRIGGER_DELAY 5014 +//SMP_CAPTURE_BEFORE_TRIGGER_SAMPLE_COUNT sets how much samples will be preserved before trigger event. Value 0 is traditional, +n starts capture n sample cycles before trigger, -n after trigger. In positive delay, the maximum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected. +#define SMP_CAPTURE_BEFORE_TRIGGER_SAMPLE_COUNT 5014 //this is looped 0-n to make samples 0-n readable from SMP_CAPTURE_BUFFER_GET_VALUE #define SMP_CAPTURE_BUFFER_GET_ADDR 5333 #define SMP_CAPTURE_BUFFER_GET_VALUE 5334 @@ -738,6 +739,7 @@ #define DEVICE_CAPABILITY_SENSORLESS_COMMUTATION BV(8) #define DEVICE_CAPABILITY_ANALOG_OUTPUT BV(9) #define DEVICE_CAPABILITY_SCOPE_TRIGGER_DELAY BV(10) + #define DEVICE_CAPABILITY_SCOPE_EXTERNAL_TRIGGER BV(11) #define SMP_FIRMWARE_VERSION 6010 #define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011 -- cgit v1.2.3 From 46fd11de562cc7824a32b32b5bc074fa23877512 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Mon, 8 May 2017 15:12:54 +0300 Subject: Scope capture changes (mainly used by Granity) --- simplemotion_defs.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 4d438ab..fc8d8c0 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -709,14 +709,15 @@ #define TRIG_FAULT 2 #define TRIG_TARGETCHANGE 3 #define TRIG_TARGETCHANGE_POS 4 - #define TRIG_SERIALCMD 5 - #define TRIG_EXTERNAL_INPUT 6 + #define TRIG_EXTERNAL_INPUT 5 #define SMP_CAPTURE_SAMPLERATE 5012 //rdonly #define SMP_CAPTURE_BUF_LENGHT 5013 -//SMP_CAPTURE_BEFORE_TRIGGER_SAMPLE_COUNT sets how much samples will be preserved before trigger event. Value 0 is traditional, +n starts capture n sample cycles before trigger, -n after trigger. In positive delay, the maximum effective value is SMP_CAPTURE_BUF_LENGHT divided by number of channels selected. -#define SMP_CAPTURE_BEFORE_TRIGGER_SAMPLE_COUNT 5014 +//SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS sets how much samples will be preserved before trigger event. Value 0 is traditional, +n starts capture n percents before trigger (relative to whole capture length), -n after trigger. Value range -1000000%..+100%. +#define SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS 5014 +//SMP_CAPTURE_STATE, states: 0=idle (capture complete or not started), 1=waiting for trigger, 2=capturing. to start capture, write value 1 here starting from IONI FW V1110 +#define SMP_CAPTURE_STATE 5015 //this is looped 0-n to make samples 0-n readable from SMP_CAPTURE_BUFFER_GET_VALUE #define SMP_CAPTURE_BUFFER_GET_ADDR 5333 #define SMP_CAPTURE_BUFFER_GET_VALUE 5334 @@ -738,7 +739,7 @@ #define DEVICE_CAPABILITY_AUTOSETUP_COMMUTATION_SENSOR BV(7) #define DEVICE_CAPABILITY_SENSORLESS_COMMUTATION BV(8) #define DEVICE_CAPABILITY_ANALOG_OUTPUT BV(9) - #define DEVICE_CAPABILITY_SCOPE_TRIGGER_DELAY BV(10) + #define DEVICE_CAPABILITY_SCOPE_TRIGGER_DELAY BV(10) /*also means that params SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS and SMP_CAPTURE_STATE exist */ #define DEVICE_CAPABILITY_SCOPE_EXTERNAL_TRIGGER BV(11) #define SMP_FIRMWARE_VERSION 6010 -- cgit v1.2.3 From b5023d0af5bee1272fcc7217b5d1be23bd113430 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Tue, 9 May 2017 21:59:33 +0300 Subject: Changed definition of SMP_SERIAL_ENC_BITS 574 --- simplemotion_defs.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index fc8d8c0..15a749f 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -450,11 +450,21 @@ /*for BiSS/SSI encoder * bits defined as (from LSB): - * bits 0-7: single turn bits, accepted value range 4-24 + * bits 0-7: single turn bits, accepted value range 4-32 * bits 8-15: multi turn bits, accepted value range 0-16 - * bits 16-18: serial encoder mode: 000=BiSS, 001=SSI, 010=AMS SSI (SSI+CS+error monitoring), 011=SPI (SSI+CS) 100=GRAY SSI (i.e. SICK TTK70) + * ^^ sum of MT and ST must be max 32 + * bits 16-19: serial encoder mode (see below) * rest: reserved for future use (always 0) */ + /* + * Serial encoder read modes + * 0 BiSS C + * 1 BiSS C gray + * 2 BiSS B + * 3 SSI + * 4 SSI gray + * 5 AMS SSI + */ #define SMP_SERIAL_ENC_BITS 574 //primary feedback loop 200-299 -- cgit v1.2.3 From 927d9bdd30027b0e8add782e7f54624463dab369 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Wed, 10 May 2017 01:11:09 +0300 Subject: Changed serial encoder mode definitions --- simplemotion_defs.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 15a749f..a23af35 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -455,15 +455,13 @@ * ^^ sum of MT and ST must be max 32 * bits 16-19: serial encoder mode (see below) * rest: reserved for future use (always 0) - */ - /* - * Serial encoder read modes + * + * Serial encoder read modes (bits 16-19): * 0 BiSS C - * 1 BiSS C gray - * 2 BiSS B - * 3 SSI - * 4 SSI gray - * 5 AMS SSI + * 1 BiSS B + * 2 SSI + * 3 SSI gray + * 4 AMS SSI */ #define SMP_SERIAL_ENC_BITS 574 -- cgit v1.2.3 From 1f53eeda1b497f044e76affc784e5123be172f44 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 11 May 2017 18:57:12 +0300 Subject: Redefine DEVICE_CAPABILITIES parameters --- simplemotion_defs.h | 47 ++++++++++++++++++++++++++++++++++------------- 1 file changed, 34 insertions(+), 13 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 20d2c88..619c1c1 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -749,19 +749,40 @@ //read only bit field that is can be used to identify device capabilities //the list below is subject to extend -#define SMP_DEVICE_CAPABILITIES 6006 - #define DEVICE_CAPABILITY_HOMING BV(0) - #define DEVICE_CAPABILITY_SERIAL_FBD BV(1) - #define DEVICE_CAPABILITY_RESTORE_SAVED_CONFIG BV(2) - #define DEVICE_CAPABILITY_MEASURE_RL BV(3) - #define DEVICE_CAPABILITY_TORQUE_RIPPLE_COMPENSATION BV(4) - #define DEVICE_CAPABILITY_NOTCH_FILTER BV(5) - #define DEVICE_CAPABILITY_TORQUE_EFFECTS BV(6) - #define DEVICE_CAPABILITY_AUTOSETUP_COMMUTATION_SENSOR BV(7) - #define DEVICE_CAPABILITY_SENSORLESS_COMMUTATION BV(8) - #define DEVICE_CAPABILITY_ANALOG_OUTPUT BV(9) - #define DEVICE_CAPABILITY_SCOPE_TRIGGER_DELAY BV(10) /*also means that params SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS and SMP_CAPTURE_STATE exist */ - #define DEVICE_CAPABILITY_SCOPE_EXTERNAL_TRIGGER BV(11) +#define SMP_DEVICE_CAPABILITIES1 6006 + #define DEVICE_CAPABILITY1_PMDC BV(0) + #define DEVICE_CAPABILITY1_PMAC BV(1) + #define DEVICE_CAPABILITY1_STEPPER BV(2) + #define DEVICE_CAPABILITY1_TORQUE BV(3) + #define DEVICE_CAPABILITY1_POSITIONING BV(4) + #define DEVICE_CAPABILITY1_VELOCITY BV(5) + #define DEVICE_CAPABILITY1_TRAJ_PLANNER BV(6) + #define DEVICE_CAPABILITY1_HALLS BV(7) + #define DEVICE_CAPABILITY1_INDEXER BV(8) + #define DEVICE_CAPABILITY1_HOMING BV(9) + #define DEVICE_CAPABILITY1_REF_PULSETRAIN BV(10) + #define DEVICE_CAPABILITY1_REF_PWM BV(11) + #define DEVICE_CAPABILITY1_REF_ANALOG BV(12) + #define DEVICE_CAPABILITY1_REF_QUADRATURE BV(13) + #define DEVICE_CAPABILITY1_FB_QUADRATURE BV(14) + #define DEVICE_CAPABILITY1_FB_SSI BV(15) + #define DEVICE_CAPABILITY1_FB_BISS BV(16) + #define DEVICE_CAPABILITY1_FB_SINCOS BV(17) + #define DEVICE_CAPABILITY1_GEARING BV(18) + #define DEVICE_CAPABILITY1_AUTOSETUP_COMMUTATION_SENSOR BV(19) + +//read only bit field that is can be used to identify device capabilities +//the list below is subject to extend +#define SMP_DEVICE_CAPABILITIES2 6006 + #define DEVICE_CAPABILITY2_RESTORE_SAVED_CONFIG BV(0) + #define DEVICE_CAPABILITY2_MEASURE_RL BV(1) + #define DEVICE_CAPABILITY2_TORQUE_RIPPLE_COMPENSATION BV(2) + #define DEVICE_CAPABILITY2_NOTCH_FILTER BV(3) + #define DEVICE_CAPABILITY2_TORQUE_EFFECTS BV(4) + #define DEVICE_CAPABILITY2_SENSORLESS_COMMUTATION BV(5) + #define DEVICE_CAPABILITY2_ANALOG_OUTPUT BV(6) + #define DEVICE_CAPABILITY2_SCOPE_TRIGGER_DELAY BV(7) /*also means that params SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS and SMP_CAPTURE_STATE exist */ + #define DEVICE_CAPABILITY2_SCOPE_EXTERNAL_TRIGGER BV(8) #define SMP_FIRMWARE_VERSION 6010 #define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011 -- cgit v1.2.3 From 0eb3390c9e8620a510061927279178fda6c81ac3 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 11 May 2017 19:04:44 +0300 Subject: Fix duplicate parameter address on previous commit --- simplemotion_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 619c1c1..f7bb19d 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -773,7 +773,7 @@ //read only bit field that is can be used to identify device capabilities //the list below is subject to extend -#define SMP_DEVICE_CAPABILITIES2 6006 +#define SMP_DEVICE_CAPABILITIES2 6007 #define DEVICE_CAPABILITY2_RESTORE_SAVED_CONFIG BV(0) #define DEVICE_CAPABILITY2_MEASURE_RL BV(1) #define DEVICE_CAPABILITY2_TORQUE_RIPPLE_COMPENSATION BV(2) -- cgit v1.2.3 From 998a39044725968bcf4aad37de5891bb4cbaa2fb Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 11 May 2017 19:29:37 +0300 Subject: Add definitions for motor sound notifications --- simplemotion_defs.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index f7bb19d..8a8b68c 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -437,6 +437,7 @@ #define FLAG_USE_HALLS BV(14) /*becoming obsolete, no effect on device where param SMP_COMMUTATION_SENSOR_CONFIG is present */ #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) + #define FLAG_ENABLE_MOTOR_SOUND_NOTIFICATIONS BV(17) #define SMP_MOTION_FAULT_THRESHOLD 568 #define SMP_HV_VOLTAGE_HI_LIMIT 569 #define SMP_HV_VOLTAGE_LOW_LIMIT 570 @@ -783,6 +784,7 @@ #define DEVICE_CAPABILITY2_ANALOG_OUTPUT BV(6) #define DEVICE_CAPABILITY2_SCOPE_TRIGGER_DELAY BV(7) /*also means that params SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS and SMP_CAPTURE_STATE exist */ #define DEVICE_CAPABILITY2_SCOPE_EXTERNAL_TRIGGER BV(8) + #define DEVICE_CAPABILITY2_SOUND_NOTIFICATIONS_FROM_MOTOR BV(9) #define SMP_FIRMWARE_VERSION 6010 #define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011 -- cgit v1.2.3 From f096c464c7063b1405b9aa1ac26d36236f997225 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Fri, 12 May 2017 16:38:52 +0300 Subject: Add info about valid IP address format --- busdevice.c | 1 + simplemotion.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/busdevice.c b/busdevice.c index 6a89396..b73da3f 100644 --- a/busdevice.c +++ b/busdevice.c @@ -52,6 +52,7 @@ void smBDinit() bdInitialized=smtrue; } +//accepted TCP/IP address format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number static int validateIpAddress(const char *s, const char **pip_end, const char **pport_start) { diff --git a/simplemotion.h b/simplemotion.h index 725d3dc..da9a26f 100644 --- a/simplemotion.h +++ b/simplemotion.h @@ -65,7 +65,8 @@ typedef enum _smVerbosityLevel {Off,Low,Mid,High,Trace} smVerbosityLevel; /////////////////////////////////////////////////////////////////////////////////////// /** Open SM RS485 communication bus. Parameters: - -devicename: "USB2VSD" or com port as "COMx" where x=1-16 + -devicename: "USB2VSD" or com port as "COMx" where x=1-n + -devicename for TCP/IP connection format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number for TCP/IP connection -return value: handle to be used with all other commands, -1 if fails */ LIB smbus smOpenBus( const char * devicename ); -- cgit v1.2.3 From 53ad6bae709998e84d046e11966b4382ff1c0df9 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Wed, 24 May 2017 16:57:23 +0300 Subject: Clarified/cleaned some old definitions & added new: - SMP_HOME_SWITCH_SOURCE_SELECT - SMP_AUX_OUTPUT_SOURCE_SELECT - SMP_TRAJ_PLANNER_HOMING_SECOND_APPROACH_DISTANCE - SMP_ACTUAL_POSITION_FB_NEVER_RESETTING - Listed new capabilities under DEVICE_CAPABILITY2_ flags --- simplemotion_defs.h | 131 +++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 108 insertions(+), 23 deletions(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 8a8b68c..34dca1e 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -84,9 +84,10 @@ -/* - * SimpleMotion V2 constants - */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// SimpleMotion V2 constants +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /* broadcast address, if packet is sent to SM_BROADCAST_ADDR then all nodes in the network * will read it but no-one will send a reply because it would cause packet collision*/ @@ -165,22 +166,40 @@ * */ -//if command is to set param addr to this -> NOP -#define SMP_ADDR_NOP 0x3fff - -//SMPCommand values that are returned with every SMPCommand when SMPRET_OTHER set as SMP_RETURN_PARAM_LEN: -#define SMP_CMD_STATUS_ACK 0 -#define SMP_CMD_STATUS_NACK 1 -#define SMP_CMD_STATUS_INVALID_ADDR 2 -#define SMP_CMD_STATUS_INVALID_VALUE 4 -#define SMP_CMD_STATUS_VALUE_TOO_HIGH 8 -#define SMP_CMD_STATUS_VALUE_TOO_LOW 16 -#define SMP_CMD_STATUS_OTHER_MASK32 (3L<<30) //1byte packet. payload codes: +/* Following is a list of status (SMP_CMD_STATUS_) values that are the result of every SM subpacket command. + * + * To read directly this value on each SM subpacket command, set SMPRET_OTHER as SMP_RETURN_PARAM_LEN. In this + * case, user can not read value of the variable. + * + * As alternative to direct reading like above: + * All executed SM subpacket commands also modify SMP_CUMULATIVE_STATUS. User can + * read SMP_CUMULATIVE_STATUS to check whihc status bits the previous commands have set + * (i.e. to check if all parameters were reawd/written succesfully). Once bit is set in cumulative status, it will stick there + * until user clears this value by writing 0 to SMP_CUMULATIVE_STATUS. Procedure for using SMP_CUMULATIVE_STATUS: + * + * 1. Set SMP_CUMULATIVE_STATUS to 0 + * 2. Write/read some SMP parameters + * 3. Read SMP_CUMULATIVE_STATUS and check which SMP_CMD_STATUS bits have been set. + * + * If all all above read/writes were succesfull, it contains only bit SMP_CMD_STATUS_ACK. If something else is returned, investigate which + * read/write operation caused the error (i.e. by narrowing the SMP_CUMULATIVE_STATUS monitoring range). + */ +#define SMP_CMD_STATUS_ACK 0 /* subpacket command executed successfully */ +#define SMP_CMD_STATUS_NACK 1 /* subpacket command failed, perhaps writing into read-only variable or invalid value */ +#define SMP_CMD_STATUS_INVALID_ADDR 2 /* subpacket command has been targetted in invalid SMP parameter address, perhaps parameter is not supported in target device */ +#define SMP_CMD_STATUS_INVALID_VALUE 4 /* attempt to set invalid value into parameter */ +#define SMP_CMD_STATUS_VALUE_TOO_HIGH 8 /* attempt to too high value into parameter */ +#define SMP_CMD_STATUS_VALUE_TOO_LOW 16 /* attempt to too low value into parameter */ //params 0-63 are reserved for SM specific params, application specifics are >64-8090 #define SMP_SM_RESERVED_ADDRESS_SPACE 63 +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// SimpleMotion common parameter definitions start below. These parameters are mandatory on SimpleMotion devices. +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + //SMP=Simple motion parameter + #define SMP_NULL 0 #define SMP_NODE_ADDRSS 1 #define SMP_BUS_MODE 2 @@ -197,12 +216,18 @@ #define SMP_BUS_SPEED 5 #define SMP_BUFFER_FREE_BYTES 6 #define SMP_BUFFERED_CMD_STATUS 7 + //bit mask + #define SM_BUFCMD_STAT_IDLE 1 + #define SM_BUFCMD_STAT_RUN 2 + #define SM_BUFCMD_STAT_UNDERRUN 4 + #define SM_BUFCMD_STAT_OVERRUN 8 + #define SMP_BUFFERED_CMD_PERIOD 8 #define SMP_RETURN_PARAM_ADDR 9 #define SMP_RETURN_PARAM_LEN 10 /*SMP_TIMOUT defines how long device waits for one packet to transmit before discarding it. unit 0.1 milliseconds*/ #define SMP_TIMEOUT 12 -#define SMP_CUMULATIVE_STATUS 13 //error bits are set here if any, (SMP_CMD_STATUS_... bits). clear by writing 0 +#define SMP_CUMULATIVE_STATUS 13 //error bits are set here if any, (SMP_CMD_STATUS_... bits). clear by writing 0. For practical usage of SMP_CUMULATIVE_STATUS, see definitions of SMP_CMD_STATUS_ bits above. #define SMP_ADDRESS_OFFSET 14 /*used to set or offset device address along physical method, i.e. DIP SW + offset to allow greater range of addresses than switch allows. */ /* SMP_FAULT_BEHAVIOR defines maximum amount of time between to valid received SM packets to device and other SM * fault behavior that affect drive operation. @@ -219,14 +244,70 @@ #define SMP_FAULT_BEHAVIOR 15 -//bit mask -#define SM_BUFCMD_STAT_IDLE 1 -#define SM_BUFCMD_STAT_RUN 2 -#define SM_BUFCMD_STAT_UNDERRUN 4 -#define SM_BUFCMD_STAT_OVERRUN 8 -//each DIGITAL variable is 16 bits thus holds 16 i/o's +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// SimpleMotion device specific parameter definitions start below. Note: all parameters are not available in all device types/versions. To test which are available, +// try reading them. If SMP_CMD_STATUS_INVALID_ADDR bit is being set into SMP_CUMULATIVE_STATUS on the read attempt, it means parameter doesn't exist in the target device. +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +//choose input from SMP_DIGITAL_IN_VALUES_1 to be used as home switch. +//note: this might not have effect depending on other parameters (i.e. if output is consumed by some built-in functionality) +#define SMP_HOME_SWITCH_SOURCE_SELECT 100 + +//choose signal to be forwarded into aux output of drive (on IONI it is GPO5) +/*list of aux sources + * value signal + * 0 constant 0 + * 1 constant 1 (default) + */ +#define SMP_AUX_OUTPUT_SOURCE_SELECT 101 + +//each DIGITAL variable holds one bit for each physical digital input +/*SMP_DIGITAL_IN_VALUES_1 on GD motor drives: + * bit physical input + * 0 GPI1 + * 1 GPI2 + * 2 GPI3 + * 3 GPI4 + * 4 GPI5 + * 5 HSIN1 + * 6 HSIN2 + * 7 Analog In 1 as digital value + * 8 Analog In 2 as digital value + * 9 ENC A + * 10 ENC B + * 11 ENC C + * 12 ENC D + * 13 Hall U + * 14 Hall V + * 15 Hall W + * 16 reserved + * 17 reserved + * 18 reserved + * 19 reserved + * 20 GPI6 + * 21- reserved + */ #define SMP_DIGITAL_IN_VALUES_1 128 + #define SMP_DIG_IN1_GPI1 BV(0) + #define SMP_DIG_IN1_GPI2 BV(1) + #define SMP_DIG_IN1_GPI3 BV(2) + #define SMP_DIG_IN1_GPI4 BV(3) + #define SMP_DIG_IN1_GPI5 BV(4) + #define SMP_DIG_IN1_GPI6 BV(20) + #define SMP_DIG_IN1_HSIN1 BV(5) + #define SMP_DIG_IN1_HSIN2 BV(6) + #define SMP_DIG_IN1_ANA1 BV(7) + #define SMP_DIG_IN1_ANA2 BV(8) + #define SMP_DIG_IN1_ENCA BV(9) + #define SMP_DIG_IN1_ENCB BV(10) + #define SMP_DIG_IN1_ENCC BV(11) + #define SMP_DIG_IN1_ENCD BV(12) + #define SMP_DIG_IN1_HALLU BV(13) + #define SMP_DIG_IN1_HALLV BV(14) + #define SMP_DIG_IN1_HALLW BV(15) + #define SMP_DIGITAL_IN_VALUES_2 129 #define SMP_DIGITAL_IN_VALUES_3 130 #define SMP_DIGITAL_IN_VALUES_4 131 @@ -261,8 +342,6 @@ #define SMP_ANALOG_OUT_VALUE_5 188 // continue to .. 200 - - /* * List of motor control parameters. * @@ -641,6 +720,9 @@ #define HOMING_ENABLED BV(7) /*if 0, homing cant be started */ #define _HOMING_CFG_MAX_VALUE 0x00ff +//defines from which direction & distance home switch will be approached for second time (eliminate switch hysteresis) +#define SMP_TRAJ_PLANNER_HOMING_SECOND_APPROACH_DISTANCE 807 + //starting and stopping homing: #define SMP_HOMING_CONTROL 7532 @@ -660,6 +742,7 @@ #define SMP_ACTUAL_POSITION_FB 903 #define SMP_SCOPE_CHANNEL_VALUE 904 #define SMP_SCOPE_CHANNEL_SELECT 905 +#define SMP_ACTUAL_POSITION_FB_NEVER_RESETTING 906 /*this is same than SMP_ACTUAL_POSITION_FB but does not reset to 0 on homing or init (it is always the original counter value at power-on)*/ #define SMP_MECH_BRAKE_RELEASE_DELAY 910 #define SMP_MECH_BRAKE_ENGAGE_DELAY 911 @@ -785,6 +868,8 @@ #define DEVICE_CAPABILITY2_SCOPE_TRIGGER_DELAY BV(7) /*also means that params SMP_CAPTURE_BEFORE_TRIGGER_PERCENTS and SMP_CAPTURE_STATE exist */ #define DEVICE_CAPABILITY2_SCOPE_EXTERNAL_TRIGGER BV(8) #define DEVICE_CAPABILITY2_SOUND_NOTIFICATIONS_FROM_MOTOR BV(9) + #define DEVICE_CAPABILITY2_ASSIGN_HOME_AND_AUX_IO BV(10) + #define DEVICE_CAPABILITY2_HOMING_SECOND_APPROACH BV(11) #define SMP_FIRMWARE_VERSION 6010 #define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011 -- cgit v1.2.3 From c2a59b64b486ced4dd91d4dba1005205322d6bc6 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Mon, 29 May 2017 22:14:53 +0300 Subject: -Added smLoadConfigurationFromBuffer to be operated from RAM buffer instead of file -Fix bug of smLoadConfiguration returning CFGCommunicationError repeatedly after the first connection error --- devicedeployment.c | 60 ++++++++++++++++++++++++++++++++++++++---------------- devicedeployment.h | 13 ++++++++++++ 2 files changed, 56 insertions(+), 17 deletions(-) diff --git a/devicedeployment.c b/devicedeployment.c index 222992a..18e9748 100644 --- a/devicedeployment.c +++ b/devicedeployment.c @@ -21,24 +21,32 @@ void sleep_ms(int millisecs) int globalErrorDetailCode=0; +smbool loadBinaryFile( const char *filename, smuint8 **data, int *numbytes ); + int smGetDeploymentToolErrroDetail() { return globalErrorDetailCode; } //return -1 if EOF -unsigned int readFileLine( FILE *f, int charlimit, char *output, smbool *eof) +//readPosition should be initialized to 0 and not touched by caller after that. this func will increment it after each call. +unsigned int readFileLine( const smuint8 *data, const int dataLen, int *readPosition, int charlimit, char *output, smbool *eof) { int len=0; char c; do { - c=fgetc(f); - - if(feof(f)) + if(*readPosition>=dataLen)//end of data buffer + { *eof=smtrue; + c=0; + } else + { *eof=smfalse; + c=data[*readPosition+len]; + (*readPosition)++; + } //eol or eof if( *eof==smtrue || c=='\n' || c=='\r' || len>=charlimit-1 ) @@ -62,20 +70,19 @@ typedef struct double offset; } Parameter; -smbool parseParameter(FILE *f, int idx, Parameter *param ) +smbool parseParameter( const smuint8 *drcData, const int drcDataLen, int idx, Parameter *param ) { const int maxLineLen=100; char line[maxLineLen]; char scanline[maxLineLen]; - //search correct row - fseek(f,0,SEEK_SET); smbool gotaddr=smfalse,gotvalue=smfalse, gotreadonly=smfalse, gotscale=smfalse,gotoffset=smfalse; unsigned int readbytes; + int readPosition=0; smbool eof; do//loop trhu all lines of file { - readbytes=readFileLine(f,maxLineLen,line,&eof);//read line + readbytes=readFileLine(drcData,drcDataLen,&readPosition,maxLineLen,line,&eof);//read line //try read address sprintf(scanline,"%d\\addr=",idx); @@ -136,20 +143,45 @@ smbool parseParameter(FILE *f, int idx, Parameter *param ) * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. */ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smaddress, const char *filename, unsigned int mode , int *skippedCount, int *errorCount) +{ + LoadConfigurationStatus ret; + smuint8 *drcData=NULL; + int drcDataLength; + + if(loadBinaryFile(filename,&drcData,&drcDataLength)!=smtrue) + return CFGUnableToOpenFile; + + ret = smLoadConfigurationFromBuffer( smhandle, smaddress, drcData, drcDataLength, mode, skippedCount, errorCount ); + free(drcData); + + return ret; +} + +/** + * @brief smConfigureParametersFromBuffer Same as smConfigureParameters but reads data from user specified memory address instead of file. Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param drcData Pointer to to a memory where .drc file is loaded + * @param drcDataLen Number of bytes available in the drcData buffer + * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). + * @return Enum LoadConfigurationStatus + * + * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. + */ +LIB LoadConfigurationStatus smLoadConfigurationFromBuffer( const smbus smhandle, const int smaddress, const smuint8 *drcData, const int drcDataLength, unsigned int mode, int *skippedCount, int *errorCount ) { //test connection smint32 devicetype; SM_STATUS stat; - FILE *f; int ignoredCount=0; int setErrors=0; smint32 CB1Value; int changed=0; - *skippedCount=-1; *errorCount=-1; //test connection + resetCumulativeStatus(smhandle); stat=smRead1Parameter(smhandle,smaddress,SMP_DEVICE_TYPE,&devicetype); if(stat!=SM_OK) return CFGCommunicationError; @@ -165,10 +197,6 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad if(getCumulativeStatus( smhandle )!=SM_OK ) return CFGCommunicationError; - f=fopen(filename,"rb"); - if(f==NULL) - return CFGUnableToOpenFile; - smDebug(smhandle,Low,"Setting parameters\n"); int i=1; @@ -176,7 +204,7 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad do { Parameter param; - readOk=parseParameter(f,i,¶m); + readOk=parseParameter(drcData,drcDataLength,i,¶m); if(readOk==smtrue && param.readOnly==smfalse) { @@ -226,8 +254,6 @@ LoadConfigurationStatus smLoadConfiguration(const smbus smhandle, const int smad i++; } while(readOk==smtrue); - fclose(f); - *skippedCount=ignoredCount; *errorCount=setErrors; diff --git a/devicedeployment.h b/devicedeployment.h index f6fa08e..f86c7dc 100644 --- a/devicedeployment.h +++ b/devicedeployment.h @@ -82,6 +82,19 @@ typedef enum */ LIB LoadConfigurationStatus smLoadConfiguration( const smbus smhandle, const int smaddress, const char *filename, unsigned int mode, int *skippedCount, int *errorCount ); +/** + * @brief smConfigureParametersFromBuffer Same as smConfigureParameters but reads data from user specified memory address instead of file. Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call. + * @param smhandle SM bus handle, must be opened before call + * @param smaddress Target SM device address + * @param drcData Pointer to to a memory where .drc file is loaded + * @param drcDataLen Number of bytes available in the drcData buffer + * @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values). + * @return Enum LoadConfigurationStatus + * + * Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW. + */ +LIB LoadConfigurationStatus smLoadConfigurationFromBuffer(const smbus smhandle, const int smaddress, const smuint8 *drcData, const int drcDataLength, unsigned int mode, int *skippedCount, int *errorCount ); + /** * @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed -- cgit v1.2.3