Beispiel #1
0
/*********************************************************************
 * @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
}
Beispiel #5
0
/*********************************************************************
 * @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;
}
Beispiel #7
0
/*********************************************************************
 * @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;
}
Beispiel #8
0
/*********************************************************************
 * @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;
}
Beispiel #9
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
    }
}
Beispiel #10
0
/******************************************************************************
 * @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);
    }
}
Beispiel #11
0
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);
}
Beispiel #12
0
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;
        }
    }
}
Beispiel #14
0
/* 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 );
    }
}
Beispiel #15
0
/***********************************************************************
 * @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);
    }
}
Beispiel #16
0
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;
    }
}