/********************************************************************** * @fn SendBgTrigger * Sends triggers to the algorithm background task to do processing * **********************************************************************/ static void SendBgTrigger( void ) { MessageBuffer *pSendMsg = NULLP; ASF_assert(ASFCreateMessage(MSG_TRIG_ALG_BG, sizeof(MsgNoData), &pSendMsg) == ASF_OK); ASFSendMessage(ALG_BG_TASK_ID, pSendMsg); }
/**************************************************************************************************** * @fn SendTimerExpiry * Sends the timer expiry message to the owner of the timer * * @param pTimer Pointer to the timer control block * * @return none * ***************************************************************************************************/ static void SendTimerExpiry ( AsfTimer *pTimer ) { MessageBuffer *pSendMsg = NULLP; ASF_assert( ASFCreateMessage( MSG_TIMER_EXPIRY, sizeof(MsgTimerExpiry), &pSendMsg ) == ASF_OK ); pSendMsg->msg.msgTimerExpiry.userValue = pTimer->userValue; pSendMsg->msg.msgTimerExpiry.timerId = pTimer->timerId; ASF_assert( ASFSendMessage( pTimer->owner, pSendMsg ) == ASF_OK ); }
/********************************************************************** * @fn PressureDataResultCallback * Call back for pressure data * **********************************************************************/ static void PressureDataResultCallback(MsgPressData *pMsgPressData) { MessageBuffer *pSample = NULLP; ASF_assert(ASFCreateMessage(MSG_PRESS_DATA, sizeof(MsgPressData), &pSample) == ASF_OK); pSample->msg.msgPressData = *pMsgPressData; if (!(ASFSendMessage(I2CSLAVE_COMM_TASK_ID, pSample) == ASF_OK)) { ; } if (g_logging & 0x40) { //Uncalibrated data, in Android conventions // Print_LIPS("RP,%3.4f,%4.3f", TOFLT_TIME(pMsgPressData->timeStamp), TOFLT_PRECISE(pMsgPressData->X)); } }
/********************************************************************** * @fn SensorControlActicate * Fucntion to Control the sensor activation and deactivation. * Typical usage of this function to register it as a callback * function with the algorithm library and letting the algorithm * library make decision when to enable and disable the sensor. **********************************************************************/ static OSP_STATUS_t SensorControlActivate( SensorControl_t *pControl) { MessageBuffer *pSendMsg = NULLP; InputSensor_t sensorType; if ( pControl == NULL ) return OSP_STATUS_NULL_POINTER; if ( pControl->Handle == _AccHandle ) { sensorType = _AccSensDesc.SensorType; } else if ( pControl->Handle == _MagHandle ) { sensorType = _MagSensDesc.SensorType; } else if ( pControl->Handle == _GyroHandle ) { sensorType = _GyroSensDesc.SensorType; } else { return OSP_STATUS_INVALID_HANDLE; // unrecognize handle } switch (sensorType) { case ACCEL_INPUT_SENSOR: case MAG_INPUT_SENSOR: case GYRO_INPUT_SENSOR: /* send this request to sensor acquisition task */ ASF_assert(ASFCreateMessage(MSG_SENSOR_CONTROL, sizeof(MsgSensorControlData), &pSendMsg) == ASF_OK); pSendMsg->msg.msgSensorControlData.command = pControl->Command; pSendMsg->msg.msgSensorControlData.sensorType = sensorType; pSendMsg->msg.msgSensorControlData.data = pControl->Data; ASFSendMessage(SENSOR_ACQ_TASK_ID, pSendMsg); break; default: break; // all the other sensor controls are not use by the algorithm library at this point. } return OSP_STATUS_OK; }
/********************************************************************* * @fn SendDataReadyIndication * @brief This helper function sends data ready indication to * Sensor Acq task. Called from ISR. * Best effort to send the message. If buffer or queue space is * not available, the message is dropped! * @param sensorId: Sensor identifier whose data is ready to be read * @return None * *********************************************************************/ void SendDataReadyIndication(uint8_t sensorId, uint32_t timeStamp) { MessageBuffer *pSendMsg = NULLP; /* Do not send a message until sensor acquisition task is ready */ if (0 != sTskRdyFlag){ if (ASFCreateMessage(MSG_SENSOR_DATA_RDY, sizeof(MsgSensorDataRdy), &pSendMsg) == ASF_OK) { pSendMsg->msg.msgSensorDataRdy.sensorId = sensorId; pSendMsg->msg.msgSensorDataRdy.timeStamp = timeStamp; if ( ASFSendMessage(SENSOR_ACQ_TASK_ID, pSendMsg) != ASF_OK ) { D0_printf("Error sending sensoracq message for senosr id %d\r\n", sensorId); } } else { D0_printf("Error creating sensoracq message for sensor id %d\r\n", sensorId); } } /* Mag interrupt needs to be explicitly cleaned after the interrupt is read/ignored */ else if(sensorId == MAG_INPUT_SENSOR) { Mag_ClearDataInt(); } }
/****************************************************************************** * @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); } }
static void SensorDataHandler(InputSensor_t sensorId, uint32_t timeStamp) { MsgAccelData accelData; MsgMagData magData; MsgGyroData gyroData; MsgPressData pressData; static uint8_t sMagDecimateCount = 0; static uint8_t gyroSampleCount = 0; static uint8_t accSampleCount = 0; #if defined ALGORITHM_TASK MessageBuffer *pMagSample = NULLP; MessageBuffer *pAccSample = NULLP; MessageBuffer *pGyroSample = NULLP; MessageBuffer *pPressSample = NULLP; #endif switch (sensorId) { case MAG_INPUT_SENSOR: /* Read mag Data - reading would clear interrupt also */ Mag_ReadData(&magData); if ((sMagDecimateCount++ % MAG_DECIMATE_FACTOR) == 0) { /* Replace time stamp with that captured by interrupt handler */ magData.timeStamp = timeStamp; #ifdef ALGORITHM_TASK ASF_assert(ASFCreateMessage(MSG_MAG_DATA, sizeof(MsgMagData), &pMagSample) == ASF_OK); pMagSample->msg.msgMagData = magData; if (!(ASFSendMessage(ALGORITHM_TASK_ID, pMagSample) == ASF_OK)) { queue_err++; } #endif } //D0_printf("Mag ADC: %d, %d, %d\r\n", magData.X, magData.Y, magData.Z); break; case GYRO_INPUT_SENSOR: Gyro_ReadData(&gyroData); //Reads also clears DRDY interrupt /* Replace time stamp with that captured by interrupt handler */ if ((gyroSampleCount++ % GYRO_SAMPLE_DECIMATE) == 0) { /* Read Gyro Data - reading typically clears interrupt as well */ gyroData.timeStamp = timeStamp; #ifdef ALGORITHM_TASK if (ASFCreateMessage(MSG_GYRO_DATA, sizeof(MsgGyroData), &pGyroSample) == ASF_OK) { pGyroSample->msg.msgGyroData = gyroData; if (!(ASFSendMessage(ALGORITHM_TASK_ID, pGyroSample) == ASF_OK)) { queue_err++; } } #endif // D0_printf("Gyro ADC: %d, %d, %d\r\n", gyroData.X, gyroData.Y, gyroData.Z); } break; case ACCEL_INPUT_SENSOR: #if defined TRIGGERED_MAG_SAMPLING if (accSampleCount % MAG_TRIGGER_RATE_DECIMATE == 0) { Mag_TriggerDataAcq(); //Mag is triggered relative to Accel to avoid running separate timer } #endif /* Read Accel Data - reading typically clears interrupt as well */ Accel_ReadData(&accelData); if (accSampleCount++ % ACCEL_SAMPLE_DECIMATE == 0) { /* Replace time stamp with that captured by interrupt handler */ accelData.timeStamp = timeStamp; #ifdef ALGORITHM_TASK if (ASFCreateMessage(MSG_ACC_DATA, sizeof(MsgAccelData), &pAccSample) == ASF_OK) { pAccSample->msg.msgAccelData = accelData; if (!(ASFSendMessage(ALGORITHM_TASK_ID, pAccSample) == ASF_OK)) { queue_err++; } } #endif // D0_printf("Accel ADC: %d, %d, %d\r\n", accelData.X, accelData.Y, accelData.Z); } break; case PRESSURE_INPUT_SENSOR: /* Read Accel Data - reading typically clears interrupt as well */ Pressure_ReadData(&pressData); /* Replace time stamp with that captured by interrupt handler */ pressData.timeStamp = timeStamp; /* To do: Send the pressure sensor out */ #ifdef ALGORITHM_TASK ASF_assert(ASFCreateMessage(MSG_PRESS_DATA, sizeof(MsgPressData), &pPressSample) == ASF_OK); pPressSample->msg.msgPressData = pressData; ASFSendMessage(ALGORITHM_TASK_ID, pPressSample); #endif // D0_printf("Pressure ADC: P = %d, T = %d\r\n", pressData.X, pressData.Z); break; default: D0_printf("Input Sensor ID %d is not supported\r\n", sensorId); break; } }