Rev 378 | Rev 430 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 378 | Rev 418 | ||
|---|---|---|---|
| Line 233... | Line 233... | ||
| 233 | /* Decode data */ |
233 | /* Decode data */ |
| 234 | /**************************************************************/ |
234 | /**************************************************************/ |
| 235 | void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg) |
235 | void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg) |
| 236 | { |
236 | { |
| 237 | u8 a,b,c,d; |
237 | u8 a,b,c,d; |
| 238 | u8 x,y,z; |
- | |
| 239 | u16 ptrIn = 3; // start with first data byte in rx buffer |
238 | u16 ptrIn = 3; // start with first data byte in rx buffer |
| 240 | u16 ptrOut = 3; |
239 | u16 ptrOut = 3; |
| 241 | u16 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload ) |
240 | u16 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload ) |
| - | 241 | ||
| - | 242 | len/=4; // number of 4 byte blocks |
|
| 242 | while(len) |
243 | while(len) |
| 243 | { |
244 | { |
| 244 | a = pRxBuff->pData[ptrIn++] - '='; |
245 | a = pRxBuff->pData[ptrIn++] - '='; |
| 245 | b = pRxBuff->pData[ptrIn++] - '='; |
246 | b = pRxBuff->pData[ptrIn++] - '='; |
| 246 | c = pRxBuff->pData[ptrIn++] - '='; |
247 | c = pRxBuff->pData[ptrIn++] - '='; |
| 247 | d = pRxBuff->pData[ptrIn++] - '='; |
248 | d = pRxBuff->pData[ptrIn++] - '='; |
| 248 | //if(ptrIn > ReceivedBytes - 3) break; |
- | |
| Line 249... | Line 249... | ||
| 249 | 249 | ||
| 250 | x = (a << 2) | (b >> 4); |
250 | pRxBuff->pData[ptrOut++] = (a << 2) | (b >> 4); |
| 251 | y = ((b & 0x0f) << 4) | (c >> 2); |
- | |
| 252 | z = ((c & 0x03) << 6) | d; |
- | |
| 253 | - | ||
| 254 | if(len--) pRxBuff->pData[ptrOut++] = x; else break; |
251 | pRxBuff->pData[ptrOut++] = ((b & 0x0f) << 4) | (c >> 2); |
| 255 | if(len--) pRxBuff->pData[ptrOut++] = y; else break; |
- | |
| 256 | if(len--) pRxBuff->pData[ptrOut++] = z; else break; |
252 | pRxBuff->pData[ptrOut++] = ((c & 0x03) << 6) | d; |
| 257 | } |
253 | } |
| 258 | pSerialMsg->pData = &(pRxBuff->pData[3]); |
254 | pSerialMsg->pData = &(pRxBuff->pData[3]); |
| 259 | pSerialMsg->DataLen = ptrOut - 3; // return number of data in bytes |
255 | pSerialMsg->DataLen = ptrOut - 3; // return number of data in bytes |
| 260 | pRxBuff->Position = 0; |
256 | pRxBuff->Position = 0; |