Example #1
0
/*********************************************************************
 * @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;
        }
    }
}