ISF  2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
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) 2015, 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 <isf.h>
23 #include <isf_types.h>
24 #include <isf_dsa_adapter.h>
25 #include <isf_dsa_direct.h>
26 #include <isf_bm.h>
27 #include <isf_sensor_types.h>
28 #include <isf_fifo.h>
30 #include <isf_gyrometer_types.h>
31 #include <isf_magnetometer_types.h>
32 #include <isf_altitude_types.h>
33 #include <isf_temperature_types.h>
34 #include <isf_orientation_types.h>
35 #include <isf_comm.h>
36 #include <isf_util.h>
37 #include <isf_sensors.h>
38 #include "fsl_os_abstraction.h"
40 #include "fusion_config.h"
41 #include "fusion_types.h"
42 #include "fusion_exec.h"
43 //#include "task_template_list.h"
44 #include "Ac_Fixed_utils.h"
45 #include "drivers.h"
46 /* Sensor data ready event masks. */
47 
48 
49 
50 #ifdef USE_ACCELEROMETER
51 # define ACCEL_DATA_READY_EVENT ((uint32)(1 << 0))
52 #else
53 # define ACCEL_DATA_READY_EVENT (0L)
54 #endif
55 
56 #ifdef USE_MAGNETOMETER
57 # define MAG_DATA_READY_EVENT ((uint32)(1 << 1))
58 #else
59 # define MAG_DATA_READY_EVENT (0L)
60 #endif
61 
62 #ifdef USE_GYROMETER
63 # define GYRO_DATA_READY_EVENT ((uint32)(1 << 2))
64 #else
65 # define GYRO_DATA_READY_EVENT (0L)
66 #endif
67 
68 #ifdef USE_PRESSURE_SENSOR
69 # define PRESSURE_DATA_READY_EVENT ((uint32)(1 << 3))
70 #else
71 # define PRESSURE_DATA_READY_EVENT (0L)
72 #endif
73 
74 #define MAGCAL_EVENT_FLAG (1L)
75 
76 #define PRESSURE_DATA_MASK ~PRESSURE_DATA_READY_EVENT
77 
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 OSA_TASK_DEFINE(fsl_fusion, 2000);
83 
84 OSA_TASK_DEFINE(fsl_MagCal, 3000);
85 
86 void fsl_fusion_task(uint32_t task_init_data);
87 void fsl_MagCal_task(uint32_t task_init_data);
88 
89 extern const uint8_t *mqx_task_stack_pointers[];
90 
91 /*! @brief Supported sensor and data types for the orientation sensor */
93  {
101  };
103  {
113  };
114 
115 // Converter routines for the supported data types
116 static isf_dsa_status_t quaternion_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
117 static isf_dsa_status_t euler_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
118 static isf_dsa_status_t dircosine_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
119 #ifdef USE_ACCELEROMETER
120 static isf_dsa_status_t accel_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
121 #endif
122 #ifdef USE_MAGNETOMETER
123 static isf_dsa_status_t mag_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
124 #endif
125 #ifdef USE_GYROMETER
126 static isf_dsa_status_t gyro_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
127 #endif
128 #ifdef USE_PRESSURE_SENSOR
129 static isf_dsa_status_t altitude_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
130 static isf_dsa_status_t temperature_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample );
131 #endif
132 
133 static void set_current_sample_buffer(
134  fsl_orientation_DataBuffer_t *pCurrentSampleBuffer,
135  fusion_state_t *pState,
136  fsl_fusion_Sensor_Specific_Settings_t *pSpecificSettings
137 );
138 
139 /*! @brief This is the concrete implementation of the orientation sensor adapter initialization.
140  * @details This function allocates memory for the device descriptor.
141  * @param[in] pSensorHandle This is a void pointer to the instance of the orientation sensor adapter.
142  *
143  * @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.
144  * @retval ::ISF_SUCCESS is returned when the device is initialized properly.
145  * @retval ::DSA_ERR_PARAM is returned when a wrong parameter was passed into the function, i.e. an invalid or NULL parameter.
146  * @retval ::DSA_ERR_INITIALIZE is returned when the driver could not be initialized successfully.
147 
148  *
149  * @Constraints None
150  *
151  * @Reentrant Yes
152  * @Libs fsl_fusion_virt_3D_orient.lib
153  */
155 {
156  int32 retStatus = 0;
157 
159 
160 
161  // Check the input argument
162  if (NULL == pSensorHandle) {
163  return DSA_ERR_PARAM;
164  }
165 
166  // Check if the sensor is available, and that it has already been initialized.
167  if (pSensorHandle->adapterStatus > DSA_STATE_NOT_INITIALIZED) {
168  return DSA_ERR_INITIALIZE;
169  }
170 
171  // Allocate memory for the device descriptor
172  //pSensorHandle->pDeviceDescriptor = _lwmem_alloc(sizeof(fsl_fusion_DeviceDescriptor_t));
173  pSensorHandle->pDeviceDescriptor = OSA_MemAllocZero(sizeof(fsl_fusion_DeviceDescriptor_t));
174 
175  // Hold the descriptor pointers in local variables for upcoming operations
177 
178  if (NULL == pDeviceDescriptor) {
179  return DSA_ERR_INITIALIZE;
180  }
181 
182  // Create a semaphore to synchronize the device descriptor across tasks
183  //if (MQX_OK != _lwsem_create(&(pDeviceDescriptor->deviceSemaphore), 1)){
184  if (kStatus_OSA_Success != OSA_SemaCreate(&(pDeviceDescriptor->deviceSemaphore), 1)){
185  return DSA_ERR_INITIALIZE;
186  }
187 
188 
189  // Create an event to control the MagCal Task
190  //if (MQX_OK != _lwevent_create(&(pDeviceDescriptor->MagCalEventStruct), LWEVENT_AUTO_CLEAR)){
191  if (kStatus_OSA_Success != OSA_EventCreate(&(pDeviceDescriptor->MagCalEventStruct), kEventAutoClear)){
192  return DSA_ERR_INITIALIZE;
193  }
194 
195  // Create an event to control the fusion Task
196  //if (MQX_OK != _lwevent_create(&(pDeviceDescriptor->sensorLWEvent), LWEVENT_AUTO_CLEAR)){
197  if (kStatus_OSA_Success != OSA_EventCreate(&(pDeviceDescriptor->sensorLWEvent), kEventAutoClear)){
198  return DSA_ERR_INITIALIZE;
199  }
200 
201 #ifdef USE_ACCELEROMETER
202  {
203  retStatus |= init_sensor(pSensorSpecificConfig->accelerometerSensorId, &(pDeviceDescriptor->accelHandle), &(pDeviceDescriptor->sensorLWEvent), ACCEL_DATA_READY_EVENT);
204  }
205 #endif
206 
207 #ifdef USE_MAGNETOMETER
208  retStatus |= init_sensor(pSensorSpecificConfig->magnetometerSensorId, &(pDeviceDescriptor->magHandle), &(pDeviceDescriptor->sensorLWEvent), MAG_DATA_READY_EVENT);
209 #endif
210 
211 #ifdef USE_GYROMETER
212  retStatus |= init_sensor(pSensorSpecificConfig->gyroSensorId, &(pDeviceDescriptor->gyroHandle), &(pDeviceDescriptor->sensorLWEvent), GYRO_DATA_READY_EVENT);
213 #endif
214 
215 #ifdef USE_PRESSURE_SENSOR
216  retStatus |= init_sensor(pSensorSpecificConfig->pressureSensorId, &(pDeviceDescriptor->pressureHandle), &(pDeviceDescriptor->sensorLWEvent), PRESSURE_DATA_READY_EVENT);
217 #endif
218 
219 #ifdef USE_ACCELEROMETER
220  {
222  (
223  &(pDeviceDescriptor->accelFifo),
224  NULL,
226  pSensorSpecificConfig->algorithmConfig.accelOversampleRatio
227  );
228  }
229 #endif
230 
231 #ifdef USE_MAGNETOMETER
232  {
233  uint16 sampleSize;
234  sampleSize = sizeof(isf_MagneticFieldStrength3D_RawCount_t);
235 
237  (
238  &(pDeviceDescriptor->magFifo),
239  NULL,
240  sampleSize,
241  pSensorSpecificConfig->algorithmConfig.magOversampleRatio
242  );
243  }
244 #endif
245 
246 #ifdef USE_GYROMETER
248  (
249  &(pDeviceDescriptor->gyroFifo),
250  NULL,
252  pSensorSpecificConfig->algorithmConfig.gyroOversampleRatio
253  );
254 #endif
255 
256 #ifdef USE_PRESSURE_SENSOR
258  (
259  &(pDeviceDescriptor->pressureFifo),
260  NULL,
261  pDeviceDescriptor->pressureHandle.pSensorStaticConfig->pAdapter->devInfo.nNativeDataSetSize,
262  pSensorSpecificConfig->algorithmConfig.pressureOversampleRatio
263  );
264 #endif
265 
266  pDeviceDescriptor->fusionState.algorithmToUse = pSensorSpecificConfig->algorithmToUse;
267 
268 #if defined(USE_ACCELEROMETER) && defined(USE_MAGNETOMETER)
269  if ( pSensorSpecificConfig->magnetometerSensorId != pSensorSpecificConfig->accelerometerSensorId )
270  {
271  pDeviceDescriptor->sensorDataReady = SENSOR_DATA_READY_EVENT;
272  }
273  else
274  {
275  pDeviceDescriptor->sensorDataReady = HYBRID_SENSOR_DATA_READY_EVENT;
276  }
277 #else
278  pDeviceDescriptor->sensorDataReady = SENSOR_DATA_READY_EVENT;
279 #endif
280 
281  // Create the Fusion Task
282  OSA_TaskCreate(fsl_fusion_task, /* The task function entry */
283  (uint8_t *)"Fusion_task", /* The name of this task */
284  2000, /* The stack size in byte */
285  fsl_fusion_stack, /* Pointer to the stack */
286  9, /* Initial priority of the task */
287  (task_param_t)(pSensorHandle), /* Pointer to be passed to the task when it is created */
288  true, /* This task will use not float register */
289  &fsl_fusion_task_handler); /* Pointer to the task handler */
290 
291  // Create the MagCal Task
292  OSA_TaskCreate(fsl_MagCal_task, /* The task function entry */
293  (uint8_t *)"MagCal_task", /* The name of this task */
294  3000, /* The stack size in byte */
295  fsl_MagCal_stack, /* Pointer to the stack */
296  10, /* Initial priority of the task */
297  (task_param_t)(pSensorHandle), /* Pointer to be passed to the task when it is created */
298  true, /* This task will use not float register */
299  &fsl_MagCal_task_handler); /* Pointer to the task handler */
300 
301  // Set the adapter state to be initialized.
302  pSensorHandle->adapterStatus = DSA_STATE_INITIALIZED;
303 
304  return retStatus;
305 }
306 
307 /*! @brief This is the concrete implementation of the orientation sensor adapter for validating current settings.
308  * @details This function is responsible for validating the current settings and if the current settings are not valid settings,
309  * it provides the best suitable settings.
310  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
311  * @param[in] pSettings The reference to the sensor settings.
312  *
313  * @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.
314  * @retval ::ISF_SUCCESS is returned when the device settings are successfully validated.
315  * @retval ::SM_DSA_RET_SETTINGS_CHANGED is returned when the current settings have been changed.
316  *
317  * @Constraints None
318  *
319  * @Reentrant Yes
320  * @Libs fsl_fusion_virt_3D_orient.lib
321  */
323 {
324  int32 samplePeriod;
325 
326  isf_dsa_SensorSettings_t settings;
327 
328  // Check the input arguments.
329  if ((NULL == pSensorHandle) || (NULL == pSensorSettings)) {
330  return DSA_ERR_PARAM;
331  }
332 
333  // Set the default return status
334  isf_dsa_status_t retStat = ISF_SUCCESS;
335 
336  // Get the device descriptor and sensor specific settings
339 
340  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.gyroOversampleRatio); //!< Sample period in microseconds
341  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
342  settings.resultType = TYPE_ROTATIONAL_RATE_3D; //!< The desired data type of the subscription
344 
345 #if defined(USE_ACCELEROMETER) && defined(USE_MAGNETOMETER)
346  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
347  {
348  if (pSpecificSettings->algorithmConfig.magOversampleRatio != pSpecificSettings->algorithmConfig.accelOversampleRatio )
349  {
350  return DSA_ERR_VALIDATE_SETTINGS; //In Hybrid Mode, the ODR is common to both sensors but the OSR settings for each sensor are independent and can be different.
351  } //Refer Section 4.4 & 7.5 of FXOS8700 Data Sheet. Need to check whether this validation is required or not here?
352  }
353 #endif
354 
355 #ifdef USE_GYROMETER
356  retStat = pDeviceDescriptor->gyroHandle.pSensorStaticConfig->pAdapter->control.ValidateSettings(&(pDeviceDescriptor->gyroHandle), &settings);
357  if ( retStat != ISF_SUCCESS )
358  {
359  pSensorSettings->nSamplePeriod = settings.nSamplePeriod * pSpecificSettings->algorithmConfig.gyroOversampleRatio;
360  }
361 #endif
362 
363  if (( pSensorSettings->resultType != TYPE_QUATERNION ) &&
364  ( pSensorSettings->resultType != TYPE_EULER_3D ) &&
365  ( pSensorSettings->resultType != TYPE_DIR_COSINE_MATRIX) &&
366  ( pSensorSettings->resultType != TYPE_RAW_ACCELERATION_3D) &&
367  ( pSensorSettings->resultType != TYPE_MAGNETIC_FIELD_STRENGTH_3D) &&
368  ( pSensorSettings->resultType != TYPE_ROTATIONAL_RATE_3D) &&
369  ( pSensorSettings->resultType != TYPE_ALTITUDE) &&
370  ( pSensorSettings->resultType != TYPE_TEMPERATURE) &&
371  ( pSensorSettings->resultType != TYPE_NATIVE_SENSOR_DATA_TYPE)
372  )
373  {
374  pSensorSettings->resultType = TYPE_NATIVE_SENSOR_DATA_TYPE;
376  }
377 
378  return retStat;
379 }
380 /*! @brief This is the concrete implementation of the fusion sensor adapter configuration function.
381  * @details This function resets the fusion state and calls the underlying AGM adapter's Configure functions.
382  * @param[in] pSensorHdl This is a void pointer to the instance of the fusion sensor adapter.
383  * @param[in] pConfig The reference to the sensor configuration settings.
384  *
385  * @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.
386  * @retval ::ISF_SUCCESS is returned when the device is configured successfully.
387  * @retval ::DSA_ERR_CONFIGURE is returned when the provided configuration settings could not be applied.
388  *
389  * @Constraints None
390  *
391  * @Reentrant Yes
392  * @Libs fsl_fusion_virt_3D_orient.lib
393  */
395 {
396  int32 samplePeriod;
397 
400 
401  // Check the input arguments
402  if ((NULL == pSensorHandle) || (NULL == pSensorSettings)) {
403  return DSA_ERR_PARAM;
404  }
405 
406  isf_dsa_status_t retStatus = ISF_SUCCESS;
407 
408  // Get the device descriptor and sensor specific settings
410 
411  // Check the device descriptor pointer.
412  if ((NULL == pDeviceDescriptor) || (NULL == pSpecificSettings))
413  {
414  return retStatus;
415  }
416 
417  // Lock the device descriptor.
418  //_lwsem_wait_ticks(&(pDeviceDescriptor->deviceSemaphore), 0);
419  OSA_SemaWait(&(pDeviceDescriptor->deviceSemaphore), OSA_WAIT_FOREVER);
420 
421  // Check the driver state.
422  if (DSA_STATE_INITIALIZED > pSensorHandle->adapterStatus){
423  goto unlockdescriptor;
424  }
425 
426 #ifdef USE_ACCELEROMETER
427  {
428  int32 ast = 0;
429 
430  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.accelOversampleRatio); //!< Sample period in microseconds
431  settings.resultType = TYPE_RAW_ACCELERATION_3D; //!< The desired data type of the subscription
432  settings.resultFormat = DSA_RESULT_TYPE_RAW_COUNTS; //!< The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT
433  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
434  settings.nSettingsToUse = 3; //!< 1 = current; 2=given; 3=best possible
435  settings.nFifoDepth = pSpecificSettings->algorithmConfig.accelOversampleRatio;
436 
437  ast = configure_sensor(
438  &(pDeviceDescriptor->accelHandle),
439  &settings,
440  &(pDeviceDescriptor->accelFifo));
441 
442  if (ast != ISF_SUCCESS)
443  {
444  ast = configure_sensor(
445  &(pDeviceDescriptor->accelHandle),
446  &settings,
447  &(pDeviceDescriptor->accelFifo));
448  }
449  }
450 #endif
451 
452 #ifdef USE_MAGNETOMETER
453  {
454  int32 mst = 0;
455  isf_SensorDataTypes_t resultType;
456 
457  {
458  resultType = TYPE_MAGNETIC_FIELD_STRENGTH_3D;
459 
460  }
461 
462  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.magOversampleRatio); //!< Sample period in microseconds
463  settings.resultType = resultType; //!< The desired data type of the subscription
464  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
465  settings.nFifoDepth = pSpecificSettings->algorithmConfig.magOversampleRatio;
466 
467  mst = configure_sensor(
468  &pDeviceDescriptor->magHandle,
469  &settings,
470  &pDeviceDescriptor->magFifo
471  );
472  if (mst != ISF_SUCCESS)
473  {
474  mst = configure_sensor(
475  &(pDeviceDescriptor->magHandle),
476  &settings,
477  &(pDeviceDescriptor->magFifo));
478  }
479  }
480 #endif
481 
482 #ifdef USE_GYROMETER
483  {
484  int32 gst = 0;
485 
486  samplePeriod = (pSensorSettings->nSamplePeriod) / (pSpecificSettings->algorithmConfig.gyroOversampleRatio); //!< Sample period in microseconds
487  settings.resultType = TYPE_ROTATIONAL_RATE_3D; //!< The desired data type of the subscription
488  settings.resultFormat = DSA_RESULT_TYPE_RAW_COUNTS; //!< The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT
489  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
490  settings.nFifoDepth = pSpecificSettings->algorithmConfig.gyroOversampleRatio;
491 
492  gst = configure_sensor(
493  &(pDeviceDescriptor->gyroHandle),
494  &settings,
495  &(pDeviceDescriptor->gyroFifo)
496  );
497  if (gst != ISF_SUCCESS)
498  {
499  retStatus = gst;
500  pSensorSettings->nSamplePeriod = settings.nSamplePeriod * pSpecificSettings->algorithmConfig.gyroOversampleRatio;
501  goto unlockdescriptor;
502  }
503  }
504 #endif
505 
506 #ifdef USE_PRESSURE_SENSOR
507  {
508  int32 pst = 0;
509 
510  samplePeriod = (pSensorSettings->nSamplePeriod); //!< Sample period in microseconds
511  settings.resultType = TYPE_NATIVE_SENSOR_DATA_TYPE; //!< The desired data type of the subscription
512  settings.resultFormat = DSA_RESULT_TYPE_RAW_COUNTS; //!< The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT
513  settings.nSamplePeriod = samplePeriod; //!< Sample period in microseconds
514  settings.nFifoDepth = pSpecificSettings->algorithmConfig.pressureOversampleRatio;
515 
516  pst = configure_sensor(
517  &pDeviceDescriptor->pressureHandle,
518  &settings,
519  &pDeviceDescriptor->pressureFifo
520  );
521  if (pst != ISF_SUCCESS)
522  {
523  pst = configure_sensor(
524  &(pDeviceDescriptor->pressureHandle),
525  &settings,
526  &(pDeviceDescriptor->pressureFifo));
527  }
528  }
529 #endif
530 
532  retStatus = ISF_SUCCESS;
533 
534  unlockdescriptor:
535 
536  // Unlock the device descriptor.
537  //_lwsem_post(&(pDeviceDescriptor->deviceSemaphore));
538  OSA_SemaPost(&(pDeviceDescriptor->deviceSemaphore));
539 
540  return retStatus;
541 }
542 /*! @brief This is the concrete implementation of the orientation sensor adapter for start Data.
543  * @details This function tells the Bus Manager to start the callbacks periodically as defined in the bus configuration and enable the data collection.
544  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
545  *
546  * @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.
547  * @retval ::ISF_SUCCESS is returned when the data collection callback triggered successfully.
548  * @retval ::DSA_ERR_PARAM is returned when a wrong parameter is passed into the function such as an invalid or NULL parameter.
549  *
550  * @Constraints None
551  *
552  * @Reentrant Yes
553  * @Libs fsl_fusion_virt_3D_orient.lib
554  */
556 {
557  // Check the input argument
558  if (NULL == pSensorHandle) {
559  return DSA_ERR_PARAM;
560  }
561 
562  int32_t retStat = DSA_ERR_START_DATA;
563 
564  // Get the device descriptor and specific settings
565  fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = pSensorHandle->pDeviceDescriptor;
566 
567  // Check the device descriptor.
568  if (NULL == pDeviceDescriptor)
569  return DSA_ERR_PARAM;
570 
572 
573  // Lock the device descriptor.
574  //_lwsem_wait_ticks(&(pDeviceDescriptor->deviceSemaphore), 0);
575  OSA_SemaWait(&(pDeviceDescriptor->deviceSemaphore), OSA_WAIT_FOREVER);
576 
577  // Check the driver state.
578  if (DSA_STATE_CONFIGURED_STOPPED > pSensorHandle->adapterStatus)
579  {
580  goto unlockdescriptor;
581  }
582 
583 #ifdef USE_MAGNETOMETER
584  {
585  int32 mst;
586 
587  mst = start_sensor(&(pDeviceDescriptor->magHandle));
588 
589  if (ISF_SUCCESS != mst) goto unlockdescriptor;
590  }
591 
592 #endif
593 
594 #ifdef USE_PRESSURE_SENSOR
595  {
596  int32 pst;
597 
598  pst = start_sensor(&(pDeviceDescriptor->pressureHandle));
599 
600  if (ISF_SUCCESS != pst) goto unlockdescriptor;
601  }
602 #endif
603 
604 #ifdef USE_ACCELEROMETER
605  {
606  int32 ast;
607  {
608  ast = start_sensor(&(pDeviceDescriptor->accelHandle));
609 
610  if (ISF_SUCCESS != ast) goto unlockdescriptor;
611  }
612  }
613 #endif
614 
615 #ifdef USE_GYROMETER
616  {
617  int32 gst;
618 
619  gst = start_sensor(&(pDeviceDescriptor->gyroHandle));
620 
621  if (ISF_SUCCESS != gst) goto unlockdescriptor;
622  }
623 #endif
624 
625  // Set device to active state.
627  retStat = ISF_SUCCESS;
628 
629 unlockdescriptor:
630  // Unlock the device descriptor.
631  //_lwsem_post(&(pDeviceDescriptor->deviceSemaphore));
632  OSA_SemaPost(&(pDeviceDescriptor->deviceSemaphore));
633 
634  return retStat;
635 }
636 /*! @brief This is the concrete implementation of the orientation sensor adapter for End Data.
637  * @details This function tells the Bus Manager to stop the callbacks and disable data collection.
638  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
639  *
640  * @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.
641  * @retval ::ISF_SUCCESS is returned when the data collection stopped successfully.
642  * @retval ::DSA_ERR_PARAM is returned when a wrong parameter is passed into the function such as an invalid or NULL parameter.
643  *
644  * @Constraints None
645  *
646  * @Reentrant Yes
647  * @Libs fsl_fusion_virt_3D_orient.lib
648  */
650 {
651  // Check the input argument
652  if (NULL == pSensorHandle) {
653  return DSA_ERR_PARAM;
654  }
655 
656  int32_t retStat = DSA_ERR_END_DATA;
657 
658  // Get the device descriptor and specific settings
659  fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = pSensorHandle->pDeviceDescriptor;
660 
661  // Check the device descriptor.
662  if (NULL == pDeviceDescriptor)
663  return DSA_ERR_PARAM;
664 
666 
667  // Lock the device descriptor.
668  //_lwsem_wait_ticks(&(pDeviceDescriptor->deviceSemaphore), 0);
669  OSA_SemaWait(&(pDeviceDescriptor->deviceSemaphore), OSA_WAIT_FOREVER);
670 
671  // Check the driver state.
672  if (DSA_STATE_CONFIGURED_STOPPED > pSensorHandle->adapterStatus)
673  {
674  goto unlockdescriptor;
675  }
676 
677 #ifdef USE_MAGNETOMETER
678  {
679  int32 mst;
680 
681  mst = stop_sensor(&(pDeviceDescriptor->magHandle));
682 
683  if (ISF_SUCCESS != mst) goto unlockdescriptor;
684  }
685 #endif
686 
687 #ifdef USE_PRESSURE_SENSOR
688  {
689  int32 pst;
690 
691  pst = stop_sensor(&(pDeviceDescriptor->pressureHandle));
692 
693  if (ISF_SUCCESS != pst) goto unlockdescriptor;
694  }
695 #endif
696 
697 #ifdef USE_ACCELEROMETER
698  {
699  int32 ast;
700 
701  ast = stop_sensor(&(pDeviceDescriptor->accelHandle));
702 
703  if (ISF_SUCCESS != ast) goto unlockdescriptor;
704  }
705 #endif
706 
707 #ifdef USE_GYROMETER
708  {
709  int32 gst;
710 
711  gst = stop_sensor(&(pDeviceDescriptor->gyroHandle));
712 
713  if (ISF_SUCCESS != gst) goto unlockdescriptor;
714  }
715 #endif
716 
718  retStat = ISF_SUCCESS;
719 
720 
721 unlockdescriptor:
722  // Unlock the device descriptor.
723  //_lwsem_post(&(pDeviceDescriptor->deviceSemaphore));
724  OSA_SemaPost(&(pDeviceDescriptor->deviceSemaphore));
725 
726  return retStat;
727 }
728 /*! @brief This is the concrete implementation of the orientation sensor adapter for calibration .
729  * @details The FXAS21000 does not provide the capability to be dynamically calibrated. Therefore, this function simply returns success.
730  *
731  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
732  *
733  @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.
734  * @retval ::ISF_SUCCESS is returned when the device is calibrated properly.
735  * \b Note: Currently, there is no return error type defined for this API.
736  *
737  * @Constraints None
738  *
739  * @Reentrant Yes
740  * @Libs fsl_fusion_virt_3D_orient.lib
741  */
743 {
744  return ISF_SUCCESS;
745 }
746 /*! @brief This is the concrete implementation of the orientation sensor adapter for shutdown .
747  *
748  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
749  *
750  @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.
751  * @retval ::ISF_SUCCESS is returned when the device is calibrated properly.
752  * \b Note: Currently, there is no return error type defined for this API.
753  *
754  * @Constraints None
755  *
756  * @Reentrant Yes
757  * @Libs fsl_fusion_virt_3D_orient.lib
758  */
760 {
761  if(NULL == pSensorHandle){
762  return DSA_ERR_PARAM;
763  }
764 
765  // Create helper reference pointers.
768 
769 #ifdef USE_MAGNETOMETER
770  shutdown_sensor(&(pDeviceDescriptor->magHandle));
771 #endif
772 
773 #ifdef USE_PRESSURE_SENSOR
774  shutdown_sensor(&(pDeviceDescriptor->pressureHandle));
775 #endif
776 
777 #ifdef USE_ACCELEROMETER
778  shutdown_sensor(&(pDeviceDescriptor->accelHandle));
779 #endif
780 
781 #ifdef USE_GYROMETER
782  shutdown_sensor(&(pDeviceDescriptor->gyroHandle));
783 #endif
784 
785  pSensorHandle->adapterStatus = DSA_STATE_INITIALIZED;
786  return ISF_SUCCESS;
787 }
788 /*! @brief The orientation sensor adapter's periodic processing function.
789  * @details This function is invoked periodically whenever new sensor data is available from its underlying sensors.
790  *
791  * @param[in] pSensorHdl This is a void pointer to the instance of the orientation sensor adapter.
792  *
793  * @return Void There is no return value.
794  *
795  * @Constraints None
796  *
797  * @Reentrant Yes
798  * @Libs fsl_fusion_virt_3D_orient.lib
799  */
800 
802 {
803 
804 
805  // Get the device descriptor and specific settings
806  isf_SensorHandle_t *pSensorHdl = (isf_SensorHandle_t *)pSensorHandle;
810  fusion_state_t *pState = &(pDesc->fusionState);
811 
812  isf_status_t st = 0;
813  int8 i, j, k, l; // counters
814  int32 iSum[3]; // array of sums
815  int32 itmp; // scratch
816  if (pState->softReset)
817  {
818  Fusion_Init(pState);
819  pState->softReset = 0;
820  }
821 
822  if (pState->softRINS)
823  {
824  int i;
825 #ifdef COMPUTE_9DOF_GBY_KALMAN
826  for (i = 0; i <= 2; i++)
827  {
828  pState->thisSV_9DOF_GBY_KALMAN.fVelGl[i] = 0.0F;
829  pState->thisSV_9DOF_GBY_KALMAN.fDisGl[i] = 0.0F;
830  }
831  pState->softRINS = 0;
832 #endif
833  }
834 
835 #ifdef USE_ACCELEROMETER
836  if (pSensorHdl->signalledEvents & ACCEL_DATA_READY_EVENT)
837  {
838  isf_fifo_t* pAccelFifo;
839 
840  pAccelFifo = &(pDesc->accelFifo);
841 
842  int32 sumX = 0;
843  int32 sumY = 0;
844  int32 sumZ = 0;
845 
846  isf_Acceleration3D_RawCount_t *pSampleIter = 0;
849  int8 sampleCount = 0;
851 
852  isf_fifo_lock(pAccelFifo);
853 
854  do
855  {
856  st = isf_fifo_el_traverse ( pAccelFifo, (void **)&pSampleIter );
857  pSample = pSampleIter;
858  // check for -32768 since this value cannot be negated in a later HAL operation
859 
860  for (int8 index = X; index <= Z; index++)
861  {
862  // check for -32768 since this value cannot be negated in a later HAL operation
863  if (pSample->accel[index] == -32768) pSample->accel[index]++;
864  }
865  pState->thisAccel.iGs[X] = ACCEL_GET_HAL_X_VALUE(pSample->accel[ACCEL_HAL_X_INDEX]);
866  pState->thisAccel.iGs[Y] = ACCEL_GET_HAL_Y_VALUE(pSample->accel[ACCEL_HAL_Y_INDEX]);
867  pState->thisAccel.iGs[Z] = ACCEL_GET_HAL_Z_VALUE(pSample->accel[ACCEL_HAL_Z_INDEX]);
868 
869  sumX += pState->thisAccel.iGs[X];
870  sumY += pState->thisAccel.iGs[Y];
871  sumZ += pState->thisAccel.iGs[Z];
872  ++sampleCount;
873 
874  } while(st != ISF_FIFO_NO_MORE_ENTRIES);
875 
876  pState->accelTimestamp = pSample->timestamp;
877 
878  isf_fifo_el_clear(pAccelFifo);
879  isf_fifo_unlock(pAccelFifo);
880 
881  pState->thisAccel.iGsAvg[X] = sumX / sampleCount;
882  pState->thisAccel.iGsAvg[Y] = sumY / sampleCount;
883  pState->thisAccel.iGsAvg[Z] = sumZ / sampleCount;
884 
885  pState->thisAccel.fGsAvg[X] = (float)pState->thisAccel.iGsAvg[X] * pState->thisAccel.fgPerCount;
886  pState->thisAccel.fGsAvg[Y] = (float)pState->thisAccel.iGsAvg[Y] * pState->thisAccel.fgPerCount;
887  pState->thisAccel.fGsAvg[Z] = (float)pState->thisAccel.iGsAvg[Z] * pState->thisAccel.fgPerCount;
888 
890  }
891 #endif
892 #ifdef USE_MAGNETOMETER
893  if (pSensorHdl->signalledEvents & MAG_DATA_READY_EVENT)
894  {
895  int32 sumX = 0.0F;
896  int32 sumY = 0.0F;
897  int32 sumZ = 0.0F;
898 
902  int8 sampleCount = 0;
904 
905  isf_fifo_lock(&(pDesc->magFifo));
906 
907  do
908  {
909  st = isf_fifo_el_traverse ( &(pDesc->magFifo), (void **)&pSampleIter );
910 
911  pSample = pSampleIter;
912 
913  for (int8 index = X; index <= Z; index++)
914  {
915  // check for -32768 since this value cannot be negated in a later HAL operation
916  if (pSample->fieldStrength[index] == -32768) pSample->fieldStrength[index]++;
917  }
918  pState->thisMag.iBs[X] = MAG_GET_HAL_X_VALUE(pSample->fieldStrength[MAG_HAL_X_INDEX]);
919  pState->thisMag.iBs[Y] = MAG_GET_HAL_Y_VALUE(pSample->fieldStrength[MAG_HAL_Y_INDEX]);
920  pState->thisMag.iBs[Z] = MAG_GET_HAL_Z_VALUE(pSample->fieldStrength[MAG_HAL_Z_INDEX]);
921 
922  pState->thisMag.iBsBuffer[sampleCount][X] = pState->thisMag.iBs[X];
923  pState->thisMag.iBsBuffer[sampleCount][Y] = pState->thisMag.iBs[Y];
924  pState->thisMag.iBsBuffer[sampleCount][Z] = pState->thisMag.iBs[Z];
925 
926  //sumX += pState->thisMag.iBs[X];
927  //sumY += pState->thisMag.iBs[Y];
928  //sumZ += pState->thisMag.iBs[Z];
929  ++sampleCount;
930  } while(st != ISF_FIFO_NO_MORE_ENTRIES);
931 
932  pState->magTimestamp = pSample->timestamp;
933  isf_fifo_el_clear(&(pDesc->magFifo));
934  isf_fifo_unlock(&(pDesc->magFifo));
935 
936  //pState->thisMag.iBsAvg[X] = sumX / sampleCount;
937  //pState->thisMag.iBsAvg[Y] = sumY / sampleCount;
938  //pState->thisMag.iBsAvg[Z] = sumZ / sampleCount;
939 
940  if (!pState->thisMagCal.iCalInProgress)
941  {
942  // update the magnetometer measurement buffer integer magnetometer data (typically at 25Hz)
943  iUpdateMagnetometerBuffer(&pState->thisMagBuffer, &pState->thisMag, pState->loopcounter);
944  }
945  // every OVERSAMPLE_RATIO passes calculate the block averaged and calibrated measurement using an anti-glitch filter
946  // that rejects the measurement furthest from the mean. magnetometer sensors are sensitive
947  // to occasional current pulses from power supply traces and so on and this is a simple method to remove these.
948  // calculate the channel means using all measurements
949  for (i = X; i <= Z; i++)
950  {
951  // accumulate channel sums
952  iSum[i] = 0;
953  for (j = 0; j < OVERSAMPLE_RATIO; j++)
954  iSum[i] += (int32)pState->thisMag.iBsBuffer[j][i];
955  }
956  for (i = X; i <= Z; i++)
957  {
958  for (j = 0; j < OVERSAMPLE_RATIO; j++)
959  {
960  if (abs((int32)pState->thisMag.iBsBuffer[j][i] * OVERSAMPLE_RATIO - iSum[i]) >= itmp)
961  {
962  k = i;
963  l = j;
964  itmp = abs((int32)pState->thisMag.iBsBuffer[j][i] * OVERSAMPLE_RATIO - iSum[i]);
965  }
966  }
967  }
968 
969  // re-calculate the block averaged measurement ignoring channel k in measurement l
970  if (OVERSAMPLE_RATIO == 1)
971  {
972  // use the one available measurement for averaging in this case
973  for (i = X; i <= Z; i++)
974  {
975  pState->thisMag.iBsAvg[i] = pState->thisMag.iBsBuffer[0][i];
976  }
977  } // end of compute averages for OVERSAMPLE_RATIO = 1
978  else
979  {
980  // sum all measurements ignoring channel k in measurement l
981  for (i = X; i <= Z; i++)
982  {
983  iSum[i] = 0;
984  for (j = 0; j < OVERSAMPLE_RATIO; j++)
985  {
986  if (!((i == k) && (j == l)))
987  iSum[i] += (int32)pState->thisMag.iBsBuffer[j][i];
988  }
989  }
990  // compute the average with nearest integer rounding
991  for (i = X; i <= Z; i++)
992  {
993  if (i != k)
994  {
995  // OVERSAMPLE_RATIO measurements were used
996  if (iSum[i] >= 0)
997  pState->thisMag.iBsAvg[i] = (int16)((iSum[i] + (OVERSAMPLE_RATIO >> 1)) / OVERSAMPLE_RATIO);
998  else
999  pState->thisMag.iBsAvg[i] = (int16)((iSum[i] - (OVERSAMPLE_RATIO >> 1)) / OVERSAMPLE_RATIO);
1000  }
1001  else
1002  {
1003  // OVERSAMPLE_RATIO - 1 measurements were used
1004  if (iSum[i] >= 0)
1005  pState->thisMag.iBsAvg[i] = (int16)((iSum[i] + ((OVERSAMPLE_RATIO - 1) >> 1)) / (OVERSAMPLE_RATIO - 1));
1006  else
1007  pState->thisMag.iBsAvg[i] = (int16)((iSum[i] - ((OVERSAMPLE_RATIO - 1) >> 1)) / (OVERSAMPLE_RATIO - 1));
1008  }
1009  }
1010  } // end of compute averages for OVERSAMPLE_RATIO = 1
1011 
1012 
1013  pState->thisMag.fBsAvg[X] = (float)(pState->thisMag.iBsAvg[X] * pState->thisMag.fuTPerCount);
1014  pState->thisMag.fBsAvg[Y] = (float)(pState->thisMag.iBsAvg[Y] * pState->thisMag.fuTPerCount);
1015  pState->thisMag.fBsAvg[Z] = (float)(pState->thisMag.iBsAvg[Z] * pState->thisMag.fuTPerCount);
1016 
1018  }
1019 #endif
1020 #ifdef USE_PRESSURE_SENSOR
1021  if (pSensorHdl->signalledEvents & PRESSURE_DATA_READY_EVENT)
1022  {
1023 
1024  isf_Meters1D_float_t pressureSample;
1025  isf_DegreesCelsius1D_float_t temperatureSample;
1026  void *pSampleIter = 0;
1027  isf_fifo_status_t st;
1028 
1029  isf_fifo_lock(&(pDesc->pressureFifo));
1030 
1031  do
1032  {
1033  st = isf_fifo_el_traverse ( &(pDesc->pressureFifo), &pSampleIter );
1034  convert_sensor_data(&(pDesc->pressureHandle), TYPE_ALTITUDE, DSA_RESULT_TYPE_ENG_FLOAT, pSampleIter, &pressureSample);
1035  convert_sensor_data(&(pDesc->pressureHandle), TYPE_TEMPERATURE, DSA_RESULT_TYPE_ENG_FLOAT, pSampleIter, &temperatureSample);
1036  pState->thisPressure.fHp = pressureSample.altitude;
1037  pState->thisPressure.fTp = temperatureSample.temperature;
1038  } while (st != ISF_FIFO_NO_MORE_ENTRIES);
1039 
1040  pState->pressureTimestamp = pressureSample.timestamp;
1041 
1042  isf_fifo_el_clear(&(pDesc->pressureFifo));
1043  isf_fifo_unlock(&(pDesc->pressureFifo));
1045  }
1046 #endif
1047 
1048 #ifdef USE_GYROMETER
1049  if (pSensorHdl->signalledEvents & GYRO_DATA_READY_EVENT)
1050  {
1051 
1052  isf_AngularVelocity3D_RawCount_t *pGyroSample = 0;
1053  int8 sampleIdx;
1054  isf_fifo_status_t st;
1055 
1056  isf_fifo_lock(&(pDesc->gyroFifo));
1057  sampleIdx = 0;
1058 
1059  do
1060  {
1061  st = isf_fifo_el_traverse ( &(pDesc->gyroFifo), (void **)&pGyroSample );
1062  for (int8 index = X; index <= Z; index++)
1063  {
1064  // check for -32768 since this value cannot be negated in a later HAL operation
1065  if (pGyroSample->angularVelocity[index] == -32768) pGyroSample->angularVelocity[index]++;
1066  }
1067  pState->thisGyro.iYsBuffer[sampleIdx][X] = GYRO_GET_HAL_X_VALUE(pGyroSample->angularVelocity[GYRO_HAL_X_INDEX]);
1068  pState->thisGyro.iYsBuffer[sampleIdx][Y] = GYRO_GET_HAL_Y_VALUE(pGyroSample->angularVelocity[GYRO_HAL_Y_INDEX]);
1069  pState->thisGyro.iYsBuffer[sampleIdx][Z] = GYRO_GET_HAL_Z_VALUE(pGyroSample->angularVelocity[GYRO_HAL_Z_INDEX]);
1070 
1071  ++sampleIdx;
1072  } while ( st != ISF_FIFO_NO_MORE_ENTRIES);
1073 
1074  pState->gyroTimestamp = pGyroSample->timestamp;
1075 
1076  isf_fifo_el_clear(&(pDesc->gyroFifo));
1077  isf_fifo_unlock(&(pDesc->gyroFifo));
1078 
1080  }
1081 #endif
1082 
1083  {
1084  int8 initiatemagcal;
1085 
1086  if (!(pDesc->sensorDataReady & PRESSURE_DATA_MASK)) // When all sensors are ready (except don't care about pressure), run the fusion
1087  {
1088  initiatemagcal = Fusion_Run(pState, &(pSpecificSettings->algorithmConfig));
1089 
1091 
1092  // Lock the fifo for update.
1093  if(kStatus_OSA_Success != isf_fifo_lock(pFifo)){
1094  return;
1095  }
1096 
1097  void *pFifoEntry = isf_fifo_el_get_insert_pointer(pFifo);
1098 
1099  // write the new data
1101  {
1102  set_current_sample_buffer((fsl_orientation_DataBuffer_t *)pFifoEntry, pState, pSpecificSettings);
1103  }
1104  else
1105  {
1106  fsl_orientation_DataBuffer_t currentSampleBuffer;
1107  int32 numBytes;
1108 
1109  set_current_sample_buffer(&currentSampleBuffer, pState, pSpecificSettings);
1110 
1112  pSensorHdl,
1115  &currentSampleBuffer,
1116  pFifoEntry,
1117  &numBytes);
1118  }
1119 
1120  // Increment the fifo to the next sample entry.
1121  st = isf_fifo_el_increment(pFifo);
1122 
1123  // Unlock the fifo.
1124  isf_fifo_unlock(pFifo);
1125 
1126  if (st == ISF_FIFO_FULL)
1127  {
1128  // Notify the user using their registered event information
1129  //_lwevent_set(pSensorHdl->controlData.pEventGroup,(uint32_t)pSensorHdl->controlData.nEventFieldIndex);
1130  OSA_EventSet(pSensorHdl->controlData.pEventGroup,(uint32_t)pSensorHdl->controlData.nEventFieldIndex);
1131  }
1132 
1133  if (initiatemagcal)
1134  {
1135  //_lwevent_set(&(pDesc->MagCalEventStruct), MAGCAL_EVENT_FLAG);
1136  OSA_EventSet(&(pDesc->MagCalEventStruct), MAGCAL_EVENT_FLAG);
1137  }
1138  }
1139  }
1140 }
1141 
1142 
1143 
1144 void fsl_fusion_task(uint32_t task_init_data)
1145 {
1146  isf_SensorHandle_t *pSensorHandle = (isf_SensorHandle_t *)task_init_data;
1147  uint32_t osaStat;
1148  event_flags_t signalledEvents = 0;
1149 
1150  // Check the input argument
1151  if (NULL == pSensorHandle) {
1152  return;
1153  }
1154 
1157 
1158  // Check the device descriptor
1159  if (NULL == pDesc){
1160  return;
1161  }
1162 // should be generated based on the type of sensor selected
1163 #if SF_MAG_FXOS8700
1164  FXOS8700_Mag_Init(pDesc);
1165 #elif SF_MAG_MAG3110
1166  MAG3100_Init(pDesc);
1167 #endif
1168 #if SF_GYRO_FXAS21002
1169  FXAS21000_Init(pDesc);
1170 #endif
1171 #if SF_ACCEL_FXOS8700
1172  FXOS8700_Accel_Init(pDesc);
1173 #elif SF_ACCEL_MMA8652X
1174  MMA865X_Init(pDesc);
1175 #elif SF_ACCEL_FXLS8952
1176  FXLS8952_Init(pDesc);
1177 #endif
1178  Fusion_Init(&(pDesc->fusionState));
1179 
1180  // Execute the fusion computation loop
1181  // This loop should never exit
1182  for (;;)
1183  {
1184  uint32_t readyFlags;
1185  /* wait for sensor data to arrive.*/
1186 #if 0
1187 //#if defined(USE_ACCELEROMETER) && defined(USE_MAGNETOMETER)
1188  if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
1189  {
1190  readyFlags = HYBRID_SENSOR_DATA_READY_EVENT;
1191  }
1192  else
1193  {
1194  readyFlags = SENSOR_DATA_READY_EVENT;
1195  }
1196 #else
1197  readyFlags = SENSOR_DATA_READY_EVENT;
1198 #endif
1199 
1200  //osaStat = _lwevent_wait_for(&(pDesc->sensorLWEvent), readyFlags, FALSE, NULL);
1201  //osaStat = OSA_EventWait(&(pDesc->sensorLWEvent), readyFlags, FALSE, OSA_WAIT_FOREVER, &signalledEvents);
1202  osaStat = OSA_EventWait(&(pDesc->sensorLWEvent), readyFlags, FALSE, OSA_WAIT_FOREVER, &(pSensorHandle->signalledEvents));
1203 
1204  if ( osaStat == kStatus_OSA_Success) fsl_fusion_virt_3D_orient_PeriodicCallback(pSensorHandle);
1205  }
1206 }
1207 
1208 
1209 /* User includes (#include below this line is not maintained by Processor Expert) */
1210 /*
1211 ** ===================================================================
1212 ** Event : MagCal_task (module mqx_tasks)
1213 **
1214 ** Component : MagCal_task [MQXLite_task]
1215 ** Description :
1216 ** MQX task routine. The routine is generated into mqx_tasks.c
1217 ** file.
1218 ** Parameters :
1219 ** NAME - DESCRIPTION
1220 ** task_init_data -
1221 ** Returns : Nothing
1222 ** ===================================================================
1223 */
1224 // MagCal_task
1225 void fsl_MagCal_task(uint32_t task_init_data)
1226 {
1227  uint32 signalledEvents = 0;
1228  isf_SensorHandle_t *pSensorHandle = (isf_SensorHandle_t *)task_init_data;
1229 
1231  // magnetic DOF: reset magnetic calibration and magnetometer data buffer (not needed for 3DOF)
1232 
1233  while(1)
1234  {
1235 
1236  // wait for the magnetic calibration event
1237  // this event will never be enabled for build options which don't require magnetic calibration
1238  // FALSE means any bit (of the 1 bit enabled by the mask) unblocks
1239  // and NULL means timeout is infinite
1240  //_lwevent_wait_for(&(pDesc->MagCalEventStruct), MAGCAL_EVENT_FLAG, FALSE, NULL);
1241  OSA_EventWait(&(pDesc->MagCalEventStruct), MAGCAL_EVENT_FLAG, FALSE, OSA_WAIT_FOREVER, &signalledEvents);
1242 
1243  // prevent compilation errors when magnetic calibration is not required
1244 #if defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
1245  // and run the magnetic calibration
1246  MagCal_Run(&(pDesc->fusionState));
1247 #endif
1248 
1249  } // end of infinite loop
1250 }
1251 
1252 
1253 /*!
1254  * @brief This function coverts the raw sample data to the desired output type.
1255  */
1257  (
1258  volatile isf_SensorHandle_t *pSensorHandle,
1259  isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType,
1260  void *pNativeSample,
1261  void *pConvertedSample,
1262  int32 *numBytes
1263  )
1264 {
1267 
1268  pConverter = NULL;
1269 
1270  // TODO - Only supports float types for now
1271  if (resultType != DSA_RESULT_TYPE_ENG_FLOAT) {
1273  }
1274 
1275  switch (convertToType)
1276  {
1277 #ifdef USE_ACCELEROMETER
1279  pConverter = accel_converter;
1280  break;
1281 #endif
1282 #ifdef USE_MAGNETOMETER
1284  pConverter = mag_converter;
1285  break;
1286 #endif
1287 #ifdef USE_GYROMETER
1289  pConverter = gyro_converter;
1290  break;
1291 #endif
1292  case TYPE_QUATERNION:
1293  pConverter = quaternion_converter;
1294  break;
1295  case TYPE_EULER_3D:
1296  pConverter = euler_converter;
1297  break;
1299  pConverter = dircosine_converter;
1300  break;
1301 #ifdef USE_PRESSURE_SENSOR
1302  case TYPE_ALTITUDE:
1303  pConverter = altitude_converter;
1304  break;
1305  case TYPE_TEMPERATURE:
1306  pConverter = temperature_converter;
1307  break;
1308 #endif
1309  default:
1311  }
1312 
1313  if (pConverter == NULL) {
1314  return DSA_ERR_BAD_RESULT_TYPE;
1315  }
1316 
1317  retStat = pConverter(
1319  (fsl_orientation_DataBuffer_t *)pNativeSample,
1320  pConvertedSample
1321  );
1322 
1323  return retStat;
1324 }
1325 
1326 static isf_dsa_status_t quaternion_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1327 {
1329  convertedSample->timestamp = nativeSample->timestamp;
1330  convertedSample->scalar = nativeSample->fq.q0;
1331  convertedSample->xvect = nativeSample->fq.q1;
1332  convertedSample->yvect = nativeSample->fq.q2;
1333  convertedSample->zvect = nativeSample->fq.q3;
1334  return ISF_SUCCESS;
1335 }
1336 
1337 static isf_dsa_status_t euler_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1338 {
1339  isf_Orientation3D_Euler_EngFloat_t *convertedSample = (isf_Orientation3D_Euler_EngFloat_t *)vpConvertedSample;
1340  convertedSample->timestamp = nativeSample->timestamp;
1341  convertedSample->fThe = nativeSample->fThe;
1342  convertedSample->fPhi = nativeSample->fPhi;
1343  convertedSample->fPsi = nativeSample->fPsi;
1344 
1345  return ISF_SUCCESS;
1346 }
1347 static isf_dsa_status_t dircosine_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1348 {
1349  isf_Orientation3D_Rotation_EngFloat_t *convertedSample = (isf_Orientation3D_Rotation_EngFloat_t *)vpConvertedSample;
1350  convertedSample->timestamp = nativeSample->timestamp;
1351  convertedSample->rMatrix[0][0] = nativeSample->fRmatrix[0][0];
1352  convertedSample->rMatrix[0][1] = nativeSample->fRmatrix[0][1];
1353  convertedSample->rMatrix[0][2] = nativeSample->fRmatrix[0][2];
1354  convertedSample->rMatrix[1][0] = nativeSample->fRmatrix[1][0];
1355  convertedSample->rMatrix[1][1] = nativeSample->fRmatrix[1][1];
1356  convertedSample->rMatrix[1][2] = nativeSample->fRmatrix[1][2];
1357  convertedSample->rMatrix[2][0] = nativeSample->fRmatrix[2][0];
1358  convertedSample->rMatrix[2][1] = nativeSample->fRmatrix[2][1];
1359  convertedSample->rMatrix[2][2] = nativeSample->fRmatrix[2][2];
1360 
1361  return ISF_SUCCESS;
1362 }
1363 #ifdef USE_ACCELEROMETER
1364 static isf_dsa_status_t accel_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1365 {
1366  isf_Acceleration3D_Float_t *convertedSample = (isf_Acceleration3D_Float_t *)vpConvertedSample;
1367  convertedSample->timestamp = nativeSample->accelData.timestamp;
1368  convertedSample->accel[0] = nativeSample->accelData.accel[0];
1369  convertedSample->accel[1] = nativeSample->accelData.accel[1];
1370  convertedSample->accel[2] = nativeSample->accelData.accel[2];
1371  return ISF_SUCCESS;
1372 }
1373 #endif
1374 #ifdef USE_MAGNETOMETER
1375 static isf_dsa_status_t mag_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1376 {
1377  isf_MagneticFieldStrength3D_float_t *convertedSample = (isf_MagneticFieldStrength3D_float_t *)vpConvertedSample;
1378  convertedSample->timestamp = nativeSample->magData.timestamp;
1379  convertedSample->fieldStrength[0] = nativeSample->magData.fieldStrength[0];
1380  convertedSample->fieldStrength[1] = nativeSample->magData.fieldStrength[1];
1381  convertedSample->fieldStrength[2] = nativeSample->magData.fieldStrength[2];
1382  return ISF_SUCCESS;
1383 }
1384 #endif
1385 #ifdef USE_GYROMETER
1386 static isf_dsa_status_t gyro_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1387 {
1388  isf_AngularVelocity3D__float_t *convertedSample = (isf_AngularVelocity3D__float_t *)vpConvertedSample;
1389  convertedSample->timestamp = nativeSample->magData.timestamp;
1390  convertedSample->angularVelocity[0] = nativeSample->gyroData.angularVelocity[0];
1391  convertedSample->angularVelocity[1] = nativeSample->gyroData.angularVelocity[1];
1392  convertedSample->angularVelocity[2] = nativeSample->gyroData.angularVelocity[2];
1393  return ISF_SUCCESS;
1394 }
1395 #endif
1396 
1397 #ifdef USE_PRESSURE_SENSOR
1398 static isf_dsa_status_t altitude_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1399 {
1400  isf_Meters1D_float_t *convertedSample = (isf_Meters1D_float_t *)vpConvertedSample;
1401  convertedSample->timestamp = nativeSample->pressureData.timestamp;
1402  convertedSample->altitude = nativeSample->fHp;
1403  return ISF_SUCCESS;
1404 }
1405 static isf_dsa_status_t temperature_converter(fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig, fsl_orientation_DataBuffer_t *nativeSample, void *vpConvertedSample )
1406 {
1407  isf_DegreesCelsius1D_float_t *convertedSample = (isf_DegreesCelsius1D_float_t *)vpConvertedSample;
1408  convertedSample->timestamp = nativeSample->fTp;
1409  return ISF_SUCCESS;
1410 }
1411 #endif
1412 
1413 void set_current_sample_buffer(
1414  fsl_orientation_DataBuffer_t *pCurrentSampleBuffer,
1415  fusion_state_t *pState,
1416  fsl_fusion_Sensor_Specific_Settings_t *pSpecificSettings
1417 )
1418 {
1419  int i,j;
1420  int latestGyroSampleIdx = pSpecificSettings->algorithmConfig.gyroOversampleRatio-1;
1421 
1422 #ifdef USE_PRESSURE_SENSOR
1423  pCurrentSampleBuffer->pressureData.altitude = pState->thisPressure.fHp;
1424  pCurrentSampleBuffer->temperatureData.temperature = pState->thisPressure.fTp;
1425 # if defined COMPUTE_1DOF_P_BASIC
1426  pCurrentSampleBuffer->fHp = pState->thisSV_1DOF_P_BASIC.fLPHp;
1427  pCurrentSampleBuffer->fTp = pState->thisSV_1DOF_P_BASIC.fLPTp;
1428 # endif
1429 #endif
1430 #ifdef USE_ACCELEROMETER
1431  pCurrentSampleBuffer->accelData.accel[0] = pState->thisAccel.fGsAvg[0];
1432  pCurrentSampleBuffer->accelData.accel[1] = pState->thisAccel.fGsAvg[1];
1433  pCurrentSampleBuffer->accelData.accel[2] = pState->thisAccel.fGsAvg[2];
1434  pCurrentSampleBuffer->accelData.timestamp= pState->accelTimestamp;
1435 #endif
1436 #ifdef USE_MAGNETOMETER
1437 #ifdef USE_CALIBRATED_MAG_VALUES
1438  pCurrentSampleBuffer->magData.fieldStrength[0] = pState->thisMag.fBcAvg[0];
1439  pCurrentSampleBuffer->magData.fieldStrength[1] = pState->thisMag.fBcAvg[1];
1440  pCurrentSampleBuffer->magData.fieldStrength[2] = pState->thisMag.fBcAvg[2];
1441 #else
1442  pCurrentSampleBuffer->magData.fieldStrength[0] = pState->thisMag.fBsAvg[0];
1443  pCurrentSampleBuffer->magData.fieldStrength[1] = pState->thisMag.fBsAvg[1];
1444  pCurrentSampleBuffer->magData.fieldStrength[2] = pState->thisMag.fBsAvg[2];
1445 #endif
1446  pCurrentSampleBuffer->magData.timestamp= pState->magTimestamp;
1447 #endif
1448 #ifdef USE_GYROMETER
1449  pCurrentSampleBuffer->gyroData.angularVelocity[0] = pState->thisGyro.iYsBuffer[latestGyroSampleIdx][0] * pState->thisGyro.fDegPerSecPerCount;
1450  pCurrentSampleBuffer->gyroData.angularVelocity[1] = pState->thisGyro.iYsBuffer[latestGyroSampleIdx][1] * pState->thisGyro.fDegPerSecPerCount;
1451  pCurrentSampleBuffer->gyroData.angularVelocity[2] = pState->thisGyro.iYsBuffer[latestGyroSampleIdx][2] * pState->thisGyro.fDegPerSecPerCount;
1452  pCurrentSampleBuffer->gyroData.timestamp= pState->gyroTimestamp;
1453 #endif
1454 
1455  switch (pState->algorithmToUse)
1456  {
1457 #ifdef COMPUTE_3DOF_G_BASIC
1458  case Q3:
1459 
1460  pCurrentSampleBuffer->fChi = pState->thisSV_3DOF_G_BASIC.fLPChi;
1461  pCurrentSampleBuffer->fPhi = pState->thisSV_3DOF_G_BASIC.fLPPhi;
1462  pCurrentSampleBuffer->fPsi = pState->thisSV_3DOF_G_BASIC.fLPPsi;
1463  pCurrentSampleBuffer->fRho = pState->thisSV_3DOF_G_BASIC.fLPRho;
1464  pCurrentSampleBuffer->fThe = pState->thisSV_3DOF_G_BASIC.fLPThe;
1465  pCurrentSampleBuffer->fq = pState->thisSV_3DOF_G_BASIC.fLPq;
1466  pCurrentSampleBuffer->fDelta = 0.0;
1467  pCurrentSampleBuffer->fDeltaPl = 0.0;
1468 
1469  for (i=0; i<3; i++)
1470  {
1471  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_3DOF_G_BASIC.fLPRVec[i];
1472 
1473 
1474  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_3DOF_G_BASIC.fOmega[i];
1475 
1476  pCurrentSampleBuffer->fZErr[i] = 0.0;
1477  pCurrentSampleBuffer->fqgErrPl[i] = 0.0;
1478  pCurrentSampleBuffer->fbPl[i] = 0.0;
1479  pCurrentSampleBuffer->fqmErrPl[i] = 0.0;
1480  for (j=0; j<3; j++)
1481  {
1482  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_3DOF_G_BASIC.fLPR[i][j];
1483  }
1484  }
1485 
1486  for (i=3; i<7; i++)
1487  {
1488  pCurrentSampleBuffer->fZErr[i] = 0.0;
1489  }
1490 
1491  pCurrentSampleBuffer->timestamp = pState->accelTimestamp;
1492 
1493  break;
1494 #endif
1495 #ifdef COMPUTE_3DOF_B_BASIC
1496  case Q3M:
1497  pCurrentSampleBuffer->fChi = pState->thisSV_3DOF_B_BASIC.fLPChi;
1498  pCurrentSampleBuffer->fPhi = pState->thisSV_3DOF_B_BASIC.fLPPhi;
1499  pCurrentSampleBuffer->fPsi = pState->thisSV_3DOF_B_BASIC.fLPPsi;
1500  pCurrentSampleBuffer->fRho = pState->thisSV_3DOF_B_BASIC.fLPRho;
1501  pCurrentSampleBuffer->fThe = pState->thisSV_3DOF_B_BASIC.fLPThe;
1502  pCurrentSampleBuffer->fq = pState->thisSV_3DOF_B_BASIC.fLPq;
1503  pCurrentSampleBuffer->fDelta = 0.0;
1504  pCurrentSampleBuffer->fDeltaPl = 0.0;
1505 
1506  for (i=0; i<3; i++)
1507  {
1508  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_3DOF_B_BASIC.fLPRVec[i];
1509 
1510 
1511  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_3DOF_B_BASIC.fOmega[i];
1512 
1513  pCurrentSampleBuffer->fAccGl[i] = 0.0;
1514  pCurrentSampleBuffer->fDisGl[i] = 0.0;
1515 
1516  pCurrentSampleBuffer->fZErr[i] = 0.0;
1517  pCurrentSampleBuffer->fqgErrPl[i] = 0.0;
1518  pCurrentSampleBuffer->fbPl[i] = 0.0;
1519  pCurrentSampleBuffer->fqmErrPl[i] = 0.0;
1520  for (j=0; j<3; j++)
1521  {
1522  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_3DOF_B_BASIC.fLPR[i][j];
1523  }
1524  }
1525 
1526  for (i=3; i<7; i++)
1527  {
1528  pCurrentSampleBuffer->fZErr[i] = 0.0;
1529  }
1530  pCurrentSampleBuffer->timestamp = pState->magTimestamp;
1531  break;
1532 #endif
1533 #ifdef COMPUTE_3DOF_Y_BASIC
1534  case Q3G:
1535  pCurrentSampleBuffer->fChi = pState->thisSV_3DOF_Y_BASIC.fChi;
1536  pCurrentSampleBuffer->fPhi = pState->thisSV_3DOF_Y_BASIC.fPhi;
1537  pCurrentSampleBuffer->fPsi = pState->thisSV_3DOF_Y_BASIC.fPsi;
1538  pCurrentSampleBuffer->fRho = pState->thisSV_3DOF_Y_BASIC.fRho;
1539  pCurrentSampleBuffer->fThe = pState->thisSV_3DOF_Y_BASIC.fThe;
1540  pCurrentSampleBuffer->fq = pState->thisSV_3DOF_Y_BASIC.fq;
1541  pCurrentSampleBuffer->fDelta = 0.0;
1542  pCurrentSampleBuffer->fDeltaPl = 0.0;
1543 
1544  for (i=0; i<3; i++)
1545  {
1546  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_3DOF_Y_BASIC.fRVec[i];
1547 
1548 
1549  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_3DOF_Y_BASIC.fOmega[i];
1550 
1551  pCurrentSampleBuffer->fAccGl[i] = 0.0;
1552  pCurrentSampleBuffer->fDisGl[i] = 0.0;
1553 
1554  pCurrentSampleBuffer->fZErr[i] = 0.0;
1555  pCurrentSampleBuffer->fqgErrPl[i] = 0.0;
1556  pCurrentSampleBuffer->fbPl[i] = 0.0;
1557  pCurrentSampleBuffer->fqmErrPl[i] = 0.0;
1558  for (j=0; j<3; j++)
1559  {
1560  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_3DOF_Y_BASIC.fR[i][j];
1561  }
1562  }
1563 
1564  for (i=3; i<7; i++)
1565  {
1566  pCurrentSampleBuffer->fZErr[i] = 0.0;
1567  }
1568  pCurrentSampleBuffer->timestamp = pState->gyroTimestamp;
1569 
1570  break;
1571 #endif
1572 #ifdef COMPUTE_6DOF_GB_BASIC
1573  case Q6MA:
1574  pCurrentSampleBuffer->fChi = pState->thisSV_6DOF_GB_BASIC.fLPChi;
1575  pCurrentSampleBuffer->fPhi = pState->thisSV_6DOF_GB_BASIC.fLPPhi;
1576  pCurrentSampleBuffer->fPsi = pState->thisSV_6DOF_GB_BASIC.fLPPsi;
1577  pCurrentSampleBuffer->fRho = pState->thisSV_6DOF_GB_BASIC.fLPRho;
1578  pCurrentSampleBuffer->fThe = pState->thisSV_6DOF_GB_BASIC.fLPThe;
1579  pCurrentSampleBuffer->fq = pState->thisSV_6DOF_GB_BASIC.fLPq;
1580  pCurrentSampleBuffer->fDelta = pState->thisSV_6DOF_GB_BASIC.fLPDelta;
1581  pCurrentSampleBuffer->fDeltaPl = 0.0;
1582 
1583  for (i=0; i<3; i++)
1584  {
1585  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_6DOF_GB_BASIC.fLPRVec[i];
1586 
1587 
1588  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_6DOF_GB_BASIC.fOmega[i];
1589 
1590  pCurrentSampleBuffer->fAccGl[i] = 0.0;
1591  pCurrentSampleBuffer->fDisGl[i] = 0.0;
1592 
1593  pCurrentSampleBuffer->fZErr[i] = 0.0;
1594  pCurrentSampleBuffer->fqgErrPl[i] = 0.0;
1595  pCurrentSampleBuffer->fbPl[i] = 0.0;
1596  pCurrentSampleBuffer->fqmErrPl[i] = 0.0;
1597 
1598  for (j=0; j<3; j++)
1599  {
1600  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_6DOF_GB_BASIC.fLPR[i][j];
1601  }
1602  }
1603 
1604  for (i=3; i<7; i++)
1605  {
1606  pCurrentSampleBuffer->fZErr[i] = 0.0;
1607  }
1608  if (pState->accelTimestamp > pState->magTimestamp)
1609  {
1610  pCurrentSampleBuffer->timestamp = pState->accelTimestamp;
1611  }
1612  else
1613  {
1614  pCurrentSampleBuffer->timestamp = pState->magTimestamp;
1615  }
1616  break;
1617 #endif
1618 #ifdef COMPUTE_6DOF_GY_KALMAN
1619  case Q6AG:
1620  pCurrentSampleBuffer->fChi = pState->thisSV_6DOF_GY_KALMAN.fChiPl;
1621  pCurrentSampleBuffer->fPhi = pState->thisSV_6DOF_GY_KALMAN.fPhiPl;
1622  pCurrentSampleBuffer->fPsi = pState->thisSV_6DOF_GY_KALMAN.fPsiPl;
1623  pCurrentSampleBuffer->fRho = pState->thisSV_6DOF_GY_KALMAN.fRhoPl;
1624  pCurrentSampleBuffer->fThe = pState->thisSV_6DOF_GY_KALMAN.fThePl;
1625  pCurrentSampleBuffer->fq = pState->thisSV_6DOF_GY_KALMAN.fqPl;
1626  pCurrentSampleBuffer->fDelta = 0.0;
1627  pCurrentSampleBuffer->fDeltaPl = 0.0;
1628 
1629  for (i=0; i<3; i++)
1630  {
1631  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_6DOF_GY_KALMAN.fRVecPl[i];
1632 
1633 
1634  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_6DOF_GY_KALMAN.fOmega[i];
1635 
1636  pCurrentSampleBuffer->fAccGl[i] = pState->thisSV_6DOF_GY_KALMAN.fAccGl[i];
1637  pCurrentSampleBuffer->fDisGl[i] = 0;
1638  pCurrentSampleBuffer->fZErr[i] = pState->thisSV_6DOF_GY_KALMAN.fZErr[i];
1639  pCurrentSampleBuffer->fqgErrPl[i] = pState->thisSV_6DOF_GY_KALMAN.fqgErrPl[i];
1640  pCurrentSampleBuffer->fbPl[i] = pState->thisSV_6DOF_GY_KALMAN.fbPl[i];
1641 
1642  for (j=0; j<3; j++)
1643  {
1644  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_6DOF_GY_KALMAN.fRPl[i][j];
1645  }
1646  }
1647 
1648  for (i=3; i<7; i++)
1649  {
1650  pCurrentSampleBuffer->fZErr[i] = pState->thisSV_6DOF_GY_KALMAN.fZErr[i];
1651  }
1652 
1653  pCurrentSampleBuffer->timestamp = pState->gyroTimestamp;
1654 
1655 
1656  break;
1657 #endif
1658 #ifdef COMPUTE_9DOF_GBY_KALMAN
1659  case Q9:
1660  default:
1661  pCurrentSampleBuffer->fChi = pState->thisSV_9DOF_GBY_KALMAN.fChiPl;
1662  pCurrentSampleBuffer->fPhi = pState->thisSV_9DOF_GBY_KALMAN.fPhiPl;
1663  pCurrentSampleBuffer->fPsi = pState->thisSV_9DOF_GBY_KALMAN.fPsiPl;
1664  pCurrentSampleBuffer->fRho = pState->thisSV_9DOF_GBY_KALMAN.fRhoPl;
1665  pCurrentSampleBuffer->fThe = pState->thisSV_9DOF_GBY_KALMAN.fThePl;
1666  pCurrentSampleBuffer->fq = pState->thisSV_9DOF_GBY_KALMAN.fqPl;
1667  pCurrentSampleBuffer->fDelta = pState->thisSV_9DOF_GBY_KALMAN.fDeltaPl;
1668  pCurrentSampleBuffer->fDeltaPl = pState->thisSV_9DOF_GBY_KALMAN.fDeltaPl;
1669 
1670  for (i=0; i<3; i++)
1671  {
1672  pCurrentSampleBuffer->fRVec[i] = pState->thisSV_9DOF_GBY_KALMAN.fRVecPl[i];
1673 
1674  pCurrentSampleBuffer->fOmega[i] = pState->thisSV_9DOF_GBY_KALMAN.fOmega[i];
1675 
1676  pCurrentSampleBuffer->fAccGl[i] = pState->thisSV_9DOF_GBY_KALMAN.fAccGl[i];
1677  pCurrentSampleBuffer->fDisGl[i] = pState->thisSV_9DOF_GBY_KALMAN.fDisGl[i];
1678 
1679  pCurrentSampleBuffer->fZErr[i] = pState->thisSV_9DOF_GBY_KALMAN.fZErr[i];
1680  pCurrentSampleBuffer->fqgErrPl[i] = pState->thisSV_9DOF_GBY_KALMAN.fqgErrPl[i];
1681  pCurrentSampleBuffer->fbPl[i] = pState->thisSV_9DOF_GBY_KALMAN.fbPl[i];
1682  pCurrentSampleBuffer->fqmErrPl[i] = pState->thisSV_9DOF_GBY_KALMAN.fqmErrPl[i];
1683 
1684  for (j=0; j<3; j++)
1685  {
1686  pCurrentSampleBuffer->fRmatrix[i][j] = pState->thisSV_9DOF_GBY_KALMAN.fRPl[i][j];
1687  }
1688  }
1689 
1690  for (i=3; i<7; i++)
1691  {
1692  pCurrentSampleBuffer->fZErr[i] = pState->thisSV_9DOF_GBY_KALMAN.fZErr[i];
1693  }
1694 
1695  pCurrentSampleBuffer->timestamp = pState->gyroTimestamp;
1696 
1697  break;
1698 #endif
1699  }
1700 }
1701 
isf_orientation_euler_deg_float_t fPsi
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:48
#define ACCEL_HAL_X_INDEX
Definition: fusion_config.h:38
isf_temperature_degC_float_t temperature
#define PRESSURE_DATA_MASK
Standard raw type for three axes accelerometers.
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.
#define ACCEL_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:41
isf_orientation_euler_deg_float_t fThe
#define MAG_HAL_X_INDEX
Definition: fusion_config.h:44
int32 stop_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:97
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:37
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:170
const uint8_t * mqx_task_stack_pointers[]
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:199
isf_orientation_euler_deg_float_t fPhi
isf_microTeslas_raw_16count_t fieldStrength[3]
Standard floating point type for single axis temperature sensor.
isf_fifo_status_t isf_fifo_el_increment(isf_fifo_t *pFifo)
Routine increments the insert pointer after direct access.
Definition: isf_fifo.c:237
isf_sensors.h contains the ISF Generic Sensor definitions and data structures required when a client ...
#define FALSE
Definition: isf_types.h:86
isf_SensorTypes_t fsl_fusion_SupportedSensorTypes[]
Supported sensor and data types for the orientation sensor.
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
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:217
isf_timestamp_t timestamp
Definition: matrix.h:38
isf_dsa_result_types_t resultFormat
The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT.
isf_SensorDataTypes_t TYPE_MAGNETIC_FIELD_STRENGTH_3D
#define MAG_DATA_READY_EVENT
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 ...
int32 init_sensor(uint8 nSensorID, isf_SensorHandle_t *pSensorAdapterHandle, event_t *pEventGroup, uint32 nEventFieldIndex)
This function initializes the sensor adapters in the absence of the Sensor Manager.
isf_SensorTypes_t
#define MAG_HAL_Y_INDEX
Definition: fusion_config.h:46
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...
#define GYRO_HAL_X_INDEX
Definition: fusion_config.h:49
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.
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:75
#define HYBRID_SENSOR_DATA_READY_EVENT
Standard raw type for three axes accelerometers.
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...
Standard float type for three axis accelerometers.
isf_acceleration_g_float_t accel[3]
signed short int int16
Definition: isf_types.h:73
#define ACCEL_HAL_Z_INDEX
Definition: fusion_config.h:42
uint32 isf_fifo_status_t
Definition: isf_fifo.h:16
The isf_sensor_types.h contains the enumerated list of sensor types used by ISF.
The isf_util.h file contains the utility method declarations and macros.
#define GYRO_HAL_Y_INDEX
Definition: fusion_config.h:51
#define GYRO_HAL_Z_INDEX
Definition: fusion_config.h:53
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)
int32 start_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
#define ACCEL_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:39
isf_orientation_quaternion_float_t zvect
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:24
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
isf_gyrometer_dps_raw_16count_t angularVelocity[3]
Definition: matrix.h:40
OSA_TASK_DEFINE(fsl_fusion, 2000)
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
isf_dsa_result_types_t resultFormat
isf_SensorDataTypes_t
#define MAG_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:45
#define PRESSURE_DATA_READY_EVENT
The isf_altitude_types.h file contains the ISF data type definitions for use with the ISF generic alt...
The isf_temperature_fixed_t.h file contains the ISF data type definitions for use with the ISF generi...
isf_orientation_rot_float_t rMatrix[3][3]
const isf_SensorConfig_t * pSensorStaticConfig
Definition: matrix.h:39
int32 convert_sensor_data(isf_SensorHandle_t *pSensorAdapterHandle, isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType, void *nativeSample, void *convertedSample)
#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.
isf_dsa_AdapterStatus_t adapterStatus
signed long int int32
Definition: isf_types.h:74
unsigned short int uint16
Definition: isf_types.h:77
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:50
uint32 nSamplePeriod
Sample period in microseconds.
#define ACCEL_DATA_READY_EVENT
#define GYRO_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:48
#define MAGCAL_EVENT_FLAG
#define GYRO_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:52
isf_gyrometer_dps_float_t angularVelocity[3]
int32 isf_status_t
ISF return status type.
Definition: isf.h:76
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:268
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
#define MAG_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:43
#define ISF_FIFO_NO_MORE_ENTRIES
Definition: isf_fifo.h:34
void * isf_fifo_el_get_insert_pointer(isf_fifo_t *pFifo)
Routine returns the insert pointer for direct access.
Definition: isf_fifo.c:229
unsigned long int uint32
Definition: isf_types.h:78
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:47
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:40
#define OVERSAMPLE_RATIO
Definition: fusion_config.h:21
#define ISF_FIFO_FULL
Definition: isf_fifo.h:33
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:259
isf_acceleration_g_rawCount_t accel[3]
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 ...
signed char int8
Definition: isf_types.h:72