/**********************************************************************
* @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;
}
}
