sbgEComCmdMag.c
Go to the documentation of this file.
1 #include "sbgEComCmdMag.h"
2 #include <streamBuffer/sbgStreamBuffer.h>
3 #include "transfer/sbgEComTransfer.h"
4 
5 //----------------------------------------------------------------------//
6 //- Magnetometer commands -//
7 //----------------------------------------------------------------------//
8 
15 SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, uint32 id)
16 {
17  //
18  // Call generic function with specific command name
19  //
20  return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, id);
21 }
22 
29 SbgErrorCode sbgEComCmdMagGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
30 {
31  //
32  // Call generic function with specific command name
33  //
34  return sbgEComCmdGenericGetModelInfo(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, pModelInfo);
35 }
36 
46 SbgErrorCode sbgEComCmdMagSetModel(SbgEComHandle *pHandle, const void *pBuffer, uint32 size)
47 {
48  //
49  // Call function that handle data transfer
50  //
51  return sbgEComTransferSend(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_SET_MODEL, pBuffer, size);
52 }
53 
61 SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9])
62 {
63  SbgErrorCode errorCode = SBG_NO_ERROR;
64  SbgStreamBuffer outputStream;
65  uint8 payload[12*sizeof(float)];
66  uint32 trial;
67  uint32 i;
68 
69  //
70  // Test that the protocol handle is valid
71  //
72  if (pHandle)
73  {
74  //
75  // Initialize a stream buffer to write the command payload
76  //
77  errorCode = sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload));
78 
79  //
80  // Write the offset vector
81  //
82  sbgStreamBufferWriteFloatLE(&outputStream, offset[0]);
83  sbgStreamBufferWriteFloatLE(&outputStream, offset[1]);
84  sbgStreamBufferWriteFloatLE(&outputStream, offset[2]);
85 
86  //
87  // Write the matrix
88  //
89  for (i = 0; i < 9; i++)
90  {
91  sbgStreamBufferWriteFloatLE(&outputStream, matrix[i]);
92  }
93 
94  //
95  // Make sure that the stream buffer has been initialized
96  //
97  if (errorCode == SBG_NO_ERROR)
98  {
99  //
100  // Send the command three times
101  //
102  for (trial = 0; trial < pHandle->numTrials; trial++)
103  {
104  //
105  // Send the command
106  //
107  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, payload, sbgStreamBufferGetLength(&outputStream));
108 
109  //
110  // Make sure that the command has been sent
111  //
112  if (errorCode == SBG_NO_ERROR)
113  {
114  //
115  // Try to read the device answer for 500 ms
116  //
117  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, pHandle->cmdDefaultTimeOut);
118 
119  //
120  // Test if we have received a valid ACK
121  //
122  if (errorCode == SBG_NO_ERROR)
123  {
124  //
125  // The command has been executed successfully so return
126  //
127  break;
128  }
129  }
130  else
131  {
132  //
133  // We have a write error so exit the try loop
134  //
135  break;
136  }
137  }
138  }
139  }
140  else
141  {
142  //
143  // Invalid protocol handle.
144  //
145  errorCode = SBG_NULL_POINTER;
146  }
147 
148  return errorCode;
149 }
150 
157 SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf)
158 {
159  SbgErrorCode errorCode = SBG_NO_ERROR;
160  uint32 trial;
161  size_t receivedSize;
162  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
163  SbgStreamBuffer inputStream;
164 
165  //
166  // Test that the input pointer are valid
167  //
168  if ((pHandle) && (pRejectConf))
169  {
170  //
171  // Send the command three times
172  //
173  for (trial = 0; trial < pHandle->numTrials; trial++)
174  {
175  //
176  // Send the command only since this is a no-payload command
177  //
178  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, NULL, 0);
179 
180  //
181  // Make sure that the command has been sent
182  //
183  if (errorCode == SBG_NO_ERROR)
184  {
185  //
186  // Try to read the device answer for 500 ms
187  //
188  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
189 
190  //
191  // Test if we have received frame was OK
192  //
193  if (errorCode == SBG_NO_ERROR)
194  {
195  //
196  // Initialize stream buffer to read parameters
197  //
198  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
199 
200  //
201  // Read parameters
202  //
203  pRejectConf->magneticField = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&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  else
221  {
222  //
223  // Null pointer.
224  //
225  errorCode = SBG_NULL_POINTER;
226  }
227 
228  return errorCode;
229 }
230 
237 SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf)
238 {
239  SbgErrorCode errorCode = SBG_NO_ERROR;
240  uint32 trial;
241  uint8 outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
242  SbgStreamBuffer outputStream;
243 
244  //
245  // Test that the input pointer are valid
246  //
247  if ((pHandle) && (pRejectConf))
248  {
249  //
250  // Send the command three times
251  //
252  for (trial = 0; trial < pHandle->numTrials; trial++)
253  {
254  //
255  // Init stream buffer for output
256  // Build payload
257  //
258  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
259  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)pRejectConf->magneticField);
260 
261  //
262  // Send the payload over ECom
263  //
264  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream));
265 
266  //
267  // Make sure that the command has been sent
268  //
269  if (errorCode == SBG_NO_ERROR)
270  {
271  //
272  // Try to read the device answer for 500 ms
273  //
274  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, pHandle->cmdDefaultTimeOut);
275 
276  //
277  // Test if we have received a valid ACK
278  //
279  if (errorCode == SBG_NO_ERROR)
280  {
281  //
282  // The command has been executed successfully so return
283  //
284  break;
285  }
286  }
287  else
288  {
289  //
290  // We have a write error so exit the try loop
291  //
292  break;
293  }
294  }
295  }
296  else
297  {
298  //
299  // Invalid protocol handle.
300  //
301  errorCode = SBG_NULL_POINTER;
302  }
303 
304  return errorCode;
305 }
306 
307 //----------------------------------------------------------------------//
308 //- Magnetometer onboard calibration commands -//
309 //----------------------------------------------------------------------//
310 
320 SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth)
321 {
322  SbgErrorCode errorCode = SBG_NO_ERROR;
323  SbgStreamBuffer outputStream;
324  uint8 payload[2];
325  uint32 trial;
326 
327  //
328  // Test that the protocol handle is valid
329  //
330  if (pHandle)
331  {
332  //
333  // Initialize a stream buffer to write the command payload
334  //
335  errorCode = sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload));
336 
337  //
338  // Write the calibration mode and bandwith
339  //
340  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)mode);
341  sbgStreamBufferWriteUint8LE(&outputStream, (uint8)bandwidth);
342 
343  //
344  // Make sure that the stream buffer has been initialized
345  //
346  if (errorCode == SBG_NO_ERROR)
347  {
348  //
349  // Send the command three times
350  //
351  for (trial = 0; trial < pHandle->numTrials; trial++)
352  {
353  //
354  // Send the command
355  //
356  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, payload, sbgStreamBufferGetLength(&outputStream));
357 
358  //
359  // Make sure that the command has been sent
360  //
361  if (errorCode == SBG_NO_ERROR)
362  {
363  //
364  // Try to read the device answer for 500 ms
365  //
366  errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, pHandle->cmdDefaultTimeOut);
367 
368  //
369  // Test if we have received a valid ACK
370  //
371  if (errorCode == SBG_NO_ERROR)
372  {
373  //
374  // The command has been executed successfully so return
375  //
376  break;
377  }
378  }
379  else
380  {
381  //
382  // We have a write error so exit the try loop
383  //
384  break;
385  }
386  }
387  }
388  }
389  else
390  {
391  //
392  // Invalid protocol handle.
393  //
394  errorCode = SBG_NULL_POINTER;
395  }
396 
397  return errorCode;
398 }
399 
407 SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults)
408 {
409  SbgErrorCode errorCode = SBG_NO_ERROR;
410  uint32 trial;
411  size_t receivedSize;
412  uint8 receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
413  SbgStreamBuffer inputStream;
414  uint32 i;
415 
416  //
417  // Test that the input pointer are valid
418  //
419  if ((pHandle) && (pCalibResults))
420  {
421  //
422  // Send the command three times
423  //
424  for (trial = 0; trial < pHandle->numTrials; trial++)
425  {
426  //
427  // Send the command only since this is a no-payload command
428  //
429  errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, NULL, 0);
430 
431  //
432  // Make sure that the command has been sent
433  //
434  if (errorCode == SBG_NO_ERROR)
435  {
436  //
437  // Try to read the device answer for 5 s because the onboard magnetic computation can take some time
438  //
439  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, receivedBuffer, &receivedSize, sizeof(receivedBuffer), 5000);
440 
441  //
442  // Test if we have received the correct command
443  //
444  if (errorCode == SBG_NO_ERROR)
445  {
446  //
447  // Initialize stream buffer to read parameters
448  //
449  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
450 
451  //
452  // Read quality and status parameters
453  //
454  pCalibResults->quality = (SbgEComMagCalibQuality)sbgStreamBufferReadUint8LE(&inputStream);
455  pCalibResults->confidence = (SbgEComMagCalibConfidence)sbgStreamBufferReadUint8LE(&inputStream);
456  pCalibResults->advancedStatus = sbgStreamBufferReadUint16LE(&inputStream);
457 
458  pCalibResults->beforeMeanError = sbgStreamBufferReadFloatLE(&inputStream);
459  pCalibResults->beforeStdError = sbgStreamBufferReadFloatLE(&inputStream);
460  pCalibResults->beforeMaxError = sbgStreamBufferReadFloatLE(&inputStream);
461 
462  pCalibResults->afterMeanError = sbgStreamBufferReadFloatLE(&inputStream);
463  pCalibResults->afterStdError = sbgStreamBufferReadFloatLE(&inputStream);
464  pCalibResults->afterMaxError = sbgStreamBufferReadFloatLE(&inputStream);
465 
466  pCalibResults->meanAccuracy = sbgStreamBufferReadFloatLE(&inputStream);
467  pCalibResults->stdAccuracy = sbgStreamBufferReadFloatLE(&inputStream);
468  pCalibResults->maxAccuracy = sbgStreamBufferReadFloatLE(&inputStream);
469 
470  pCalibResults->numPoints = sbgStreamBufferReadUint16LE(&inputStream);
471  pCalibResults->maxNumPoints = sbgStreamBufferReadUint16LE(&inputStream);
472 
473  //
474  // Read the computed hard iron offset vector
475  //
476  pCalibResults->offset[0] = sbgStreamBufferReadFloatLE(&inputStream);
477  pCalibResults->offset[1] = sbgStreamBufferReadFloatLE(&inputStream);
478  pCalibResults->offset[2] = sbgStreamBufferReadFloatLE(&inputStream);
479 
480  //
481  // Read the computed soft iron matrix
482  //
483  for (i = 0; i < 9; i++)
484  {
485  pCalibResults->matrix[i] = sbgStreamBufferReadFloatLE(&inputStream);
486  }
487 
488  //
489  // The command has been executed successfully so return
490  //
491  break;
492  }
493  }
494  else
495  {
496  //
497  // We have a write error so exit the try loop
498  //
499  break;
500  }
501  }
502  }
503  else
504  {
505  //
506  // Null pointer.
507  //
508  errorCode = SBG_NULL_POINTER;
509  }
510 
511  return errorCode;
512 }
sbgEComCmdMagGetModelInfo
SbgErrorCode sbgEComCmdMagGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
Definition: sbgEComCmdMag.c:29
_SbgEComMagCalibResults::beforeStdError
float beforeStdError
Definition: sbgEComCmdMag.h:117
SbgEComMagCalibConfidence
enum _SbgEComMagCalibConfidence SbgEComMagCalibConfidence
sbgStreamBufferInitForRead
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
Definition: sbgStreamBufferCommon.h:195
sbgEComReceiveCmd
SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, void *pData, size_t *pSize, size_t maxSize, uint32 timeOut)
Definition: sbgEComCmdCommon.c:191
_SbgEComMagCalibResults::confidence
SbgEComMagCalibConfidence confidence
Definition: sbgEComCmdMag.h:113
_SbgEComMagCalibResults::afterStdError
float afterStdError
Definition: sbgEComCmdMag.h:121
sbgStreamBufferReadFloatLE
SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferLE.h:616
uint32
unsigned int uint32
Definition: sbgTypes.h:58
_SbgEComMagCalibResults::numPoints
uint16 numPoints
Definition: sbgEComCmdMag.h:128
_SbgEComMagCalibResults::quality
SbgEComMagCalibQuality quality
Definition: sbgEComCmdMag.h:112
_SbgEComModelInfo
Definition: sbgEComCmdCommon.h:58
SbgEComMagCalibMode
enum _SbgEComMagCalibMode SbgEComMagCalibMode
sbgStreamBuffer.h
Used to read/write data from/to a memory buffer stream.
_SbgEComMagRejectionConf
Definition: sbgEComCmdMag.h:102
sbgEComTransferSend
SbgErrorCode sbgEComTransferSend(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, const void *pBuffer, size_t size)
Definition: sbgEComTransfer.c:502
sbgEComCmdMagStartCalib
SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth)
Definition: sbgEComCmdMag.c:320
_SbgEComHandle
Definition: sbgECom.h:80
SbgEComMagCalibBandwidth
enum _SbgEComMagCalibBandwidth SbgEComMagCalibBandwidth
sbgEComCmdMagSetModel
SbgErrorCode sbgEComCmdMagSetModel(SbgEComHandle *pHandle, const void *pBuffer, uint32 size)
Definition: sbgEComCmdMag.c:46
sbgStreamBufferReadUint16LE
SBG_INLINE uint16 sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferLE.h:106
SbgEComRejectionMode
enum _SbgEComRejectionMode SbgEComRejectionMode
sbgEComCmdMagSetModelId
SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, uint32 id)
Definition: sbgEComCmdMag.c:15
sbgEComCmdMag.h
This file implements SbgECom commands related to Magnetometer module.
sbgEComWaitForAck
SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 timeOut)
Definition: sbgEComCmdCommon.c:303
sbgEComTransfer.h
Handle large send/receive transfer for specific ECom Protocol commands.
_SbgEComHandle::protocolHandle
SbgEComProtocol protocolHandle
Definition: sbgECom.h:82
sbgStreamBufferInitForWrite
SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize)
Definition: sbgStreamBufferCommon.h:160
SbgEComMagCalibQuality
enum _SbgEComMagCalibQuality SbgEComMagCalibQuality
sbgEComCmdGenericGetModelInfo
SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, SbgEComModelInfo *pModelInfo)
Definition: sbgEComCmdCommon.c:497
_SbgEComMagCalibResults::meanAccuracy
float meanAccuracy
Definition: sbgEComCmdMag.h:124
_SbgEComMagCalibResults::matrix
float matrix[9]
Definition: sbgEComCmdMag.h:131
_SbgEComMagCalibResults::beforeMeanError
float beforeMeanError
Definition: sbgEComCmdMag.h:116
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
NULL
#define NULL
Definition: sbgDefines.h:43
_SbgEComHandle::cmdDefaultTimeOut
uint32 cmdDefaultTimeOut
Definition: sbgECom.h:87
_SbgEComMagCalibResults::offset
float offset[3]
Definition: sbgEComCmdMag.h:130
_SbgEComMagCalibResults::stdAccuracy
float stdAccuracy
Definition: sbgEComCmdMag.h:125
sbgEComCmdMagComputeCalib
SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults)
Definition: sbgEComCmdMag.c:407
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
sbgEComCmdMagSetCalibData
SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9])
Definition: sbgEComCmdMag.c:61
_SbgEComMagCalibResults::beforeMaxError
float beforeMaxError
Definition: sbgEComCmdMag.h:118
_SbgEComMagCalibResults
Definition: sbgEComCmdMag.h:110
uint8
unsigned char uint8
Definition: sbgTypes.h:56
_SbgEComMagCalibResults::maxAccuracy
float maxAccuracy
Definition: sbgEComCmdMag.h:126
_SbgEComMagCalibResults::afterMaxError
float afterMaxError
Definition: sbgEComCmdMag.h:122
_SbgEComMagCalibResults::maxNumPoints
uint16 maxNumPoints
Definition: sbgEComCmdMag.h:129
sbgEComCmdGenericSetModelId
SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8 msgClass, uint8 msg, uint32 modelId)
Definition: sbgEComCmdCommon.c:419
sbgStreamBufferGetLength
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
Definition: sbgStreamBufferCommon.h:287
_SbgEComMagRejectionConf::magneticField
SbgEComRejectionMode magneticField
Definition: sbgEComCmdMag.h:104
sbgEComCmdMagGetRejection
SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf)
Definition: sbgEComCmdMag.c:157
_SbgEComMagCalibResults::advancedStatus
uint16 advancedStatus
Definition: sbgEComCmdMag.h:114
_SbgEComMagCalibResults::afterMeanError
float afterMeanError
Definition: sbgEComCmdMag.h:120
sbgEComCmdMagSetRejection
SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf)
Definition: sbgEComCmdMag.c:237
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