sbgEComCmdSensor.c
Go to the documentation of this file.
1 #include "sbgEComCmdSensor.h"
4 
5 //----------------------------------------------------------------------//
6 //- Sensor commands -//
7 //----------------------------------------------------------------------//
8 
16 {
17  //
18  // Call generic function with specific command name
19  //
21 }
22 
23 
31 {
32  //
33  // Call generic function with specific command name
34  //
36 }
37 
48 {
49  //
50  // Call function that handle data transfer
51  //
53 }
54 
62 {
63  SbgErrorCode errorCode = SBG_NO_ERROR;
64  uint32 trial;
65  size_t receivedSize;
66  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
67  SbgStreamBuffer inputStream;
68 
69  //
70  // Test that the input pointers are valid
71  //
72  if ((pHandle) && (pConf))
73  {
74  //
75  // Send the command three times
76  //
77  for (trial = 0; trial < pHandle->numTrials; trial++)
78  {
79  //
80  // Send the command only since this is a no payload command
81  //
83 
84  //
85  // Make sure that the command has been sent
86  //
87  if (errorCode == SBG_NO_ERROR)
88  {
89  //
90  // Try to read the device answer for 500 ms
91  //
92  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
93 
94  //
95  // Test if we have received a SBG_ECOM_CMD_INIT_PARAMETERS command
96  //
97  if (errorCode == SBG_NO_ERROR)
98  {
99  //
100  // Initialize stream buffer to read parameters
101  //
102  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
103 
104  //
105  // Read parameters
106  //
107  pConf->latitude = sbgStreamBufferReadDoubleLE(&inputStream);
108  pConf->longitude = sbgStreamBufferReadDoubleLE(&inputStream);
109  pConf->altitude = sbgStreamBufferReadDoubleLE(&inputStream);
110  pConf->year = sbgStreamBufferReadUint16LE(&inputStream);
111  pConf->month = sbgStreamBufferReadUint8LE(&inputStream);
112  pConf->day = sbgStreamBufferReadUint8LE(&inputStream);
113 
114  //
115  // The command has been executed successfully so return
116  //
117  break;
118  }
119  }
120  else
121  {
122  //
123  // We have a write error so exit the try loop
124  //
125  break;
126  }
127  }
128  }
129  else
130  {
131  //
132  // Null pointer
133  //
134  errorCode = SBG_NULL_POINTER;
135  }
136 
137  return errorCode;
138 }
139 
147 {
148  SbgErrorCode errorCode = SBG_NO_ERROR;
149  uint32 trial;
150  uint8 outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
151  SbgStreamBuffer outputStream;
152 
153  //
154  // Test that the input pointer are valid
155  //
156  if ((pHandle) && (pConf))
157  {
158  //
159  // Send the command three times
160  //
161  for (trial = 0; trial < pHandle->numTrials; trial++)
162  {
163  //
164  // Init stream buffer for output
165  //
166  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
167 
168  //
169  // Build payload
170  //
171  sbgStreamBufferWriteDoubleLE(&outputStream, pConf->latitude);
172  sbgStreamBufferWriteDoubleLE(&outputStream, pConf->longitude);
173  sbgStreamBufferWriteDoubleLE(&outputStream, pConf->altitude);
174  sbgStreamBufferWriteUint16LE(&outputStream, (uint16)pConf->year);
175  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pConf->month);
176  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pConf->day);
177 
178  //
179  // Send the payload over ECom
180  //
182 
183  //
184  // Make sure that the command has been sent
185  //
186  if (errorCode == SBG_NO_ERROR)
187  {
188  //
189  // Try to read the device answer for 500 ms
190  //
192 
193  //
194  // Test if we have received a valid ACK
195  //
196  if (errorCode == SBG_NO_ERROR)
197  {
198  //
199  // The command has been executed successfully so return
200  //
201  break;
202  }
203  }
204  else
205  {
206  //
207  // We have a write error so exit the try loop
208  //
209  break;
210  }
211  }
212  }
213  else
214  {
215  //
216  // Null pointer.
217  //
218  errorCode = SBG_NULL_POINTER;
219  }
220 
221  return errorCode;
222 }
223 
231 {
232  SbgErrorCode errorCode = SBG_NO_ERROR;
233  uint32 trial;
234  size_t receivedSize;
235  uint8 receivedBuffer[16];
236  SbgStreamBuffer inputStream;
237 
238  //
239  // Test that the input pointers are valid
240  //
241  if ((pHandle) && (pConf))
242  {
243  //
244  // Send the command three times
245  //
246  for (trial = 0; trial < pHandle->numTrials; trial++)
247  {
248  //
249  // Send the command only since this is a no payload command
250  //
252 
253  //
254  // Make sure that the command has been sent
255  //
256  if (errorCode == SBG_NO_ERROR)
257  {
258  //
259  // Try to read the device answer for 500 ms
260  //
261  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
262 
263  //
264  // Test if we have received a SBG_ECOM_CMD_AIDING_ASSIGNMENT command
265  //
266  if (errorCode == SBG_NO_ERROR)
267  {
268  //
269  // Initialize stream buffer to read parameters
270  //
271  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
272 
273  //
274  // Read parameters
275  //
278  sbgStreamBufferSeek(&inputStream, 6*sizeof(uint8), SB_SEEK_CUR_INC);
280  sbgStreamBufferSeek(&inputStream, 1*sizeof(uint8), SB_SEEK_CUR_INC);
282 
283  //
284  // The command has been executed successfully so return
285  //
286  break;
287  }
288  }
289  else
290  {
291  //
292  // We have a write error so exit the try loop
293  //
294  break;
295  }
296  }
297  }
298  else
299  {
300  //
301  // Null pointer
302  //
303  errorCode = SBG_NULL_POINTER;
304  }
305 
306  return errorCode;
307 }
308 
316 {
317  SbgErrorCode errorCode = SBG_NO_ERROR;
318  uint32 trial;
319  uint8 outputBuffer[16];
320  SbgStreamBuffer outputStream;
321 
322  //
323  // Test that the input pointer are valid
324  //
325  if ((pHandle) && (pConf))
326  {
327  //
328  // Send the command three times
329  //
330  for (trial = 0; trial < pHandle->numTrials; trial++)
331  {
332  //
333  // Init stream buffer for output
334  //
335  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
336 
337  //
338  // Build payload
339  //
340  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pConf->gps1Port);
341  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pConf->gps1Sync);
342  sbgStreamBufferSeek(&outputStream, 6*sizeof(uint8), SB_SEEK_CUR_INC);
343  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pConf->rtcmPort);
344  sbgStreamBufferSeek(&outputStream, 1*sizeof(uint8), SB_SEEK_CUR_INC);
345  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pConf->odometerPinsConf);
346 
347  //
348  // Send the payload over ECom
349  //
351 
352  //
353  // Make sure that the command has been sent
354  //
355  if (errorCode == SBG_NO_ERROR)
356  {
357  //
358  // Try to read the device answer for 500 ms
359  //
361 
362  //
363  // Test if we have received a valid ACK
364  //
365  if (errorCode == SBG_NO_ERROR)
366  {
367  //
368  // The command has been executed successfully so return
369  //
370  break;
371  }
372  }
373  else
374  {
375  //
376  // We have a write error so exit the try loop
377  //
378  break;
379  }
380  }
381  }
382  else
383  {
384  //
385  // Null pointer.
386  //
387  errorCode = SBG_NULL_POINTER;
388  }
389 
390  return errorCode;
391 }
392 
401 {
402  SbgErrorCode errorCode = SBG_NO_ERROR;
403  uint32 trial;
404  size_t receivedSize;
405  uint8 receivedBuffer[32];
406  SbgStreamBuffer inputStream;
407 
408  //
409  // Test that the input pointer are valid
410  //
411  if ((pHandle) && (pAlignConf) && (leverArm))
412  {
413  //
414  // Send the command three times
415  //
416  for (trial = 0; trial < pHandle->numTrials; trial++)
417  {
418  //
419  // Send the command only since this is a no-payload command
420  //
422 
423  //
424  // Make sure that the command has been sent
425  //
426  if (errorCode == SBG_NO_ERROR)
427  {
428  //
429  // Try to read the device answer for 500 ms
430  //
431  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
432 
433  //
434  // Test if we have received a SBG_ECOM_CMD_IMU_ALIGNMENT command
435  //
436  if (errorCode == SBG_NO_ERROR)
437  {
438  //
439  // Initialize stream buffer to read parameters
440  //
441  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
442 
443  //
444  // Read parameters
445  //
448  pAlignConf->misRoll = sbgStreamBufferReadFloatLE(&inputStream);
449  pAlignConf->misPitch = sbgStreamBufferReadFloatLE(&inputStream);
450  pAlignConf->misYaw = sbgStreamBufferReadFloatLE(&inputStream);
451  leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream);
452  leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream);
453  leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream);
454 
455  //
456  // The command has been executed successfully so return
457  //
458  break;
459  }
460  }
461  else
462  {
463  //
464  // We have a write error so exit the try loop
465  //
466  break;
467  }
468  }
469  }
470  else
471  {
472  //
473  // Null pointer.
474  //
475  errorCode = SBG_NULL_POINTER;
476  }
477 
478  return errorCode;
479 }
480 
489 {
490  SbgErrorCode errorCode = SBG_NO_ERROR;
491  uint32 trial;
492  uint8 outputBuffer[32];
493  SbgStreamBuffer outputStream;
494 
495  //
496  // Test that the input pointer are valid
497  //
498  if ((pHandle) && (pAlignConf) && (leverArm))
499  {
500  //
501  // Send the command three times
502  //
503  for (trial = 0; trial < pHandle->numTrials; trial++)
504  {
505  //
506  // Init stream buffer for output
507  //
508  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
509 
510  //
511  // Build payload
512  //
513  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pAlignConf->axisDirectionX);
514  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pAlignConf->axisDirectionY);
515  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misRoll);
516  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misPitch);
517  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misYaw);
518  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[0]);
519  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[1]);
520  sbgStreamBufferWriteFloatLE(&outputStream, leverArm[2]);
521 
522  //
523  // Send the payload over ECom
524  //
526 
527  //
528  // Make sure that the command has been sent
529  //
530  if (errorCode == SBG_NO_ERROR)
531  {
532  //
533  // Try to read the device answer for 500 ms
534  //
536 
537  //
538  // Test if we have received a valid ACK
539  //
540  if (errorCode == SBG_NO_ERROR)
541  {
542  //
543  // The command has been executed successfully so return
544  //
545  break;
546  }
547  }
548  else
549  {
550  //
551  // We have a write error so exit the try loop
552  //
553  break;
554  }
555  }
556  }
557  else
558  {
559  //
560  // Invalid protocol handle.
561  //
562  errorCode = SBG_NULL_POINTER;
563  }
564 
565  return errorCode;
566 }
SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float leverArm[3])
SbgEComAxisDirection axisDirectionX
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
This file implements SbgECom commands related to sensor.
SBG_INLINE SbgErrorCode sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin)
SbgEComModulePortAssignment gps1Port
SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
unsigned int uint32
Definition: sbgTypes.h:58
SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf)
SbgEComAxisDirection axisDirectionY
SbgErrorCode sbgEComCmdSensorGetMotionProfileInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
Used to read/write data from/to a memory buffer stream.
SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleLE(SbgStreamBuffer *pHandle, double value)
SbgErrorCode sbgEComTransferSend(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, const void *pBuffer, size_t size)
SBG_INLINE uint16 sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 timeOut)
Handle large send/receive transfer for specific ECom Protocol commands.
SbgEComProtocol protocolHandle
Definition: sbgECom.h:82
SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize)
SbgEComModuleSyncAssignment gps1Sync
SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, SbgEComModelInfo *pModelInfo)
SbgErrorCode sbgEComCmdSensorSetAidingAssignment(SbgEComHandle *pHandle, const SbgEComAidingAssignConf *pConf)
enum _SbgEComModuleSyncAssignment SbgEComModuleSyncAssignment
enum _SbgEComOdometerPinAssignment SbgEComOdometerPinAssignment
SbgEComModulePortAssignment rtcmPort
SbgErrorCode sbgEComCmdSensorSetMotionProfile(SbgEComHandle *pHandle, const void *pBuffer, uint32 size)
SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8 msgClass, uint8 msg, const void *pData, size_t size)
#define sbgStreamBufferWriteUint8LE
SBG_INLINE void * sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
uint32 numTrials
Definition: sbgECom.h:86
SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, uint32 id)
#define NULL
Definition: sbgDefines.h:43
uint32 cmdDefaultTimeOut
Definition: sbgECom.h:87
SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf)
SbgEComOdometerPinAssignment odometerPinsConf
SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16LE(SbgStreamBuffer *pHandle, uint16 value)
#define sbgStreamBufferReadUint8LE
SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value)
enum _SbgEComAxisDirection SbgEComAxisDirection
enum _SbgEComModulePortAssignment SbgEComModulePortAssignment
unsigned char uint8
Definition: sbgTypes.h:56
SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 modelId)
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
SBG_INLINE double sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle)
unsigned short uint16
Definition: sbgTypes.h:57
SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf)
enum _SbgErrorCode SbgErrorCode
#define SBG_ECOM_MAX_BUFFER_SIZE
SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float leverArm[3])


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