| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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