sbgEComCmdGnss.c
Go to the documentation of this file.
1 /* sbgCommonLib headers */
2 #include <sbgCommon.h>
4 
5 /* Project headers */
6 
7 /* Local headers */
8 #include "sbgEComCmdGnss.h"
10 
11 //----------------------------------------------------------------------//
12 //- Private methods -//
13 //----------------------------------------------------------------------//
14 
23 static SbgErrorCode sbgEComCmdGnssSetModelId(SbgEComHandle *pHandle, uint32_t id, SbgEComCmd cmdId)
24 {
25  assert(pHandle);
26 
27  //
28  // Call generic function with specific command name
29  //
30  return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, id);
31 }
32 
42 {
43  assert(pHandle);
44  assert(pModelInfo);
45 
46  //
47  // Call generic function with specific command name
48  //
49  return sbgEComCmdGenericGetModelInfo(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pModelInfo);
50 }
51 
61 {
62  SbgErrorCode errorCode = SBG_NO_ERROR;
63  uint32_t trial;
64  size_t receivedSize;
65  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
66  SbgStreamBuffer inputStream;
67 
68  assert(pHandle);
69  assert(pGnssInstallation);
70 
71  //
72  // Send the command three times
73  //
74  for (trial = 0; trial < pHandle->numTrials; trial++)
75  {
76  //
77  // Send the command only since this is a no-payload command
78  //
79  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, NULL, 0);
80 
81  //
82  // Make sure that the command has been sent
83  //
84  if (errorCode == SBG_NO_ERROR)
85  {
86  //
87  // Try to read the device answer for 500 ms
88  //
89  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
90 
91  //
92  // Test if we have received a valid answer
93  //
94  if (errorCode == SBG_NO_ERROR)
95  {
96  //
97  // Initialize stream buffer to read parameters
98  //
99  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
100 
101  //
102  // Read parameters
103  //
104  pGnssInstallation->leverArmPrimary[0] = sbgStreamBufferReadFloatLE(&inputStream);
105  pGnssInstallation->leverArmPrimary[1] = sbgStreamBufferReadFloatLE(&inputStream);
106  pGnssInstallation->leverArmPrimary[2] = sbgStreamBufferReadFloatLE(&inputStream);
107  pGnssInstallation->leverArmPrimaryPrecise = sbgStreamBufferReadBooleanLE(&inputStream);
108 
109  pGnssInstallation->leverArmSecondary[0] = sbgStreamBufferReadFloatLE(&inputStream);
110  pGnssInstallation->leverArmSecondary[1] = sbgStreamBufferReadFloatLE(&inputStream);
111  pGnssInstallation->leverArmSecondary[2] = sbgStreamBufferReadFloatLE(&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  return errorCode;
130 }
131 
141 {
142  SbgErrorCode errorCode = SBG_NO_ERROR;
143  uint32_t trial;
144  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
145  SbgStreamBuffer outputStream;
146 
147  assert(pHandle);
148  assert(pGnssInstallation);
149 
150  //
151  // Send the command three times
152  //
153  for (trial = 0; trial < pHandle->numTrials; trial++)
154  {
155  //
156  // Initialize stream buffer for output
157  //
158  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
159 
160  //
161  // Build payload
162  //
163  sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[0]);
164  sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[1]);
165  sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[2]);
166  sbgStreamBufferWriteBooleanLE(&outputStream, pGnssInstallation->leverArmPrimaryPrecise);
167 
168  sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[0]);
169  sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[1]);
170  sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[2]);
171  sbgStreamBufferWriteUint8LE(&outputStream, pGnssInstallation->leverArmSecondaryMode);
172 
173  //
174  // Send the payload over ECom
175  //
176  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
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 = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pHandle->cmdDefaultTimeOut);
187 
188  //
189  // Test if we have received a valid ACK
190  //
191  if (errorCode == SBG_NO_ERROR)
192  {
193  //
194  // The command has been executed successfully so return
195  //
196  break;
197  }
198  }
199  else
200  {
201  //
202  // We have a write error so exit the try loop
203  //
204  break;
205  }
206  }
207 
208  return errorCode;
209 }
210 
220 {
221  SbgErrorCode errorCode = SBG_NO_ERROR;
222  uint32_t trial;
223  size_t receivedSize;
224  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
225  SbgStreamBuffer inputStream;
226 
227  assert(pHandle);
228  assert(pRejectConf);
229 
230  //
231  // Send the command three times
232  //
233  for (trial = 0; trial < pHandle->numTrials; trial++)
234  {
235  //
236  // Send the command only since this is a no-payload command
237  //
238  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, NULL, 0);
239 
240  //
241  // Make sure that the command has been sent
242  //
243  if (errorCode == SBG_NO_ERROR)
244  {
245  //
246  // Try to read the device answer for 500 ms
247  //
248  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
249 
250  //
251  // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command
252  //
253  if (errorCode == SBG_NO_ERROR)
254  {
255  //
256  // Initialize stream buffer to read parameters
257  //
258  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
259 
260  //
261  // Read parameters
262  //
263  pRejectConf->position = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream);
264  pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream);
265  sbgStreamBufferReadUint8LE(&inputStream); // Skipped for backward compatibility
266  pRejectConf->hdt = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream);
267 
268  //
269  // The command has been executed successfully so return
270  //
271  break;
272  }
273  }
274  else
275  {
276  //
277  // We have a write error so exit the try loop
278  //
279  break;
280  }
281  }
282 
283  return errorCode;
284 }
285 
295 {
296  SbgErrorCode errorCode = SBG_NO_ERROR;
297  uint32_t trial;
298  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
299  SbgStreamBuffer outputStream;
300 
301  assert(pHandle);
302  assert(pRejectConf);
303 
304  //
305  // Send the command three times
306  //
307  for (trial = 0; trial < pHandle->numTrials; trial++)
308  {
309  //
310  // Init stream buffer for output
311  //
312  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
313 
314  //
315  // Build payload
316  //
317  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->position);
318  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->velocity);
319  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)SBG_ECOM_NEVER_ACCEPT_MODE); // Reserved parameter
320  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->hdt);
321 
322  //
323  // Send the payload over ECom
324  //
325  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
326 
327  //
328  // Make sure that the command has been sent
329  //
330  if (errorCode == SBG_NO_ERROR)
331  {
332  //
333  // Try to read the device answer for 500 ms
334  //
335  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pHandle->cmdDefaultTimeOut);
336 
337  //
338  // Test if we have received a valid ACK
339  //
340  if (errorCode == SBG_NO_ERROR)
341  {
342  //
343  // The command has been executed successfully so return
344  //
345  break;
346  }
347  }
348  else
349  {
350  //
351  // We have a write error so exit the try loop
352  //
353  break;
354  }
355  }
356 
357  return errorCode;
358 }
359 
360 //----------------------------------------------------------------------//
361 //- GNSS public commands -//
362 //----------------------------------------------------------------------//
363 
365 {
366  assert(pHandle);
367 
369 }
370 
372 {
373  assert(pHandle);
374  assert(pModelInfo);
375 
376  return sbgEComCmdGnssGetModelInfo(pHandle, pModelInfo, SBG_ECOM_CMD_GNSS_1_MODEL_ID);
377 }
378 
380 {
381  SbgErrorCode errorCode = SBG_NO_ERROR;
382  uint32_t trial;
383  size_t receivedSize;
384  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
385  SbgStreamBuffer inputStream;
386 
387  assert(pHandle);
388  assert(pAlignConf);
389 
390  //
391  // Send the command three times
392  //
393  for (trial = 0; trial < pHandle->numTrials; trial++)
394  {
395  //
396  // Send the command only since this is a no-payload command
397  //
399 
400  //
401  // Make sure that the command has been sent
402  //
403  if (errorCode == SBG_NO_ERROR)
404  {
405  //
406  // Try to read the device answer for 500 ms
407  //
408  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
409 
410  //
411  // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command
412  //
413  if (errorCode == SBG_NO_ERROR)
414  {
415  //
416  // Initialize stream buffer to read parameters
417  //
418  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
419 
420  //
421  // Read parameters
422  //
423  pAlignConf->leverArmX = sbgStreamBufferReadFloatLE(&inputStream);
424  pAlignConf->leverArmY = sbgStreamBufferReadFloatLE(&inputStream);
425  pAlignConf->leverArmZ = sbgStreamBufferReadFloatLE(&inputStream);
426  pAlignConf->pitchOffset = sbgStreamBufferReadFloatLE(&inputStream);
427  pAlignConf->yawOffset = sbgStreamBufferReadFloatLE(&inputStream);
428  pAlignConf->antennaDistance = sbgStreamBufferReadFloatLE(&inputStream);
429 
430  //
431  // The command has been executed successfully so return
432  //
433  break;
434  }
435  }
436  else
437  {
438  //
439  // We have a write error so exit the try loop
440  //
441  break;
442  }
443  }
444 
445  return errorCode;
446 }
447 
449 {
450  SbgErrorCode errorCode = SBG_NO_ERROR;
451  uint32_t trial;
452  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
453  SbgStreamBuffer outputStream;
454 
455  assert(pHandle);
456  assert(pAlignConf);
457 
458  //
459  // Send the command three times
460  //
461  for (trial = 0; trial < pHandle->numTrials; trial++)
462  {
463  //
464  // Initialize stream buffer for output
465  //
466  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
467 
468  //
469  // Build payload
470  //
471  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->leverArmX);
472  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->leverArmY);
473  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->leverArmZ);
474  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->pitchOffset);
475  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->yawOffset);
476  sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->antennaDistance);
477 
478  //
479  // Send the payload over ECom
480  //
482 
483  //
484  // Make sure that the command has been sent
485  //
486  if (errorCode == SBG_NO_ERROR)
487  {
488  //
489  // Try to read the device answer for 500 ms
490  //
492 
493  //
494  // Test if we have received a valid ACK
495  //
496  if (errorCode == SBG_NO_ERROR)
497  {
498  //
499  // The command has been executed successfully so return
500  //
501  break;
502  }
503  }
504  else
505  {
506  //
507  // We have a write error so exit the try loop
508  //
509  break;
510  }
511  }
512 
513  return errorCode;
514 }
515 
517 {
518  assert(pHandle);
519  assert(pGnssInstallation);
520 
521  return sbgEComCmdGnssInstallationGet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION);
522 }
523 
525 {
526  assert(pHandle);
527  assert(pGnssInstallation);
528 
529  return sbgEComCmdGnssInstallationSet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION);
530 }
531 
533 {
534  assert(pHandle);
535  assert(pRejectConf);
536 
537  return sbgEComCmdGnssGetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES);
538 }
539 
541 {
542  assert(pHandle);
543  assert(pRejectConf);
544 
545  return sbgEComCmdGnssSetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES);
546 }
SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf)
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
SbgEComRejectionMode velocity
enum _SbgEComCmd SbgEComCmd
SbgErrorCode sbgEComCmdGnss1SetLeverArmAlignment(SbgEComHandle *pHandle, const SbgEComGnssAlignmentInfo *pAlignConf)
enum _SbgEComGnssInstallationMode SbgEComGnssInstallationMode
SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgEComModelInfo *pModelInfo)
static SbgErrorCode sbgEComCmdGnssSetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf, SbgEComCmd cmdId)
SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
Used to read/write data from/to a memory buffer stream.
SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, uint32_t id)
SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pHandle, uint8_t msgClass, uint8_t msg, const void *pData, size_t size)
SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t modelId)
SbgEComRejectionMode hdt
#define sbgStreamBufferWriteBooleanLE
static SbgErrorCode sbgEComCmdGnssInstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation, SbgEComCmd cmdId)
enum _SbgEComRejectionMode SbgEComRejectionMode
Handle large send/receive transfer for specific ECom Protocol commands.
SbgEComProtocol protocolHandle
Definition: sbgECom.h:72
SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize)
SbgEComGnssInstallationMode leverArmSecondaryMode
SbgErrorCode sbgEComCmdGnss1GetLeverArmAlignment(SbgEComHandle *pHandle, SbgEComGnssAlignmentInfo *pAlignConf)
static SbgErrorCode sbgEComCmdGnssGetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf, SbgEComCmd cmdId)
SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf)
SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut)
SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation)
uint32_t cmdDefaultTimeOut
Definition: sbgECom.h:78
#define sbgStreamBufferWriteUint8LE
SBG_INLINE void * sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
#define NULL
Definition: sbgDefines.h:81
Main header file for SBG Systems common C library.
SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut)
SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation)
#define sbgStreamBufferReadUint8LE
uint32_t numTrials
Definition: sbgECom.h:77
SbgErrorCode sbgEComCmdGnss1GetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value)
static SbgErrorCode sbgEComCmdGnssInstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation, SbgEComCmd cmdId)
#define sbgStreamBufferReadBooleanLE
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
SbgEComRejectionMode position
static SbgErrorCode sbgEComCmdGnssSetModelId(SbgEComHandle *pHandle, uint32_t id, SbgEComCmd cmdId)
static SbgErrorCode sbgEComCmdGnssGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo, SbgEComCmd cmdId)
enum _SbgErrorCode SbgErrorCode
This file implements SbgECom commands related to GNSS module.
#define SBG_ECOM_MAX_BUFFER_SIZE


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