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