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

sbg_driver
Author(s): SBG Systems
autogenerated on Fri Oct 11 2024 02:13:40