/**************************************************************************************************** * @fn UnCalMagDataResultCallback * Call back for Uncalibrated magnetometer data * ***************************************************************************************************/ static void UnCalMagDataResultCallback(OutputSensorHandle_t outputHandle, Android_UncalibratedMagOutputData_t* pOutput) { if (g_logging & 0x40) //Uncalibrated data, in Android conventions { Print_LIPS("RM,%.6f,%.6f,%.6f,%.6f", TOFLT_TIME(pOutput->TimeStamp), TOFLT_EXTENDED(pOutput->X), TOFLT_EXTENDED(pOutput->Y), TOFLT_EXTENDED(pOutput->Z)); } }
/****************************************************************************** * @fn GenericDataResultCallback() * Generic callback function for sensor result data * This function should be registered as a callback function when subscribes * for a sensor result from the algorithm library. When result is available * from the algorithm library, it will call this callback function and providing * a pointer pOutput to a structure containing the sensor result. User should * cast this pointer to an appropriate result structure to extract the data. * User should cast the resultHandle to ResultDescriptor_t type to obtain the * sensor type value. */ static void GenericDataResultCallback(ResultHandle_t resultHandle, void* pOutput) { #define MAX_BUFF_SIZE (128) char outBuff[MAX_BUFF_SIZE]; ASensorType_t sensorType; enum MessageIdTag msg_type; MessageBuffer *pSample = NULLP; osp_bool_t sendMessage = TRUE; // A callback with NULL handle should never happen but check it anyway if ( resultHandle == NULL ) { D0_printf("Result callback with NULL handle\r\n"); return; } sensorType = ((ResultDescriptor_t *) resultHandle)->SensorType; // Android sensor result switch (sensorType) { case SENSOR_ACCELEROMETER: { Android_TriAxisPreciseData_t* pData = (Android_TriAxisPreciseData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_CAL_ACC_DATA, sizeof(MsgAccelData), &pSample) == ASF_OK); pSample->msg.msgAccelData.X = pData->X; pSample->msg.msgAccelData.Y = pData->Y; pSample->msg.msgAccelData.Z = pData->Z; pSample->msg.msgAccelData.timeStamp = pData->TimeStamp; if ( g_logging & 0x40 ) { snprintf(outBuff, MAX_BUFF_SIZE, "A, %6.3f, %03.4f, %03.4f, %03.4f", TOFLT_TIME(pData->TimeStamp), TOFLT_PRECISE(pData->X), TOFLT_PRECISE(pData->Y), TOFLT_PRECISE(pData->Z)); } break; } case SENSOR_MAGNETIC_FIELD: { Android_TriAxisExtendedData_t* pData = (Android_TriAxisExtendedData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_CAL_MAG_DATA, sizeof(MsgMagData), &pSample) == ASF_OK); pSample->msg.msgMagData.X = pData->X; pSample->msg.msgMagData.Y = pData->Y; pSample->msg.msgMagData.Z = pData->Z; pSample->msg.msgMagData.timeStamp = pData->TimeStamp; if (g_logging & 0x40) { snprintf(outBuff, MAX_BUFF_SIZE,"M, %6.3f, %03.4f, %03.4f, %03.4f", TOFLT_TIME(pData->TimeStamp), TOFLT_EXTENDED(pData->X), TOFLT_EXTENDED(pData->Y), TOFLT_EXTENDED(pData->Z)); } break; } case SENSOR_GYROSCOPE: { Android_TriAxisPreciseData_t* pData = (Android_TriAxisPreciseData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_CAL_GYRO_DATA, sizeof(MsgGyroData), &pSample) == ASF_OK); pSample->msg.msgGyroData.X = pData->X; pSample->msg.msgGyroData.Y = pData->Y; pSample->msg.msgGyroData.Z = pData->Z; pSample->msg.msgGyroData.timeStamp = pData->TimeStamp; if (g_logging & 0x40) { snprintf(outBuff, MAX_BUFF_SIZE,"G, %6.3f, %03.4f, %03.4f, %03.4f", TOFLT_TIME(pData->TimeStamp), TOFLT_PRECISE(pData->X), TOFLT_PRECISE(pData->Y), TOFLT_PRECISE(pData->Z)); } break; } case SENSOR_ROTATION_VECTOR: case SENSOR_GAME_ROTATION_VECTOR: case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: { Android_RotationVectorResultData_t *pRotVecOut = (Android_RotationVectorResultData_t *)pOutput; switch(sensorType) { case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: msg_type = MSG_GEO_QUATERNION_DATA; break; case SENSOR_GAME_ROTATION_VECTOR: msg_type = MSG_GAME_QUATERNION_DATA; break; case SENSOR_ROTATION_VECTOR: default: msg_type = MSG_QUATERNION_DATA; break; } ASF_assert(ASFCreateMessage(msg_type, sizeof(MsgQuaternionData), &pSample) == ASF_OK); pSample->msg.msgQuaternionData.W = pRotVecOut->W; pSample->msg.msgQuaternionData.X = pRotVecOut->X; pSample->msg.msgQuaternionData.Y = pRotVecOut->Y; pSample->msg.msgQuaternionData.Z = pRotVecOut->Z; pSample->msg.msgQuaternionData.HeadingError = pRotVecOut->HeadingErrorEst; pSample->msg.msgQuaternionData.TiltError = pRotVecOut->TiltErrorEst; pSample->msg.msgQuaternionData.timeStamp = pRotVecOut->TimeStamp; if (g_logging & 0x40 ) { int32_t offset; switch(sensorType) { case SENSOR_ROTATION_VECTOR: snprintf(outBuff, MAX_BUFF_SIZE, "Q "); offset = 2; break; case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: snprintf(outBuff, MAX_BUFF_SIZE, "GC "); offset = 3; break; default: snprintf(outBuff, MAX_BUFF_SIZE, "GV "); offset = 3; break; } snprintf(&outBuff[offset], MAX_BUFF_SIZE - offset, "%6.3f, %3.4f, %3.4f, %3.4f, %3.4f, %3.4f, %3.4f", TOFLT_TIME(pRotVecOut->TimeStamp), TOFLT_PRECISE(pRotVecOut->W), TOFLT_PRECISE(pRotVecOut->X), TOFLT_PRECISE(pRotVecOut->Y), TOFLT_PRECISE(pRotVecOut->Z), TOFLT_PRECISE(pRotVecOut->HeadingErrorEst), TOFLT_PRECISE(pRotVecOut->TiltErrorEst)); } break; } case SENSOR_ORIENTATION: { Android_OrientationResultData_t *pOrientOut = (Android_OrientationResultData_t *)pOutput; ASF_assert(ASFCreateMessage(MSG_ORIENTATION_DATA, sizeof(MsgOrientationData), &pSample) == ASF_OK); pSample->msg.msgOrientationData.X = pOrientOut->Pitch; pSample->msg.msgOrientationData.Y = pOrientOut->Roll; pSample->msg.msgOrientationData.Z = pOrientOut->Yaw; pSample->msg.msgOrientationData.timeStamp = pOrientOut->TimeStamp; if ( g_logging & 0x40 ) { snprintf(outBuff, MAX_BUFF_SIZE,"I, %6.3f, %3.4f, %3.4f, %3.4f", TOFLT_TIME(pOrientOut->TimeStamp), TOFLT_EXTENDED(pOrientOut->Yaw), TOFLT_EXTENDED(pOrientOut->Pitch), TOFLT_EXTENDED(pOrientOut->Roll)); } break; } case SENSOR_GRAVITY: case SENSOR_LINEAR_ACCELERATION: { Android_TriAxisPreciseData_t *pTriAxisOut = (Android_TriAxisPreciseData_t *)pOutput; if (sensorType == SENSOR_GRAVITY) { msg_type = MSG_GRAVITY_DATA; } else { msg_type = MSG_LINEAR_ACCELERATION_DATA; } ASF_assert(ASFCreateMessage(msg_type, sizeof(MsgGenericTriAxisData), &pSample) == ASF_OK); /* msgGravityData and msgLinearAcceleration are a union of the same type */ pSample->msg.msgGravityData.X = pTriAxisOut->X; pSample->msg.msgGravityData.Y = pTriAxisOut->Y; pSample->msg.msgGravityData.Z = pTriAxisOut->Z; pSample->msg.msgGravityData.timeStamp = pTriAxisOut->TimeStamp; if ( g_logging & 0x40 ) { if ( sensorType == SENSOR_GRAVITY ) { snprintf(outBuff, MAX_BUFF_SIZE,"GR: %6.3f, %3.4f, %3.4f, %3.4f", TOFLT_TIME(pTriAxisOut->TimeStamp), TOFLT_PRECISE(pTriAxisOut->X), TOFLT_PRECISE(pTriAxisOut->Y), TOFLT_PRECISE(pTriAxisOut->Z)); } if ( sensorType == SENSOR_LINEAR_ACCELERATION ) { snprintf(outBuff, MAX_BUFF_SIZE,"LN: %6.3f, %3.4f, %3.4f, %3.4f", TOFLT_TIME(pTriAxisOut->TimeStamp), TOFLT_PRECISE(pTriAxisOut->X), TOFLT_PRECISE(pTriAxisOut->Y), TOFLT_PRECISE(pTriAxisOut->Z)); } } break; } case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: { Android_UncalibratedTriAxisExtendedData_t *pData = (Android_UncalibratedTriAxisExtendedData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_MAG_DATA, sizeof(MsgMagData), &pSample) == ASF_OK); pSample->msg.msgMagData.X = pData->X; pSample->msg.msgMagData.Y = pData->Y; pSample->msg.msgMagData.Z = pData->Z; pSample->msg.msgMagData.timeStamp = pData->TimeStamp; if (g_logging & 0x40) { snprintf(outBuff, MAX_BUFF_SIZE,"RM, %6.3f, %03.4f, %03.4f, %03.4f", TOFLT_TIME(pData->TimeStamp), TOFLT_EXTENDED(pData->X), TOFLT_EXTENDED(pData->Y), TOFLT_EXTENDED(pData->Z)); } break; } case SENSOR_GYROSCOPE_UNCALIBRATED: { Android_UncalibratedTriAxisPreciseData_t *pData = (Android_UncalibratedTriAxisPreciseData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_GYRO_DATA, sizeof(MsgGyroData), &pSample) == ASF_OK); pSample->msg.msgGyroData.X = pData->X; pSample->msg.msgGyroData.Y = pData->Y; pSample->msg.msgGyroData.Z = pData->Z; pSample->msg.msgGyroData.timeStamp = pData->TimeStamp; if (g_logging & 0x40) { snprintf(outBuff, MAX_BUFF_SIZE,"RG, %6.3f, %03.4f, %03.4f, %03.4f", TOFLT_TIME(pData->TimeStamp), TOFLT_EXTENDED(pData->X), TOFLT_EXTENDED(pData->Y), TOFLT_EXTENDED(pData->Z)); } break; } case SENSOR_SIGNIFICANT_MOTION: { Android_BooleanResultData_t *pData = (Android_BooleanResultData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_SIG_MOTION_DATA, sizeof(MsgSigMotionData), &pSample) == ASF_OK); pSample->msg.msgSigMotionData.active = pData->data; pSample->msg.msgSigMotionData.timeStamp = pData->TimeStamp; snprintf(outBuff, MAX_BUFF_SIZE,"SM,%+03.4f,%d", TOFLT_TIME(pData->TimeStamp), pData->data); break; } case SENSOR_STEP_COUNTER: { Android_StepCounterResultData_t *pData = (Android_StepCounterResultData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_STEP_COUNT_DATA, sizeof(MsgStepData), &pSample) == ASF_OK); pSample->msg.msgStepCountData.X = pData->StepCount; pSample->msg.msgStepCountData.Y = 0; // not use pSample->msg.msgStepCountData.Z = 0; // not use pSample->msg.msgStepCountData.timeStamp = pData->TimeStamp; snprintf(outBuff, MAX_BUFF_SIZE,"SC,%+03.4f,%d,0", TOFLT_TIME(pData->TimeStamp), pData->StepCount); break; } case SENSOR_STEP_DETECTOR: { Android_BooleanResultData_t *pData = (Android_BooleanResultData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_STEP_DETECT_DATA, sizeof(MsgStepDetData), &pSample) == ASF_OK); pSample->msg.msgStepDetData.active = TRUE; pSample->msg.msgStepDetData.timeStamp = pData->TimeStamp; snprintf(outBuff, MAX_BUFF_SIZE,"SD,%+03.4f", TOFLT_TIME(pData->TimeStamp)); break; } case AP_PSENSOR_ACCELEROMETER_UNCALIBRATED: { Android_UncalibratedTriAxisPreciseData_t *pData = (Android_UncalibratedTriAxisPreciseData_t *) pOutput; ASF_assert(ASFCreateMessage(MSG_ACC_DATA, sizeof(MsgAccelData), &pSample) == ASF_OK); pSample->msg.msgAccelData.X = pData->X; pSample->msg.msgAccelData.Y = pData->Y; pSample->msg.msgAccelData.Z = pData->Z; pSample->msg.msgAccelData.timeStamp = pData->TimeStamp; if (g_logging & 0x40) { snprintf(outBuff, MAX_BUFF_SIZE,"RA, %6.3f, %03.4f, %03.4f, %03.4f", TOFLT_TIME(pData->TimeStamp), TOFLT_PRECISE(pData->X), TOFLT_PRECISE(pData->Y), TOFLT_PRECISE(pData->Z)); } break; } default: D0_printf("%s not handling sensor type %i\r\n", __FUNCTION__, sensorType); sendMessage = FALSE; break; } /* Now send the created message to the I2C slave task to route to host.*/ if ( sendMessage ) { if (!(ASFSendMessage(I2CSLAVE_COMM_TASK_ID, pSample) == ASF_OK)) { ; } if ( g_logging & 0x40) Print_LIPS("%s", outBuff); } }