ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
fsl_fusion_virt_3D_orient.c
Go to the documentation of this file.
1 /*!
2  ********************************************************************************
3  * File: fsl_fusion_virt_3D_orient.c
4  *
5  * Copyright (c) 2013, Freescale Semiconductor, Inc.
6  *
7  *******************************************************************************/
8 /*!
9  * @file fsl_fusion_virt_3D_orient.c
10  * @brief The orientation sensor adapter is a virtual adapter that uses sensor fusion algorithms
11  * to compute orientation data using accelerometer, magnetomer and gyrometer data.
12  * Multiple fusion algorithms are available to which each require different combinations of these
13  * sensors.
14  * A tilt algorithm can compute pitch and roll from only accelerometer data.
15  * An automotive compass algorithm computes magnetic heading from only magnetometer data
16  * A rotation algorithm computes pitch, roll, and yaw from only gyro data.
17  * A tilt-compensated Ecompass algorithm computes pitch, roll and magnetic heading from accel + mag data
18  * A gaming handset algorithm computes pitch, roll, and yaw from accel + gyro data
19  * A gyro-stabilized Compass computes pitch, roll, and magnetic heading from accel + mag + gyro data.
20  *
21  */
22 #include <mqxlite.h>
23 #include <isf.h>
24 #include <isf_types.h>
25 #include <lwmem.h>
26 #include <lwsem.h>
27 #include <lwevent.h>
28 #include <isf_dsa_adapter.h>
29 #include <isf_dsa_direct.h>
30 #include <isf_bm.h>
31 #include <isf_sensor_types.h>
32 #include <isf_fifo.h>
34 #include <isf_gyrometer_types.h>
35 #include <isf_magnetometer_types.h>
36 #include <isf_orientation_types.h>
37 #include <isf_comm.h>
38 #include <isf_util.h>
39 #include <isf_sensors.h>
40 #include "fsl_i2c_master_driver.h"
42 #include "lwmem.h"
43 #include "fusion_config.h"
44 #include "fusion_types.h"
45 #include "fusion_exec.h"
46 #include "task_template_list.h"
47 #include "Ac_Fixed_utils.h"
48 
49 /* Sensor data ready event masks. */
50 
51 
52 
53 #ifdef USE_ACCELEROMETER
54 # define ACCEL_DATA_READY_EVENT ((uint32)(1 << 0))
55 #else
56 # define ACCEL_DATA_READY_EVENT (0L)
57 #endif
58 
59 #ifdef USE_MAGNETOMETER
60 # define MAG_DATA_READY_EVENT ((uint32)(1 << 1))
61 #else
62 # define MAG_DATA_READY_EVENT (0L)
63 #endif
64 
65 #ifdef USE_GYROMETER
66 # define GYRO_DATA_READY_EVENT ((uint32)(1 << 2))
67 #else
68 # define GYRO_DATA_READY_EVENT (0L)
69 #endif
70 
71 #ifdef USE_PRESSURE_SENSOR
72 # define PRESSURE_DATA_READY_EVENT ((uint32)(1 << 3))
73 #else
74 # define PRESSURE_DATA_READY_EVENT (0L)
75 #endif
76 
77 #define MAGCAL_EVENT_FLAG (1L)
78 
79 #define SENSOR_DATA_READY_EVENT GYRO_DATA_READY_EVENT | ACCEL_DATA_READY_EVENT | MAG_DATA_READY_EVENT | PRESSURE_DATA_READY_EVENT
80 #define HYBRID_SENSOR_DATA_READY_EVENT GYRO_DATA_READY_EVENT | MAG_DATA_READY_EVENT | PRESSURE_DATA_READY_EVENT
81 
82 extern const uint8_t *mqx_task_stack_pointers[];
83 
84 /*! @brief Supported sensor and data types for the orientation sensor */
87  {
95  };
96 
97 // Converter routines for the supported data types
98 static isf_dsa_status_t quaternion_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
99 static isf_dsa_status_t euler_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
100 static isf_dsa_status_t dircosine_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
101 static isf_dsa_status_t accel_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
102 static isf_dsa_status_t mag_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
103 static isf_dsa_status_t gyro_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
104 
105 static void set_current_sample_buffer(
106  fsl_orientation_DataBuffer_t *pCurrentSampleBuffer,
107  fusion_state_t *pState,
108  fsl_fusion_Sensor_Specific_Settings_t *pSpecificSettings
109 );
110 
111 /*! @brief This is the concrete implementation of the orientation sensor adapter initialization.
112  * @details This function allocates memory for the device descriptor.
113  * @param[in] pSensorHandle This is a void pointer to the instance of the orientation sensor adapter.
114  *
115  * @return ::fsl_fusion_virt_3D_orient_Initialize() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
116  * @retval ::ISF_SUCCESS is returned when the device is initialized properly.
117  * @retval ::DSA_ERR_PARAM is returned when a wrong parameter was passed into the function, i.e. an invalid or NULL parameter.
118  * @retval ::DSA_ERR_INITIALIZE is returned when the driver could not be initialized successfully.
119 
120  *
121  * @Constraints None
122  *
123  * @Reentrant Yes
124  * @Libs fsl_fusion_virt_3D_orient.lib
125  */
127 {
128  int32 retStatus = 0;
129 
131 
132 
133  // Check the input argument
134  if (NULL == pSensorHandle) {
135  return DSA_ERR_PARAM;
136  }
137 
138  // Check if the sensor is available, and that it has already been initialized.
139  if (pSensorHandle->adapterStatus > DSA_STATE_NOT_INITIALIZED) {
140  return DSA_ERR_INITIALIZE;
141  }
142 
143  // Allocate memory for the device descriptor
144  pSensorHandle->pDeviceDescriptor = _lwmem_alloc(sizeof(fsl_fusion_DeviceDescriptor_t));
145 
146  // Hold the descriptor pointers in local variables for upcoming operations
148 
149  if (NULL == pDeviceDescriptor) {
150  return DSA_ERR_INITIALIZE;
151  }
152 
153  // Create a semaphore to synchronize the device descriptor across tasks
154  if (MQX_OK != _lwsem_create(&(pDeviceDescriptor->deviceSemaphore), 1)){
155  return DSA_ERR_INITIALIZE;
156  }
157 
158 
159  // Create an event to control the MagCal Task
160  if (MQX_OK != _lwevent_create(&(pDeviceDescriptor->MagCalEventStruct), LWEVENT_AUTO_CLEAR)){
161  return DSA_ERR_INITIALIZE;
162  }
163 
164  // Create an event to control the fusion Task
165  if (MQX_OK != _lwevent_create(&(pDeviceDescriptor->sensorLWEvent), LWEVENT_AUTO_CLEAR)){
166  return DSA_ERR_INITIALIZE;
167  }
168 
169 #ifdef USE_MAGNETOMETER
170  retStatus |= init_sensor(pSensorSpecificConfig->magnetometerSensorId, &(pDeviceDescriptor->magHandle), &(pDeviceDescriptor->sensorLWEvent), MAG_DATA_READY_EVENT);
171 #endif
172 
173 #ifdef USE_PRESSURE_SENSOR
174  retStatus |= init_sensor(pSensorSpecificConfig->pressureSensorId, &(pDeviceDescriptor->accelHandle), &(pDeviceDescriptor->sensorLWEvent), PRESSURE_DATA_READY_EVENT);
175 #endif
176 
177 #ifdef USE_ACCELEROMETER
178  if ( pSensorSpecificConfig->magnetometerSensorId != pSensorSpecificConfig->accelerometerSensorId )
179  {
180  retStatus |= init_sensor(pSensorSpecificConfig->accelerometerSensorId, &(pDeviceDescriptor->accelHandle), &(pDeviceDescriptor->sensorLWEvent), ACCEL_DATA_READY_EVENT);
181  }
182 #endif
183 
184 #ifdef USE_GYROMETER
185  retStatus |= init_sensor(pSensorSpecificConfig->gyroSensorId, &(pDeviceDescriptor->gyroHandle), &(pDeviceDescriptor->sensorLWEvent), GYRO_DATA_READY_EVENT);
186 #endif
187 
188  pDeviceDescriptor->fusionState.algorithmToUse = pSensorSpecificConfig->algorithmToUse;
189 
190  if ( pSensorSpecificConfig->magnetometerSensorId != pSensorSpecificConfig->accelerometerSensorId )
191  {
192  pDeviceDescriptor->sensorDataReady = SENSOR_DATA_READY_EVENT;
193  }
194  else
195  {
196  pDeviceDescriptor->sensorDataReady = HYBRID_SENSOR_DATA_READY_EVENT;
197  }
198 
199  // _task_create fails because it does not properly allocate the stack pointer. See: https://community.freescale.com/thread/324757
200  //_task_create(0, _task_get_index_from_id(FUSION_TASK), (uint_32) pSensorHandle);
201  //_task_create(0, _task_get_index_from_id(MAGCAL_TASK), (uint_32) pSensorHandle);
202 
203 
204  /* Instead of creating the tasks when we want, we can have them started automatically but have them immediately call _task_block()
205  * and wait until execution gets here to wake them up again with _task_ready()
206  */
207  {
208  _task_id task_id = _task_get_id_from_name("fusion_task");
209  pointer td = _task_get_td(task_id);
210 
211  _task_set_parameter_for((uint32)pSensorHandle, task_id);
212  _task_ready(td);
213  }
214 
215  {
216  _task_id task_id = _task_get_id_from_name("magcal_task");
217  pointer td = _task_get_td(task_id);
218 
219  _task_set_parameter_for((uint32)pSensorHandle, task_id);
220  _task_ready(td);
221 
222  }
223 
224  // Set the adapter state to be initialized.
225  pSensorHandle->adapterStatus = DSA_STATE_INITIALIZED;
226 
227  return retStatus;
228 }
229 
230 /*! @brief This is the concrete implementation of the orientation sensor adapter for validating current settings.
231  * @details This function is responsible for validating the current settings and if the current settings are not valid settings,
232  * it provides the best suitable settings.
233  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
234  * @param[in] pSettings The reference to the sensor settings.
235  *
236  * @return ::fsl_fusion_virt_3D_orient_ValidateSettings() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
237  * @retval ::ISF_SUCCESS is returned when the device settings are successfully validated.
238  * @retval ::SM_DSA_RET_SETTINGS_CHANGED is returned when the current settings have been changed.
239  *
240  * @Constraints None
241  *
242  * @Reentrant Yes
243  * @Libs fsl_fusion_virt_3D_orient.lib
244  */
246 {
247  int32 samplePeriod;
248 
249  isf_dsa_SensorSettings_t settings;
250 
251  // Check the input arguments.
252  if ((NULL == pSensorHandle) || (NULL == pSensorSettings)) {
253  return DSA_ERR_PARAM;
254  }
255 
256  // Set the default return status
257  isf_dsa_status_t retStat = ISF_SUCCESS;
258 
259  // Get the device descriptor and sensor specific settings
262 
263  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.gyroOversampleRatio); //!< Sample period in microseconds
264  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
265  settings.resultType = TYPE_ROTATIONAL_RATE_3D; //!< The desired data type of the subscription
267 
268  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
269  {
270  if (pSpecificSettings->algorithmConfig.magOversampleRatio != pSpecificSettings->algorithmConfig.accelOversampleRatio )
271  {
273  }
274  }
275 
276  retStat = pDeviceDescriptor->gyroHandle.pSensorStaticConfig->pAdapter->control.ValidateSettings(&(pDeviceDescriptor->gyroHandle), &settings);
277  if ( retStat != ISF_SUCCESS )
278  {
279  pSensorSettings->nSamplePeriod = settings.nSamplePeriod * pSpecificSettings->algorithmConfig.gyroOversampleRatio;
280  }
281 
282  if (( pSensorSettings->resultType != TYPE_QUATERNION ) &&
283  ( pSensorSettings->resultType != TYPE_EULER_3D ) &&
284  ( pSensorSettings->resultType != TYPE_DIR_COSINE_MATRIX) &&
285  ( pSensorSettings->resultType != TYPE_NATIVE_SENSOR_DATA_TYPE)
286  )
287  {
288  pSensorSettings->resultType = TYPE_NATIVE_SENSOR_DATA_TYPE;
290  }
291 
292  return retStat;
293 }
294 /*! @brief This is the concrete implementation of the fusion sensor adapter configuration function.
295  * @details This function resets the fusion state and calls the underlying AGM adapter's Configure functions.
296  * @param[in] pSensorHdl This is a void pointer to the instance of the fusion sensor adapter.
297  * @param[in] pConfig The reference to the sensor configuration settings.
298  *
299  * @return ::fsl_fusion_virt_3D_orient_Configure() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
300  * @retval ::ISF_SUCCESS is returned when the device is configured successfully.
301  * @retval ::DSA_ERR_CONFIGURE is returned when the provided configuration settings could not be applied.
302  *
303  * @Constraints None
304  *
305  * @Reentrant Yes
306  * @Libs fsl_fusion_virt_3D_orient.lib
307  */
309 {
310  int32 samplePeriod;
311 
314 
315  // Check the input arguments
316  if ((NULL == pSensorHandle) || (NULL == pSensorSettings)) {
317  return DSA_ERR_PARAM;
318  }
319 
320  isf_dsa_status_t retStatus = ISF_SUCCESS;
321 
322  // Get the device descriptor and sensor specific settings
324 
325  // Check the device descriptor pointer.
326  if ((NULL == pDeviceDescriptor) || (NULL == pSpecificSettings))
327  {
328  return retStatus;
329  }
330 
331  // Lock the device descriptor.
332  _lwsem_wait_ticks(&(pDeviceDescriptor->deviceSemaphore), 0);
333 
334  // Check the driver state.
335  if (DSA_STATE_INITIALIZED > pSensorHandle->adapterStatus){
336  goto unlockdescriptor;
337  }
338 
339 #ifdef USE_MAGNETOMETER
340  {
341  uint16 sampleSize;
342 
343  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
344  {
345  sampleSize = sizeof(isf_MagneticFieldStrength3D_float_t);
346  }
347  else
348  {
349  sampleSize = pSensorHandle->pSensorStaticConfig->pAdapter->devInfo.nNativeDataSetSize;
350  }
351 
353  (
354  &(pDeviceDescriptor->magFifo),
355  NULL,
356  sampleSize,
357  pSpecificSettings->algorithmConfig.magOversampleRatio
358  );
359  }
360 #endif
361 
362 #ifdef USE_PRESSURE_SENSOR
364  (
365  &pDeviceDescriptor->pressureFifo,
366  NULL,
368  pSpecificSettings->algorithmConfig.pressureOversampleRatio
369  );
370 #endif
371 
372 #ifdef USE_ACCELEROMETER
373  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
374  {
376  (
377  &(pDeviceDescriptor->accelFifo),
378  NULL,
380  pSpecificSettings->algorithmConfig.accelOversampleRatio
381  );
382  }
383 #endif
384 
385 #ifdef USE_GYROMETER
387  (
388  &(pDeviceDescriptor->gyroFifo),
389  NULL,
391  pSpecificSettings->algorithmConfig.gyroOversampleRatio
392  );
393 #endif
394 
395 #ifdef USE_ACCELEROMETER
396  {
397  int32 ast = 0;
398 
399  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
400  {
401  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.accelOversampleRatio); //!< Sample period in microseconds
402  settings.resultType = TYPE_RAW_ACCELERATION_3D; //!< The desired data type of the subscription
403  settings.resultFormat = DSA_RESULT_TYPE_ENG_FLOAT; //!< The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT
404  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
405  settings.nSettingsToUse = 3; //!< 1 = current; 2=given; 3=best possible
406  settings.nFifoDepth = pSpecificSettings->algorithmConfig.accelOversampleRatio;
407 
408  ast = configure_sensor(
409  &(pDeviceDescriptor->accelHandle),
410  &settings,
411  &(pDeviceDescriptor->accelFifo));
412 
413  if (ast != ISF_SUCCESS)
414  {
415  ast = configure_sensor(
416  &(pDeviceDescriptor->accelHandle),
417  &settings,
418  &(pDeviceDescriptor->accelFifo));
419  }
420  }
421  }
422 #endif
423 
424 #ifdef USE_MAGNETOMETER
425  {
426  int32 mst = 0;
427  isf_SensorDataTypes_t resultType;
428 
429  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
430  {
431  resultType = TYPE_MAGNETIC_FIELD_STRENGTH_3D;
432  }
433  else
434  {
435  resultType = TYPE_NATIVE_SENSOR_DATA_TYPE;
436  }
437 
438  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.magOversampleRatio); //!< Sample period in microseconds
439  settings.resultType = resultType; //!< The desired data type of the subscription
440  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
441  settings.nFifoDepth = pSpecificSettings->algorithmConfig.magOversampleRatio;
442 
443  mst = configure_sensor(
444  &pDeviceDescriptor->magHandle,
445  &settings,
446  &pDeviceDescriptor->magFifo
447  );
448  if (mst != ISF_SUCCESS)
449  {
450  mst = configure_sensor(
451  &(pDeviceDescriptor->magHandle),
452  &settings,
453  &(pDeviceDescriptor->magFifo));
454  }
455  }
456 #endif
457 
458 #ifdef USE_GYROMETER
459  {
460  int32 gst = 0;
461 
462  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.gyroOversampleRatio); //!< Sample period in microseconds
463  settings.resultType = TYPE_ROTATIONAL_RATE_3D; //!< The desired data type of the subscription
464  settings.resultFormat = DSA_RESULT_TYPE_ENG_FLOAT; //!< The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT
465  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
466  settings.nFifoDepth = pSpecificSettings->algorithmConfig.gyroOversampleRatio;
467 
468  gst = configure_sensor(
469  &(pDeviceDescriptor->gyroHandle),
470  &settings,
471  &(pDeviceDescriptor->gyroFifo)
472  );
473  if (gst != ISF_SUCCESS)
474  {
475  retStatus = gst;
476  pSensorSettings->nSamplePeriod = settings.nSamplePeriod * pSpecificSettings->algorithmConfig.gyroOversampleRatio;
477  goto unlockdescriptor;
478  }
479  }
480 #endif
481 
482 #ifdef USE_PRESSURE_SENSOR
483  {
484  int32 pst = 0;
485 
486  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.pressureOversampleRatio); //!< Sample period in microseconds
487  settings.resultType = TYPE_ALTITUDE; //!< The desired data type of the subscription
488  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
489  settings.nFifoDepth = pSpecificSettings->algorithmConfig.pressureOversampleRatio;
490 
491  pst = configure_sensor(
492  &pDeviceDescriptor->pressureHandle,
493  &settings,
494  &pDeviceDescriptor->pressureFifo
495  );
496  if (pst != ISF_SUCCESS)
497  {
498  retStatus = pst;
499  pSensorSettings->nSamplePeriod = settings.nSamplePeriod * pSpecificSettings->algorithmConfig.pressureOversampleRatio;
500  goto unlockdescriptor;
501  }
502  }
503 #endif
504 
506  retStatus = ISF_SUCCESS;
507 
508  unlockdescriptor:
509 
510  // Unlock the device descriptor.
511  _lwsem_post(&(pDeviceDescriptor->deviceSemaphore));
512 
513  return retStatus;
514 }
515 /*! @brief This is the concrete implementation of the orientation sensor adapter for start Data.
516  * @details This function tells the Bus Manager to start the callbacks periodically as defined in the bus configuration and enable the data collection.
517  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
518  *
519  * @return ::fsl_fusion_virt_3D_orient_StartData() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
520  * @retval ::ISF_SUCCESS is returned when the data collection callback triggered successfully.
521  * @retval ::DSA_ERR_PARAM is returned when a wrong parameter is passed into the function such as an invalid or NULL parameter.
522  *
523  * @Constraints None
524  *
525  * @Reentrant Yes
526  * @Libs fsl_fusion_virt_3D_orient.lib
527  */
529 {
530  // Check the input argument
531  if (NULL == pSensorHandle) {
532  return DSA_ERR_PARAM;
533  }
534 
535  int32_t retStat = DSA_ERR_START_DATA;
536 
537  // Get the device descriptor and specific settings
538  fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = pSensorHandle->pDeviceDescriptor;
539 
540  // Check the device descriptor.
541  if (NULL == pDeviceDescriptor)
542  return DSA_ERR_PARAM;
543 
545 
546  // Lock the device descriptor.
547  _lwsem_wait_ticks(&(pDeviceDescriptor->deviceSemaphore), 0);
548 
549  // Check the driver state.
550  if (DSA_STATE_CONFIGURED_STOPPED > pSensorHandle->adapterStatus)
551  {
552  goto unlockdescriptor;
553  }
554 
555 #ifdef USE_MAGNETOMETER
556  {
557  int32 mst;
558 
559  mst = start_sensor(&(pDeviceDescriptor->magHandle));
560 
561  if (ISF_SUCCESS != mst) goto unlockdescriptor;
562  }
563 
564 #endif
565 
566 #ifdef USE_PRESSURE_SENSOR
567  {
568  int32 pst;
569 
570  pst = start_sensor(&(pDeviceDescriptor->pressureHandle));
571 
572  if (ISF_SUCCESS != pst) goto unlockdescriptor;
573  }
574 #endif
575 
576 #ifdef USE_ACCELEROMETER
577  {
578  int32 ast;
579  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
580  {
581  ast = start_sensor(&(pDeviceDescriptor->accelHandle));
582 
583  if (ISF_SUCCESS != ast) goto unlockdescriptor;
584  }
585  }
586 #endif
587 
588 #ifdef USE_GYROMETER
589  {
590  int32 gst;
591 
592  gst = start_sensor(&(pDeviceDescriptor->gyroHandle));
593 
594  if (ISF_SUCCESS != gst) goto unlockdescriptor;
595  }
596 #endif
597 
598  // Set device to active state.
600  retStat = ISF_SUCCESS;
601 
602 unlockdescriptor:
603  // Unlock the device descriptor.
604  _lwsem_post(&(pDeviceDescriptor->deviceSemaphore));
605 
606  return retStat;
607 }
608 /*! @brief This is the concrete implementation of the orientation sensor adapter for End Data.
609  * @details This function tells the Bus Manager to stop the callbacks and disable data collection.
610  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
611  *
612  * @return ::fsl_fusion_virt_3D_orient_EndData() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
613  * @retval ::ISF_SUCCESS is returned when the data collection stopped successfully.
614  * @retval ::DSA_ERR_PARAM is returned when a wrong parameter is passed into the function such as an invalid or NULL parameter.
615  *
616  * @Constraints None
617  *
618  * @Reentrant Yes
619  * @Libs fsl_fusion_virt_3D_orient.lib
620  */
622 {
623  // Check the input argument
624  if (NULL == pSensorHandle) {
625  return DSA_ERR_PARAM;
626  }
627 
628  int32_t retStat = DSA_ERR_END_DATA;
629 
630  // Get the device descriptor and specific settings
631  fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = pSensorHandle->pDeviceDescriptor;
632 
633  // Check the device descriptor.
634  if (NULL == pDeviceDescriptor)
635  return DSA_ERR_PARAM;
636 
638 
639  // Lock the device descriptor.
640  _lwsem_wait_ticks(&(pDeviceDescriptor->deviceSemaphore), 0);
641 
642  // Check the driver state.
643  if (DSA_STATE_CONFIGURED_STOPPED > pSensorHandle->adapterStatus)
644  {
645  goto unlockdescriptor;
646  }
647 
648 #ifdef USE_MAGNETOMETER
649  {
650  int32 mst;
651 
652  mst = stop_sensor(&(pDeviceDescriptor->magHandle));
653 
654  if (ISF_SUCCESS != mst) goto unlockdescriptor;
655  }
656 #endif
657 
658 #ifdef USE_PRESSURE_SENSOR
659  {
660  int32 pst;
661 
662  pst = stop_sensor(&(pDeviceDescriptor->pressureHandle));
663 
664  if (ISF_SUCCESS != pst) goto unlockdescriptor;
665  }
666 #endif
667 
668 #ifdef USE_ACCELEROMETER
669  {
670  int32 ast;
671  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
672  {
673  ast = stop_sensor(&(pDeviceDescriptor->accelHandle));
674 
675  if (ISF_SUCCESS != ast) goto unlockdescriptor;
676  }
677  }
678 #endif
679 
680 #ifdef USE_GYROMETER
681  {
682  int32 gst;
683 
684  gst = stop_sensor(&(pDeviceDescriptor->gyroHandle));
685 
686  if (ISF_SUCCESS != gst) goto unlockdescriptor;
687  }
688 #endif
689 
691  retStat = ISF_SUCCESS;
692 
693 
694 unlockdescriptor:
695  // Unlock the device descriptor.
696  _lwsem_post(&(pDeviceDescriptor->deviceSemaphore));
697 
698  return retStat;
699 }
700 /*! @brief This is the concrete implementation of the orientation sensor adapter for calibration .
701  * @details The FXAS21000 does not provide the capability to be dynamically calibrated. Therefore, this function simply returns success.
702  *
703  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
704  *
705  @return ::fsl_fusion_virt_3D_orient_Calibrate() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
706  * @retval ::ISF_SUCCESS is returned when the device is calibrated properly.
707  * \b Note: Currently, there is no return error type defined for this API.
708  *
709  * @Constraints None
710  *
711  * @Reentrant Yes
712  * @Libs fsl_fusion_virt_3D_orient.lib
713  */
715 {
716  return ISF_SUCCESS;
717 }
718 /*! @brief This is the concrete implementation of the orientation sensor adapter for shutdown .
719  *
720  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
721  *
722  @return ::fsl_fusion_virt_3D_orient_Shutdown() returns a value of type ::isf_dsa_status_t indicating the success or failure of the function call.
723  * @retval ::ISF_SUCCESS is returned when the device is calibrated properly.
724  * \b Note: Currently, there is no return error type defined for this API.
725  *
726  * @Constraints None
727  *
728  * @Reentrant Yes
729  * @Libs fsl_fusion_virt_3D_orient.lib
730  */
732 {
733  if(NULL == pSensorHandle){
734  return DSA_ERR_PARAM;
735  }
736 
737  // Create helper reference pointers.
740 
741 #ifdef USE_MAGNETOMETER
742  shutdown_sensor(&(pDeviceDescriptor->magHandle));
743 #endif
744 
745 #ifdef USE_PRESSURE_SENSOR
746  shutdown_sensor(&(pDeviceDescriptor->pressureHandle));
747 #endif
748 
749 #ifdef USE_ACCELEROMETER
750  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
751  {
752  shutdown_sensor(&(pDeviceDescriptor->accelHandle));
753  }
754 #endif
755 
756 #ifdef USE_GYROMETER
757  shutdown_sensor(&(pDeviceDescriptor->gyroHandle));
758 #endif
759 
760  pSensorHandle->adapterStatus = DSA_STATE_INITIALIZED;
761  return ISF_SUCCESS;
762 }
763 /*! @brief The orientation sensor adapter's periodic processing function.
764  * @details This function is invoked periodically whenever new sensor data is available from its underlying sensors.
765  *
766  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
767  *
768  * @return Void There is no return value.
769  *
770  * @Constraints None
771  *
772  * @Reentrant Yes
773  * @Libs fsl_fusion_virt_3D_orient.lib
774  */
776 {
777 
778 
779  // Get the device descriptor and specific settings
780  isf_SensorHandle_t *pSensorHdl = (isf_SensorHandle_t *)pSensorHandle;
784  fusion_state_t *pState = &(pDesc->fusionState);
785 
786  isf_status_t st = 0;
787 
788  uint32 signalledEvents = _lwevent_get_signalled();
789 
790  if (pState->softReset)
791  {
792  Fusion_Init(pState);
793  pState->softReset = 0;
794  }
795 
796 #ifdef USE_ACCELEROMETER
797  if (
798  (signalledEvents & ACCEL_DATA_READY_EVENT) ||
799  ((pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId) && (signalledEvents & MAG_DATA_READY_EVENT))
800  )
801  {
802  isf_fifo_t* pAccelFifo;
803 
804  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
805  {
806  pAccelFifo = &(pDesc->accelFifo);
807  }
808  else
809  {
810  pAccelFifo = &(pDesc->magFifo);
811  }
812 
813  float sumX = 0.0F;
814  float sumY = 0.0F;
815  float sumZ = 0.0F;
816 
817  isf_Acceleration3D_Float_t *pSampleIter = 0;
819  isf_Acceleration3D_Float_t tmpSample;
820  int8 sampleCount = 0;
822 
823  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
824  {
825  pSample = &tmpSample;
826  }
827 
828  isf_fifo_lock(pAccelFifo);
829 
830  do
831  {
832  st = isf_fifo_el_traverse ( pAccelFifo, (void **)&pSampleIter );
833 
834  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
835  {
836  convert_sensor_data(&(pDesc->magHandle), TYPE_RAW_ACCELERATION_3D, DSA_RESULT_TYPE_ENG_FLOAT, pSampleIter, &tmpSample);
837  }
838  else
839  {
840  pSample = pSampleIter;
841  }
842 
843  pState->thisAccel.fGpFast[X] = ACCEL_GET_HAL_X_VALUE(pSample->accel[ACCEL_HAL_X_INDEX]);
844  pState->thisAccel.fGpFast[Y] = ACCEL_GET_HAL_Y_VALUE(pSample->accel[ACCEL_HAL_Y_INDEX]);
845  pState->thisAccel.fGpFast[Z] = ACCEL_GET_HAL_Z_VALUE(pSample->accel[ACCEL_HAL_Z_INDEX]);
846 
847  sumX += pState->thisAccel.fGpFast[X];
848  sumY += pState->thisAccel.fGpFast[Y];
849  sumZ += pState->thisAccel.fGpFast[Z];
850  ++sampleCount;
851 
852  } while(st != ISF_FIFO_NO_MORE_ENTRIES);
853 
854  pState->accelTimestamp = pSample->timestamp;
855 
856  if ( pSpecificSettings->magnetometerSensorId != pSpecificSettings->accelerometerSensorId )
857  {
858  isf_fifo_el_clear(pAccelFifo);
859  }
860  isf_fifo_unlock(pAccelFifo);
861 
862  pState->thisAccel.fGp[X] = sumX / sampleCount;
863  pState->thisAccel.fGp[Y] = sumY / sampleCount;
864  pState->thisAccel.fGp[Z] = sumZ / sampleCount;
865 
866  pState->thisAccel.iGp[X] = pState->thisAccel.fGp[X] * 10000.0F;
867  pState->thisAccel.iGp[Y] = pState->thisAccel.fGp[Y] * 10000.0F;
868  pState->thisAccel.iGp[Z] = pState->thisAccel.fGp[Z] * 10000.0F;
869 
870  pDesc->sensorDataReady &= ~ACCEL_DATA_READY_EVENT;
871  }
872 #endif
873 #ifdef USE_MAGNETOMETER
874  if (signalledEvents & MAG_DATA_READY_EVENT)
875  {
876  float sumX = 0.0F;
877  float sumY = 0.0F;
878  float sumZ = 0.0F;
879 
880  isf_MagneticFieldStrength3D_float_t *pSampleIter = 0;
883  int8 sampleCount = 0;
885 
886  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
887  {
888  pSample = &tmpSample;
889  }
890 
891  isf_fifo_lock(&(pDesc->magFifo));
892 
893  do
894  {
895  st = isf_fifo_el_traverse ( &(pDesc->magFifo), (void **)&pSampleIter );
896 
897  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
898  {
899  convert_sensor_data(&(pDesc->magHandle), TYPE_MAGNETIC_FIELD_STRENGTH_3D, DSA_RESULT_TYPE_ENG_FLOAT, pSampleIter, pSample);
900  }
901  else
902  {
903  pSample = pSampleIter;
904  }
905 
906  pState->thisMag.fBpFast[X] = MAG_GET_HAL_X_VALUE(pSample->fieldStrength[MAG_HAL_X_INDEX]);
907  pState->thisMag.fBpFast[Y] = MAG_GET_HAL_Y_VALUE(pSample->fieldStrength[MAG_HAL_Y_INDEX]);
908  pState->thisMag.fBpFast[Z] = MAG_GET_HAL_Z_VALUE(pSample->fieldStrength[MAG_HAL_Z_INDEX]);
909 
910  sumX += pState->thisMag.fBpFast[X];
911  sumY += pState->thisMag.fBpFast[Y];
912  sumZ += pState->thisMag.fBpFast[Z];
913  ++sampleCount;
914  } while(st != ISF_FIFO_NO_MORE_ENTRIES);
915 
916  pState->magTimestamp = pSample->timestamp;
917  isf_fifo_el_clear(&(pDesc->magFifo));
918  isf_fifo_unlock(&(pDesc->magFifo));
919 
920  pState->thisMag.fBp[X] = sumX / sampleCount;
921  pState->thisMag.fBp[Y] = sumY / sampleCount;
922  pState->thisMag.fBp[Z] = sumZ / sampleCount;
923 
924  {
925 #if 0
926  pState->thisMag.iBp[X] = float32_To_AcFixed(pState->thisMag.fBp[X], 16, 8, 1, &st);
927  pState->thisMag.iBp[Y] = float32_To_AcFixed(pState->thisMag.fBp[Y], 16, 8, 1, &st);
928  pState->thisMag.iBp[Z] = float32_To_AcFixed(pState->thisMag.fBp[Z], 16, 8, 1, &st);
929 #else
930  pState->thisMag.iBp[X] = (int16)(pState->thisMag.fBp[X] * 10.0F);
931  pState->thisMag.iBp[Y] = (int16)(pState->thisMag.fBp[Y] * 10.0F);
932  pState->thisMag.iBp[Z] = (int16)(pState->thisMag.fBp[Z] * 10.0F);
933 #endif
934  }
935  pDesc->sensorDataReady &= ~MAG_DATA_READY_EVENT;
936  }
937 #endif
938 
939 #ifdef USE_PRESSURE_SENSOR
940  if (signalledEvents & PRESSURE_DATA_READY_EVENT)
941  {
942 
943  isf_Meters1D_float_t *pPressureSample = 0;
945 
946  isf_fifo_lock(&(pDesc->pressureFifo));
947 
948  do
949  {
950  st = isf_fifo_el_traverse ( &(pDesc->pressureFifo), &pPressureSample );
951  pState->thisPressure.fHp = pPressureSample->altitude;
952  } while (st != ISF_FIFO_NO_MORE_ENTRIES);
953 
954  pState->pressureTimestamp = pPressureSample->timestamp;
955 
956  isf_fifo_el_clear(&(pDesc->pressureFifo));
957  isf_fifo_unlock(&(pDesc->pressureFifo));
958  pDesc->sensorDataReady &= ~PRESSURE_DATA_READY_EVENT;
959  }
960 #endif
961 
962 #ifdef USE_GYROMETER
963  if (signalledEvents & GYRO_DATA_READY_EVENT)
964  {
965 
966  isf_AngularVelocity3D__float_t *pGyroSample = 0;
967  int8 sampleIdx;
969 
970  isf_fifo_lock(&(pDesc->gyroFifo));
971  sampleIdx = 0;
972 
973  do
974  {
975  st = isf_fifo_el_traverse ( &(pDesc->gyroFifo), (void **)&pGyroSample );
976  pState->thisGyro.fYpBuffer[sampleIdx][X] = GYRO_GET_HAL_X_VALUE(pGyroSample->angularVelocity[GYRO_HAL_X_INDEX]);
977  pState->thisGyro.fYpBuffer[sampleIdx][Y] = GYRO_GET_HAL_Y_VALUE(pGyroSample->angularVelocity[GYRO_HAL_Y_INDEX]);
978  pState->thisGyro.fYpBuffer[sampleIdx][Z] = GYRO_GET_HAL_Z_VALUE(pGyroSample->angularVelocity[GYRO_HAL_Z_INDEX]);
979 
980  ++sampleIdx;
981  } while ( st != ISF_FIFO_NO_MORE_ENTRIES);
982 
983  pState->gyroTimestamp = pGyroSample->timestamp;
984 
985  isf_fifo_el_clear(&(pDesc->gyroFifo));
986  isf_fifo_unlock(&(pDesc->gyroFifo));
987 
988  pDesc->sensorDataReady &= ~GYRO_DATA_READY_EVENT;
989  }
990 #endif
991 
992  {
993  int8 initiatemagcal;
994 
995  if (!pDesc->sensorDataReady)
996  {
997  initiatemagcal = Fusion_Run(pState, &(pSpecificSettings->algorithmConfig));
998 
999  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
1000  {
1002  }
1003  else
1004  {
1006  }
1007 
1008  // Lock the fifo for update.
1009  if(MQX_OK != isf_fifo_lock(pFifo)){
1010  return;
1011  }
1012 
1013  void *pFifoEntry = isf_fifo_el_get_insert_pointer(pFifo);
1014 
1015  // write the new data
1017  {
1018  set_current_sample_buffer((fsl_orientation_DataBuffer_t *)pFifoEntry, pState, pSpecificSettings);
1019  }
1020  else
1021  {
1022  fsl_orientation_DataBuffer_t currentSampleBuffer;
1023  int32 numBytes;
1024 
1025  set_current_sample_buffer(&currentSampleBuffer, pState, pSpecificSettings);
1026 
1028  pSensorHdl,
1031  &currentSampleBuffer,
1032  pFifoEntry,
1033  &numBytes);
1034  }
1035 
1036  // Increment the fifo to the next sample entry.
1037  st = isf_fifo_el_increment(pFifo);
1038 
1039  // Unlock the fifo.
1040  isf_fifo_unlock(pFifo);
1041 
1042  if (st == ISF_FIFO_FULL)
1043  {
1044  // Notify the user using their registered event information
1045  _lwevent_set(pSensorHdl->controlData.pEventGroup,(_mqx_uint)pSensorHdl->controlData.nEventFieldIndex);
1046  }
1047 
1048  if (initiatemagcal)
1049  {
1050  _lwevent_set(&(pDesc->MagCalEventStruct), MAGCAL_EVENT_FLAG);
1051  }
1052  }
1053  }
1054 }
1055 
1056 void fsl_fusion_task(uint32_t task_init_data)
1057 {
1058  isf_SensorHandle_t *pSensorHandle = (isf_SensorHandle_t *)task_init_data;
1059  _mqx_uint mqxStat;
1060 
1061  // Check the input argument
1062  if (NULL == pSensorHandle) {
1063  return;
1064  }
1065 
1068 
1069  // Check the device descriptor
1070  if (NULL == pDesc){
1071  return;
1072  }
1073 
1074  Fusion_Init(&(pDesc->fusionState));
1075 
1076  // Execute the fusion computation loop
1077  // This loop should never exit
1078  for (;;)
1079  {
1080  _mqx_uint readyFlags;
1081  /* wait for sensor data to arrive.*/
1082  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
1083  {
1084  readyFlags = HYBRID_SENSOR_DATA_READY_EVENT;
1085  }
1086  else
1087  {
1088  readyFlags = SENSOR_DATA_READY_EVENT;
1089  }
1090 
1091  mqxStat = _lwevent_wait_for(&(pDesc->sensorLWEvent), readyFlags, FALSE, NULL);
1092 
1093  if ( mqxStat == MQX_OK) fsl_fusion_virt_3D_orient_PeriodicCallback(pSensorHandle);
1094  }
1095 }
1096 
1097 
1098 /* User includes (#include below this line is not maintained by Processor Expert) */
1099 /*
1100 ** ===================================================================
1101 ** Event : MagCal_task (module mqx_tasks)
1102 **
1103 ** Component : MagCal_task [MQXLite_task]
1104 ** Description :
1105 ** MQX task routine. The routine is generated into mqx_tasks.c
1106 ** file.
1107 ** Parameters :
1108 ** NAME - DESCRIPTION
1109 ** task_init_data -
1110 ** Returns : Nothing
1111 ** ===================================================================
1112 */
1113 // MagCal_task
1114 void fsl_MagCal_task(uint32_t task_init_data)
1115 {
1116  isf_SensorHandle_t *pSensorHandle = (isf_SensorHandle_t *)task_init_data;
1117 
1119  // magnetic DOF: reset magnetic calibration and magnetometer data buffer (not needed for 3DOF)
1120 
1121  while(1)
1122  {
1123 
1124  // wait for the magnetic calibration event
1125  // this event will never be enabled for build options which don't require magnetic calibration
1126  // FALSE means any bit (of the 1 bit enabled by the mask) unblocks
1127  // and NULL means timeout is infinite
1128  _lwevent_wait_for(&(pDesc->MagCalEventStruct), MAGCAL_EVENT_FLAG, FALSE, NULL);
1129 
1130  // prevent compilation errors when magnetic calibration is not required
1131 #if defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
1132  // and run the magnetic calibration
1133  MagCal_Run(&(pDesc->fusionState));
1134 #endif
1135 
1136  } // end of infinite loop
1137 }
1138 
1139 
1140 /*!
1141  * @brief This function coverts the raw sample data to the desired output type.
1142  */
1144  (
1145  volatile isf_SensorHandle_t *pSensorHandle,
1146  isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType,
1147  void *pNativeSample,
1148  void *pConvertedSample,
1149  int32 *numBytes
1150  )
1151 {
1154 
1155  pConverter = NULL;
1156 
1157  // TODO - Only supports float types for now
1158  if (resultType != DSA_RESULT_TYPE_ENG_FLOAT) {
1160  }
1161 
1162  switch (convertToType)
1163  {
1165  pConverter = accel_converter;
1166  break;
1168  pConverter = mag_converter;
1169  break;
1171  pConverter = gyro_converter;
1172  break;
1173  case TYPE_QUATERNION:
1174  pConverter = quaternion_converter;
1175  break;
1176  case TYPE_EULER_3D:
1177  pConverter = euler_converter;
1178  break;
1180  pConverter = dircosine_converter;
1181  break;
1182  default:
1184  }
1185 
1186  if (pConverter == NULL) {
1187  return DSA_ERR_BAD_RESULT_TYPE;
1188  }
1189 
1190  retStat = pConverter(
1192  (fsl_orientation_DataBuffer_t *)pNativeSample,
1193  pConvertedSample
1194  );
1195 
1196  return retStat;
1197 }
1198 
1199 static isf_dsa_status_t quaternion_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1200 {
1202  convertedSample->timestamp = nativeSample->timestamp;
1203  convertedSample->scalar = nativeSample->fq.q0;
1204  convertedSample->xvect = nativeSample->fq.q1;
1205  convertedSample->yvect = nativeSample->fq.q2;
1206  convertedSample->zvect = nativeSample->fq.q3;
1207  return ISF_SUCCESS;
1208 }
1209 
1210 static isf_dsa_status_t euler_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1211 {
1212  isf_Orientation3D_Euler_EngFloat_t *convertedSample = (isf_Orientation3D_Euler_EngFloat_t *)vpConvertedSample;
1213  convertedSample->timestamp = nativeSample->timestamp;
1214  convertedSample->fThe = nativeSample->fThe;
1215  convertedSample->fPhi = nativeSample->fPhi;
1216  convertedSample->fPsi = nativeSample->fPsi;
1217 
1218  return ISF_SUCCESS;
1219 }
1220 static isf_dsa_status_t dircosine_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1221 {
1222  isf_Orientation3D_Rotation_EngFloat_t *convertedSample = (isf_Orientation3D_Rotation_EngFloat_t *)vpConvertedSample;
1223  convertedSample->timestamp = nativeSample->timestamp;
1224  convertedSample->rMatrix[0][0] = nativeSample->fRmatrix[0][0];
1225  convertedSample->rMatrix[0][1] = nativeSample->fRmatrix[0][1];
1226  convertedSample->rMatrix[0][2] = nativeSample->fRmatrix[0][2];
1227  convertedSample->rMatrix[1][0] = nativeSample->fRmatrix[1][0];
1228  convertedSample->rMatrix[1][1] = nativeSample->fRmatrix[1][1];
1229  convertedSample->rMatrix[1][2] = nativeSample->fRmatrix[1][2];
1230  convertedSample->rMatrix[2][0] = nativeSample->fRmatrix[2][0];
1231  convertedSample->rMatrix[2][1] = nativeSample->fRmatrix[2][1];
1232  convertedSample->rMatrix[2][2] = nativeSample->fRmatrix[2][2];
1233 
1234  return ISF_SUCCESS;
1235 }
1236 static isf_dsa_status_t accel_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1237 {
1238  isf_Acceleration3D_Float_t *convertedSample = (isf_Acceleration3D_Float_t *)vpConvertedSample;
1239  convertedSample->timestamp = nativeSample->accelData.timestamp;
1240  convertedSample->accel[0] = nativeSample->accelData.accel[0];
1241  convertedSample->accel[1] = nativeSample->accelData.accel[1];
1242  convertedSample->accel[2] = nativeSample->accelData.accel[2];
1243  return ISF_SUCCESS;
1244 }
1245 static isf_dsa_status_t mag_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1246 {
1247  isf_MagneticFieldStrength3D_float_t *convertedSample = (isf_MagneticFieldStrength3D_float_t *)vpConvertedSample;
1248  convertedSample->timestamp = nativeSample->magData.timestamp;
1249  convertedSample->fieldStrength[0] = nativeSample->magData.fieldStrength[0];
1250  convertedSample->fieldStrength[1] = nativeSample->magData.fieldStrength[1];
1251  convertedSample->fieldStrength[2] = nativeSample->magData.fieldStrength[2];
1252  return ISF_SUCCESS;
1253 }
1254 static isf_dsa_status_t gyro_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1255 {
1256  isf_AngularVelocity3D__float_t *convertedSample = (isf_AngularVelocity3D__float_t *)vpConvertedSample;
1257  convertedSample->timestamp = nativeSample->magData.timestamp;
1258  convertedSample->angularVelocity[0] = nativeSample->gyroData.angularVelocity[0];
1259  convertedSample->angularVelocity[1] = nativeSample->gyroData.angularVelocity[1];
1260  convertedSample->angularVelocity[2] = nativeSample->gyroData.angularVelocity[2];
1261  return ISF_SUCCESS;
1262 }
1263 
1264 void set_current_sample_buffer(
1265  fsl_orientation_DataBuffer_t *pCurrentSampleBuffer,
1266  fusion_state_t *pState,
1267  fsl_fusion_Sensor_Specific_Settings_t *pSpecificSettings
1268 )
1269 {
1270  int i,j;
1271  int latestGyroSampleIdx = pSpecificSettings->algorithmConfig.gyroOversampleRatio-1;
1272 
1273 #ifdef USE_PRESSURE_SENSOR
1274  pCurrentSampleBuffer->pressure = pState->thisPressure.fHp;
1275 #endif
1276 #ifdef USE_ACCELEROMETER
1277  pCurrentSampleBuffer->accelData.accel[0] = pState->thisAccel.fGpFast[0];
1278  pCurrentSampleBuffer->accelData.accel[1] = pState->thisAccel.fGpFast[1];
1279  pCurrentSampleBuffer->accelData.accel[2] = pState->thisAccel.fGpFast[2];
1280  pCurrentSampleBuffer->accelData.timestamp= pState->accelTimestamp;
1281 #endif
1282 #ifdef USE_MAGNETOMETER
1283 #ifdef USE_CALIBRATED_MAG_VALUES
1284  pCurrentSampleBuffer->magData.fieldStrength[0] = pState->thisMag.fBc[0];
1285  pCurrentSampleBuffer->magData.fieldStrength[1] = pState->thisMag.fBc[1];
1286  pCurrentSampleBuffer->magData.fieldStrength[2] = pState->thisMag.fBc[2];
1287 #else
1288  pCurrentSampleBuffer->magData.fieldStrength[0] = pState->thisMag.fBpFast[0];
1289  pCurrentSampleBuffer->magData.fieldStrength[1] = pState->thisMag.fBpFast[1];
1290  pCurrentSampleBuffer->magData.fieldStrength[2] = pState->thisMag.fBpFast[2];
1291 #endif
1292  pCurrentSampleBuffer->magData.timestamp= pState->magTimestamp;
1293 #endif
1294 #ifdef USE_GYROMETER
1295  pCurrentSampleBuffer->gyroData.angularVelocity[0] = pState->thisGyro.fYpBuffer[latestGyroSampleIdx][0];
1296  pCurrentSampleBuffer->gyroData.angularVelocity[1] = pState->thisGyro.fYpBuffer[latestGyroSampleIdx][1];
1297  pCurrentSampleBuffer->gyroData.angularVelocity[2] = pState->thisGyro.fYpBuffer[latestGyroSampleIdx][2];
1298  pCurrentSampleBuffer->gyroData.timestamp= pState->gyroTimestamp;
1299 #endif
1300 
1301  switch (pState->algorithmToUse)
1302  {
1303 #ifdef COMPUTE_3DOF_G_BASIC
1304  case Q3:
1305 
1306  pCurrentSampleBuffer->fChi = pState->thisSV_3DOF_G_BASIC.fLPChi;
1307  pCurrentSampleBuffer->fPhi = pState->thisSV_3DOF_G_BASIC.fLPPhi;
1308  pCurrentSampleBuffer->fPsi = pState->thisSV_3DOF_G_BASIC.fLPPsi;
1309  pCurrentSampleBuffer->fRho = pState->thisSV_3DOF_G_BASIC.fLPRho;
1310  pCurrentSampleBuffer->fThe = pState->thisSV_3DOF_G_BASIC.fLPThe;
1311  pCurrentSampleBuffer->fq = pState->thisSV_3DOF_G_BASIC.fLPq;
1312  pCurrentSampleBuffer->fDelta = 0.0;
1313 
1314  for (i=0; i<3; i++)
1315  {
1316  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_3DOF_G_BASIC.fLPRVec[i];
1317 
1318 
1319  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_3DOF_G_BASIC.fOmega[i];
1320 
1321  pCurrentSampleBuffer->faSe[i] = 0.0;
1322  pCurrentSampleBuffer->fbErr[i] = 0.0;
1323  pCurrentSampleBuffer->fb[i] = 0.0;
1324  pCurrentSampleBuffer->fThErr[i] = 0.0;
1325  for (j=0; j<3; j++)
1326  {
1327  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_3DOF_G_BASIC.fLPR[i][j];
1328  }
1329  }
1330  pCurrentSampleBuffer->timestamp = pState->accelTimestamp;
1331 
1332  break;
1333 #endif
1334 #ifdef COMPUTE_3DOF_B_BASIC
1335  case Q3M:
1336  pCurrentSampleBuffer->fChi = pState->thisSV_3DOF_B_BASIC.fLPChi;
1337  pCurrentSampleBuffer->fPhi = pState->thisSV_3DOF_B_BASIC.fLPPhi;
1338  pCurrentSampleBuffer->fPsi = pState->thisSV_3DOF_B_BASIC.fLPPsi;
1339  pCurrentSampleBuffer->fRho = pState->thisSV_3DOF_B_BASIC.fLPRho;
1340  pCurrentSampleBuffer->fThe = pState->thisSV_3DOF_B_BASIC.fLPThe;
1341  pCurrentSampleBuffer->fq = pState->thisSV_3DOF_B_BASIC.fq;
1342  pCurrentSampleBuffer->fDelta = 0.0;
1343 
1344  for (i=0; i<3; i++)
1345  {
1346  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_3DOF_B_BASIC.fLPRVec[i];
1347 
1348 
1349  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_3DOF_B_BASIC.fOmega[i];
1350 
1351  pCurrentSampleBuffer->faSe[i] = 0.0;
1352  pCurrentSampleBuffer->fbErr[i] = 0.0;
1353  pCurrentSampleBuffer->fb[i] = 0.0;
1354  pCurrentSampleBuffer->fThErr[i] = 0.0;
1355  for (j=0; j<3; j++)
1356  {
1357  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_3DOF_B_BASIC.fLPR[i][j];
1358  }
1359  }
1360  pCurrentSampleBuffer->timestamp = pState->magTimestamp;
1361 
1362  break;
1363 #endif
1364 #ifdef COMPUTE_3DOF_Y_BASIC
1365  case Q3G:
1366  pCurrentSampleBuffer->fChi = pState->thisSV_3DOF_Y_BASIC.fChi;
1367  pCurrentSampleBuffer->fPhi = pState->thisSV_3DOF_Y_BASIC.fPhi;
1368  pCurrentSampleBuffer->fPsi = pState->thisSV_3DOF_Y_BASIC.fPsi;
1369  pCurrentSampleBuffer->fRho = pState->thisSV_3DOF_Y_BASIC.fRho;
1370  pCurrentSampleBuffer->fThe = pState->thisSV_3DOF_Y_BASIC.fThe;
1371  pCurrentSampleBuffer->fq = pState->thisSV_3DOF_Y_BASIC.fq;
1372  pCurrentSampleBuffer->fDelta = 0.0;
1373 
1374  for (i=0; i<3; i++)
1375  {
1376  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_3DOF_Y_BASIC.fRVec[i];
1377 
1378 
1379  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_3DOF_Y_BASIC.fOmega[i];
1380 
1381  pCurrentSampleBuffer->faSe[i] = 0.0;
1382  pCurrentSampleBuffer->fbErr[i] = 0.0;
1383  pCurrentSampleBuffer->fb[i] = 0.0;
1384  pCurrentSampleBuffer->fThErr[i] = 0.0;
1385  for (j=0; j<3; j++)
1386  {
1387  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_3DOF_Y_BASIC.fR[i][j];
1388  }
1389  }
1390 
1391  pCurrentSampleBuffer->timestamp = pState->gyroTimestamp;
1392 
1393  break;
1394 #endif
1395 #ifdef COMPUTE_6DOF_GB_BASIC
1396  case Q6MA:
1397  pCurrentSampleBuffer->fChi = pState->thisSV_6DOF_GB_BASIC.fLPChi;
1398  pCurrentSampleBuffer->fPhi = pState->thisSV_6DOF_GB_BASIC.fLPPhi;
1399  pCurrentSampleBuffer->fPsi = pState->thisSV_6DOF_GB_BASIC.fLPPsi;
1400  pCurrentSampleBuffer->fRho = pState->thisSV_6DOF_GB_BASIC.fLPRho;
1401  pCurrentSampleBuffer->fThe = pState->thisSV_6DOF_GB_BASIC.fLPThe;
1402  pCurrentSampleBuffer->fq = pState->thisSV_6DOF_GB_BASIC.fLPq;
1403  pCurrentSampleBuffer->fDelta = pState->thisSV_6DOF_GB_BASIC.fLPDelta;
1404 
1405  for (i=0; i<3; i++)
1406  {
1407  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_6DOF_GB_BASIC.fLPRVec[i];
1408 
1409 
1410  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_6DOF_GB_BASIC.fOmega[i];
1411 
1412  pCurrentSampleBuffer->faSe[i] = 0.0;
1413  pCurrentSampleBuffer->fbErr[i] = 0.0;
1414  pCurrentSampleBuffer->fb[i] = 0.0;
1415  pCurrentSampleBuffer->fThErr[i] = 0.0;
1416  for (j=0; j<3; j++)
1417  {
1418  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_6DOF_GB_BASIC.fLPR[i][j];
1419  }
1420  }
1421  if (pState->accelTimestamp > pState->magTimestamp)
1422  {
1423  pCurrentSampleBuffer->timestamp = pState->accelTimestamp;
1424  }
1425  else
1426  {
1427  pCurrentSampleBuffer->timestamp = pState->magTimestamp;
1428  }
1429  break;
1430 #endif
1431 #ifdef COMPUTE_6DOF_GY_KALMAN
1432  case Q6AG:
1433  pCurrentSampleBuffer->fChi = pState->thisSV_6DOF_GY_KALMAN.fChiPl;
1434  pCurrentSampleBuffer->fPhi = pState->thisSV_6DOF_GY_KALMAN.fPhiPl;
1435  pCurrentSampleBuffer->fPsi = pState->thisSV_6DOF_GY_KALMAN.fPsiPl;
1436  pCurrentSampleBuffer->fRho = pState->thisSV_6DOF_GY_KALMAN.fRhoPl;
1437  pCurrentSampleBuffer->fThe = pState->thisSV_6DOF_GY_KALMAN.fThePl;
1438  pCurrentSampleBuffer->fq = pState->thisSV_6DOF_GY_KALMAN.fqPl;
1439  pCurrentSampleBuffer->fDelta = 0.0;
1440 
1441  for (i=0; i<3; i++)
1442  {
1443  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_6DOF_GY_KALMAN.fRVecPl[i];
1444 
1445 
1446  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_6DOF_GY_KALMAN.fOmega[i];
1447 
1448  pCurrentSampleBuffer->faSe[i] = pState->thisSV_6DOF_GY_KALMAN.faSePl[i];
1449  pCurrentSampleBuffer->fbErr[i] = pState->thisSV_6DOF_GY_KALMAN.fbErrPl[i];
1450  pCurrentSampleBuffer->fb[i] = pState->thisSV_6DOF_GY_KALMAN.fbPl[i];
1451  pCurrentSampleBuffer->fThErr[i] = pState->thisSV_6DOF_GY_KALMAN.fThErrPl[i];
1452  for (j=0; j<3; j++)
1453  {
1454  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_6DOF_GY_KALMAN.fRPl[i][j];
1455  }
1456  }
1457 
1458  pCurrentSampleBuffer->timestamp = pState->gyroTimestamp;
1459 
1460 
1461  break;
1462 #endif
1463 #ifdef COMPUTE_9DOF_GBY_KALMAN
1464  case Q9:
1465  default:
1466  pCurrentSampleBuffer->fChi = pState->thisSV_9DOF_GBY_KALMAN.fChiPl;
1467  pCurrentSampleBuffer->fPhi = pState->thisSV_9DOF_GBY_KALMAN.fPhiPl;
1468  pCurrentSampleBuffer->fPsi = pState->thisSV_9DOF_GBY_KALMAN.fPsiPl;
1469  pCurrentSampleBuffer->fRho = pState->thisSV_9DOF_GBY_KALMAN.fRhoPl;
1470  pCurrentSampleBuffer->fThe = pState->thisSV_9DOF_GBY_KALMAN.fThePl;
1471  pCurrentSampleBuffer->fq = pState->thisSV_9DOF_GBY_KALMAN.fqPl;
1472  pCurrentSampleBuffer->fDelta = 0.0;
1473 
1474  for (i=0; i<3; i++)
1475  {
1476  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_9DOF_GBY_KALMAN.fRVecPl[i];
1477 
1478 
1479  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_9DOF_GBY_KALMAN.fOmega[i];
1480 
1481  pCurrentSampleBuffer->faSe[i] = pState->thisSV_9DOF_GBY_KALMAN.faSePl[i];
1482  pCurrentSampleBuffer->fbErr[i] = pState->thisSV_9DOF_GBY_KALMAN.fbErrPl[i];
1483  pCurrentSampleBuffer->fb[i] = pState->thisSV_9DOF_GBY_KALMAN.fbPl[i];
1484  pCurrentSampleBuffer->fThErr[i] = pState->thisSV_9DOF_GBY_KALMAN.fThErrPl[i];
1485  for (j=0; j<3; j++)
1486  {
1487  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_9DOF_GBY_KALMAN.fRPl[i][j];
1488  }
1489  }
1490 
1491  pCurrentSampleBuffer->timestamp = pState->gyroTimestamp;
1492 
1493  break;
1494 #endif
1495  }
1496 }
1497 
1498 
1499 
1500 
1501 
1502 
1503 
1504 
1505 
1506 
1507 
1508 
1509 
1510 
1511 
1512 
1513 
1514 
1515 
1516 
1517 
1518 
1519 
1520 
1521 
1522 
1523 
1524 
1525 
1526 
1527 
1528 
1529 
1530 
1531 
1532 
1533 
1534 
1535 
1536 
1537 
1538 
1539 
1540 
1541 
1542 
1543 
1544 
1545 
1546 
1547 
1548 
1549 
1550 
1551 
1552 
1553 
1554 
1555 
1556 
1557 
1558 
1559 
1560 
1561 
1562 
1563 
1564 
1565 
1566 
1567 
1568 
1569 
1570 
1571 
1572 
1573 
1574 
1575 
1576 
1577 
1578 
1579 
1580 
1581 
1582 
1583 
1584 
1585 
1586 
1587 
1588 
1589 
1590 
1591 
1592 
1593 
1594 
1595 
1596 
1597 
1598 
1599 
1600 
1601 
1602 
1603 
float fLPRVec[3]
Definition: fusion_types.h:86
isf_orientation_euler_deg_float_t fPsi
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:49
#define ACCEL_HAL_X_INDEX
Definition: fusion_config.h:46
void * pSensorSpecificSettings
isf_dsa_status_t fsl_fusion_virt_3D_orient_Initialize(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter initialization.
float fBpFast[3]
struct fquaternion fLPq
Definition: fusion_types.h:59
fsl_i2c_master_driver.h defines structures and types for the i2c master driver.
#define ACCEL_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:49
isf_orientation_euler_deg_float_t fThe
#define MAG_HAL_X_INDEX
Definition: fusion_config.h:52
int32 stop_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:99
Standard fixed type for three axis accelerometers.
The isf_magnetometer_types.h file contains the ISF data type definitions for use with the ISF generic...
#define ACCEL_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:45
isf_SensorDataTypes_t fsl_fusion_SupportedDataTypes[]
isf_dsa_status_t fsl_fusion_virt_3D_orient_Configure(isf_SensorHandle_t *pSensorHandle, isf_dsa_SensorSettings_t *pSensorSettings)
This is the concrete implementation of the fusion sensor adapter configuration function.
#define SENSOR_DATA_READY_EVENT
isf_dsa_ControlData_t controlData
isf_fifo_status_t isf_fifo_lock(isf_fifo_t *pFifo)
Lock a sample buffer for exclusive access.
Definition: isf_fifo.c:171
void fsl_MagCal_task(uint32_t task_init_data)
isf_fifo_status_t isf_fifo_unlock(isf_fifo_t *pFifo)
Release the exclusive access lock on a sample buffer.
Definition: isf_fifo.c:202
isf_orientation_euler_deg_float_t fPhi
struct MagSensor thisMag
Definition: fusion_exec.h:59
const isf_dsa_Adapter_t * pAdapter
struct SV_9DOF_GBY_KALMAN thisSV_9DOF_GBY_KALMAN
Definition: fusion_exec.h:102
isf_fifo_status_t isf_fifo_el_increment(isf_fifo_t *pFifo)
Routine increments the insert pointer after direct access.
Definition: isf_fifo.c:240
isf_sensors.h contains the ISF Generic Sensor definitions and data structures required when a client ...
float fRPl[3][3]
Definition: fusion_types.h:163
float fRPl[3][3]
Definition: fusion_types.h:212
#define FALSE
Definition: isf_types.h:56
isf_SensorTypes_t fsl_fusion_SupportedSensorTypes[]
Supported sensor and data types for the orientation sensor.
struct AccelSensor thisAccel
Definition: fusion_exec.h:55
struct fquaternion fqPl
Definition: fusion_types.h:213
isf_fieldStrength_uT_float_t fieldStrength[3]
API definitions, types, and macros for the Intelligent Sensing Framework (ISF) Bus Manager (BM)...
isf_altitude_meters_float_t altitude
#define USE_MAGNETOMETER
Definition: fusion_config.h:40
isf_orientation_quaternion_float_t xvect
This defines the DSA sensor device handle structure used to invoke the adapter access functions...
isf_fifo_status_t isf_fifo_el_clear(isf_fifo_t *pFifo)
Routine to clear the fifo.
Definition: isf_fifo.c:220
float fLPRVec[3]
Definition: fusion_types.h:60
struct fquaternion fq
Definition: fusion_types.h:93
isf_timestamp_t timestamp
isf_dsa_result_types_t resultFormat
The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT.
isf_timestamp_t gyroTimestamp
Definition: fusion_exec.h:67
#define MAG_DATA_READY_EVENT
Definition: matrix.h:33
signed char int8
Definition: basic_types.h:12
unsigned long uint32
This defines uint32 as unsigned long.
Definition: isf_types.h:36
struct SV_6DOF_GY_KALMAN thisSV_6DOF_GY_KALMAN
Definition: fusion_exec.h:97
isf_orientation_quaternion_float_t yvect
isf_dsa_status_t fsl_fusion_virt_3D_orient_Shutdown(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter for shutdown ...
isf_SensorTypes_t
#define MAG_HAL_Y_INDEX
Definition: fusion_config.h:54
isf_orientation_quaternion_float_t scalar
uint8 nFifoDepth
1 = no FIFO, or another value < SM_MAX_FIFO_DEPTH
The isf_types.h file contains the ISF data type definitions and some of the globally used macros...
float fLPR[3][3]
Definition: fusion_types.h:84
#define GYRO_HAL_X_INDEX
Definition: fusion_config.h:58
struct fquaternion fqPl
Definition: fusion_types.h:164
isf_dsa_status_t fsl_fusion_virt_3D_orient_EndData(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter for End Data.
#define HYBRID_SENSOR_DATA_READY_EVENT
float fR[3][3]
Definition: fusion_types.h:110
isf_dsa_DeviceInfoBlock_t devInfo
float fLPR[3][3]
Definition: fusion_types.h:135
int32 isf_dsa_status_t
This is the Sensor Manager API return type definition.
The isf_gyrometer_types.h file contains the ISF data type definitions for use with the ISF generic gy...
#define MAG_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:55
Standard fixed type for three axis accelerometers.
isf_timestamp_t accelTimestamp
Definition: fusion_exec.h:54
isf_acceleration_g_float_t accel[3]
#define ACCEL_HAL_Z_INDEX
Definition: fusion_config.h:50
uint32 isf_fifo_status_t
Definition: isf_fifo.h:15
The isf_sensor_types.h contains the enumerated list of sensor types used by ISF.
int32 init_sensor(uint8 nSensorID, isf_SensorHandle_t *pSensorAdapterHandle, LWEVENT_STRUCT *pEventGroup, uint32 nEventFieldIndex)
This function initializes the sensor adapters in the absence of the Sensor Manager.
struct SV_3DOF_B_BASIC thisSV_3DOF_B_BASIC
Definition: fusion_exec.h:82
The isf_util.h file contains the utility method declarations and macros.
#define GYRO_HAL_Y_INDEX
Definition: fusion_config.h:60
#define GYRO_HAL_Z_INDEX
Definition: fusion_config.h:62
enum isf_dsa_result_enums isf_dsa_result_types_t
int32 configure_sensor(isf_SensorHandle_t *pSensorAdapterHandle, isf_SubscriptionSettings_t *pRequiredSettings, isf_fifo_t *pfifo)
uint32 float32_To_AcFixed(float fN, uint8 W, uint8 I, uint8 S, int32 *status)
This function converts an IEEE-754 32-bit floating point number into a fixed point integer format...
int32 start_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
#define ACCEL_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:47
const uint8_t * mqx_task_stack_pointers[]
Definition: MQX1.c:73
isf_orientation_quaternion_float_t zvect
float fYpBuffer[GYRO_OVERSAMPLE_RATIO][3]
isf_fifo_status_t isf_fifo_init(isf_fifo_t *pFifo, void *pData, uint16 sampleSize, uint16 bufferCapacity)
Initializes a new fifo structure with a provided buffer.
Definition: isf_fifo.c:19
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
isf_dsa_result_types_t resultFormat
float fOmega[3]
Definition: fusion_types.h:88
float fLPR[3][3]
Definition: fusion_types.h:58
isf_SensorDataTypes_t
#define MAG_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:53
#define PRESSURE_DATA_READY_EVENT
struct fquaternion fq
Definition: fusion_types.h:111
isf_orientation_rot_float_t rMatrix[3][3]
float fGpFast[3]
const isf_SensorConfig_t * pSensorStaticConfig
int32 convert_sensor_data(isf_SensorHandle_t *pSensorAdapterHandle, isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType, void *nativeSample, void *convertedSample)
long int32
This defines int32 as long.
Definition: isf_types.h:32
struct SV_6DOF_GB_BASIC thisSV_6DOF_GB_BASIC
Definition: fusion_exec.h:92
float fOmega[3]
Definition: fusion_types.h:62
Definition: matrix.h:34
#define GYRO_DATA_READY_EVENT
isf_SensorDataTypes_t resultType
The desired data type of the subscription.
isf_SensorDataTypes_t resultType
Main ISF header file. Contains code common to all ISF components.
LWEVENT_STRUCT * pEventGroup
short int16
This defines int16 as short.
Definition: isf_types.h:23
isf_dsa_AdapterStatus_t adapterStatus
float fBc[3]
isf_dsa_status_t fsl_fusion_virt_3D_orient_ValidateSettings(isf_SensorHandle_t *pSensorHandle, isf_dsa_SensorSettings_t *pSensorSettings)
This is the concrete implementation of the orientation sensor adapter for validating current settings...
void fsl_fusion_virt_3D_orient_PeriodicCallback(void *pSensorHandle)
The orientation sensor adapter's periodic processing function.
uint8 nSettingsToUse
1 = current; 2=given; 3=best possible
Standard float type for three axis accelerometers.
isf_dsa_status_t fsl_fusion_virt_3D_orient_StartData(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter for start Data.
#define GYRO_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:59
uint32 nSamplePeriod
Sample period in microseconds.
isf_timestamp_t magTimestamp
Definition: fusion_exec.h:62
struct SV_3DOF_G_BASIC thisSV_3DOF_G_BASIC
Definition: fusion_exec.h:77
#define ACCEL_DATA_READY_EVENT
Definition: matrix.h:35
#define GYRO_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:57
#define MAGCAL_EVENT_FLAG
unsigned short uint16
This defines uint16 as unsigned short.
Definition: isf_types.h:27
#define GYRO_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:61
isf_gyrometer_dps_float_t angularVelocity[3]
int32 isf_status_t
ISF return status type.
Definition: isf.h:51
void fsl_fusion_task(uint32_t task_init_data)
Standard floating point type for single axis altitude sensor.
void MagCal_Run(fusion_state_t *pState)
Definition: fusion_exec.c:257
This defines the DSA sensor configuration parameter structure configuring the sensor settings by a su...
isf_comm.h defines the common types for the Communications Service Family of the Intelligent Sensing ...
isf_dsa_SensorSettings_t sensorSettings
struct SV_3DOF_Y_BASIC thisSV_3DOF_Y_BASIC
Definition: fusion_exec.h:87
struct GyroSensor thisGyro
Definition: fusion_exec.h:66
#define MAG_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:51
#define ISF_FIFO_NO_MORE_ENTRIES
Definition: isf_fifo.h:33
void * isf_fifo_el_get_insert_pointer(isf_fifo_t *pFifo)
Routine returns the insert pointer for direct access.
Definition: isf_fifo.c:232
isf_dsa_status_t fsl_fusion_virt_3D_orient_Convert(volatile isf_SensorHandle_t *pSensorHandle, isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType, void *pNativeSample, void *pConvertedSample, int32 *numBytes)
This function coverts the raw sample data to the desired output type.
#define MAG_HAL_Z_INDEX
Definition: fusion_config.h:56
int32 shutdown_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
The isf_accelerometer_types.h file contains the ISF data type definitions for use with the ISF generi...
#define ACCEL_HAL_Y_INDEX
Definition: fusion_config.h:48
#define ISF_FIFO_FULL
Definition: isf_fifo.h:32
isf_fifo_status_t isf_fifo_el_traverse(isf_fifo_t *pFifo, void **pSamplePtr)
Routine to traverse a fifo To initiate the traversal set pSamplePtr to NULL. The function will set th...
Definition: isf_fifo.c:262
struct fquaternion fLPq
Definition: fusion_types.h:136
isf_dsa_status_t fsl_fusion_virt_3D_orient_Calibrate(isf_SensorHandle_t *pSensorHandle, void *pCalData)
This is the concrete implementation of the orientation sensor adapter for calibration ...