| ISF 2.2 rev 5 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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>
29 #include <isf_accelerometer_types.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"
39 #include "fsl_fusion_virt_3D_orient.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 */
92 isf_SensorTypes_t fsl_fusion_SupportedSensorTypes[] =
93 {
94 TYPE_ORIENTATION,
95 TYPE_ACCELEROMETER_3D,
96 TYPE_MAGNETOMETER_3D,
97 TYPE_GYROMETER_3D,
98 TYPE_ALTIMETER,
99 TYPE_THERMOMETER,
100 TYPE_NATIVE_SENSOR_TYPE
101 };
102 isf_SensorDataTypes_t fsl_fusion_SupportedDataTypes[] =
103 {
104 TYPE_QUATERNION,
105 TYPE_EULER_3D,
106 TYPE_DIR_COSINE_MATRIX,
107 TYPE_RAW_ACCELERATION_3D,
108 TYPE_MAGNETIC_FIELD_STRENGTH_3D,
109 TYPE_ROTATIONAL_RATE_3D,
110 TYPE_ALTITUDE,
111 TYPE_TEMPERATURE,
112 TYPE_NATIVE_SENSOR_DATA_TYPE
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 */
154 isf_dsa_status_t fsl_fusion_virt_3D_orient_Initialize(isf_SensorHandle_t *pSensorHandle)
155 {
156 int32 retStatus = 0;
157
158 fsl_fusion_Sensor_Specific_Settings_t *pSensorSpecificConfig = (fsl_fusion_Sensor_Specific_Settings_t*)pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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
176 fsl_fusion_DeviceDescriptor_t *pDeviceDescriptor = (fsl_fusion_DeviceDescriptor_t*)pSensorHandle->pDeviceDescriptor;
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 {
221 isf_fifo_init
222 (
223 &(pDeviceDescriptor->accelFifo),
224 NULL,
225 sizeof(isf_Acceleration3D_RawCount_t),
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
236 isf_fifo_init
237 (
238 &(pDeviceDescriptor->magFifo),
239 NULL,
240 sampleSize,
241 pSensorSpecificConfig->algorithmConfig.magOversampleRatio
242 );
243 }
244 #endif
245
246 #ifdef USE_GYROMETER
247 isf_fifo_init
248 (
249 &(pDeviceDescriptor->gyroFifo),
250 NULL,
251 sizeof(isf_AngularVelocity3D_RawCount_t),
252 pSensorSpecificConfig->algorithmConfig.gyroOversampleRatio
253 );
254 #endif
255
256 #ifdef USE_PRESSURE_SENSOR
257 isf_fifo_init
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 */
322 isf_dsa_status_t fsl_fusion_virt_3D_orient_ValidateSettings(isf_SensorHandle_t *pSensorHandle, isf_dsa_SensorSettings_t *pSensorSettings)
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
337 fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = (fsl_fusion_DeviceDescriptor_t*)pSensorHandle->pDeviceDescriptor;
338 fsl_fusion_Sensor_Specific_Settings_t* pSpecificSettings = pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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
343 settings.resultFormat = DSA_RESULT_TYPE_ENG_FLOAT;
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;
375 retStat = DSA_ERR_BAD_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 */
394 isf_dsa_status_t fsl_fusion_virt_3D_orient_Configure(isf_SensorHandle_t *pSensorHandle, isf_dsa_SensorSettings_t *pSensorSettings)
395 {
396 int32 samplePeriod;
397
398 isf_SubscriptionSettings_t settings;
399 fsl_fusion_Sensor_Specific_Settings_t* pSpecificSettings = pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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
409 fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = (fsl_fusion_DeviceDescriptor_t*)pSensorHandle->pDeviceDescriptor;
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
531 pSensorHandle->adapterStatus = DSA_STATE_CONFIGURED_STOPPED;
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 */
555 isf_dsa_status_t fsl_fusion_virt_3D_orient_StartData(isf_SensorHandle_t *pSensorHandle)
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
571 fsl_fusion_Sensor_Specific_Settings_t* pSpecificSettings = pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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.
626 pSensorHandle->adapterStatus = DSA_STATE_CONFIGURED_STARTED;
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 */
649 isf_dsa_status_t fsl_fusion_virt_3D_orient_EndData(isf_SensorHandle_t *pSensorHandle)
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
665 fsl_fusion_Sensor_Specific_Settings_t* pSpecificSettings = pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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
717 pSensorHandle->adapterStatus = DSA_STATE_CONFIGURED_STOPPED;
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 */
742 isf_dsa_status_t fsl_fusion_virt_3D_orient_Calibrate(isf_SensorHandle_t *pSensorHandle, void *pCalData )
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 */
759 isf_dsa_status_t fsl_fusion_virt_3D_orient_Shutdown(isf_SensorHandle_t *pSensorHandle )
760 {
761 if(NULL == pSensorHandle){
762 return DSA_ERR_PARAM;
763 }
764
765 // Create helper reference pointers.
766 fsl_fusion_DeviceDescriptor_t* pDeviceDescriptor = (fsl_fusion_DeviceDescriptor_t*)pSensorHandle->pDeviceDescriptor;
767 fsl_fusion_Sensor_Specific_Settings_t* pSpecificSettings = pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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
801 void fsl_fusion_virt_3D_orient_PeriodicCallback(void* pSensorHandle)
802 {
803
804
805 // Get the device descriptor and specific settings
806 isf_SensorHandle_t *pSensorHdl = (isf_SensorHandle_t *)pSensorHandle;
807 isf_fifo_t *pFifo = pSensorHdl->controlData.sensorSettings.pSampleBufferInfo;
808 fsl_fusion_DeviceDescriptor_t *pDesc = (fsl_fusion_DeviceDescriptor_t*)pSensorHdl->pDeviceDescriptor;
809 fsl_fusion_Sensor_Specific_Settings_t *pSpecificSettings = pSensorHdl->pSensorStaticConfig->pSensorSpecificSettings;
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;
847 isf_Acceleration3D_RawCount_t *pSample;
848 isf_Acceleration3D_RawCount_t tmpSample;
849 int8 sampleCount = 0;
850 isf_fifo_status_t st;
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
889 pDesc->sensorDataReady &= ~ACCEL_DATA_READY_EVENT;
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
899 isf_MagneticFieldStrength3D_RawCount_t *pSampleIter = 0;
900 isf_MagneticFieldStrength3D_RawCount_t *pSample;
901 isf_MagneticFieldStrength3D_RawCount_t tmpSample;
902 int8 sampleCount = 0;
903 isf_fifo_status_t st;
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
1017 pDesc->sensorDataReady &= ~MAG_DATA_READY_EVENT;
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));
1044 pDesc->sensorDataReady &= ~PRESSURE_DATA_READY_EVENT;
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
1079 pDesc->sensorDataReady &= ~GYRO_DATA_READY_EVENT;
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
1090 pDesc->sensorDataReady = SENSOR_DATA_READY_EVENT;
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
1100 if (pSensorHdl->controlData.sensorSettings.resultType == TYPE_NATIVE_SENSOR_DATA_TYPE )
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
1111 fsl_fusion_virt_3D_orient_Convert(
1112 pSensorHdl,
1113 pSensorHdl->controlData.sensorSettings.resultType,
1114 pSensorHdl->controlData.sensorSettings.resultFormat,
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
1155 fsl_fusion_DeviceDescriptor_t *pDesc = (fsl_fusion_DeviceDescriptor_t*)pSensorHandle->pDeviceDescriptor;
1156 fsl_fusion_Sensor_Specific_Settings_t *pSpecificSettings = pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings;
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
1230 fsl_fusion_DeviceDescriptor_t *pDesc = (fsl_fusion_DeviceDescriptor_t*)pSensorHandle->pDeviceDescriptor;
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 */
1256 isf_dsa_status_t fsl_fusion_virt_3D_orient_Convert
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 {
1265 isf_dsa_status_t retStat = DSA_ERR_BAD_SENSOR_DATA_TYPE;
1266 isf_dsa_status_t (*pConverter)(fsl_fusion_Sensor_Specific_Settings_t *, fsl_orientation_DataBuffer_t *, void *);
1267
1268 pConverter = NULL;
1269
1270 // TODO - Only supports float types for now
1271 if (resultType != DSA_RESULT_TYPE_ENG_FLOAT) {
1272 return DSA_ERR_BAD_SENSOR_DATA_TYPE;
1273 }
1274
1275 switch (convertToType)
1276 {
1277 #ifdef USE_ACCELEROMETER
1278 case TYPE_RAW_ACCELERATION_3D:
1279 pConverter = accel_converter;
1280 break;
1281 #endif
1282 #ifdef USE_MAGNETOMETER
1283 case TYPE_MAGNETIC_FIELD_STRENGTH_3D:
1284 pConverter = mag_converter;
1285 break;
1286 #endif
1287 #ifdef USE_GYROMETER
1288 case TYPE_ROTATIONAL_RATE_3D:
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;
1298 case TYPE_DIR_COSINE_MATRIX:
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:
1310 return DSA_ERR_BAD_SENSOR_DATA_TYPE;
1311 }
1312
1313 if (pConverter == NULL) {
1314 return DSA_ERR_BAD_RESULT_TYPE;
1315 }
1316
1317 retStat = pConverter(
1318 (fsl_fusion_Sensor_Specific_Settings_t*)pSensorHandle->pSensorStaticConfig->pSensorSpecificSettings,
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 {
1328 isf_Orientation3D_Quaterion_EngFloat_t *convertedSample = (isf_Orientation3D_Quaterion_EngFloat_t *)vpConvertedSample;
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_SensorHandle_st::pDeviceDescriptor
void * pDeviceDescriptor
Definition: isf_dsa_adapter.h:109
isf_AngularVelocity3D__float_t::timestamp
isf_timestamp_t timestamp
Definition: isf_gyrometer_types.h:77
isf_Orientation3D_Euler_EngFloat_t::fPsi
isf_orientation_euler_deg_float_t fPsi
Definition: isf_orientation_types.h:56
Fusion_Init
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:48
ACCEL_HAL_X_INDEX
#define ACCEL_HAL_X_INDEX
Definition: fusion_config.h:38
isf_DegreesCelsius1D_float_t::temperature
isf_temperature_degC_float_t temperature
Definition: isf_temperature_types.h:37
PRESSURE_DATA_MASK
#define PRESSURE_DATA_MASK
Definition: fsl_fusion_virt_3D_orient.c:76
isf_Acceleration3D_RawCount_t
Standard raw type for three axes accelerometers.
Definition: isf_accelerometer_types.h:58
SensorConfig_st::pSensorSpecificSettings
void * pSensorSpecificSettings
Definition: isf_dsa_adapter.h:157
fsl_fusion_virt_3D_orient_Initialize
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.
Definition: fsl_fusion_virt_3D_orient.c:154
fsl_fusion_Sensor_Specific_Settings_t
Definition: fsl_fusion_virt_3D_orient.h:129
isf_dsa_adapter.h
DSA_ERR_BAD_RESULT_TYPE
Definition: isf_dsa_adapter.h:39
ACCEL_GET_HAL_Z_VALUE
#define ACCEL_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:41
fsl_orientation_DataBuffer_t::fAccGl
float fAccGl[3]
Definition: fsl_fusion_virt_3D_orient.h:115
fsl_orientation_DataBuffer_t::fChi
float fChi
Definition: fsl_fusion_virt_3D_orient.h:102
isf_AngularVelocity3D_RawCount_t::timestamp
isf_timestamp_t timestamp
Definition: isf_gyrometer_types.h:56
isf_SensorHandle_st::signalledEvents
uint32 signalledEvents
Definition: isf_dsa_adapter.h:113
isf_Orientation3D_Euler_EngFloat_t::fThe
isf_orientation_euler_deg_float_t fThe
Definition: isf_orientation_types.h:54
MAG_HAL_X_INDEX
#define MAG_HAL_X_INDEX
Definition: fusion_config.h:44
stop_sensor
int32 stop_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
Definition: isf_dsa_direct.c:119
Fusion_Run
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:97
isf_Acceleration3D_Float_t::timestamp
isf_timestamp_t timestamp
Definition: isf_accelerometer_types.h:81
isf_MagneticFieldStrength3D_float_t
Standard fixed type for three axis accelerometers.
Definition: isf_magnetometer_types.h:80
isf_magnetometer_types.h
The isf_magnetometer_types.h file contains the ISF data type definitions for use with the ISF generic...
ISF_SUCCESS
Definition: isf.h:31
fsl_orientation_DataBuffer_t::fHp
float fHp
Definition: fsl_fusion_virt_3D_orient.h:112
Q6AG
Definition: fusion_types.h:51
ACCEL_GET_HAL_X_VALUE
#define ACCEL_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:37
fsl_fusion_SupportedDataTypes
isf_SensorDataTypes_t fsl_fusion_SupportedDataTypes[]
Definition: fsl_fusion_virt_3D_orient.c:102
fsl_fusion_virt_3D_orient_Configure
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.
Definition: fsl_fusion_virt_3D_orient.c:394
DSA_ERR_BAD_SENSOR_DATA_TYPE
Definition: isf_dsa_adapter.h:38
SENSOR_DATA_READY_EVENT
#define SENSOR_DATA_READY_EVENT
Definition: fsl_fusion_virt_3D_orient.c:79
isf_Orientation3D_Euler_EngFloat_t::timestamp
isf_timestamp_t timestamp
Definition: isf_orientation_types.h:53
isf_SensorHandle_st::controlData
isf_dsa_ControlData_t controlData
Definition: isf_dsa_adapter.h:111
isf_fifo_lock
isf_fifo_status_t isf_fifo_lock(isf_fifo_t *pFifo)
Lock a sample buffer for exclusive access.
Definition: isf_fifo.c:170
fsl_fusion_Sensor_Specific_Settings_t::algorithmToUse
quaternion_type algorithmToUse
Definition: fsl_fusion_virt_3D_orient.h:144
mqx_task_stack_pointers
const uint8_t * mqx_task_stack_pointers[]
fsl_MagCal_task
void fsl_MagCal_task(uint32_t task_init_data)
Definition: fsl_fusion_virt_3D_orient.c:1225
fusion_algorithmConfig_t::gyroOversampleRatio
uint8 gyroOversampleRatio
Definition: fusion_types.h:269
fsl_orientation_DataBuffer_t
Definition: fsl_fusion_virt_3D_orient.h:77
isf_fifo_unlock
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_Orientation3D_Euler_EngFloat_t::fPhi
isf_orientation_euler_deg_float_t fPhi
Definition: isf_orientation_types.h:55
isf_MagneticFieldStrength3D_RawCount_t::fieldStrength
isf_microTeslas_raw_16count_t fieldStrength[3]
Definition: isf_magnetometer_types.h:61
isf_MagneticFieldStrength3D_RawCount_t::timestamp
isf_timestamp_t timestamp
Definition: isf_magnetometer_types.h:60
isf_Orientation3D_Quaterion_EngFloat_t::timestamp
isf_timestamp_t timestamp
Definition: isf_orientation_types.h:41
isf_DegreesCelsius1D_float_t
Standard floating point type for single axis temperature sensor.
Definition: isf_temperature_types.h:35
DSA_STATE_CONFIGURED_STARTED
Definition: isf_dsa_adapter.h:60
isf_fifo_el_increment
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
isf_sensors.h contains the ISF Generic Sensor definitions and data structures required when a client ...
Q3M
Definition: fusion_types.h:51
FALSE
#define FALSE
Definition: isf_types.h:86
fsl_fusion_SupportedSensorTypes
isf_SensorTypes_t fsl_fusion_SupportedSensorTypes[]
Supported sensor and data types for the orientation sensor.
Definition: fsl_fusion_virt_3D_orient.c:92
isf_MagneticFieldStrength3D_float_t::fieldStrength
isf_fieldStrength_uT_float_t fieldStrength[3]
Definition: isf_magnetometer_types.h:82
isf_bm.h
API definitions, types, and macros for the Intelligent Sensing Framework (ISF) Bus Manager (BM)...
fsl_orientation_DataBuffer_t::fqgErrPl
float fqgErrPl[3]
Definition: fsl_fusion_virt_3D_orient.h:122
isf_Meters1D_float_t::altitude
isf_altitude_meters_float_t altitude
Definition: isf_altitude_types.h:38
fsl_fusion_DeviceDescriptor_t::sensorLWEvent
event_t sensorLWEvent
Definition: fsl_fusion_virt_3D_orient.h:149
TYPE_QUATERNION
Definition: isf_sensor_types.h:88
fsl_orientation_DataBuffer_t::fPsi
float fPsi
Definition: fsl_fusion_virt_3D_orient.h:100
isf_Orientation3D_Quaterion_EngFloat_t::xvect
isf_orientation_quaternion_float_t xvect
Definition: isf_orientation_types.h:43
isf_SensorHandle_st
This defines the DSA sensor device handle structure used to invoke the adapter access functions...
Definition: isf_dsa_adapter.h:107
fquaternion::q1
float q1
Definition: fusion_types.h:61
isf_fifo_el_clear
isf_fifo_status_t isf_fifo_el_clear(isf_fifo_t *pFifo)
Routine to clear the fifo.
Definition: isf_fifo.c:217
DSA_RESULT_TYPE_RAW_COUNTS
Definition: isf_dsa_adapter.h:47
isf_Meters1D_float_t::timestamp
isf_timestamp_t timestamp
Definition: isf_altitude_types.h:37
X
Definition: matrix.h:38
fsl_fusion_DeviceDescriptor_t
Definition: fsl_fusion_virt_3D_orient.h:147
Q6MA
Definition: fusion_types.h:51
isf_SubscriptionSettings_t::resultFormat
isf_dsa_result_types_t resultFormat
The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT.
Definition: isf_dsa_direct_types.h:25
isf_MagneticFieldStrength3D_RawCount_t
isf_SensorDataTypes_t TYPE_MAGNETIC_FIELD_STRENGTH_3D
Definition: isf_magnetometer_types.h:59
Q3
Definition: fusion_types.h:51
isf_dsa_SensorSettings_st::nSamplePeriod
uint32 nSamplePeriod
Definition: isf_dsa_adapter.h:82
TYPE_THERMOMETER
Definition: isf_sensor_types.h:39
fsl_orientation_DataBuffer_t::fZErr
float fZErr[7]
Definition: fsl_fusion_virt_3D_orient.h:121
fsl_fusion_DeviceDescriptor_t::deviceSemaphore
semaphore_t deviceSemaphore
Definition: fsl_fusion_virt_3D_orient.h:151
MAG_DATA_READY_EVENT
#define MAG_DATA_READY_EVENT
Definition: fsl_fusion_virt_3D_orient.c:59
fusion_exec.h
fsl_fusion_DeviceDescriptor_t::sensorDataReady
int32 sensorDataReady
Definition: fsl_fusion_virt_3D_orient.h:178
fusion_algorithmConfig_t::accelOversampleRatio
uint8 accelOversampleRatio
Definition: fusion_types.h:267
TYPE_TEMPERATURE
Definition: isf_sensor_types.h:95
fsl_fusion_DeviceDescriptor_t::fusionState
fusion_state_t fusionState
Definition: fsl_fusion_virt_3D_orient.h:179
TYPE_ALTIMETER
Definition: isf_sensor_types.h:42
isf_Orientation3D_Quaterion_EngFloat_t::yvect
isf_orientation_quaternion_float_t yvect
Definition: isf_orientation_types.h:44
fsl_fusion_virt_3D_orient_Shutdown
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 ...
Definition: fsl_fusion_virt_3D_orient.c:759
init_sensor
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.
Definition: isf_dsa_direct.c:41
isf_SensorTypes_t
isf_SensorTypes_t
Definition: isf_sensor_types.h:14
fusion_algorithmConfig_t::magOversampleRatio
uint8 magOversampleRatio
Definition: fusion_types.h:268
isf_dsa_direct.h
isf_dsa_ControlData_st::pEventGroup
event_t * pEventGroup
Definition: isf_dsa_adapter.h:92
MAG_HAL_Y_INDEX
#define MAG_HAL_Y_INDEX
Definition: fusion_config.h:46
isf_Orientation3D_Quaterion_EngFloat_t::scalar
isf_orientation_quaternion_float_t scalar
Definition: isf_orientation_types.h:42
isf_SubscriptionSettings_t::nFifoDepth
uint8 nFifoDepth
1 = no FIFO, or another value < SM_MAX_FIFO_DEPTH
Definition: isf_dsa_direct_types.h:28
isf_Orientation3D_Rotation_EngFloat_t
Definition: isf_orientation_types.h:62
fsl_orientation_DataBuffer_t::fOmega
float fOmega[3]
Definition: fsl_fusion_virt_3D_orient.h:109
Q9
Definition: fusion_types.h:51
isf_types.h
The isf_types.h file contains the ISF data type definitions and some of the globally used macros...
DSA_ERR_INITIALIZE
Definition: isf_dsa_adapter.h:29
fsl_orientation_DataBuffer_t::fPhi
float fPhi
Definition: fsl_fusion_virt_3D_orient.h:98
GYRO_HAL_X_INDEX
#define GYRO_HAL_X_INDEX
Definition: fusion_config.h:49
fsl_orientation_DataBuffer_t::fDelta
float fDelta
Definition: fsl_fusion_virt_3D_orient.h:103
fsl_orientation_DataBuffer_t::fqmErrPl
float fqmErrPl[3]
Definition: fsl_fusion_virt_3D_orient.h:125
TYPE_GYROMETER_3D
Definition: isf_sensor_types.h:30
fsl_fusion_virt_3D_orient_EndData
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.
Definition: fsl_fusion_virt_3D_orient.c:649
Q3G
Definition: fusion_types.h:51
iUpdateMagnetometerBuffer
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:75
DSA_ERR_END_DATA
Definition: isf_dsa_adapter.h:33
HYBRID_SENSOR_DATA_READY_EVENT
#define HYBRID_SENSOR_DATA_READY_EVENT
Definition: fsl_fusion_virt_3D_orient.c:80
isf_AngularVelocity3D_RawCount_t
Standard raw type for three axes accelerometers.
Definition: isf_gyrometer_types.h:55
DSA_STATE_INITIALIZED
Definition: isf_dsa_adapter.h:58
isf_dsa_status_t
int32 isf_dsa_status_t
This is the Sensor Manager API return type definition.
Definition: isf_dsa_adapter.h:21
TYPE_NATIVE_SENSOR_TYPE
Definition: isf_sensor_types.h:15
isf_gyrometer_types.h
The isf_gyrometer_types.h file contains the ISF data type definitions for use with the ISF generic gy...
isf_AngularVelocity3D__float_t
Standard float type for three axis accelerometers.
Definition: isf_gyrometer_types.h:76
isf_Acceleration3D_Float_t::accel
isf_acceleration_g_float_t accel[3]
Definition: isf_accelerometer_types.h:82
int16
signed short int int16
Definition: isf_types.h:73
fsl_orientation_DataBuffer_t::fRmatrix
float fRmatrix[3][3]
Definition: fsl_fusion_virt_3D_orient.h:105
ACCEL_HAL_Z_INDEX
#define ACCEL_HAL_Z_INDEX
Definition: fusion_config.h:42
fsl_orientation_DataBuffer_t::fDisGl
float fDisGl[3]
Definition: fsl_fusion_virt_3D_orient.h:116
isf_fifo_status_t
uint32 isf_fifo_status_t
Definition: isf_fifo.h:16
fsl_orientation_DataBuffer_t::fTp
float fTp
Definition: fsl_fusion_virt_3D_orient.h:113
isf_sensor_types.h
The isf_sensor_types.h contains the enumerated list of sensor types used by ISF.
isf_util.h
The isf_util.h file contains the utility method declarations and macros.
GYRO_HAL_Y_INDEX
#define GYRO_HAL_Y_INDEX
Definition: fusion_config.h:51
GYRO_HAL_Z_INDEX
#define GYRO_HAL_Z_INDEX
Definition: fusion_config.h:53
isf_dsa_result_types_t
enum isf_dsa_result_enums isf_dsa_result_types_t
configure_sensor
int32 configure_sensor(isf_SensorHandle_t *pSensorAdapterHandle, isf_SubscriptionSettings_t *pRequiredSettings, isf_fifo_t *pfifo)
Definition: isf_dsa_direct.c:88
start_sensor
int32 start_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
Definition: isf_dsa_direct.c:112
fsl_orientation_DataBuffer_t::fThe
float fThe
Definition: fsl_fusion_virt_3D_orient.h:99
ACCEL_GET_HAL_Y_VALUE
#define ACCEL_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:39
isf_Orientation3D_Quaterion_EngFloat_t::zvect
isf_orientation_quaternion_float_t zvect
Definition: isf_orientation_types.h:45
isf_fifo_init
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
TYPE_MAGNETIC_FIELD_STRENGTH_3D
Definition: isf_sensor_types.h:80
fusion_state_t::algorithmToUse
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
fusion_algorithmConfig_t::pressureOversampleRatio
uint8 pressureOversampleRatio
Definition: fusion_types.h:270
isf_AngularVelocity3D_RawCount_t::angularVelocity
isf_gyrometer_dps_raw_16count_t angularVelocity[3]
Definition: isf_gyrometer_types.h:57
fsl_fusion_DeviceDescriptor_t::MagCalEventStruct
event_t MagCalEventStruct
Definition: fsl_fusion_virt_3D_orient.h:150
Z
Definition: matrix.h:40
OSA_TASK_DEFINE
OSA_TASK_DEFINE(fsl_fusion, 2000)
fusion_config.h
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
isf_dsa_SensorSettings_st::resultFormat
isf_dsa_result_types_t resultFormat
Definition: isf_dsa_adapter.h:84
fsl_fusion_Sensor_Specific_Settings_t::algorithmConfig
fusion_algorithmConfig_t algorithmConfig
Definition: fsl_fusion_virt_3D_orient.h:130
TYPE_ORIENTATION
Definition: isf_sensor_types.h:33
fusion_types.h
isf_SensorDataTypes_t
isf_SensorDataTypes_t
Definition: isf_sensor_types.h:61
MAG_GET_HAL_Y_VALUE
#define MAG_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:45
isf_Acceleration3D_RawCount_t::timestamp
isf_timestamp_t timestamp
Definition: isf_accelerometer_types.h:59
TYPE_MAGNETOMETER_3D
Definition: isf_sensor_types.h:25
PRESSURE_DATA_READY_EVENT
#define PRESSURE_DATA_READY_EVENT
Definition: fsl_fusion_virt_3D_orient.c:71
TYPE_ALTITUDE
Definition: isf_sensor_types.h:98
isf_altitude_types.h
The isf_altitude_types.h file contains the ISF data type definitions for use with the ISF generic alt...
isf_temperature_types.h
The isf_temperature_fixed_t.h file contains the ISF data type definitions for use with the ISF generi...
fsl_orientation_DataBuffer_t::fRho
float fRho
Definition: fsl_fusion_virt_3D_orient.h:101
fsl_orientation_DataBuffer_t::fDeltaPl
float fDeltaPl
Definition: fsl_fusion_virt_3D_orient.h:124
isf_Orientation3D_Rotation_EngFloat_t::rMatrix
isf_orientation_rot_float_t rMatrix[3][3]
Definition: isf_orientation_types.h:64
isf_fifo_t
Definition: isf_fifo.h:19
isf_SensorHandle_st::pSensorStaticConfig
const isf_SensorConfig_t * pSensorStaticConfig
Definition: isf_dsa_adapter.h:110
Y
Definition: matrix.h:39
convert_sensor_data
int32 convert_sensor_data(isf_SensorHandle_t *pSensorAdapterHandle, isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType, void *nativeSample, void *convertedSample)
Definition: isf_dsa_direct.c:133
GYRO_DATA_READY_EVENT
#define GYRO_DATA_READY_EVENT
Definition: fsl_fusion_virt_3D_orient.c:65
isf_SubscriptionSettings_t::resultType
isf_SensorDataTypes_t resultType
The desired data type of the subscription.
Definition: isf_dsa_direct_types.h:24
fusion_state_t
Definition: fusion_exec.h:36
fquaternion::q2
float q2
Definition: fusion_types.h:62
isf_MagneticFieldStrength3D_float_t::timestamp
isf_timestamp_t timestamp
Definition: isf_magnetometer_types.h:81
isf_dsa_SensorSettings_st::resultType
isf_SensorDataTypes_t resultType
Definition: isf_dsa_adapter.h:83
isf.h
Main ISF header file. Contains code common to all ISF components.
DSA_ERR_VALIDATE_SETTINGS
Definition: isf_dsa_adapter.h:30
TYPE_ACCELEROMETER_3D
Definition: isf_sensor_types.h:20
isf_SensorHandle_st::adapterStatus
isf_dsa_AdapterStatus_t adapterStatus
Definition: isf_dsa_adapter.h:112
int32
signed long int int32
Definition: isf_types.h:74
uint16
unsigned short int uint16
Definition: isf_types.h:77
DSA_STATE_CONFIGURED_STOPPED
Definition: isf_dsa_adapter.h:59
fsl_fusion_virt_3D_orient_ValidateSettings
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...
Definition: fsl_fusion_virt_3D_orient.c:322
DSA_ERR_PARAM
Definition: isf_dsa_adapter.h:36
fsl_fusion_virt_3D_orient_PeriodicCallback
void fsl_fusion_virt_3D_orient_PeriodicCallback(void *pSensorHandle)
The orientation sensor adapter's periodic processing function.
Definition: fsl_fusion_virt_3D_orient.c:801
isf_SubscriptionSettings_t::nSettingsToUse
uint8 nSettingsToUse
1 = current; 2=given; 3=best possible
Definition: isf_dsa_direct_types.h:27
isf_Acceleration3D_Float_t
Standard float type for three axis accelerometers.
Definition: isf_accelerometer_types.h:80
isf_orientation_types.h
fsl_fusion_virt_3D_orient_StartData
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.
Definition: fsl_fusion_virt_3D_orient.c:555
GYRO_GET_HAL_Y_VALUE
#define GYRO_GET_HAL_Y_VALUE(v)
Definition: fusion_config.h:50
DSA_RESULT_TYPE_ENG_FLOAT
Definition: isf_dsa_adapter.h:49
isf_SubscriptionSettings_t::nSamplePeriod
uint32 nSamplePeriod
Sample period in microseconds.
Definition: isf_dsa_direct_types.h:26
fsl_fusion_virt_3D_orient.h
isf_SubscriptionSettings_t
Definition: isf_dsa_direct_types.h:22
isf_DegreesCelsius1D_float_t::timestamp
isf_timestamp_t timestamp
Definition: isf_temperature_types.h:36
isf_dsa_ControlData_st::nEventFieldIndex
uint32 nEventFieldIndex
Definition: isf_dsa_adapter.h:93
ACCEL_DATA_READY_EVENT
#define ACCEL_DATA_READY_EVENT
Definition: fsl_fusion_virt_3D_orient.c:53
isf_dsa_SensorSettings_st::pSampleBufferInfo
isf_fifo_t * pSampleBufferInfo
Definition: isf_dsa_adapter.h:81
GYRO_GET_HAL_X_VALUE
#define GYRO_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:48
TYPE_ROTATIONAL_RATE_3D
Definition: isf_sensor_types.h:85
MAGCAL_EVENT_FLAG
#define MAGCAL_EVENT_FLAG
Definition: fsl_fusion_virt_3D_orient.c:74
isf_Orientation3D_Rotation_EngFloat_t::timestamp
isf_timestamp_t timestamp
Definition: isf_orientation_types.h:63
GYRO_GET_HAL_Z_VALUE
#define GYRO_GET_HAL_Z_VALUE(v)
Definition: fusion_config.h:52
isf_AngularVelocity3D__float_t::angularVelocity
isf_gyrometer_dps_float_t angularVelocity[3]
Definition: isf_gyrometer_types.h:78
isf_status_t
int32 isf_status_t
ISF return status type.
Definition: isf.h:76
fsl_fusion_task
void fsl_fusion_task(uint32_t task_init_data)
Definition: fsl_fusion_virt_3D_orient.c:1144
fsl_orientation_DataBuffer_t::fbPl
float fbPl[3]
Definition: fsl_fusion_virt_3D_orient.h:123
isf_fifo.h
isf_Meters1D_float_t
Standard floating point type for single axis altitude sensor.
Definition: isf_altitude_types.h:36
MagCal_Run
void MagCal_Run(fusion_state_t *pState)
Definition: fusion_exec.c:268
DSA_STATE_NOT_INITIALIZED
Definition: isf_dsa_adapter.h:57
isf_dsa_SensorSettings_st
This defines the DSA sensor configuration parameter structure configuring the sensor settings by a su...
Definition: isf_dsa_adapter.h:79
isf_comm.h
isf_comm.h defines the common types for the Communications Service Family of the Intelligent Sensing ...
fsl_orientation_DataBuffer_t::timestamp
isf_timestamp_t timestamp
Definition: fsl_fusion_virt_3D_orient.h:95
isf_dsa_ControlData_st::sensorSettings
isf_dsa_SensorSettings_t sensorSettings
Definition: isf_dsa_adapter.h:94
fquaternion::q0
float q0
Definition: fusion_types.h:60
TYPE_DIR_COSINE_MATRIX
Definition: isf_sensor_types.h:92
MAG_GET_HAL_X_VALUE
#define MAG_GET_HAL_X_VALUE(v)
Definition: fusion_config.h:43
ISF_FIFO_NO_MORE_ENTRIES
#define ISF_FIFO_NO_MORE_ENTRIES
Definition: isf_fifo.h:34
isf_fifo_el_get_insert_pointer
void * isf_fifo_el_get_insert_pointer(isf_fifo_t *pFifo)
Routine returns the insert pointer for direct access.
Definition: isf_fifo.c:229
fquaternion::q3
float q3
Definition: fusion_types.h:63
DSA_ERR_START_DATA
Definition: isf_dsa_adapter.h:32
uint32
unsigned long int uint32
Definition: isf_types.h:78
fsl_fusion_virt_3D_orient_Convert
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.
Definition: fsl_fusion_virt_3D_orient.c:1257
MAG_HAL_Z_INDEX
#define MAG_HAL_Z_INDEX
Definition: fusion_config.h:47
shutdown_sensor
int32 shutdown_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
Definition: isf_dsa_direct.c:126
fsl_orientation_DataBuffer_t::fRVec
float fRVec[3]
Definition: fsl_fusion_virt_3D_orient.h:107
fsl_orientation_DataBuffer_t::fq
struct fquaternion fq
Definition: fsl_fusion_virt_3D_orient.h:106
TYPE_RAW_ACCELERATION_3D
Definition: isf_sensor_types.h:71
isf_accelerometer_types.h
The isf_accelerometer_types.h file contains the ISF data type definitions for use with the ISF generi...
TYPE_EULER_3D
Definition: isf_sensor_types.h:91
ACCEL_HAL_Y_INDEX
#define ACCEL_HAL_Y_INDEX
Definition: fusion_config.h:40
OVERSAMPLE_RATIO
#define OVERSAMPLE_RATIO
Definition: fusion_config.h:21
TYPE_NATIVE_SENSOR_DATA_TYPE
Definition: isf_sensor_types.h:63
ISF_FIFO_FULL
#define ISF_FIFO_FULL
Definition: isf_fifo.h:33
isf_Orientation3D_Euler_EngFloat_t
Definition: isf_orientation_types.h:52
isf_fifo_el_traverse
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_Acceleration3D_RawCount_t::accel
isf_acceleration_g_rawCount_t accel[3]
Definition: isf_accelerometer_types.h:60
isf_Orientation3D_Quaterion_EngFloat_t
Definition: isf_orientation_types.h:39
fsl_fusion_virt_3D_orient_Calibrate
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 ...
Definition: fsl_fusion_virt_3D_orient.c:742
int8
signed char int8
Definition: isf_types.h:72
- © Freescale Semiconductor, Inc. 2016. All Rights Reserved.