/****************************************************************************************************
* @fn UnCalGyroDataResultCallback
* Call back for Uncalibrated gyroscope data
*
***************************************************************************************************/
static void UnCalGyroDataResultCallback(OutputSensorHandle_t outputHandle,
Android_UncalibratedGyroOutputData_t* pOutput)
{
if (g_logging & 0x40) //Uncalibrated data, in Android conventions
{
Print_LIPS("RG,%.6f,%.6f,%.6f,%.6f", TOFLT_TIME(pOutput->TimeStamp), TOFLT_PRECISE(pOutput->X),
TOFLT_PRECISE(pOutput->Y), TOFLT_PRECISE(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);
}
}
/****************************************************************************************************
* @fn sigMotionOutputCallback
* Call back for Significant Motion results
*
***************************************************************************************************/
static void sigMotionOutputCallback(OutputSensorHandle_t outputHandle,
Android_SignificantMotionOutputData_t* pOutput)
{
Print_LIPS("SM,%+03.4f,%d",TOFLT_TIME(pOutput->TimeStamp), pOutput->significantMotionDetected);
}
/****************************************************************************************************
* @fn stepDetectorResultCallback
* Call back for Step Detector results
*
***************************************************************************************************/
static void stepCounterOutputCallback(OutputSensorHandle_t OutputHandle,
Android_StepCounterOutputData_t* pOutput)
{
Print_LIPS("STC,%+03.4f,%d,0", TOFLT_TIME(pOutput->TimeStamp), pOutput->StepCount);
}
/****************************************************************************************************
* @fn SetNewStepSegment
* <brief>
*
***************************************************************************************************/
static void SetNewStepSegment(StepSegment_t * segment){
NTTIME dt;
//Check for start of walk sequence
if(segment->type == firstStep){
stepDetectData.startWalkTime = segment->startTime;
stepDetectData.step.numStepsSinceWalking = 0;
}
//Set times and increment counters
stepDetectData.step.startTime = segment->startTime;
stepDetectData.step.stopTime = segment->stopTime;
stepDetectData.step.numStepsTotal++;
stepDetectData.step.numStepsSinceWalking++;
//Estimate step frequency and length
dt = segment->stopTime - stepDetectData.startWalkTime;
stepDetectData.step.stepFrequency = ((osp_float_t)stepDetectData.step.numStepsSinceWalking)/TOFLT_TIME(dt);
//Callback to subscribers if any
if(stepDetectData.stepResultReadyCallback){
stepDetectData.stepResultReadyCallback(&stepDetectData.step);
}
if(stepDetectData.stepSegmentResultReadyCallback){
stepDetectData.stepSegmentResultReadyCallback(segment);
}
}
