sbgEComCmdCommon.c
Go to the documentation of this file.
1 #include "sbgEComCmdCommon.h"
2 #include <streamBuffer/sbgStreamBuffer.h>
3 
4 //----------------------------------------------------------------------//
5 //- Common command reception operations -//
6 //----------------------------------------------------------------------//
7 
25 SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8 *pMsgClass, uint8 *pMsg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
26 {
27  SbgErrorCode errorCode = SBG_NO_ERROR;
28  SbgBinaryLogData logData;
29  uint8 receivedMsg;
30  uint8 receivedMsgClass;
31  uint16 receivedCmd;
32  size_t payloadSize;
33  uint8 payloadData[SBG_ECOM_MAX_PAYLOAD_SIZE];
34  uint32 lastValidTime;
35 
36  //
37  // Test that we have a valid protocol handle
38  //
39  if (pHandle)
40  {
41  //
42  // Compute the last valid time according to the time out
43  //
44  lastValidTime = sbgGetTime() + timeOut;
45 
46  //
47  // Try to receive the desired frame within the specified time out
48  //
49  do
50  {
51  //
52  // Read a received frame
53  //
54  errorCode = sbgEComProtocolReceive(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsg, payloadData, &payloadSize, sizeof(payloadData));
55 
56  //
57  // Test if we have received a valid frame
58  //
59  if (errorCode == SBG_NO_ERROR)
60  {
61  //
62  // Test if the received frame is a binary log
63  //
64  if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass))
65  {
66  //
67  // The received frame is a binary log one
68  //
69  errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, receivedMsg, payloadData, payloadSize, &logData);
70 
71  //
72  // Test if the incoming log has been parsed successfully
73  //
74  if (errorCode == SBG_NO_ERROR)
75  {
76  //
77  // Test if we have a valid callback to handle received logs
78  //
79  if (pHandle->pReceiveCallback)
80  {
81  //
82  // Call the binary log callback using the deprecated method
83  //
84  receivedCmd = SBG_ECOM_BUILD_ID(receivedMsgClass, receivedMsg);
85  pHandle->pReceiveCallback(pHandle, (SbgEComCmdId)receivedCmd, &logData, pHandle->pUserArg);
86  }
87  else if (pHandle->pReceiveLogCallback)
88  {
89  //
90  // Call the binary log callback using the new method
91  //
92  errorCode = pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsg, &logData, pHandle->pUserArg);
93  }
94  }
95  else
96  {
97  //
98  // Call the on error callback
99  //
100  }
101  }
102  else
103  {
104  //
105  // Return the received command
106  //
107  if (pMsg)
108  {
109  *pMsg = receivedMsg;
110  }
111  if (pMsgClass)
112  {
113  *pMsgClass = receivedMsgClass;
114  }
115 
116  //
117  // We have received a command so return the payload size
118  //
119  if (pSize)
120  {
121  *pSize = payloadSize;
122  }
123 
124  //
125  // Test if we have a payload to return
126  //
127  if (payloadSize > 0)
128  {
129  //
130  // Make sure that the payload can be stored and fit in the destination buffer
131  //
132  if ( (pData) && (payloadSize <= maxSize) )
133  {
134  //
135  // Copy the payload
136  //
137  memcpy(pData, payloadData, payloadSize);
138  }
139  else
140  {
141  //
142  // We have a buffer overflow
143  //
144  return SBG_BUFFER_OVERFLOW;
145  }
146  }
147 
148  //
149  // We have received the frame we are looking for so return
150  //
151  return SBG_NO_ERROR;
152  }
153  }
154  else if (errorCode == SBG_NOT_READY)
155  {
156  //
157  // No more data are present in the reception buffer so release some CPU before the next try
158  //
159  sbgSleep(1);
160  }
161  } while (lastValidTime >= sbgGetTime());
162 
163  //
164  // The time out has expired so return time out error
165  //
166  errorCode = SBG_TIME_OUT;
167  }
168  else
169  {
170  errorCode = SBG_NULL_POINTER;
171  }
172 
173  return errorCode;
174 }
175 
191 SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
192 {
193  SbgErrorCode errorCode = SBG_NO_ERROR;
194  SbgErrorCode ackErrorCode;
195  uint8 receivedMsg;
196  uint8 receivedMsgClass;
197  uint8 ackMsg;
198  uint8 ackMsgClass;
199  uint32 lastValidTime;
200  SbgStreamBuffer inputStream;
201 
202  //
203  // Test that we have a valid protocol handle
204  //
205  if (pHandle)
206  {
207  //
208  // Compute the last valid time according to the time out
209  //
210  lastValidTime = sbgGetTime() + timeOut;
211 
212  //
213  // Try to receive the desired frame within the specified time out
214  //
215  do
216  {
217  //
218  // Read a received frame - no timeout as this is handled in this function
219  // Content is directly copied to user parameters in order to avoid multiple data copy
220  //
221  errorCode = sbgEComReceiveAnyCmd(pHandle, &receivedMsgClass, &receivedMsg, pData, pSize, maxSize, 0);
222 
223  //
224  // Test if we have received a valid frame
225  //
226  if (errorCode == SBG_NO_ERROR)
227  {
228  //
229  // Test if the received frame the requested one
230  //
231  if ((receivedMsgClass == msgClass) && (receivedMsg == msg))
232  {
233  //
234  // We have received the frame we are looking for so return
235  // Output parameters are already filled by sbgEComReceiveAnyCmd
236  //
237  return SBG_NO_ERROR;
238  }
239  else if ((receivedMsgClass == SBG_ECOM_CLASS_LOG_CMD_0) && (receivedMsg == SBG_ECOM_CMD_ACK))
240  {
241  //
242  // We have received an ACK frame.
243  // We can now parse the message and check if this gives an negative answer to our expected command
244  //
245  sbgStreamBufferInitForRead(&inputStream, pData, *pSize);
246  ackMsg = sbgStreamBufferReadUint8LE(&inputStream);
247  ackMsgClass = sbgStreamBufferReadUint8LE(&inputStream);
248  ackErrorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream);
249 
250  //
251  // Check if the ACK corresponds to the frame we expected
252  //
253  if ((ackMsg == msg) && (ackMsgClass == msgClass))
254  {
255  //
256  // return the error code if this is a negative one. in case of NO_ERROR, return SBG_ERROR instead
257  //
258  if (ackErrorCode != SBG_NO_ERROR)
259  {
260  return ackErrorCode;
261  }
262  else
263  {
264  return SBG_ERROR;
265  }
266  }
267  }
268  }
269  else if (errorCode == SBG_NOT_READY)
270  {
271  //
272  // No more data are present in the reception buffer so release some CPU before the next try
273  //
274  sbgSleep(1);
275  }
276  } while (lastValidTime >= sbgGetTime());
277 
278  //
279  // The time out has expired so return time out error
280  //
281  errorCode = SBG_TIME_OUT;
282  }
283  else
284  {
285  errorCode = SBG_NULL_POINTER;
286  }
287 
288  return errorCode;
289 }
290 
291 //----------------------------------------------------------------------//
292 //- ACK related commands operations -//
293 //----------------------------------------------------------------------//
294 
303 SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 timeOut)
304 {
305  SbgErrorCode errorCode = SBG_NO_ERROR;
306  uint8 payload[2*sizeof(uint16)];
307  SbgStreamBuffer inputStream;
308  size_t receivedSize;
309  uint8 ackClass;
310  uint8 ackMsg;
311 
312  //
313  // Check input arguments
314  //
315  SBG_ASSERT(pHandle);
316 
317  //
318  // Try to receive the ACK
319  //
320  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, payload, &receivedSize, sizeof(payload), timeOut);
321 
322  //
323  // Test if an ACK frame has been received
324  //
325  if (errorCode == SBG_NO_ERROR)
326  {
327  //
328  // Validate the received ACK frame
329  //
330  if (receivedSize == 2*sizeof(uint16))
331  {
332  //
333  // Initialize a stream buffer to parse the received payload
334  //
335  sbgStreamBufferInitForRead(&inputStream, payload, sizeof(payload));
336 
337  //
338  // The ACK frame contains the ack message ID and class, and a uint16 for the return error code
339  // We make sure that the ACK is for the correct command
340  //
341  ackMsg = sbgStreamBufferReadUint8LE(&inputStream);
342  ackClass = sbgStreamBufferReadUint8LE(&inputStream);
343 
344  if ((ackMsg == msg) && (ackClass == msgClass))
345  {
346  //
347  // Parse the error code and return it
348  //
349  errorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream);
350  }
351  else
352  {
353  //
354  // We have received an ACK but not for this frame!
355  //
356  errorCode = SBG_INVALID_FRAME;
357  }
358  }
359  else
360  {
361  //
362  // The ACK is invalid
363  //
364  errorCode = SBG_INVALID_FRAME;
365  }
366  }
367 
368  return errorCode;
369 }
370 
379 SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, SbgErrorCode cmdError)
380 {
381  SbgStreamBuffer outputStream;
382  uint8 payload[2*sizeof(uint8)+sizeof(uint16)];
383 
384  //
385  // Check input arguments
386  //
387  SBG_ASSERT(pHandle);
388 
389  //
390  // Initialize a stream buffer to write the command payload
391  //
392  sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload));
393 
394  //
395  // Write the message ID and class and then the error code
396  //
397  sbgStreamBufferWriteUint8LE(&outputStream, msg);
398  sbgStreamBufferWriteUint8LE(&outputStream, msgClass);
399  sbgStreamBufferWriteUint16LE(&outputStream, (uint16)cmdError);
400 
401  //
402  // Send the ACK command
403  //
404  return sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
405 }
406 
407 //----------------------------------------------------------------------//
408 //- Generic command definitions -//
409 //----------------------------------------------------------------------//
410 
419 SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 modelId)
420 {
421  SbgErrorCode errorCode = SBG_NO_ERROR;
422  uint32 trial;
423  uint8 outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
424  SbgStreamBuffer outputStream;
425 
426 
427  //
428  // Test that the input pointers are valid
429  //
430  if (pHandle)
431  {
432  //
433  // Init stream buffer for output and Build payload
434  //
435  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
436  sbgStreamBufferWriteUint32LE(&outputStream, modelId);
437 
438  //
439  // Send the command three times
440  //
441  for (trial = 0; trial < pHandle->numTrials; trial++)
442  {
443  //
444  // Send the payload over ECom
445  //
446  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
447 
448  //
449  // Make sure that the command has been sent
450  //
451  if (errorCode == SBG_NO_ERROR)
452  {
453  //
454  // Try to read the device answer for 500 ms
455  //
456  errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut);
457 
458  //
459  // Test if we have received a valid ACK
460  //
461  if (errorCode == SBG_NO_ERROR)
462  {
463  //
464  // The command has been executed successfully so return
465  //
466  break;
467  }
468  }
469  else
470  {
471  //
472  // We have a write error so exit the try loop
473  //
474  break;
475  }
476  }
477  }
478  else
479  {
480  //
481  // Null pointer
482  //
483  errorCode = SBG_NULL_POINTER;
484  }
485 
486  return errorCode;
487 }
488 
497 SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, SbgEComModelInfo *pModelInfo)
498 {
499  SbgErrorCode errorCode = SBG_NO_ERROR;
500  uint32 trial;
501  size_t receivedSize;
502  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
503  SbgStreamBuffer inputStream;
504 
505  //
506  // Test that the input pointers are valid
507  //
508  if ((pHandle) && (pModelInfo))
509  {
510  //
511  // Send the command three times
512  //
513  for (trial = 0; trial < pHandle->numTrials; trial++)
514  {
515  //
516  // Send the command only since this is a no payload command
517  //
518  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, NULL, 0);
519 
520  //
521  // Make sure that the command has been sent
522  //
523  if (errorCode == SBG_NO_ERROR)
524  {
525  //
526  // Try to read the device answer for 500 ms
527  //
528  errorCode = sbgEComReceiveCmd(pHandle, msgClass, msg, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
529 
530  //
531  // Test if we have received a the specified command
532  //
533  if (errorCode == SBG_NO_ERROR)
534  {
535  //
536  // Initialize stream buffer to read parameters
537  //
538  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
539 
540  //
541  // Read parameters
542  //
543  pModelInfo->id = sbgStreamBufferReadUint32LE(&inputStream);
544  pModelInfo->revision = sbgStreamBufferReadUint32LE(&inputStream);
545 
546  //
547  // The command has been executed successfully so return
548  //
549  break;
550  }
551  }
552  else
553  {
554  //
555  // We have a write error so exit the try loop
556  //
557  break;
558  }
559  }
560  }
561  else
562  {
563  //
564  // Null pointer
565  //
566  errorCode = SBG_NULL_POINTER;
567  }
568 
569  return errorCode;
570 }
sbgEComCmdCommon.h
This file groups all common definitions required by all commands.
sbgStreamBufferInitForRead
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
Definition: sbgStreamBufferCommon.h:195
sbgEComReceiveCmd
SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
Definition: sbgEComCmdCommon.c:191
uint32
unsigned int uint32
Definition: sbgTypes.h:58
_SbgEComModelInfo
Definition: sbgEComCmdCommon.h:58
sbgEComReceiveAnyCmd
SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8 *pMsgClass, uint8 *pMsg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
Definition: sbgEComCmdCommon.c:25
sbgStreamBuffer.h
Used to read/write data from/to a memory buffer stream.
_SbgEComHandle::pReceiveCallback
SbgEComReceiveFunc pReceiveCallback
Definition: sbgECom.h:83
sbgSleep
void sbgSleep(uint32 ms)
Definition: sbgPlatform.c:54
_SbgEComHandle
Definition: sbgECom.h:80
sbgStreamBufferReadUint16LE
SBG_INLINE uint16 sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferLE.h:106
sbgEComWaitForAck
SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 timeOut)
Definition: sbgEComCmdCommon.c:303
_SbgEComHandle::protocolHandle
SbgEComProtocol protocolHandle
Definition: sbgECom.h:82
sbgStreamBufferInitForWrite
SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize)
Definition: sbgStreamBufferCommon.h:160
sbgEComCmdGenericGetModelInfo
SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, SbgEComModelInfo *pModelInfo)
Definition: sbgEComCmdCommon.c:497
SbgEComCmdId
uint16 SbgEComCmdId
Definition: sbgEComIds.h:272
sbgEComProtocolSend
SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8 msgClass, uint8 msg, const void *pData, size_t size)
Definition: sbgEComProtocol.c:66
sbgStreamBufferWriteUint8LE
#define sbgStreamBufferWriteUint8LE
Definition: sbgStreamBufferCommon.h:101
sbgStreamBufferGetLinkedBuffer
SBG_INLINE void * sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferCommon.h:421
SBG_ECOM_BUILD_ID
#define SBG_ECOM_BUILD_ID(classId, logId)
Definition: sbgEComIds.h:37
sbgEComProtocolReceive
SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pHandle, uint8 *pMsgClass, uint8 *pMsg, void *pData, size_t *pSize, size_t maxSize)
Definition: sbgEComProtocol.c:152
_SbgEComModelInfo::revision
uint32 revision
Definition: sbgEComCmdCommon.h:61
_SbgEComHandle::numTrials
uint32 numTrials
Definition: sbgECom.h:86
NULL
#define NULL
Definition: sbgDefines.h:43
_SbgEComHandle::pReceiveLogCallback
SbgEComReceiveLogFunc pReceiveLogCallback
Definition: sbgECom.h:84
_SbgEComHandle::cmdDefaultTimeOut
uint32 cmdDefaultTimeOut
Definition: sbgECom.h:87
sbgStreamBufferReadUint32LE
SBG_INLINE uint32 sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferLE.h:360
sbgStreamBufferWriteUint16LE
SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16LE(SbgStreamBuffer *pHandle, uint16 value)
Definition: sbgStreamBufferLE.h:780
_SbgEComHandle::pUserArg
void * pUserArg
Definition: sbgECom.h:85
sbgStreamBufferReadUint8LE
#define sbgStreamBufferReadUint8LE
Definition: sbgStreamBufferCommon.h:97
sbgGetTime
uint32 sbgGetTime(void)
Definition: sbgPlatform.c:24
_SbgStreamBuffer
Definition: sbgStreamBufferCommon.h:140
sbgStreamBufferWriteUint32LE
SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint32LE(SbgStreamBuffer *pHandle, uint32 value)
Definition: sbgStreamBufferLE.h:1029
_SbgEComModelInfo::id
uint32 id
Definition: sbgEComCmdCommon.h:60
uint8
unsigned char uint8
Definition: sbgTypes.h:56
SBG_ASSERT
#define SBG_ASSERT(expression)
Definition: sbgDebug.h:52
SbgEComClass
enum _SbgEComClass SbgEComClass
sbgEComCmdGenericSetModelId
SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 modelId)
Definition: sbgEComCmdCommon.c:419
sbgStreamBufferGetLength
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferCommon.h:287
sbgEComMsgClassIsALog
SBG_INLINE bool sbgEComMsgClassIsALog(SbgEComClass msgClass)
Definition: sbgEComIds.h:289
_SbgBinaryLogData
Definition: sbgEComBinaryLogs.h:46
uint16
unsigned short uint16
Definition: sbgTypes.h:57
SBG_ECOM_MAX_PAYLOAD_SIZE
#define SBG_ECOM_MAX_PAYLOAD_SIZE
Definition: sbgEComProtocol.h:36
SbgErrorCode
enum _SbgErrorCode SbgErrorCode
sbgEComSendAck
SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, SbgErrorCode cmdError)
Definition: sbgEComCmdCommon.c:379
sbgEComBinaryLogParse
SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData)
Definition: sbgEComBinaryLogs.c:16
SBG_ECOM_MAX_BUFFER_SIZE
#define SBG_ECOM_MAX_BUFFER_SIZE
Definition: sbgEComProtocol.h:35

sbg_driver
Author(s):
autogenerated on Sun Jan 27 2019 03:42:20