/***********************************************************************
* @fn HandleSensorData
* Handles sensor input data and feeds it to the sensor algorithms
*
***********************************************************************/
static void HandleSensorData(MessageBuffer *pRcvMsg)
{
osp_status_t status;
OSP_InputSensorData_t sensorData;
switch(pRcvMsg->msgId) {
case MSG_ACC_DATA:
sensorData.data.data[0] = pRcvMsg->msg.msgAccelData.X;
sensorData.data.data[1] = pRcvMsg->msg.msgAccelData.Y;
sensorData.data.data[2] = pRcvMsg->msg.msgAccelData.Z;
sensorData.data.TimeStamp = pRcvMsg->msg.msgAccelData.timeStamp;
status = OSP_SetInputData(_AccHandle, &sensorData);
ASF_assert(status == OSP_STATUS_OK);
break;
case MSG_MAG_DATA:
sensorData.data.data[0] = pRcvMsg->msg.msgMagData.X;
sensorData.data.data[1] = pRcvMsg->msg.msgMagData.Y;
sensorData.data.data[2] = pRcvMsg->msg.msgMagData.Z;
sensorData.data.TimeStamp = pRcvMsg->msg.msgMagData.timeStamp;
status = OSP_SetInputData(_MagHandle, &sensorData);
ASF_assert(status == OSP_STATUS_OK);
break;
case MSG_GYRO_DATA:
sensorData.data.data[0] = pRcvMsg->msg.msgGyroData.X;
sensorData.data.data[1] = pRcvMsg->msg.msgGyroData.Y;
sensorData.data.data[2] = pRcvMsg->msg.msgGyroData.Z;
sensorData.data.TimeStamp = pRcvMsg->msg.msgGyroData.timeStamp;
status = OSP_SetInputData(_GyroHandle, &sensorData);
ASF_assert(status == OSP_STATUS_OK);
break;
default:
D1_printf("ALG: Bad message: %d\r\n", pRcvMsg->msgId);
break;
}
}
/****************************************************************************************************
* @fn ASFKillTimer
* Kills the timer that was created earlier
*
* @param pTimer Pointer to timer control block containing the attributes of the timer to be
* created.
*
* @return none
*
* @see ASFTimerStart()
***************************************************************************************************/
void _ASFKillTimer ( AsfTimer *pTimer, char *_file, int _line )
{
TimerId ret;
ASF_assert( pTimer != NULLP );
ret = os_tmr_kill( pTimer->timerId );
ASF_assert( ret == NULL );
pTimer->sysUse = (uint32_t)-1; //Timer no longer in use
}
/****************************************************************************************************
* @fn ASFTimerStart
* Creates a new timer in the system with the given attributes.
*
* @param pTimer Pointer to timer control block containing the attributes of the timer to be
* created.
*
* @return none
*
* @see ASFDeleteTimer()
***************************************************************************************************/
static void _TimerStart ( AsfTimer *pTimer, char *_file, int _line )
{
uint16_t info = (uint16_t)((uint32_t)pTimer); //We only need to store the LSB16 of the pointer as the system RAM is < 64K
ASF_assert( pTimer != NULLP );
ASF_assert( pTimer->sysUse != TIMER_SYS_ID ); //In case we are trying to restart a running timer
pTimer->sysUse = TIMER_SYS_ID;
pTimer->timerId = os_tmr_create( pTimer->ticks, info );
ASF_assert( pTimer->timerId != NULL );
}
/****************************************************************************************************
* @fn ASFKillTimer
* Kills the timer that was created earlier
*
* @param pTimer Pointer to timer control block containing the attributes of the timer to be
* created.
*
* @return none
*
* @see ASFTimerStart()
***************************************************************************************************/
void _ASFKillTimer ( AsfTimer *pTimer, char *_file, int _line )
{
osStatus os_ret = osErrorOS;
ASF_assert( pTimer != NULLP );
os_ret = osTimerDelete(pTimer->timerId);
ASF_assert( os_ret == osOK );
pTimer->sysUse = (uint32_t)-1; //Timer no longer in use
AsfTimerRemoveTimerFromList(pTimer->info);
}
/****************************************************************************************************
* @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 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);
}
int gpio_irq_init(gpio_irq_t *obj, PinName pin, gpio_irq_handler handler, uint32_t id) {
ASF_assert(pin != NC);
Chip_INMUX_PinIntSel(id, DECODE_PORT(pin), DECODE_PIN(pin));
gpio_irq_enable(obj);
return 0;
}
/**
* Configure pin (input, output, alternate function or analog) + output speed + AF
*/
void pin_function(PinName pin, uint32_t data) {
uint32_t port_index = DECODE_PORT(pin);
uint32_t pin_index = DECODE_PIN(pin);
ASF_assert(pin != (PinName)NC);
// Configure GPIO
Chip_IOCON_PinMuxSet(LPC_IOCON, port_index, pin_index, data);
}
static uint16_t AsfTimerAddTimerToList(AsfTimer *pTimer)
{
uint16_t i;
for ( i = 0; i < OS_TIMERCNT; i++ ) {
if ( _asfTimerList[i] == NULL ) {
_asfTimerList[i] = pTimer;
return i;
}
}
/* Trigger assert. Use too many timers. */
ASF_assert(FALSE);
}
/**
* Configure pin pull-up/pull-down
*/
void pin_mode(PinName pin, PinMode mode) {
#if 0
ASF_assert(pin != (PinName)NC);
GPIO_InitTypeDef GPIO_InitStructure;
uint32_t port_index = STM_PORT(pin);
uint32_t pin_index = STM_PIN(pin);
// Enable GPIO clock
uint32_t gpio_add = Set_GPIO_Clock(port_index);
GPIO_TypeDef *gpio = (GPIO_TypeDef *)gpio_add;
// Configure open-drain and pull-up/down
switch (mode) {
case AnalogInput:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
break;
case Floating:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
break;
case PullUp:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
break;
case PullDown:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
break;
case OpenDrain:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
break;
case PushPullOutput:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
break;
case OpenDrainOutputAF:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
break;
case PushPullOutputAF:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
break;
default:
D1_printf("PINMAP: Bad GPIO Mode: %d\r\n", mode);
break;
}
// Configure GPIO
GPIO_InitStructure.GPIO_Pin = (uint16_t)pin_index;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(gpio, &GPIO_InitStructure);
#endif
}
/****************************************************************************************************
* @fn i2cslave_rx
* Slave data receive callback function
*
***************************************************************************************************/
static uint8_t i2cslave_rx(uint8_t data)
{
uint8_t *p8;
uint8_t ret_val = I2C_SLVCTL_SLVCONTINUE;
i2c_t *obj = pi2c_slave_obj; /* get the record*/
obj->i2c_operation = I2C_READ_IN_PROGRESS; /* read operation */
p8 = obj->pXfer.rxBuff;
p8[obj->pXfer.bytesRecv++] = (uint8_t) data;
ASF_assert(obj->pXfer.bytesRecv < RX_LENGTH);
return ret_val;
}
/**********************************************************************
* @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 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 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
}
}
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;
}
}
/****************************************************************************************************
* @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;
OSP_GetVersion(&version);
D1_printf("OSP Version: %s\r\n", version->VersionString);
OSP_Status = OSP_Initialize(&gSystemDesc);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_Initialize Failed");
// Register the input sensors
OSP_Status = OSP_RegisterInputSensor(&_AccSensDesc, &_AccHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_RegisterSensor (accel) Failed");
OSP_Status = OSP_RegisterInputSensor(&_MagSensDesc, &_MagHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_RegisterSensor (mag) Failed");
OSP_Status = OSP_RegisterInputSensor(&_GyroSensDesc, &_GyroHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_RegisterSensor (gyro) Failed");
// Register output sensors/results
OSP_Status = OSP_SubscribeOutputSensor(&stepCounterRequest, &_stepCounterHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_STEP_COUNTER) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&sigMotionRequest, &_sigMotionHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_CONTEXT_DEVICE_MOTION) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&UnCalAccelRequest, &_unCalAccelHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_ACCELEROMETER) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&UnCalMagRequest, &_unCalMagHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_MAGNETIC_FIELD) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&UnCalGyroRequest, &_unCalGyroHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_GYROSCOPE) Failed");
while (1)
{
ASFReceiveMessage( ALGORITHM_TASK_ID, &rcvMsg );
switch (rcvMsg->msgId)
{
case MSG_MAG_DATA:
SendBgTrigger();
case MSG_ACC_DATA:
case MSG_GYRO_DATA:
HandleSensorData(rcvMsg);
do
{
OSP_Status = OSP_DoForegroundProcessing();
ASF_assert(OSP_Status != OSP_STATUS_ERROR);
} while(OSP_Status != OSP_STATUS_IDLE)
; //keep doing foreground computation until its finished
break;
default:
/* Unhandled messages */
D1_printf("Alg-FG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
}
}
