/*********************************************************************
* @fn AlgBackGndTask
* This task is responsible for running the background
* routines (e.g. calibration)
*
* @param none
*
* @return none
*
*********************************************************************/
ASF_TASK void AlgBackGndTask (ASF_TASK_ARG)
{
MessageBuffer *rcvMsg = NULLP;
D0_printf(__func__);
D0_printf("\r\n");
while (1) {
ASFReceiveMessage(ALG_BG_TASK_ID, &rcvMsg);
switch (rcvMsg->msgId) {
case MSG_TRIG_ALG_BG:
/*
* background compute.
* Note that it's safe to call background processing
* more often than needed
*/
/* Bump clock speed while processing? HY-DBG */
while(OSP_DoBackgroundProcessing() != OSP_STATUS_IDLE);
break;
default:
/* Unhandled messages */
D1_printf("Alg-BG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
ASFDeleteMessage( ALG_BG_TASK_ID, &rcvMsg );
}
}
/****************************************************************************************************
* @fn I2C_Start_Transfer
* Call this function to send a prepared data. Also include how many bytes that should be
* sent/read including the address byte. The function will initiate the transfer and return
* immediately (or return with error if previous transfer pending). User must wait for
* transfer to complete by calling I2C_Wait_Completion
*
* @param //TODO
*
* @return none
*
***************************************************************************************************/
uint8_t I2C_Start_Transfer( uint8_t slaveAddr, uint16_t regAddr, uint8_t *pData, uint8_t dataSize, I2C_SendMode_t sendMode )
{
/* Check that no transfer is already pending*/
if (asyncXfer.i2c_txrx_status == I2C_TXRX_STATUS_ACTIVE)
{
D0_printf("I2C_Start_Transfer: A transfer is already pending\n\r");
return I2C_ERR_BUSY;
}
/* Update the transfer descriptor */
asyncXfer.i2c_slave_addr = slaveAddr << 1;
asyncXfer.i2c_slave_reg = regAddr;
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_ACTIVE;
asyncXfer.pData = pData;
asyncXfer.num = dataSize;
asyncXfer.byte_index = 0;
asyncXfer.i2c_txrx_phase = 0;
i2c_mode = sendMode;
/* Enable read interrupt and start the transfer */
I2C_SENSOR_BUS->MSTDAT = asyncXfer.i2c_slave_addr; //SLA+W
I2C_SENSOR_BUS->MSTCTL = I2C_MSTCTL_MSTSTART; //generate a START before enabling the interrupt
I2C_SENSOR_BUS->INTENSET = I2C_STAT_MSTPENDING;
return I2C_ERR_OK;
}
/****************************************************************************************************
* @fn I2C_Wait_Completion
* This function allows application to pend on completion
*
* @param none
*
* @return interrupt enabled status
*
***************************************************************************************************/
void I2C_Wait_Completion( void )
{
OS_RESULT result;
result = os_evt_wait_or( I2C_TXRX_STATUS_FAILED | I2C_TXRX_STATUS_PASSED, MSEC_TO_TICS(20));
if (result == OS_R_TMO)
{
D0_printf("### WARNING - Timedout on I2C completion ###\r\n");
}
}
/****************************************************************************************************
* @fn I2C_Wait_Completion
* This function allows application to pend on completion
*
* @param none
*
* @return none
*
***************************************************************************************************/
void I2C_Wait_Completion( void )
{
#if 1
OS_RESULT result;
result = os_evt_wait_or( I2C_TXRX_STATUS_FAILED | I2C_TXRX_STATUS_PASSED, MSEC_TO_TICS(10));
if (result == OS_R_TMO)
{
D0_printf("### WARNING - Timedout on I2C completion [%02X - %02X] ###\r\n", asyncXfer.i2c_slave_addr, asyncXfer.i2c_slave_reg);
}
#else
while( asyncXfer.i2c_txrx_status == I2C_TXRX_STATUS_ACTIVE );
#endif
}
/*********************************************************************
* @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 I2C_Start_Transfer
* Call this function to send a prepared data. Also include how many bytes that should be
* sent/read including the address byte. The function will initiate the transfer and return
* immediately (or return with error if previous transfer pending). User must wait for
* transfer to complete by calling I2C_Wait_Completion
*
* @param //TODO
*
* @return none
*
***************************************************************************************************/
uint8_t I2C_Start_Transfer( uint8_t slaveAddr, uint16_t regAddr, uint8_t *pData, uint8_t dataSize, I2C_SendMode_t sendMode )
{
/* Check that no transfer is already pending*/
if (asyncXfer.i2c_txrx_status == I2C_TXRX_STATUS_ACTIVE)
{
D0_printf("I2C_Start_Transfer: A transfer is already pending\n\r");
return I2C_ERR_BUSY;
}
if ((sendMode == I2C_MASTER_READ) || (sendMode == I2C_MASTER_WRITE))
{
/* Update the transfer descriptor */
asyncXfer.i2c_slave_addr = (slaveAddr << 1);
asyncXfer.i2c_slave_reg = regAddr;
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_ACTIVE;
asyncXfer.pData = pData;
asyncXfer.num = dataSize;
asyncXfer.byte_index = 0;
asyncXfer.i2c_txrx_phase = 0;
gSendMode = sendMode;
/* Enable interrupts and clear flags */
I2C_ITConfig( I2C_SENSOR_BUS, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, ENABLE );
I2C_ClearITPendingBit( I2C_SENSOR_BUS, I2C_IT_SMBALERT | I2C_IT_TIMEOUT | I2C_IT_PECERR | I2C_IT_OVR
| I2C_IT_AF | I2C_IT_ARLO | I2C_IT_BERR );
/* Enable ACK as it is disabled in interrupt handler after each transaction */
I2C_SENSOR_BUS->CR1 |= CR1_ACK_Set;
/* Initiate start */
I2C_GenerateSTART( I2C_SENSOR_BUS, ENABLE );
}
else
{
return I2C_ERR_REQ;
}
return I2C_ERR_OK;
}
/*********************************************************************
* @fn Algorithm_UnsubscribeSensor
* To un-subscribe a sensor result that was previously
* subscribed with the algorithm module.
*
*
* @param specify a sensor of ASensorType_t to un-subscribe
*
* @return OSP_STATUS_OK if successsful.
* OSP_STATUS_NOT_SUBSCRIBED if failure.
*
*********************************************************************/
OSP_STATUS_t Algorithm_UnsubscribeSensor( ASensorType_t sensor)
{
OSP_STATUS_t status = OSP_STATUS_NOT_SUBSCRIBED;
if ( IsPrivateAndroidSensor(sensor) ) {
// Private Android sensor
PSensorType_t PSensor = M_AndroidToPSensorBase(sensor);
if ( _outPSensorHandles[PSensor] != NULL ) {
status = OSP_UnsubscribeSensorResult(_outPSensorHandles[PSensor]);
_outPSensorHandles[PSensor] = NULL;
}
} else {
// Standard Android Sensor
if ( _outSensorHandles[sensor] != NULL ) {
status = OSP_UnsubscribeSensorResult(_outSensorHandles[sensor]);
_outSensorHandles[sensor] = NULL;
}
}
if ( status != OSP_STATUS_OK)
D0_printf("\n%s: Failed to unsubscribed sensor 0x%x\r\n", __FUNCTION__, sensor);
return status;
}
/*********************************************************************
* @fn Algorithm_SubscribeSensor
* To subscribe a sensor result with the algorithm module.
*
*
* @param specify a subscribe sensor of type ASensorType_t
*
* @return OSP_STATUS_OK if successsful.
* OSP_STATUS_SENSOR_UNSUPPORTED if failure.
*
*********************************************************************/
OSP_STATUS_t Algorithm_SubscribeSensor( ASensorType_t sensor)
{
ResultDescriptor_t *pResultDesc;
OSP_STATUS_t status;
switch( sensor ) {
case SENSOR_ACCELEROMETER:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_ACCELEROMETER);
break;
case SENSOR_MAGNETIC_FIELD:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_MAGNETIC_FIELD);
break;
case SENSOR_ORIENTATION:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_ORIENTATION);
break;
case SENSOR_GYROSCOPE:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_GYROSCOPE);
break;
case SENSOR_LIGHT:
// pResultDesc = & SENSOR_REQ_NAME(SENSOR_LIGHT);
return OSP_STATUS_SENSOR_UNSUPPORTED;
case SENSOR_PRESSURE:
// pResultDesc = & SENSOR_REQ_NAME(SENSOR_PRESSURE);
return OSP_STATUS_SENSOR_UNSUPPORTED;
case SENSOR_TEMPERATURE:
// pResultDesc = & SENSOR_REQ_NAME(SENSOR_TEMPERATURE);
return OSP_STATUS_SENSOR_UNSUPPORTED;
case SENSOR_PROXIMITY:
// pResultDesc = & SENSOR_REQ_NAME(SENSOR_PROXIMITY);
return OSP_STATUS_SENSOR_UNSUPPORTED;
case SENSOR_GRAVITY:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_GRAVITY);
break;
case SENSOR_LINEAR_ACCELERATION:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_LINEAR_ACCELERATION);
break;
case SENSOR_ROTATION_VECTOR:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_ROTATION_VECTOR);
break;
case SENSOR_RELATIVE_HUMIDITY:
// pResultDesc = & SENSOR_REQ_NAME(SENSOR_RELATIVE_HUMIDITY);
return OSP_STATUS_SENSOR_UNSUPPORTED;
case SENSOR_AMBIENT_TEMPERATURE:
// pResultDesc = & SENSOR_REQ_NAME(SENSOR_AMBIENT_TEMPERATURE);
return OSP_STATUS_SENSOR_UNSUPPORTED;
case SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_MAGNETIC_FIELD_UNCALIBRATED);
break;
case SENSOR_GAME_ROTATION_VECTOR:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_GAME_ROTATION_VECTOR);
break;
case SENSOR_GYROSCOPE_UNCALIBRATED:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_GYROSCOPE_UNCALIBRATED);
break;
case SENSOR_SIGNIFICANT_MOTION:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_SIGNIFICANT_MOTION);
break;
case SENSOR_STEP_DETECTOR:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_STEP_DETECTOR);
break;
case SENSOR_STEP_COUNTER:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_STEP_COUNTER);
break;
case SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
pResultDesc = & SENSOR_REQ_NAME(SENSOR_GEOMAGNETIC_ROTATION_VECTOR);
break;
case AP_PSENSOR_ACCELEROMETER_UNCALIBRATED:
pResultDesc = & SENSOR_REQ_NAME(AP_PSENSOR_ACCELEROMETER_UNCALIBRATED);
break;
default:
D0_printf("Unknown or unsupport sensor type %d \n", sensor);
return OSP_STATUS_SENSOR_UNSUPPORTED;
}
// Now subscribe the sensor result
if ( IsPrivateAndroidSensor(sensor) ) {
PSensorType_t PSensor = M_AndroidToPSensorBase(sensor);
status = OSP_SubscribeSensorResult(pResultDesc, &_outPSensorHandles[PSensor]);
} else {
status = OSP_SubscribeSensorResult(pResultDesc, &_outSensorHandles[sensor]);
}
if ( status != OSP_STATUS_OK ) {
D0_printf("\n%s: Failed subscribe to sensor type %d\r\n", __FUNCTION__, sensor);
}
return status;
}
/*********************************************************************
* @fn AlgorithmTask
* This task is responsible for running the sensor algorithms
* on the incoming sensor data (could be raw or filtered) and
* processing output results
*
* @param none
*
* @return none
*
**********************************************************************/
ASF_TASK void AlgorithmTask (ASF_TASK_ARG)
{
MessageBuffer *rcvMsg = NULLP;
OSP_STATUS_t OSP_Status;
int alg_count;
OSP_GetLibraryVersion(&version);
D1_printf("OSP Version: %s\r\n", version->VersionString);
/* Initialize the mutex */
mutex_id = osMutexCreate(osMutex(mutexCritSection));
OSP_Status = OSP_Initialize(&gSystemDesc);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_Initialize Failed");
OSP_SetCalibrationConfig( 0x1); // disable rotational cal.
D0_printf("--Alg Task %i\r\n", __LINE__);
// Register the input sensors
OSP_Status = OSP_RegisterInputSensor(&_AccSensDesc, &_AccHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_RegisterInputSensor (accel) Failed");
OSP_Status = OSP_RegisterInputSensor(&_MagSensDesc, &_MagHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_RegisterInputSensor (mag) Failed");
OSP_Status = OSP_RegisterInputSensor(&_GyroSensDesc, &_GyroHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_RegisterInputSensor (gyro) Failed");
#if 0
SENSOR_SUBSCRIBE(SENSOR_STEP_COUNTER);
SENSOR_SUBSCRIBE(SENSOR_STEP_DETECTOR);
SENSOR_SUBSCRIBE(SENSOR_SIGNIFICANT_MOTION);
SENSOR_SUBSCRIBE(SENSOR_GYROSCOPE_UNCALIBRATED);
SENSOR_SUBSCRIBE(SENSOR_MAGNETIC_FIELD_UNCALIBRATED);
SENSOR_SUBSCRIBE(SENSOR_GYROSCOPE);
SENSOR_SUBSCRIBE(SENSOR_ACCELEROMETER);
SENSOR_SUBSCRIBE(SENSOR_MAGNETIC_FIELD);
SENSOR_SUBSCRIBE(SENSOR_ORIENTATION);
SENSOR_SUBSCRIBE(SENSOR_GRAVITY);
SENSOR_SUBSCRIBE(SENSOR_LINEAR_ACCELERATION);
SENSOR_SUBSCRIBE(SENSOR_ROTATION_VECTOR);
SENSOR_SUBSCRIBE(SENSOR_GAME_ROTATION_VECTOR);
SENSOR_SUBSCRIBE(SENSOR_GEOMAGNETIC_ROTATION_VECTOR);
// Subscribing private sensor results
PRIVATE_SENSOR_SUBSCRIBE(AP_PSENSOR_ACCELEROMETER_UNCALIBRATED);
#endif
D0_printf("%s: --Alg Task init done\r\n", __func__);
while (1) {
ASFReceiveMessage(ALGORITHM_TASK_ID, &rcvMsg);
if (!(mycount % 64)) {
LED_Toggle(LED_GREEN);
}
switch (rcvMsg->msgId) {
case MSG_MAG_DATA:
// SendBgTrigger();
case MSG_ACC_DATA:
case MSG_GYRO_DATA:
mycount++;
HandleSensorData(rcvMsg);
//keep doing foreground computation until its finished
/* Bump clock speed while processing? HY-DBG */
alg_count = 0;
do {
OSP_Status = OSP_DoForegroundProcessing();
ASF_assert(OSP_Status != OSP_STATUS_UNSPECIFIED_ERROR);
alg_count++;
if (alg_count > 5) {
D0_printf("%s:%i Taking too long\r\n", __func__, __LINE__);
break;
}
} while(OSP_Status != OSP_STATUS_IDLE);
/* DBG:
* Run background here as the backgound taks doesn't seem to run enough */
while(OSP_DoBackgroundProcessing() != OSP_STATUS_IDLE);
break;
case MSG_PRESS_DATA:
PressureDataResultCallback(&rcvMsg->msg.msgPressData);
break;
default:
/* Unhandled messages */
D1_printf("Alg-FG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
ASFDeleteMessage( ALGORITHM_TASK_ID, &rcvMsg );
#ifdef DEBUG_TEST_SENSOR_SUBSCRIPTION
// Testing subscribe and unsubscribe sensors
DebugTestSensorSubscription();
#endif
}
}
/******************************************************************************
* @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);
}
}
void DebugTestSensorSubscription(void)
{
osp_bool_t privateSensor = FALSE;
uint32_t mask;
if ( (subscribeSensorReq == -1) && (unsubscribeSensorReq == -1 ) )
return;
if ( subscribeSensorReq != -1 ) {
// Handle subscription
privateSensor = IsPrivateAndroidSensor(subscribeSensorReq);
if ( privateSensor ) {
int PSensor = M_AndroidToPSensorBase(subscribeSensorReq);
mask = 0x1UL << PSensor;
if ( _subscribedPrivateSensors & mask ) {
D0_printf("\n%s: Requested Sensor 0x%x already subscribed\r\n", __FUNCTION__, subscribeSensorReq);
subscribeSensorReq = -1; //reset flag
return; // already subscribed
} else {
// update private subscription bit mask
_subscribedPrivateSensors |= mask;
}
} else {
mask = 0x1UL << subscribeSensorReq;
if ( _subscribedAndroidSensors & mask ) {
D0_printf("\n%s: Requested Sensor 0x%x already subscribed\r\n", __FUNCTION__, subscribeSensorReq);
subscribeSensorReq = -1; //reset flag
return; // already subscribed
} else {
// update android subscription bit mask
_subscribedAndroidSensors |= mask;
}
}
// Now subscribe the sensor
Algorithm_SubscribeSensor( (ASensorType_t) subscribeSensorReq);
subscribeSensorReq = -1; //reset flag
}
// Handle un-subscription request
if ( unsubscribeSensorReq != -1 ) {
// handle unsubscription
uint32_t mask;
privateSensor = IsPrivateAndroidSensor(unsubscribeSensorReq);
if ( privateSensor ) {
int PSensor = M_AndroidToPSensorBase(unsubscribeSensorReq);
mask = 0x1UL << PSensor ;
if ( !(_subscribedPrivateSensors & mask) ) {
D0_printf("\n%s: Requested sensor 0x%x is not subscribed\r\n", __FUNCTION__, unsubscribeSensorReq);
unsubscribeSensorReq = -1; //reset flag
return; // Sensor is not currently subscribed so cannot un-subscribe
} else {
_subscribedPrivateSensors &= ~mask; // clear the subscription bit mask
}
} else {
mask = 0x1 << unsubscribeSensorReq;
if ( !(_subscribedAndroidSensors & mask) ) {
D0_printf("\n%s: Requested sensor 0x%x is not subscribed\r\n", __FUNCTION__, unsubscribeSensorReq);
unsubscribeSensorReq = -1;
return;
} else {
_subscribedAndroidSensors &= ~mask; // clear the subscription bit mask
}
}
// Now un-subscribe the sensor
Algorithm_UnsubscribeSensor((ASensorType_t) unsubscribeSensorReq);
unsubscribeSensorReq = -1; // reset flag
}
D0_printf("SensorsSubscribed PrivateSensor = 0x%x, AndroidSensor = 0x%x\r\n", _subscribedPrivateSensors, _subscribedAndroidSensors);
}
static int FastData_add(volatile struct FastData *FD, Buffer_t *pHIFPkt)
{
volatile struct DataBuf *DB;
static int state = 0;
static uint32_t lastTimeStamp = 0;
uint32_t curTime;
if (ClearQueue) {
FastData_init(FD);
ClearQueue = 0;
}
DB = &(FD->Databuf[FD->write]);
if (pHIFPkt->Header.Length+DB->length > FASTDATA_LEN_Q) {
if (((FD->write+1)%FASTDATA_NUM_Q) == FD->read) {
drops++;
state = 1;
return 1; /* No more Q's available */
} else {
if (state == 1) {
D0_printf("drops = %i\r\n", drops);
}
state = 0;
FD->write++;
FD->write %= FASTDATA_NUM_Q;
DB = &(FD->Databuf[FD->write]);
DB->length = 0;
DB->state = 0;
}
}
memcpy(DB->DataChunk + DB->length, &(pHIFPkt->DataStart), pHIFPkt->Header.Length);
DB->length += pHIFPkt->Header.Length;
DB->state++;
curTime = rtc_read();
if (lastTimeStamp != 0) {
if ((curTime - lastTimeStamp) > 1000) {
if (((FD->write+1)%FASTDATA_NUM_Q) == FD->read) {
return 1;
} else {
FD->write++;
FD->write%=FASTDATA_NUM_Q;
DB = &(FD->Databuf[FD->write]);
DB->length = 0;
DB->state = 0;
}
}
}
lastTimeStamp = curTime;
if (DB->state > 300) {
if (((FD->write+1)%FASTDATA_NUM_Q) == FD->read) {
return 1;
} else {
FD->write++;
FD->write%=FASTDATA_NUM_Q;
DB = &(FD->Databuf[FD->write]);
DB->length = 0;
DB->state = 0;
}
}
return 0;
}
/****************************************************************************************************
* @fn I2C_IRQHandler
* Handler for I2C Tx/Rx related interrupt
*
***************************************************************************************************/
void I2C_IRQHandler(LPC_I2C_T *pI2C)
{
uint32_t status = Chip_I2CM_GetStatus(pI2C);
uint32_t i2cmststate = Chip_I2CM_GetMasterState(pI2C); /* Only check Master and Slave State */
uint32_t mstCtrl = I2C_MSTCTL_MSTCONTINUE;
if ( status & I2C_STAT_MSTRARBLOSS )
{
/* Master Lost Arbitration */
/* Clear Status Flags */
Chip_I2CM_ClearStatus(pI2C, I2C_STAT_MSTRARBLOSS);
/* Master continue */
if ( status & I2C_STAT_MSTPENDING ) {
pI2C->MSTCTL = I2C_MSTCTL_MSTCONTINUE;
}
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_FAILED;
isr_evt_set(I2C_TXRX_STATUS_FAILED, asfTaskHandleTable[SENSOR_ACQ_TASK_ID].handle );
D0_printf("I2C-ISR: Arb Loss Err!\r\n");
}
else if ( status & I2C_STAT_MSTSTSTPERR )
{
/* Master Start Stop Error */
/* Clear Status Flags */
Chip_I2CM_ClearStatus(pI2C, I2C_STAT_MSTSTSTPERR);
/* Master continue */
if ( status & I2C_STAT_MSTPENDING ) {
pI2C->MSTCTL = I2C_MSTCTL_MSTCONTINUE;
}
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_FAILED;
isr_evt_set(I2C_TXRX_STATUS_FAILED, asfTaskHandleTable[SENSOR_ACQ_TASK_ID].handle );
D0_printf("I2C-ISR: Start Stop Err!\r\n");
}
else if ( status & I2C_STAT_MSTPENDING )
{
pI2C->INTENCLR = I2C_STAT_MSTPENDING;
/* Below five states are for Master mode: IDLE, TX, RX, NACK_ADDR, NAC_TX.
IDLE is not under consideration for now. */
switch ( i2cmststate )
{
case I2C_STAT_MSTCODE_IDLE:
/* Do not send the message to the waiting task until we are done with the transaction */
if ((asyncXfer.i2c_txrx_status == I2C_TXRX_STATUS_ACTIVE) && (asyncXfer.num == 0)) {
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_PASSED;
isr_evt_set(I2C_TXRX_STATUS_PASSED, asfTaskHandleTable[SENSOR_ACQ_TASK_ID].handle );
}
break;
/* Received data. Address plus Read was previously sent and Acknowledged by slave */
case I2C_STAT_MSTCODE_RXREADY:
asyncXfer.pData[asyncXfer.byte_index++] = pI2C->MSTDAT;
asyncXfer.num--;
if (asyncXfer.num == 0) { //we are done with the read
mstCtrl |= I2C_MSTCTL_MSTSTOP;
}
pI2C->MSTCTL = mstCtrl;
pI2C->INTENSET = I2C_STAT_MSTPENDING;
break;
/* Ready to Transmit data. Address plus Write was previously sent and Acknowledged by slave */
case I2C_STAT_MSTCODE_TXREADY:
if (i2c_mode == I2C_MASTER_WRITE) {
if (asyncXfer.i2c_txrx_phase == 0) {
pI2C->MSTDAT = asyncXfer.i2c_slave_reg; //Send register addr
asyncXfer.i2c_txrx_phase = 1;
} else {
if (asyncXfer.num == 0) { //Done with write transaction
mstCtrl |= I2C_MSTCTL_MSTSTOP;
} else {
pI2C->MSTDAT = asyncXfer.pData[asyncXfer.byte_index++];
asyncXfer.num--;
}
}
pI2C->MSTCTL = mstCtrl;
pI2C->INTENSET = I2C_STAT_MSTPENDING;
} else { // MASTER_READ
if (asyncXfer.i2c_txrx_phase == 0) {
pI2C->MSTDAT = asyncXfer.i2c_slave_reg; //Send register addr
asyncXfer.i2c_txrx_phase = 1;
} else { //Send Re-Start with SLA+R
pI2C->MSTDAT = asyncXfer.i2c_slave_addr | 1;
mstCtrl |= I2C_MSTCTL_MSTSTART;
}
}
pI2C->MSTCTL = mstCtrl;
pI2C->INTENSET = I2C_STAT_MSTPENDING;
break;
case I2C_STAT_MSTCODE_NACKADR: //Slave NACKed address
/* For now just stop the transaction */
pI2C->MSTCTL = I2C_MSTCTL_MSTSTOP | I2C_MSTCTL_MSTCONTINUE;
pI2C->INTENSET = I2C_STAT_MSTPENDING;
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_FAILED;
isr_evt_set(I2C_TXRX_STATUS_FAILED, asfTaskHandleTable[SENSOR_ACQ_TASK_ID].handle );
D0_printf("I2C-ISR: Slave NACKed Addr!\r\n");
break;
default:
case I2C_STAT_MSTCODE_NACKDAT: //Slave NACKed transmitted data
/* For now just stop the transaction */
pI2C->MSTCTL = I2C_MSTCTL_MSTSTOP | I2C_MSTCTL_MSTCONTINUE;
pI2C->INTENSET = I2C_STAT_MSTPENDING;
asyncXfer.i2c_txrx_status = I2C_TXRX_STATUS_FAILED;
isr_evt_set(I2C_TXRX_STATUS_FAILED, asfTaskHandleTable[SENSOR_ACQ_TASK_ID].handle );
D0_printf("I2C-ISR: Slave NACKed Data!\r\n");
break;
}
}
}
/* Sensor data flow:
* 1. Sensor interrupts on data ready.
* 2. IRQ handler is solely expected to call SendDataReadyIndication.
* 3. SensorAcqTask will see a message data is available.
* 4. Data is read. Sensor driver is expected to clear
* the interrupt from the read function.
*/
ASF_TASK void SensorAcqTask(ASF_TASK_ARG)
{
MessageBuffer *rcvMsg = NULLP;
volatile uint8_t i;
#ifndef WAIT_FOR_HOST_SYNC
osDelay(50);
#else
WaitForHostSync(); //This also allows for startup time for sensors
#endif
/* Setup I2C bus here? */
dev_i2c_init();
/* Setup interface for the Magnetometer */
Mag_HardwareSetup(true);
Mag_Initialize();
/* Setup interface for the accelerometers */
Accel_HardwareSetup(true);
Accel_Initialize(INIT_NORMAL);
/* Setup Gyro */
Gyro_HardwareSetup(true);
Gyro_Initialize();
D0_printf("Gyro init done:\r\n");
/* Setup Pressure */
Pressure_HardwareSetup(true);
Pressure_Initialize();
ASFTimerStart(SENSOR_ACQ_TASK_ID, TIMER_REF_PRESSURE_READ,
PRESSURE_SAMPLE_PERIOD, &sPressureTimer);
#ifndef INTERRUPT_BASED_SAMPLING
/* Start sample period timer */
ASFTimerStart(SENSOR_ACQ_TASK_ID, TIMER_REF_SENSOR_READ,
SENSOR_SAMPLE_PERIOD, &sSensorTimer);
#else
/* Enable Sensor interrupts */
Mag_ConfigDataInt(true);
Accel_ConfigDataInt(true);
Gyro_ConfigDataInt(true);
# ifdef TRIGGERED_MAG_SAMPLING
D0_printf("Set mag to low power mode\r\n");
Mag_SetLowPowerMode(); //Low power mode until triggered
# endif
#endif
D0_printf("%s initialized\r\n", __func__);
/* Magnetometer sensor does not re-generate interrupt if its outputs are not read. */
Mag_ClearDataInt();
/* Indicate sensor init done */
sTskRdyFlag = 1;
while (1) {
ASFReceiveMessage(SENSOR_ACQ_TASK_ID, &rcvMsg);
// D0_printf("rcvMsg->msgId = %d\r\n",rcvMsg->msgId);
switch (rcvMsg->msgId) {
case MSG_TIMER_EXPIRY:
HandleTimers(&rcvMsg->msg.msgTimerExpiry);
break;
case MSG_CAL_EVT_NOTIFY:
#ifdef ENABLE_FLASH_STORE
StoreCalibrationData((CalEvent_t)rcvMsg->msg.msgCalEvtNotify.byte);
#else
D0_printf("#### WARNING - NV Storage Not Implemented! #####\r\n");
#endif
break;
case MSG_SENSOR_DATA_RDY:
//D0_printf("MSG_SENSOR_DATA_RDY msg id %d\r\n", rcvMsg->msgId);
#ifdef INTERRUPT_BASED_SAMPLING
SensorDataHandler((InputSensor_t)rcvMsg->msg.msgSensorDataRdy.sensorId,
rcvMsg->msg.msgSensorDataRdy.timeStamp);
#endif
break;
case MSG_SENSOR_CONTROL:
//D0_printf("MSG_SENSOR_CONTROL msg id %d\r\n",rcvMsg->msgId);
SensorControlCmdHandler(&rcvMsg->msg.msgSensorControlData);
break;
default:
/* Unhandled messages */
D2_printf("SensorAcqTask:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
ASFDeleteMessage( SENSOR_ACQ_TASK_ID, &rcvMsg );
}
}
/***********************************************************************
* @fn SensorControlCmdHandler
* Handle sensor parameter setting request
*
***********************************************************************/
void SensorControlCmdHandler(MsgSensorControlData *pData)
{
InputSensor_t sensorType;
SensorControlCommand_t command;
sensorType = pData->sensorType;
command = (SensorControlCommand_t) pData->command;
switch(command){
case SENSOR_CONTROL_SENSOR_OFF:
// e.g. put sensor into power saving mode
D0_printf("Sensor Control set sensor idx %d OFF\r\n", sensorType);
switch ( sensorType){
case ACCEL_INPUT_SENSOR:
#ifdef SENSOR_CONTROL_ENABLE
// Maybe add a user configurable 'powerMode' setting and here we
// configure the sensor hardware according to its value.
Accel_ConfigDataInt(false); // do not disable accel so watch window input will work
#endif
break;
case MAG_INPUT_SENSOR:
#ifdef SENSOR_CONTROL_ENABLE
Mag_ConfigDataInt(false);
#endif
break;
case GYRO_INPUT_SENSOR:
#ifdef SENSOR_CONTROL_ENABLE
Gyro_ConfigDataInt(false);
#endif
break;
default:
break;
}
break;
case SENSOR_CONTROL_SENSOR_SLEEP:
// put sensor into sleep mode
D0_printf("Sensor Control set sensor idx %d to SLEEP mode\r\n", sensorType);
break;
case SENSOR_CONTROL_SENSOR_ON:
// Turn sensor into normal operating mode
D0_printf("Sensor Control set sensor idx %d ON\r\n", sensorType);
switch ( sensorType){
case ACCEL_INPUT_SENSOR:
#ifdef SENSOR_CONTROL_ENABLE
Accel_ConfigDataInt(true);
#endif
break;
case MAG_INPUT_SENSOR:
#ifdef SENSOR_CONTROL_ENABLE
Mag_ConfigDataInt(true);
#endif
break;
case GYRO_INPUT_SENSOR:
#ifdef SENSOR_CONTROL_ENABLE
Gyro_ConfigDataInt(true);
#endif
break;
default:
break;
}
break;
case SENSOR_CONTROL_SET_SAMPLE_RATE:
// Update the sensor output rate
D0_printf("Sensor Control set sensor idx %d sampe rate to %d\r\n",
sensorType, pData->data);
break;
case SENSOR_CONTROL_SET_LPF_FREQ:
D0_printf("Sensor Control set sensor idx %d LPF to %d\r\n",
sensorType, pData->data);
break;
case SENSOR_CONTROL_SET_HPF_FREQ:
D0_printf("Sensor Control set sensor idx %d HPF to %d\r\n",
sensorType, pData->data);
break;
default:
D0_printf("Invalid sensor control command value (%d)\r\n", pData->command);
}
}
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;
}
}