38 #include "fsl_os_abstraction.h"
44 #include "Ac_Fixed_utils.h"
50 #ifdef USE_ACCELEROMETER
51 # define ACCEL_DATA_READY_EVENT ((uint32)(1 << 0))
53 # define ACCEL_DATA_READY_EVENT (0L)
56 #ifdef USE_MAGNETOMETER
57 # define MAG_DATA_READY_EVENT ((uint32)(1 << 1))
59 # define MAG_DATA_READY_EVENT (0L)
63 # define GYRO_DATA_READY_EVENT ((uint32)(1 << 2))
65 # define GYRO_DATA_READY_EVENT (0L)
68 #ifdef USE_PRESSURE_SENSOR
69 # define PRESSURE_DATA_READY_EVENT ((uint32)(1 << 3))
71 # define PRESSURE_DATA_READY_EVENT (0L)
74 #define MAGCAL_EVENT_FLAG (1L)
76 #define PRESSURE_DATA_MASK ~PRESSURE_DATA_READY_EVENT
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
119 #ifdef USE_ACCELEROMETER
122 #ifdef USE_MAGNETOMETER
128 #ifdef USE_PRESSURE_SENSOR
133 static void set_current_sample_buffer(
162 if (NULL == pSensorHandle) {
178 if (NULL == pDeviceDescriptor) {
184 if (kStatus_OSA_Success != OSA_SemaCreate(&(pDeviceDescriptor->
deviceSemaphore), 1)){
191 if (kStatus_OSA_Success != OSA_EventCreate(&(pDeviceDescriptor->
MagCalEventStruct), kEventAutoClear)){
197 if (kStatus_OSA_Success != OSA_EventCreate(&(pDeviceDescriptor->
sensorLWEvent), kEventAutoClear)){
201 #ifdef USE_ACCELEROMETER
207 #ifdef USE_MAGNETOMETER
215 #ifdef USE_PRESSURE_SENSOR
219 #ifdef USE_ACCELEROMETER
223 &(pDeviceDescriptor->accelFifo),
231 #ifdef USE_MAGNETOMETER
238 &(pDeviceDescriptor->magFifo),
249 &(pDeviceDescriptor->gyroFifo),
256 #ifdef USE_PRESSURE_SENSOR
259 &(pDeviceDescriptor->pressureFifo),
261 pDeviceDescriptor->pressureHandle.pSensorStaticConfig->pAdapter->devInfo.nNativeDataSetSize,
268 #if defined(USE_ACCELEROMETER) && defined(USE_MAGNETOMETER)
269 if ( pSensorSpecificConfig->magnetometerSensorId != pSensorSpecificConfig->accelerometerSensorId )
283 (uint8_t *)
"Fusion_task",
287 (task_param_t)(pSensorHandle),
289 &fsl_fusion_task_handler);
293 (uint8_t *)
"MagCal_task",
297 (task_param_t)(pSensorHandle),
299 &fsl_MagCal_task_handler);
329 if ((NULL == pSensorHandle) || (NULL == pSensorSettings)) {
340 samplePeriod = (pSensorSettings->
nSamplePeriod) / (pSpecificSettings->algorithmConfig.gyroOversampleRatio);
345 #if defined(USE_ACCELEROMETER) && defined(USE_MAGNETOMETER)
346 if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
348 if (pSpecificSettings->algorithmConfig.magOversampleRatio != pSpecificSettings->algorithmConfig.accelOversampleRatio )
356 retStat = pDeviceDescriptor->gyroHandle.pSensorStaticConfig->pAdapter->control.ValidateSettings(&(pDeviceDescriptor->gyroHandle), &settings);
402 if ((NULL == pSensorHandle) || (NULL == pSensorSettings)) {
412 if ((NULL == pDeviceDescriptor) || (NULL == pSpecificSettings))
423 goto unlockdescriptor;
426 #ifdef USE_ACCELEROMETER
438 &(pDeviceDescriptor->accelHandle),
440 &(pDeviceDescriptor->accelFifo));
445 &(pDeviceDescriptor->accelHandle),
447 &(pDeviceDescriptor->accelFifo));
452 #ifdef USE_MAGNETOMETER
468 &pDeviceDescriptor->magHandle,
470 &pDeviceDescriptor->magFifo
475 &(pDeviceDescriptor->magHandle),
477 &(pDeviceDescriptor->magFifo));
493 &(pDeviceDescriptor->gyroHandle),
495 &(pDeviceDescriptor->gyroFifo)
501 goto unlockdescriptor;
506 #ifdef USE_PRESSURE_SENSOR
517 &pDeviceDescriptor->pressureHandle,
519 &pDeviceDescriptor->pressureFifo
524 &(pDeviceDescriptor->pressureHandle),
526 &(pDeviceDescriptor->pressureFifo));
558 if (NULL == pSensorHandle) {
568 if (NULL == pDeviceDescriptor)
580 goto unlockdescriptor;
583 #ifdef USE_MAGNETOMETER
594 #ifdef USE_PRESSURE_SENSOR
598 pst =
start_sensor(&(pDeviceDescriptor->pressureHandle));
604 #ifdef USE_ACCELEROMETER
652 if (NULL == pSensorHandle) {
662 if (NULL == pDeviceDescriptor)
674 goto unlockdescriptor;
677 #ifdef USE_MAGNETOMETER
681 mst =
stop_sensor(&(pDeviceDescriptor->magHandle));
687 #ifdef USE_PRESSURE_SENSOR
691 pst =
stop_sensor(&(pDeviceDescriptor->pressureHandle));
697 #ifdef USE_ACCELEROMETER
701 ast =
stop_sensor(&(pDeviceDescriptor->accelHandle));
711 gst =
stop_sensor(&(pDeviceDescriptor->gyroHandle));
761 if(NULL == pSensorHandle){
769 #ifdef USE_MAGNETOMETER
773 #ifdef USE_PRESSURE_SENSOR
777 #ifdef USE_ACCELEROMETER
816 if (pState->softReset)
819 pState->softReset = 0;
822 if (pState->softRINS)
825 #ifdef COMPUTE_9DOF_GBY_KALMAN
826 for (i = 0; i <= 2; i++)
828 pState->thisSV_9DOF_GBY_KALMAN.fVelGl[i] = 0.0F;
829 pState->thisSV_9DOF_GBY_KALMAN.fDisGl[i] = 0.0F;
831 pState->softRINS = 0;
835 #ifdef USE_ACCELEROMETER
840 pAccelFifo = &(pDesc->accelFifo);
849 int8 sampleCount = 0;
857 pSample = pSampleIter;
860 for (
int8 index =
X; index <=
Z; index++)
863 if (pSample->
accel[index] == -32768) pSample->
accel[index]++;
869 sumX += pState->thisAccel.iGs[
X];
870 sumY += pState->thisAccel.iGs[
Y];
871 sumZ += pState->thisAccel.iGs[
Z];
876 pState->accelTimestamp = pSample->
timestamp;
881 pState->thisAccel.iGsAvg[
X] = sumX / sampleCount;
882 pState->thisAccel.iGsAvg[
Y] = sumY / sampleCount;
883 pState->thisAccel.iGsAvg[
Z] = sumZ / sampleCount;
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;
892 #ifdef USE_MAGNETOMETER
902 int8 sampleCount = 0;
911 pSample = pSampleIter;
913 for (
int8 index =
X; index <=
Z; index++)
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];
932 pState->magTimestamp = pSample->
timestamp;
940 if (!pState->thisMagCal.iCalInProgress)
949 for (i =
X; i <=
Z; i++)
954 iSum[i] += (
int32)pState->thisMag.iBsBuffer[j][i];
956 for (i =
X; i <=
Z; i++)
960 if (abs((
int32)pState->thisMag.iBsBuffer[j][i] * OVERSAMPLE_RATIO - iSum[i]) >= itmp)
964 itmp = abs((
int32)pState->thisMag.iBsBuffer[j][i] * OVERSAMPLE_RATIO - iSum[i]);
973 for (i =
X; i <=
Z; i++)
975 pState->thisMag.iBsAvg[i] = pState->thisMag.iBsBuffer[0][i];
981 for (i =
X; i <=
Z; i++)
986 if (!((i == k) && (j == l)))
987 iSum[i] += (
int32)pState->thisMag.iBsBuffer[j][i];
991 for (i =
X; i <=
Z; i++)
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);
1020 #ifdef USE_PRESSURE_SENSOR
1026 void *pSampleIter = 0;
1036 pState->thisPressure.fHp = pressureSample.
altitude;
1037 pState->thisPressure.fTp = temperatureSample.
temperature;
1040 pState->pressureTimestamp = pressureSample.
timestamp;
1048 #ifdef USE_GYROMETER
1062 for (
int8 index =
X; index <=
Z; index++)
1074 pState->gyroTimestamp = pGyroSample->
timestamp;
1084 int8 initiatemagcal;
1088 initiatemagcal =
Fusion_Run(pState, &(pSpecificSettings->algorithmConfig));
1109 set_current_sample_buffer(¤tSampleBuffer, pState, pSpecificSettings);
1115 ¤tSampleBuffer,
1148 event_flags_t signalledEvents = 0;
1151 if (NULL == pSensorHandle) {
1164 FXOS8700_Mag_Init(pDesc);
1165 #elif SF_MAG_MAG3110
1166 MAG3100_Init(pDesc);
1168 #if SF_GYRO_FXAS21002
1169 FXAS21000_Init(pDesc);
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);
1184 uint32_t readyFlags;
1188 if ( pSpecificSettings->magnetometerSensorId == pSpecificSettings->accelerometerSensorId )
1227 uint32 signalledEvents = 0;
1244 #if defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
1260 void *pNativeSample,
1261 void *pConvertedSample,
1275 switch (convertToType)
1277 #ifdef USE_ACCELEROMETER
1279 pConverter = accel_converter;
1282 #ifdef USE_MAGNETOMETER
1284 pConverter = mag_converter;
1287 #ifdef USE_GYROMETER
1289 pConverter = gyro_converter;
1293 pConverter = quaternion_converter;
1296 pConverter = euler_converter;
1299 pConverter = dircosine_converter;
1301 #ifdef USE_PRESSURE_SENSOR
1303 pConverter = altitude_converter;
1306 pConverter = temperature_converter;
1313 if (pConverter == NULL) {
1317 retStat = pConverter(
1319 (fsl_orientation_DataBuffer_t *)pNativeSample,
1330 convertedSample->
scalar = nativeSample->
fq.
q0;
1331 convertedSample->
xvect = nativeSample->
fq.
q1;
1332 convertedSample->
yvect = nativeSample->
fq.
q2;
1333 convertedSample->
zvect = nativeSample->
fq.
q3;
1341 convertedSample->
fThe = nativeSample->
fThe;
1342 convertedSample->
fPhi = nativeSample->
fPhi;
1343 convertedSample->
fPsi = nativeSample->
fPsi;
1363 #ifdef USE_ACCELEROMETER
1368 convertedSample->
accel[0] = nativeSample->accelData.accel[0];
1369 convertedSample->
accel[1] = nativeSample->accelData.accel[1];
1370 convertedSample->
accel[2] = nativeSample->accelData.accel[2];
1374 #ifdef USE_MAGNETOMETER
1379 convertedSample->
fieldStrength[0] = nativeSample->magData.fieldStrength[0];
1380 convertedSample->
fieldStrength[1] = nativeSample->magData.fieldStrength[1];
1381 convertedSample->
fieldStrength[2] = nativeSample->magData.fieldStrength[2];
1385 #ifdef USE_GYROMETER
1390 convertedSample->
angularVelocity[0] = nativeSample->gyroData.angularVelocity[0];
1391 convertedSample->
angularVelocity[1] = nativeSample->gyroData.angularVelocity[1];
1392 convertedSample->
angularVelocity[2] = nativeSample->gyroData.angularVelocity[2];
1397 #ifdef USE_PRESSURE_SENSOR
1413 void set_current_sample_buffer(
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;
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;
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];
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];
1446 pCurrentSampleBuffer->magData.
timestamp= pState->magTimestamp;
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;
1457 #ifdef COMPUTE_3DOF_G_BASIC
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;
1471 pCurrentSampleBuffer->
fRVec[i] = pState->thisSV_3DOF_G_BASIC.fLPRVec[i];
1474 pCurrentSampleBuffer->
fOmega[i] = pState->thisSV_3DOF_G_BASIC.fOmega[i];
1476 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1477 pCurrentSampleBuffer->
fqgErrPl[i] = 0.0;
1478 pCurrentSampleBuffer->
fbPl[i] = 0.0;
1479 pCurrentSampleBuffer->
fqmErrPl[i] = 0.0;
1482 pCurrentSampleBuffer->
fRmatrix[i][j] = pState->thisSV_3DOF_G_BASIC.fLPR[i][j];
1488 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1491 pCurrentSampleBuffer->
timestamp = pState->accelTimestamp;
1495 #ifdef COMPUTE_3DOF_B_BASIC
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;
1508 pCurrentSampleBuffer->
fRVec[i] = pState->thisSV_3DOF_B_BASIC.fLPRVec[i];
1511 pCurrentSampleBuffer->
fOmega[i] = pState->thisSV_3DOF_B_BASIC.fOmega[i];
1513 pCurrentSampleBuffer->
fAccGl[i] = 0.0;
1514 pCurrentSampleBuffer->
fDisGl[i] = 0.0;
1516 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1517 pCurrentSampleBuffer->
fqgErrPl[i] = 0.0;
1518 pCurrentSampleBuffer->
fbPl[i] = 0.0;
1519 pCurrentSampleBuffer->
fqmErrPl[i] = 0.0;
1522 pCurrentSampleBuffer->
fRmatrix[i][j] = pState->thisSV_3DOF_B_BASIC.fLPR[i][j];
1528 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1530 pCurrentSampleBuffer->
timestamp = pState->magTimestamp;
1533 #ifdef COMPUTE_3DOF_Y_BASIC
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;
1546 pCurrentSampleBuffer->
fRVec[i] = pState->thisSV_3DOF_Y_BASIC.fRVec[i];
1549 pCurrentSampleBuffer->
fOmega[i] = pState->thisSV_3DOF_Y_BASIC.fOmega[i];
1551 pCurrentSampleBuffer->
fAccGl[i] = 0.0;
1552 pCurrentSampleBuffer->
fDisGl[i] = 0.0;
1554 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1555 pCurrentSampleBuffer->
fqgErrPl[i] = 0.0;
1556 pCurrentSampleBuffer->
fbPl[i] = 0.0;
1557 pCurrentSampleBuffer->
fqmErrPl[i] = 0.0;
1560 pCurrentSampleBuffer->
fRmatrix[i][j] = pState->thisSV_3DOF_Y_BASIC.fR[i][j];
1566 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1568 pCurrentSampleBuffer->
timestamp = pState->gyroTimestamp;
1572 #ifdef COMPUTE_6DOF_GB_BASIC
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;
1585 pCurrentSampleBuffer->
fRVec[i] = pState->thisSV_6DOF_GB_BASIC.fLPRVec[i];
1588 pCurrentSampleBuffer->
fOmega[i] = pState->thisSV_6DOF_GB_BASIC.fOmega[i];
1590 pCurrentSampleBuffer->
fAccGl[i] = 0.0;
1591 pCurrentSampleBuffer->
fDisGl[i] = 0.0;
1593 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1594 pCurrentSampleBuffer->
fqgErrPl[i] = 0.0;
1595 pCurrentSampleBuffer->
fbPl[i] = 0.0;
1596 pCurrentSampleBuffer->
fqmErrPl[i] = 0.0;
1600 pCurrentSampleBuffer->
fRmatrix[i][j] = pState->thisSV_6DOF_GB_BASIC.fLPR[i][j];
1606 pCurrentSampleBuffer->
fZErr[i] = 0.0;
1608 if (pState->accelTimestamp > pState->magTimestamp)
1610 pCurrentSampleBuffer->
timestamp = pState->accelTimestamp;
1614 pCurrentSampleBuffer->
timestamp = pState->magTimestamp;
1618 #ifdef COMPUTE_6DOF_GY_KALMAN
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;
1631 pCurrentSampleBuffer->
fRVec[i] = pState->thisSV_6DOF_GY_KALMAN.fRVecPl[i];
1634 pCurrentSampleBuffer->
fOmega[i] = pState->thisSV_6DOF_GY_KALMAN.fOmega[i];
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];
1644 pCurrentSampleBuffer->
fRmatrix[i][j] = pState->thisSV_6DOF_GY_KALMAN.fRPl[i][j];
1650 pCurrentSampleBuffer->
fZErr[i] = pState->thisSV_6DOF_GY_KALMAN.fZErr[i];
1653 pCurrentSampleBuffer->
timestamp = pState->gyroTimestamp;
1658 #ifdef COMPUTE_9DOF_GBY_KALMAN
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;
1672 pCurrentSampleBuffer->
fRVec[i] = pState->thisSV_9DOF_GBY_KALMAN.fRVecPl[i];
1674 pCurrentSampleBuffer->
fOmega[i] = pState->thisSV_9DOF_GBY_KALMAN.fOmega[i];
1676 pCurrentSampleBuffer->
fAccGl[i] = pState->thisSV_9DOF_GBY_KALMAN.fAccGl[i];
1677 pCurrentSampleBuffer->
fDisGl[i] = pState->thisSV_9DOF_GBY_KALMAN.fDisGl[i];
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];
1686 pCurrentSampleBuffer->
fRmatrix[i][j] = pState->thisSV_9DOF_GBY_KALMAN.fRPl[i][j];
1692 pCurrentSampleBuffer->
fZErr[i] = pState->thisSV_9DOF_GBY_KALMAN.fZErr[i];
1695 pCurrentSampleBuffer->
timestamp = pState->gyroTimestamp;
isf_timestamp_t timestamp
isf_orientation_euler_deg_float_t fPsi
void Fusion_Init(fusion_state_t *pState)
#define ACCEL_HAL_X_INDEX
isf_temperature_degC_float_t temperature
#define PRESSURE_DATA_MASK
Standard raw type for three axes accelerometers.
void * pSensorSpecificSettings
isf_dsa_status_t fsl_fusion_virt_3D_orient_Initialize(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter initialization.
#define ACCEL_GET_HAL_Z_VALUE(v)
isf_timestamp_t timestamp
isf_orientation_euler_deg_float_t fThe
int32 stop_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
isf_timestamp_t timestamp
Standard fixed type for three axis accelerometers.
The isf_magnetometer_types.h file contains the ISF data type definitions for use with the ISF generic...
#define ACCEL_GET_HAL_X_VALUE(v)
isf_SensorDataTypes_t fsl_fusion_SupportedDataTypes[]
isf_dsa_status_t fsl_fusion_virt_3D_orient_Configure(isf_SensorHandle_t *pSensorHandle, isf_dsa_SensorSettings_t *pSensorSettings)
This is the concrete implementation of the fusion sensor adapter configuration function.
#define SENSOR_DATA_READY_EVENT
isf_timestamp_t timestamp
isf_dsa_ControlData_t controlData
isf_fifo_status_t isf_fifo_lock(isf_fifo_t *pFifo)
Lock a sample buffer for exclusive access.
quaternion_type algorithmToUse
const uint8_t * mqx_task_stack_pointers[]
void fsl_MagCal_task(uint32_t task_init_data)
uint8 gyroOversampleRatio
isf_fifo_status_t isf_fifo_unlock(isf_fifo_t *pFifo)
Release the exclusive access lock on a sample buffer.
isf_orientation_euler_deg_float_t fPhi
isf_microTeslas_raw_16count_t fieldStrength[3]
isf_timestamp_t timestamp
isf_timestamp_t timestamp
Standard floating point type for single axis temperature sensor.
isf_fifo_status_t isf_fifo_el_increment(isf_fifo_t *pFifo)
Routine increments the insert pointer after direct access.
isf_sensors.h contains the ISF Generic Sensor definitions and data structures required when a client ...
isf_SensorTypes_t fsl_fusion_SupportedSensorTypes[]
Supported sensor and data types for the orientation sensor.
isf_fieldStrength_uT_float_t fieldStrength[3]
API definitions, types, and macros for the Intelligent Sensing Framework (ISF) Bus Manager (BM)...
isf_altitude_meters_float_t altitude
isf_orientation_quaternion_float_t xvect
This defines the DSA sensor device handle structure used to invoke the adapter access functions...
isf_fifo_status_t isf_fifo_el_clear(isf_fifo_t *pFifo)
Routine to clear the fifo.
isf_timestamp_t timestamp
isf_dsa_result_types_t resultFormat
The format of the data to be returned- 0=RAW, 1=FIXED, 2=FLOAT.
isf_SensorDataTypes_t TYPE_MAGNETIC_FIELD_STRENGTH_3D
semaphore_t deviceSemaphore
#define MAG_DATA_READY_EVENT
uint8 accelOversampleRatio
fusion_state_t fusionState
isf_orientation_quaternion_float_t yvect
isf_dsa_status_t fsl_fusion_virt_3D_orient_Shutdown(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter for shutdown ...
int32 init_sensor(uint8 nSensorID, isf_SensorHandle_t *pSensorAdapterHandle, event_t *pEventGroup, uint32 nEventFieldIndex)
This function initializes the sensor adapters in the absence of the Sensor Manager.
isf_orientation_quaternion_float_t scalar
uint8 nFifoDepth
1 = no FIFO, or another value < SM_MAX_FIFO_DEPTH
The isf_types.h file contains the ISF data type definitions and some of the globally used macros...
isf_dsa_status_t fsl_fusion_virt_3D_orient_EndData(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter for End Data.
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
#define HYBRID_SENSOR_DATA_READY_EVENT
Standard raw type for three axes accelerometers.
int32 isf_dsa_status_t
This is the Sensor Manager API return type definition.
The isf_gyrometer_types.h file contains the ISF data type definitions for use with the ISF generic gy...
Standard float type for three axis accelerometers.
isf_acceleration_g_float_t accel[3]
#define ACCEL_HAL_Z_INDEX
The isf_sensor_types.h contains the enumerated list of sensor types used by ISF.
The isf_util.h file contains the utility method declarations and macros.
enum isf_dsa_result_enums isf_dsa_result_types_t
int32 configure_sensor(isf_SensorHandle_t *pSensorAdapterHandle, isf_SubscriptionSettings_t *pRequiredSettings, isf_fifo_t *pfifo)
int32 start_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
#define ACCEL_GET_HAL_Y_VALUE(v)
isf_orientation_quaternion_float_t zvect
isf_fifo_status_t isf_fifo_init(isf_fifo_t *pFifo, void *pData, uint16 sampleSize, uint16 bufferCapacity)
Initializes a new fifo structure with a provided buffer.
quaternion_type algorithmToUse
uint8 pressureOversampleRatio
isf_gyrometer_dps_raw_16count_t angularVelocity[3]
event_t MagCalEventStruct
OSA_TASK_DEFINE(fsl_fusion, 2000)
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
isf_dsa_result_types_t resultFormat
fusion_algorithmConfig_t algorithmConfig
#define MAG_GET_HAL_Y_VALUE(v)
isf_timestamp_t timestamp
#define PRESSURE_DATA_READY_EVENT
The isf_altitude_types.h file contains the ISF data type definitions for use with the ISF generic alt...
The isf_temperature_fixed_t.h file contains the ISF data type definitions for use with the ISF generi...
isf_orientation_rot_float_t rMatrix[3][3]
const isf_SensorConfig_t * pSensorStaticConfig
int32 convert_sensor_data(isf_SensorHandle_t *pSensorAdapterHandle, isf_SensorDataTypes_t convertToType, isf_dsa_result_types_t resultType, void *nativeSample, void *convertedSample)
#define GYRO_DATA_READY_EVENT
isf_SensorDataTypes_t resultType
The desired data type of the subscription.
isf_timestamp_t timestamp
isf_SensorDataTypes_t resultType
Main ISF header file. Contains code common to all ISF components.
isf_dsa_AdapterStatus_t adapterStatus
unsigned short int uint16
isf_dsa_status_t fsl_fusion_virt_3D_orient_ValidateSettings(isf_SensorHandle_t *pSensorHandle, isf_dsa_SensorSettings_t *pSensorSettings)
This is the concrete implementation of the orientation sensor adapter for validating current settings...
void fsl_fusion_virt_3D_orient_PeriodicCallback(void *pSensorHandle)
The orientation sensor adapter's periodic processing function.
uint8 nSettingsToUse
1 = current; 2=given; 3=best possible
Standard float type for three axis accelerometers.
isf_dsa_status_t fsl_fusion_virt_3D_orient_StartData(isf_SensorHandle_t *pSensorHandle)
This is the concrete implementation of the orientation sensor adapter for start Data.
#define GYRO_GET_HAL_Y_VALUE(v)
uint32 nSamplePeriod
Sample period in microseconds.
isf_timestamp_t timestamp
#define ACCEL_DATA_READY_EVENT
isf_fifo_t * pSampleBufferInfo
#define GYRO_GET_HAL_X_VALUE(v)
#define MAGCAL_EVENT_FLAG
isf_timestamp_t timestamp
#define GYRO_GET_HAL_Z_VALUE(v)
isf_gyrometer_dps_float_t angularVelocity[3]
int32 isf_status_t
ISF return status type.
void fsl_fusion_task(uint32_t task_init_data)
Standard floating point type for single axis altitude sensor.
void MagCal_Run(fusion_state_t *pState)
This defines the DSA sensor configuration parameter structure configuring the sensor settings by a su...
isf_comm.h defines the common types for the Communications Service Family of the Intelligent Sensing ...
isf_timestamp_t timestamp
isf_dsa_SensorSettings_t sensorSettings
#define MAG_GET_HAL_X_VALUE(v)
#define ISF_FIFO_NO_MORE_ENTRIES
void * isf_fifo_el_get_insert_pointer(isf_fifo_t *pFifo)
Routine returns the insert pointer for direct access.
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.
int32 shutdown_sensor(isf_SensorHandle_t *pSensorAdapterHandle)
The isf_accelerometer_types.h file contains the ISF data type definitions for use with the ISF generi...
#define ACCEL_HAL_Y_INDEX
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...
isf_acceleration_g_rawCount_t accel[3]
isf_dsa_status_t fsl_fusion_virt_3D_orient_Calibrate(isf_SensorHandle_t *pSensorHandle, void *pCalData)
This is the concrete implementation of the orientation sensor adapter for calibration ...