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