sbgEComCmdOdo.c
Go to the documentation of this file.
1 
19 #include "sbgEComCmdOdo.h"
21 
22 //----------------------------------------------------------------------//
23 //- Odometer commands -//
24 //----------------------------------------------------------------------//
25 
27 {
28  SbgErrorCode errorCode = SBG_NO_ERROR;
29  uint32_t trial;
30  size_t receivedSize;
31  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
32  SbgStreamBuffer inputStream;
33 
34  assert(pHandle);
35  assert(pOdometerConf);
36 
37  //
38  // Send the command three times
39  //
40  for (trial = 0; trial < pHandle->numTrials; trial++)
41  {
42  //
43  // Send the command only since this is a no-payload command
44  //
46 
47  //
48  // Make sure that the command has been sent
49  //
50  if (errorCode == SBG_NO_ERROR)
51  {
52  //
53  // Try to read the device answer for 500 ms
54  //
55  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
56 
57  //
58  // Test if we have received a SBG_ECOM_CMD_ODO_CONF command
59  //
60  if (errorCode == SBG_NO_ERROR)
61  {
62  //
63  // Initialize stream buffer to read parameters
64  //
65  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
66 
67  //
68  // Read parameters
69  //
70  pOdometerConf->gain = sbgStreamBufferReadFloatLE(&inputStream);
71  pOdometerConf->gainError = sbgStreamBufferReadUint8LE(&inputStream);
72  pOdometerConf->reverseMode = sbgStreamBufferReadBooleanLE(&inputStream);
73 
74  //
75  // The command has been executed successfully so return
76  //
77  break;
78  }
79  }
80  else
81  {
82  //
83  // We have a write error so exit the try loop
84  //
85  break;
86  }
87  }
88 
89  return errorCode;
90 }
91 
93 {
94  SbgErrorCode errorCode = SBG_NO_ERROR;
95  uint32_t trial;
96  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
97  SbgStreamBuffer outputStream;
98 
99  assert(pHandle);
100  assert(pOdometerConf);
101 
102  //
103  // Send the command three times
104  //
105  for (trial = 0; trial < pHandle->numTrials; trial++)
106  {
107  //
108  // Init stream buffer for output
109  //
110  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
111 
112  //
113  // Build payload
114  //
115  sbgStreamBufferWriteFloatLE(&outputStream, pOdometerConf->gain);
116  sbgStreamBufferWriteUint8LE(&outputStream, pOdometerConf->gainError);
117  sbgStreamBufferWriteBooleanLE(&outputStream, pOdometerConf->reverseMode);
118 
119  //
120  // Send the payload over ECom
121  //
123 
124  //
125  // Make sure that the command has been sent
126  //
127  if (errorCode == SBG_NO_ERROR)
128  {
129  //
130  // Try to read the device answer for 500 ms
131  //
133 
134  //
135  // Test if we have received a valid ACK
136  //
137  if (errorCode == SBG_NO_ERROR)
138  {
139  //
140  // The command has been executed successfully so return
141  //
142  break;
143  }
144  }
145  else
146  {
147  //
148  // We have a write error so exit the try loop
149  //
150  break;
151  }
152  }
153 
154  return errorCode;
155 }
156 
158 {
159  SbgErrorCode errorCode = SBG_NO_ERROR;
160  uint32_t trial;
161  size_t receivedSize;
162  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
163  SbgStreamBuffer inputStream;
164 
165  assert(pHandle);
166  assert(leverArm);
167 
168  //
169  // Send the command three times
170  //
171  for (trial = 0; trial < pHandle->numTrials; trial++)
172  {
173  //
174  // Send the command only since this is a no-payload command
175  //
177 
178  //
179  // Make sure that the command has been sent
180  //
181  if (errorCode == SBG_NO_ERROR)
182  {
183  //
184  // Try to read the device answer for 500 ms
185  //
186  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
187 
188  //
189  // Test if we have received a correct answer
190  //
191  if (errorCode == SBG_NO_ERROR)
192  {
193  //
194  // Initialize stream buffer to read parameters
195  //
196  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
197 
198  //
199  // Read parameters
200  //
201  leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream);
202  leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream);
203  leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream);
204 
205  //
206  // The command has been executed successfully so return
207  //
208  break;
209  }
210  }
211  else
212  {
213  //
214  // We have a write error so exit the try loop
215  //
216  break;
217  }
218  }
219 
220  return errorCode;
221 }
222 
223 SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3])
224 {
225  SbgErrorCode errorCode = SBG_NO_ERROR;
226  uint32_t trial;
227  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
228  SbgStreamBuffer outputStream;
229 
230  assert(pHandle);
231  assert(leverArm);
232 
233  //
234  // Send the command three times
235  //
236  for (trial = 0; trial < pHandle->numTrials; trial++)
237  {
238  //
239  // Init stream buffer for output
240  //
241  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
242 
243  //
244  // Build payload
245  //
246  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[0]);
247  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[1]);
248  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[2]);
249 
250  //
251  // Send the payload over ECom
252  //
254 
255  //
256  // Make sure that the command has been sent
257  //
258  if (errorCode == SBG_NO_ERROR)
259  {
260  //
261  // Try to read the device answer for 500 ms
262  //
264 
265  //
266  // Test if we have received a valid ACK
267  //
268  if (errorCode == SBG_NO_ERROR)
269  {
270  //
271  // The command has been executed successfully so return
272  //
273  break;
274  }
275  }
276  else
277  {
278  //
279  // We have a write error so exit the try loop
280  //
281  break;
282  }
283  }
284 
285  return errorCode;
286 }
287 
289 {
290  SbgErrorCode errorCode = SBG_NO_ERROR;
291  uint32_t trial;
292  size_t receivedSize;
293  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
294  SbgStreamBuffer inputStream;
295 
296  assert(pHandle);
297  assert(pRejectConf);
298 
299  //
300  // Send the command three times
301  //
302  for (trial = 0; trial < pHandle->numTrials; trial++)
303  {
304  //
305  // Send the command only since this is a no-payload command
306  //
308 
309  //
310  // Make sure that the command has been sent
311  //
312  if (errorCode == SBG_NO_ERROR)
313  {
314  //
315  // Try to read the device answer for 500 ms
316  //
317  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
318 
319  //
320  // Test if we have received a correct answer
321  //
322  if (errorCode == SBG_NO_ERROR)
323  {
324  //
325  // Initialize stream buffer to read parameters
326  //
327  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
328 
329  //
330  // Read parameters
331  //
332  pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream);
333 
334  //
335  // The command has been executed successfully so return
336  //
337  break;
338  }
339  }
340  else
341  {
342  //
343  // We have a write error so exit the try loop
344  //
345  break;
346  }
347  }
348 
349  return errorCode;
350 }
351 
353 {
354  SbgErrorCode errorCode = SBG_NO_ERROR;
355  uint32_t trial;
356  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
357  SbgStreamBuffer outputStream;
358 
359  assert(pHandle);
360  assert(pRejectConf);
361 
362  //
363  // Send the command three times
364  //
365  for (trial = 0; trial < pHandle->numTrials; trial++)
366  {
367  //
368  // Init stream buffer for output
369  //
370  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
371 
372  //
373  // Build payload
374  //
375  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->velocity);
376 
377  //
378  // Send the payload over ECom
379  //
381 
382  //
383  // Make sure that the command has been sent
384  //
385  if (errorCode == SBG_NO_ERROR)
386  {
387  //
388  // Try to read the device answer for 500 ms
389  //
391 
392  //
393  // Test if we have received a valid ACK
394  //
395  if (errorCode == SBG_NO_ERROR)
396  {
397  //
398  // The command has been executed successfully so return
399  //
400  break;
401  }
402  }
403  else
404  {
405  //
406  // We have a write error so exit the try loop
407  //
408  break;
409  }
410  }
411 
412  return errorCode;
413 }
414 
416 {
417  SbgErrorCode errorCode = SBG_NO_ERROR;
418  uint32_t trial;
419  uint8_t cmdPayload[16];
420  size_t receivedSize;
421  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
422  SbgStreamBuffer outputStream;
423  SbgStreamBuffer inputStream;
424 
425  assert(pHandle);
426  assert(pOdoCanConf);
427  assert(canChannel <= UCHAR_MAX);
428 
429  //
430  // Build the command payload used to ask the CAN odometer configuration for a specific channel
431  //
432  sbgStreamBufferInitForWrite(&outputStream, cmdPayload, sizeof(cmdPayload));
433  sbgStreamBufferWriteUint8LE(&outputStream, canChannel);
434 
435  //
436  // Send the command three times
437  //
438  for (trial = 0; trial < pHandle->numTrials; trial++)
439  {
440  //
441  // Send the command only since this is a no-payload command
442  //
444 
445  //
446  // Make sure that the command has been sent
447  //
448  if (errorCode == SBG_NO_ERROR)
449  {
450  //
451  // Try to read the device answer for 500 ms
452  //
453  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
454 
455  //
456  // Test if we have received a SBG_ECOM_CMD_ODO_CAN_SPEED_CONF command
457  //
458  if (errorCode == SBG_NO_ERROR)
459  {
460  //
461  // Initialize stream buffer to read parameters
462  //
463  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
464 
465  //
466  // Read fields from payload
467  //
468  canChannel = sbgStreamBufferReadUint8LE(&inputStream);
469 
470  pOdoCanConf->options = sbgStreamBufferReadUint16LE(&inputStream);
471  pOdoCanConf->canId = sbgStreamBufferReadUint32LE(&inputStream);
472 
473  pOdoCanConf->startBit = sbgStreamBufferReadUint8LE(&inputStream);
474  pOdoCanConf->dataSize = sbgStreamBufferReadUint8LE(&inputStream);
475 
476  pOdoCanConf->scale = sbgStreamBufferReadFloatLE(&inputStream);
477  pOdoCanConf->offset = sbgStreamBufferReadFloatLE(&inputStream);
478  pOdoCanConf->minValue = sbgStreamBufferReadFloatLE(&inputStream);
479  pOdoCanConf->maxValue = sbgStreamBufferReadFloatLE(&inputStream);
480 
481  //
482  // The command has been executed successfully so return
483  //
484  break;
485  }
486  }
487  else
488  {
489  //
490  // We have a write error so exit the try loop
491  //
492  break;
493  }
494  }
495 
496  return errorCode;
497 }
498 
500 {
501  SbgErrorCode errorCode = SBG_NO_ERROR;
502  uint32_t trial;
503  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
504  SbgStreamBuffer outputStream;
505 
506  assert(pHandle);
507  assert(pOdoCanConf);
508  assert(canChannel <= UCHAR_MAX);
509 
510  //
511  // A CAN message has a payload of up to 64 bits so the offset can range from 0 to 63 and size from 1 to 64
512  //
513  assert(pOdoCanConf->startBit < 64);
514  assert(pOdoCanConf->dataSize > 0);
515  assert(pOdoCanConf->dataSize <= 64);
516 
517  //
518  // Build the command payload
519  //
520  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
521 
522  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)canChannel);
523 
524  sbgStreamBufferWriteUint16LE(&outputStream, pOdoCanConf->options);
525  sbgStreamBufferWriteUint32LE(&outputStream, pOdoCanConf->canId);
526 
527  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pOdoCanConf->startBit);
528  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pOdoCanConf->dataSize);
529 
530  sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->scale);
531  sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->offset);
532  sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->minValue);
533  sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->maxValue);
534 
535  //
536  // Send the command three times
537  //
538  for (trial = 0; trial < pHandle->numTrials; trial++)
539  {
540  //
541  // Send the payload over ECom
542  //
544 
545  //
546  // Make sure that the command has been sent
547  //
548  if (errorCode == SBG_NO_ERROR)
549  {
550  //
551  // Try to read the device answer for 500 ms
552  //
554 
555  //
556  // Test if we have received a valid ACK
557  //
558  if (errorCode == SBG_NO_ERROR)
559  {
560  //
561  // The command has been executed successfully so return
562  //
563  break;
564  }
565  }
566  else
567  {
568  //
569  // We have a write error so exit the try loop
570  //
571  break;
572  }
573  }
574 
575  return errorCode;
576 }
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf)
Definition: sbgEComCmdOdo.c:26
SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
enum _SbgEComCmdOdoCanChannel SbgEComCmdOdoCanChannel
Used to read/write data from/to a memory buffer stream.
SbgErrorCode sbgEComCmdOdoCanGetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, SbgEComCmdOdoCanConf *pOdoCanConf)
SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8_t msgClass, uint8_t msg, const void *pData, size_t size)
SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float leverArm[3])
SbgEComRejectionMode velocity
Definition: sbgEComCmdOdo.h:44
#define sbgStreamBufferWriteBooleanLE
enum _SbgEComRejectionMode SbgEComRejectionMode
SbgEComProtocol protocolHandle
Definition: sbgECom.h:72
SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize)
SBG_INLINE uint16_t sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut)
uint32_t cmdDefaultTimeOut
Definition: sbgECom.h:78
#define sbgStreamBufferWriteUint8LE
SBG_INLINE void * sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf)
This file implements SbgECom commands related to Odometer module.
#define NULL
Definition: sbgDefines.h:81
SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf)
SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut)
#define sbgStreamBufferReadUint8LE
uint32_t numTrials
Definition: sbgECom.h:77
SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value)
SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint32LE(SbgStreamBuffer *pHandle, uint32_t value)
SbgErrorCode sbgEComCmdOdoCanSetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, const SbgEComCmdOdoCanConf *pOdoCanConf)
#define sbgStreamBufferReadBooleanLE
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
SBG_INLINE uint32_t sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle)
SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16LE(SbgStreamBuffer *pHandle, uint16_t value)
SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3])
SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf)
Definition: sbgEComCmdOdo.c:92
enum _SbgErrorCode SbgErrorCode
#define SBG_ECOM_MAX_BUFFER_SIZE


sbg_driver
Author(s): SBG Systems
autogenerated on Sat Sep 3 2022 02:53:36