diff options
| author | Tero K <tero.k@granitedevices.fi> | 2016-01-04 12:52:41 +0200 |
|---|---|---|
| committer | Tero K <tero.k@granitedevices.fi> | 2016-01-04 12:52:41 +0200 |
| commit | b912f6291444b32015ff302af6669cf2b760e985 (patch) | |
| tree | dde0730a14cf00db93ea165a5689e3cbe564e8a6 /simplemotion.c | |
| parent | 5d68b4d61c13de9ee63530c1ea8bfaf32df3ce51 (diff) | |
| download | SimpleMotionV2-b912f6291444b32015ff302af6669cf2b760e985.tar.gz SimpleMotionV2-b912f6291444b32015ff302af6669cf2b760e985.zip | |
Added smFastUpdateCycle function
Diffstat (limited to 'simplemotion.c')
| -rw-r--r-- | simplemotion.c | 72 |
1 files changed, 71 insertions, 1 deletions
diff --git a/simplemotion.c b/simplemotion.c index 5b09860..fb53522 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -94,7 +94,6 @@ void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...) va_end(fmtargs);
fprintf(smDebugOut,"%s: %s",smBus[handle].busDeviceName, buffer);
}
-
}
#else
#define smDebug(...)
@@ -143,6 +142,19 @@ smuint16 calcCRC16Buf(const char *buffer, smuint16 buffer_length) return (crc_hi << 8 | crc_lo);
}
+smuint8 calcCRC8Buf( smuint8 *buf, int len, int crcinit )
+{
+ int i;
+ smuint8 crc=crcinit;
+
+ for(i=0;i<len;i++)
+ {
+ crc=table_crc8[crc^buf[i]];
+ }
+
+ return crc;
+}
+
SM_STATUS smSetTimeout( smuint16 millsecs )
{
if(millsecs<=5000)
@@ -242,6 +254,8 @@ char *cmdidToStr(smuint8 cmdid ) #endif
case SMCMD_GET_CLOCK: str="SMCMD_GET_CLOCK";break;
case SMCMD_GET_CLOCK_RET :str="SMCMD_GET_CLOCK_RET";break;
+ case SMCMD_FAST_UPDATE_CYCLE:str="SMCMD_FAST_UPDATE_CYCLE";break;
+ case SMCMD_FAST_UPDATE_CYCLE_RET:str="SMCMD_FAST_UPDATE_CYCLE_RET";break;
default: str="unknown cmdid";break;
}
//puts(str);
@@ -323,6 +337,62 @@ SM_STATUS smSendSMCMD( smbus handle, smuint8 cmdid, smuint8 addr, smuint8 datale }
+SM_STATUS smFastUpdateCycle( smbus handle, smuint8 nodeAddress, smuint16 write1, smuint16 write2, smuint16 *read1, smuint16 *read2)
+{
+ //check if bus handle is valid & opened
+ if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
+
+ smDebug(handle, Mid, "> %s (addr=%d, w1=%d, w2=%d)\n",cmdidToStr(SMCMD_FAST_UPDATE_CYCLE),
+ nodeAddress,
+ write1,write2);
+
+
+ //form the tx packet
+ smuint8 cmd[8];
+ int i;
+ cmd[0]=SMCMD_FAST_UPDATE_CYCLE;
+ cmd[1]=nodeAddress;
+ bufput16bit(cmd,2,write1);
+ bufput16bit(cmd,4,write2);
+ cmd[6]=calcCRC8Buf(cmd,6,0x52);
+
+ //send
+ for(i=0;i<7;i++)
+ {
+ if( smWriteByte(handle,cmd[i], NULL) != smtrue )
+ return recordStatus(handle,SM_ERR_BUS);
+ }
+ smTransmitBuffer(handle);//this sends the bytes entered with smWriteByte
+
+ smDebug(handle, High, " Reading reply packet\n");
+ for(i=0;i<6;i++)
+ {
+ smbool success;
+ smuint8 rx;
+ success=smBDRead(smBus[handle].bdHandle,&rx);
+ cmd[i]=rx;
+ if(success!=smtrue)
+ return recordStatus(handle,SM_ERR_BUS|SM_ERR_LENGTH);//no enough data received
+ }
+
+ //parse
+ if( cmd[5]==calcCRC8Buf(cmd,4,0x52) || cmd[0]!=SMCMD_FAST_UPDATE_CYCLE_RET )
+ {
+ return recordStatus(handle,SM_ERR_COMMUNICATION);//packet error
+ }
+ if(read1!=NULL)
+ *read1=bufget16bit(cmd,1);
+ if(read2!=NULL)
+ *read2=bufget16bit(cmd,3);
+
+ //return data read complete
+ smDebug(handle,Mid, "< %s (id=%d, r1=%d, r2=%d)\n",
+ cmdidToStr( cmd[0] ),
+ cmd[0],*read1,*read2);
+
+ return recordStatus(handle,SM_OK);
+}
+
SM_STATUS smReceiveErrorHandler( smbus handle, smbool flushrx )
|