sbgEComCmdOdo.c
Go to the documentation of this file.
1 #include "sbgEComCmdOdo.h"
2 #include <streamBuffer/sbgStreamBuffer.h>
3 
4 //----------------------------------------------------------------------//
5 //- Odometer commands -//
6 //----------------------------------------------------------------------//
7 
14 SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf)
15 {
16  SbgErrorCode errorCode = SBG_NO_ERROR;
17  uint32 trial;
18  size_t receivedSize;
19  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
20  SbgStreamBuffer inputStream;
21 
22  //
23  // Test that the input pointer are valid
24  //
25  if ((pHandle) && (pOdometerConf))
26  {
27  //
28  // Send the command three times
29  //
30  for (trial = 0; trial < pHandle->numTrials; trial++)
31  {
32  //
33  // Send the command only since this is a no-payload command
34  //
35  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, NULL, 0);
36 
37  //
38  // Make sure that the command has been sent
39  //
40  if (errorCode == SBG_NO_ERROR)
41  {
42  //
43  // Try to read the device answer for 500 ms
44  //
45  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
46 
47  //
48  // Test if we have received a SBG_ECOM_CMD_ODO_CONF command
49  //
50  if (errorCode == SBG_NO_ERROR)
51  {
52  //
53  // Initialize stream buffer to read parameters
54  //
55  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
56 
57  //
58  // Read parameters
59  //
60  pOdometerConf->gain = sbgStreamBufferReadFloatLE(&inputStream);
61  pOdometerConf->gainError = sbgStreamBufferReadUint8LE(&inputStream);
62  pOdometerConf->reverseMode = sbgStreamBufferReadUint8LE(&inputStream);
63 
64  //
65  // The command has been executed successfully so return
66  //
67  break;
68  }
69  }
70  else
71  {
72  //
73  // We have a write error so exit the try loop
74  //
75  break;
76  }
77  }
78  }
79  else
80  {
81  //
82  // Null pointer.
83  //
84  errorCode = SBG_NULL_POINTER;
85  }
86 
87  return errorCode;
88 }
89 
96 SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf)
97 {
98  SbgErrorCode errorCode = SBG_NO_ERROR;
99  uint32 trial;
100  uint8 outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
101  SbgStreamBuffer outputStream;
102 
103  //
104  // Test that the input pointer are valid
105  //
106  if ((pHandle) && (pOdometerConf))
107  {
108  //
109  // Send the command three times
110  //
111  for (trial = 0; trial < pHandle->numTrials; trial++)
112  {
113  //
114  // Init stream buffer for output
115  //
116  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
117 
118  //
119  // Build payload
120  //
121  sbgStreamBufferWriteFloatLE(&outputStream, pOdometerConf->gain);
122  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pOdometerConf->gainError);
123  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pOdometerConf->reverseMode);
124 
125  //
126  // Send the payload over ECom
127  //
128  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
129 
130  //
131  // Make sure that the command has been sent
132  //
133  if (errorCode == SBG_NO_ERROR)
134  {
135  //
136  // Try to read the device answer for 500 ms
137  //
138  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, pHandle->cmdDefaultTimeOut);
139 
140  //
141  // Test if we have received a valid ACK
142  //
143  if (errorCode == SBG_NO_ERROR)
144  {
145  //
146  // The command has been executed successfully so return
147  //
148  break;
149  }
150  }
151  else
152  {
153  //
154  // We have a write error so exit the try loop
155  //
156  break;
157  }
158  }
159  }
160  else
161  {
162  //
163  // Invalid protocol handle.
164  //
165  errorCode = SBG_NULL_POINTER;
166  }
167 
168  return errorCode;
169 }
170 
177 SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float leverArm[3])
178 {
179  SbgErrorCode errorCode = SBG_NO_ERROR;
180  uint32 trial;
181  size_t receivedSize;
182  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
183  SbgStreamBuffer inputStream;
184 
185  //
186  // Test that the input pointer are valid
187  //
188  if (pHandle)
189  {
190  //
191  // Send the command three times
192  //
193  for (trial = 0; trial < pHandle->numTrials; trial++)
194  {
195  //
196  // Send the command only since this is a no-payload command
197  //
198  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, NULL, 0);
199 
200  //
201  // Make sure that the command has been sent
202  //
203  if (errorCode == SBG_NO_ERROR)
204  {
205  //
206  // Try to read the device answer for 500 ms
207  //
208  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
209 
210  //
211  // Test if we have received a correct answer
212  //
213  if (errorCode == SBG_NO_ERROR)
214  {
215  //
216  // Initialize stream buffer to read parameters
217  //
218  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
219 
220  //
221  // Read parameters
222  //
223  leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream);
224  leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream);
225  leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream);
226 
227  //
228  // The command has been executed successfully so return
229  //
230  break;
231  }
232  }
233  else
234  {
235  //
236  // We have a write error so exit the try loop
237  //
238  break;
239  }
240  }
241  }
242  else
243  {
244  //
245  // Null pointer.
246  //
247  errorCode = SBG_NULL_POINTER;
248  }
249 
250  return errorCode;
251 }
252 
259 SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3])
260 {
261  SbgErrorCode errorCode = SBG_NO_ERROR;
262  uint32 trial;
263  uint8 outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
264  SbgStreamBuffer outputStream;
265 
266  //
267  // Test that the input pointer are valid
268  //
269  if (pHandle)
270  {
271  //
272  // Send the command three times
273  //
274  for (trial = 0; trial < pHandle->numTrials; trial++)
275  {
276  //
277  // Init stream buffer for output
278  //
279  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
280 
281  //
282  // Build payload
283  //
284  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[0]);
285  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[1]);
286  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[2]);
287 
288  //
289  // Send the payload over ECom
290  //
291  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
292 
293  //
294  // Make sure that the command has been sent
295  //
296  if (errorCode == SBG_NO_ERROR)
297  {
298  //
299  // Try to read the device answer for 500 ms
300  //
301  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, pHandle->cmdDefaultTimeOut);
302 
303  //
304  // Test if we have received a valid ACK
305  //
306  if (errorCode == SBG_NO_ERROR)
307  {
308  //
309  // The command has been executed successfully so return
310  //
311  break;
312  }
313  }
314  else
315  {
316  //
317  // We have a write error so exit the try loop
318  //
319  break;
320  }
321  }
322  }
323  else
324  {
325  //
326  // Invalid protocol handle.
327  //
328  errorCode = SBG_NULL_POINTER;
329  }
330 
331  return errorCode;
332 }
333 
340 SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf)
341 {
342  SbgErrorCode errorCode = SBG_NO_ERROR;
343  uint32 trial;
344  size_t receivedSize;
345  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
346  SbgStreamBuffer inputStream;
347 
348  //
349  // Test that the input pointer are valid
350  //
351  if ((pHandle) && (pRejectConf))
352  {
353  //
354  // Send the command three times
355  //
356  for (trial = 0; trial < pHandle->numTrials; trial++)
357  {
358  //
359  // Send the command only since this is a no-payload command
360  //
361  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, NULL, 0);
362 
363  //
364  // Make sure that the command has been sent
365  //
366  if (errorCode == SBG_NO_ERROR)
367  {
368  //
369  // Try to read the device answer for 500 ms
370  //
371  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
372 
373  //
374  // Test if we have received a correct answer
375  //
376  if (errorCode == SBG_NO_ERROR)
377  {
378  //
379  // Initialize stream buffer to read parameters
380  //
381  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
382 
383  //
384  // Read parameters
385  //
386  pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream);
387 
388  //
389  // The command has been executed successfully so return
390  //
391  break;
392  }
393  }
394  else
395  {
396  //
397  // We have a write error so exit the try loop
398  //
399  break;
400  }
401  }
402  }
403  else
404  {
405  //
406  // Null pointer.
407  //
408  errorCode = SBG_NULL_POINTER;
409  }
410 
411  return errorCode;
412 }
413 
420 SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf)
421 {
422  SbgErrorCode errorCode = SBG_NO_ERROR;
423  uint32 trial;
424  uint8 outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
425  SbgStreamBuffer outputStream;
426 
427  //
428  // Test that the input pointer are valid
429  //
430  if ((pHandle) && (pRejectConf))
431  {
432  //
433  // Send the command three times
434  //
435  for (trial = 0; trial < pHandle->numTrials; trial++)
436  {
437  //
438  // Init stream buffer for output
439  //
440  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
441 
442  //
443  // Build payload
444  //
445  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pRejectConf->velocity);
446 
447  //
448  // Send the payload over ECom
449  //
450  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
451 
452  //
453  // Make sure that the command has been sent
454  //
455  if (errorCode == SBG_NO_ERROR)
456  {
457  //
458  // Try to read the device answer for 500 ms
459  //
460  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, pHandle->cmdDefaultTimeOut);
461 
462  //
463  // Test if we have received a valid ACK
464  //
465  if (errorCode == SBG_NO_ERROR)
466  {
467  //
468  // The command has been executed successfully so return
469  //
470  break;
471  }
472  }
473  else
474  {
475  //
476  // We have a write error so exit the try loop
477  //
478  break;
479  }
480  }
481  }
482  else
483  {
484  //
485  // Invalid protocol handle.
486  //
487  errorCode = SBG_NULL_POINTER;
488  }
489 
490  return errorCode;
491 }
sbgStreamBufferInitForRead
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
Definition: sbgStreamBufferCommon.h:195
sbgEComCmdOdoGetConf
SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf)
Definition: sbgEComCmdOdo.c:14
sbgEComReceiveCmd
SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
Definition: sbgEComCmdCommon.c:191
sbgStreamBufferReadFloatLE
SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferLE.h:616
uint32
unsigned int uint32
Definition: sbgTypes.h:58
sbgStreamBuffer.h
Used to read/write data from/to a memory buffer stream.
_SbgEComOdoConf::reverseMode
bool reverseMode
Definition: sbgEComCmdOdo.h:40
sbgEComCmdOdoGetLeverArm
SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float leverArm[3])
Definition: sbgEComCmdOdo.c:177
_SbgEComHandle
Definition: sbgECom.h:80
_SbgEComOdoRejectionConf::velocity
SbgEComRejectionMode velocity
Definition: sbgEComCmdOdo.h:48
SbgEComRejectionMode
enum _SbgEComRejectionMode SbgEComRejectionMode
_SbgEComOdoConf::gain
float gain
Definition: sbgEComCmdOdo.h:38
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
_SbgEComOdoConf
Definition: sbgEComCmdOdo.h:36
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
_SbgEComHandle::numTrials
uint32 numTrials
Definition: sbgECom.h:86
sbgEComCmdOdoSetRejection
SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf)
Definition: sbgEComCmdOdo.c:420
sbgEComCmdOdo.h
This file implements SbgECom commands related to Odometer module.
NULL
#define NULL
Definition: sbgDefines.h:43
_SbgEComOdoConf::gainError
uint8 gainError
Definition: sbgEComCmdOdo.h:39
_SbgEComHandle::cmdDefaultTimeOut
uint32 cmdDefaultTimeOut
Definition: sbgECom.h:87
sbgEComCmdOdoGetRejection
SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf)
Definition: sbgEComCmdOdo.c:340
sbgStreamBufferReadUint8LE
#define sbgStreamBufferReadUint8LE
Definition: sbgStreamBufferCommon.h:97
sbgStreamBufferWriteFloatLE
SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value)
Definition: sbgStreamBufferLE.h:1283
_SbgStreamBuffer
Definition: sbgStreamBufferCommon.h:140
_SbgEComOdoRejectionConf
Definition: sbgEComCmdOdo.h:46
uint8
unsigned char uint8
Definition: sbgTypes.h:56
sbgStreamBufferGetLength
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferCommon.h:287
sbgEComCmdOdoSetLeverArm
SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3])
Definition: sbgEComCmdOdo.c:259
sbgEComCmdOdoSetConf
SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf)
Definition: sbgEComCmdOdo.c:96
SbgErrorCode
enum _SbgErrorCode SbgErrorCode
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