sbgEComCmdMag.c
Go to the documentation of this file.
1 #include "sbgEComCmdMag.h"
4 
5 //----------------------------------------------------------------------//
6 //- Magnetometer commands -//
7 //----------------------------------------------------------------------//
8 
16 {
17  //
18  // Call generic function with specific command name
19  //
21 }
22 
30 {
31  //
32  // Call generic function with specific command name
33  //
35 }
36 
44 SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9])
45 {
46  SbgErrorCode errorCode = SBG_NO_ERROR;
47  SbgStreamBuffer outputStream;
48  uint8_t payload[12*sizeof(float)];
49  uint32_t trial;
50  uint32_t i;
51 
52  assert(pHandle);
53  assert(offset);
54  assert(matrix);
55 
56  //
57  // Initialize a stream buffer to write the command payload
58  //
59  errorCode = sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload));
60 
61  //
62  // Write the offset vector
63  //
64  sbgStreamBufferWriteFloatLE(&outputStream, offset[0]);
65  sbgStreamBufferWriteFloatLE(&outputStream, offset[1]);
66  sbgStreamBufferWriteFloatLE(&outputStream, offset[2]);
67 
68  //
69  // Write the matrix
70  //
71  for (i = 0; i < 9; i++)
72  {
73  sbgStreamBufferWriteFloatLE(&outputStream, matrix[i]);
74  }
75 
76  //
77  // Make sure that the stream buffer has been initialized
78  //
79  if (errorCode == SBG_NO_ERROR)
80  {
81  //
82  // Send the command three times
83  //
84  for (trial = 0; trial < pHandle->numTrials; trial++)
85  {
86  //
87  // Send the command
88  //
90 
91  //
92  // Make sure that the command has been sent
93  //
94  if (errorCode == SBG_NO_ERROR)
95  {
96  //
97  // Try to read the device answer for 500 ms
98  //
100 
101  //
102  // Test if we have received a valid ACK
103  //
104  if (errorCode == SBG_NO_ERROR)
105  {
106  //
107  // The command has been executed successfully so return
108  //
109  break;
110  }
111  }
112  else
113  {
114  //
115  // We have a write error so exit the try loop
116  //
117  break;
118  }
119  }
120  }
121 
122  return errorCode;
123 }
124 
132 {
133  SbgErrorCode errorCode = SBG_NO_ERROR;
134  uint32_t trial;
135  size_t receivedSize;
136  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
137  SbgStreamBuffer inputStream;
138 
139  assert(pHandle);
140  assert(pRejectConf);
141 
142  //
143  // Send the command three times
144  //
145  for (trial = 0; trial < pHandle->numTrials; trial++)
146  {
147  //
148  // Send the command only since this is a no-payload command
149  //
151 
152  //
153  // Make sure that the command has been sent
154  //
155  if (errorCode == SBG_NO_ERROR)
156  {
157  //
158  // Try to read the device answer for 500 ms
159  //
160  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, receivedBuffer, &receivedSize, sizeof(receivedBuffer), pHandle->cmdDefaultTimeOut);
161 
162  //
163  // Test if we have received frame was OK
164  //
165  if (errorCode == SBG_NO_ERROR)
166  {
167  //
168  // Initialize stream buffer to read parameters
169  //
170  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
171 
172  //
173  // Read parameters
174  //
175  pRejectConf->magneticField = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream);
176 
177  //
178  // The command has been executed successfully so return
179  //
180  break;
181  }
182  }
183  else
184  {
185  //
186  // We have a write error so exit the try loop
187  //
188  break;
189  }
190  }
191 
192  return errorCode;
193 }
194 
202 {
203  SbgErrorCode errorCode = SBG_NO_ERROR;
204  uint32_t trial;
205  uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
206  SbgStreamBuffer outputStream;
207 
208  assert(pHandle);
209  assert(pRejectConf);
210 
211  //
212  // Send the command three times
213  //
214  for (trial = 0; trial < pHandle->numTrials; trial++)
215  {
216  //
217  // Init stream buffer for output
218  // Build payload
219  //
220  sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer));
221  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->magneticField);
222 
223  //
224  // Send the payload over ECom
225  //
227 
228  //
229  // Make sure that the command has been sent
230  //
231  if (errorCode == SBG_NO_ERROR)
232  {
233  //
234  // Try to read the device answer for 500 ms
235  //
237 
238  //
239  // Test if we have received a valid ACK
240  //
241  if (errorCode == SBG_NO_ERROR)
242  {
243  //
244  // The command has been executed successfully so return
245  //
246  break;
247  }
248  }
249  else
250  {
251  //
252  // We have a write error so exit the try loop
253  //
254  break;
255  }
256  }
257 
258  return errorCode;
259 }
260 
261 //----------------------------------------------------------------------//
262 //- Magnetometer onboard calibration commands -//
263 //----------------------------------------------------------------------//
264 
275 {
276  SbgErrorCode errorCode = SBG_NO_ERROR;
277  SbgStreamBuffer outputStream;
278  uint8_t payload[2];
279  uint32_t trial;
280 
281  assert(pHandle);
282 
283  //
284  // Initialize a stream buffer to write the command payload
285  //
286  errorCode = sbgStreamBufferInitForWrite(&outputStream, payload, sizeof(payload));
287 
288  //
289  // Write the calibration mode and bandwith
290  //
291  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)mode);
292  sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)bandwidth);
293 
294  //
295  // Make sure that the stream buffer has been initialized
296  //
297  if (errorCode == SBG_NO_ERROR)
298  {
299  //
300  // Send the command three times
301  //
302  for (trial = 0; trial < pHandle->numTrials; trial++)
303  {
304  //
305  // Send the 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  //
318 
319  //
320  // Test if we have received a valid ACK
321  //
322  if (errorCode == SBG_NO_ERROR)
323  {
324  //
325  // The command has been executed successfully so return
326  //
327  break;
328  }
329  }
330  else
331  {
332  //
333  // We have a write error so exit the try loop
334  //
335  break;
336  }
337  }
338  }
339 
340  return errorCode;
341 }
342 
351 {
352  SbgErrorCode errorCode = SBG_NO_ERROR;
353  uint32_t trial;
354  size_t receivedSize;
355  uint8_t receivedBuffer[SBG_ECOM_MAX_BUFFER_SIZE];
356  SbgStreamBuffer inputStream;
357  uint32_t i;
358 
359  assert(pHandle);
360  assert(pCalibResults);
361 
362  //
363  // Send the command three times
364  //
365  for (trial = 0; trial < pHandle->numTrials; trial++)
366  {
367  //
368  // Send the command only since this is a no-payload command
369  //
371 
372  //
373  // Make sure that the command has been sent
374  //
375  if (errorCode == SBG_NO_ERROR)
376  {
377  //
378  // Try to read the device answer for 5 s because the onboard magnetic computation can take some time
379  //
380  errorCode = sbgEComReceiveCmd(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, receivedBuffer, &receivedSize, sizeof(receivedBuffer), 5000);
381 
382  //
383  // Test if we have received the correct command
384  //
385  if (errorCode == SBG_NO_ERROR)
386  {
387  //
388  // Initialize stream buffer to read parameters
389  //
390  sbgStreamBufferInitForRead(&inputStream, receivedBuffer, receivedSize);
391 
392  //
393  // Read quality and status parameters
394  //
395  pCalibResults->quality = (SbgEComMagCalibQuality)sbgStreamBufferReadUint8LE(&inputStream);
396  pCalibResults->confidence = (SbgEComMagCalibConfidence)sbgStreamBufferReadUint8LE(&inputStream);
397  pCalibResults->advancedStatus = sbgStreamBufferReadUint16LE(&inputStream);
398 
399  pCalibResults->beforeMeanError = sbgStreamBufferReadFloatLE(&inputStream);
400  pCalibResults->beforeStdError = sbgStreamBufferReadFloatLE(&inputStream);
401  pCalibResults->beforeMaxError = sbgStreamBufferReadFloatLE(&inputStream);
402 
403  pCalibResults->afterMeanError = sbgStreamBufferReadFloatLE(&inputStream);
404  pCalibResults->afterStdError = sbgStreamBufferReadFloatLE(&inputStream);
405  pCalibResults->afterMaxError = sbgStreamBufferReadFloatLE(&inputStream);
406 
407  pCalibResults->meanAccuracy = sbgStreamBufferReadFloatLE(&inputStream);
408  pCalibResults->stdAccuracy = sbgStreamBufferReadFloatLE(&inputStream);
409  pCalibResults->maxAccuracy = sbgStreamBufferReadFloatLE(&inputStream);
410 
411  pCalibResults->numPoints = sbgStreamBufferReadUint16LE(&inputStream);
412  pCalibResults->maxNumPoints = sbgStreamBufferReadUint16LE(&inputStream);
413 
414  //
415  // Read the computed hard iron offset vector
416  //
417  pCalibResults->offset[0] = sbgStreamBufferReadFloatLE(&inputStream);
418  pCalibResults->offset[1] = sbgStreamBufferReadFloatLE(&inputStream);
419  pCalibResults->offset[2] = sbgStreamBufferReadFloatLE(&inputStream);
420 
421  //
422  // Read the computed soft iron matrix
423  //
424  for (i = 0; i < 9; i++)
425  {
426  pCalibResults->matrix[i] = sbgStreamBufferReadFloatLE(&inputStream);
427  }
428 
429  //
430  // The command has been executed successfully so return
431  //
432  break;
433  }
434  }
435  else
436  {
437  //
438  // We have a write error so exit the try loop
439  //
440  break;
441  }
442  }
443 
444  return errorCode;
445 }
SbgErrorCode sbgEComCmdMagGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
Definition: sbgEComCmdMag.c:29
enum _SbgEComMagCalibConfidence SbgEComMagCalibConfidence
SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
SbgEComMagCalibConfidence confidence
SbgErrorCode sbgEComCmdGenericGetModelInfo(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgEComModelInfo *pModelInfo)
SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
SbgEComMagCalibQuality quality
enum _SbgEComMagCalibMode SbgEComMagCalibMode
Used to read/write data from/to a memory buffer stream.
SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth)
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)
enum _SbgEComMagCalibBandwidth SbgEComMagCalibBandwidth
enum _SbgEComRejectionMode SbgEComRejectionMode
This file implements SbgECom commands related to Magnetometer module.
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)
enum _SbgEComMagCalibQuality SbgEComMagCalibQuality
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)
#define NULL
Definition: sbgDefines.h:81
SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults)
SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, uint32_t id)
Definition: sbgEComCmdMag.c:15
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)
SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9])
Definition: sbgEComCmdMag.c:44
SBG_INLINE size_t sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
SbgEComRejectionMode magneticField
SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf)
SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf)
enum _SbgErrorCode SbgErrorCode
#define SBG_ECOM_MAX_BUFFER_SIZE


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