static void USB_CCID_GetDataFromUSB (void)
{
    int i;
    int USB_Datalen_s32;

    // Bool cbw_error;

    Usb_reset_endpoint_fifo_access (EP_CCID_OUT);

    // Get all data from USB
    USB_CCID_data_st.CCID_datalen = 0;

    while (Is_usb_out_received (EP_CCID_OUT))
    {
        Usb_reset_endpoint_fifo_access (EP_CCID_OUT);

        USB_Datalen_s32 = Usb_byte_count (EP_CCID_OUT);
        USB_Log_st.CCID_WriteCalls_u32++;
        USB_Log_st.CCID_BytesWrite_u32 += USB_Datalen_s32;

        // CI_TickLocalPrintf ("Get CCID USB block %3d byte - %3d\n",USB_Datalen_s32,USB_CCID_data_st.CCID_datalen);

        if (CCID_MAX_XFER_LENGTH <= USB_Datalen_s32 + USB_CCID_data_st.CCID_datalen)    // Check for oversize
        {
            CI_LocalPrintf ("*** CCID buffer to small %d ***\n", CCID_MAX_XFER_LENGTH);
            Usb_ack_out_received_free (EP_CCID_OUT);
            return;
        }

        for (i = 0; i < USB_Datalen_s32; i++)
        {
            USB_CCID_data_st.USB_data[USB_CCID_data_st.CCID_datalen] = Usb_read_endpoint_data (EP_CCID_OUT, 8);
            USB_CCID_data_st.CCID_datalen++;
        }

        Usb_ack_out_received_free (EP_CCID_OUT);
        DelayMs (1);
    }

    LED_RedOn ();

//    USB_CCID_Datalen_s32 = USB_CCID_data_st.CCID_datalen;

    USB_to_CRD_DispatchUSBMessage_v (&USB_CCID_data_st);

//    memset (USB_CCID_data_st.USB_data,0,USB_Datalen_s32);
    LED_RedOff ();

    // Usb_ack_out_received_free(EP_CCID_OUT);

}
Esempio n. 2
0
int main(void)
{
    /* initialize the core clock and the systick timer */
    InitClock();
    InitSysTick();

    /* initialize the RGB led */
    LED_Init();

    /* Initialize UART0 */
    InitUart0();

    /* double rainbow all across the sky */
    DoubleFlash();

    /* initialize the I2C bus */
    I2C_Init();

#if DATA_FUSE_MODE

    /* signaling for fusion */
    FusionSignal_Init();

#endif // DATA_FUSE_MODE

    /* initialize UART fifos */
    RingBuffer_Init(&uartInputFifo, &uartInputData, UART_RX_BUFFER_SIZE);
    RingBuffer_Init(&uartOutputFifo, &uartOutputData, UART_TX_BUFFER_SIZE);

    /* initialize UART0 interrupts */
    Uart0_InitializeIrq(&uartInputFifo, &uartOutputFifo);
    Uart0_EnableReceiveIrq();

    /* initialize I2C arbiter */
    InitI2CArbiter();

    /* initialize the IMUs */
    InitHMC5883L();
    InitMPU6050();
//    InitMPU6050();

#if ENABLE_MMA8451Q
    InitMMA8451Q();
#endif

    /* Wait for the config messages to get flushed */
    //TrafficLight();
    DoubleFlash();
    RingBuffer_BlockWhileNotEmpty(&uartOutputFifo);

#if ENABLE_MMA8451Q
    /* initialize the MMA8451Q data structure for accelerometer data fetching */
    mma8451q_acc_t acc;
    MMA8451Q_InitializeData(&acc);
#endif

    /* initialize the MPU6050 data structure */
    mpu6050_sensor_t accgyrotemp, previous_accgyrotemp;
    MPU6050_InitializeData(&accgyrotemp);
    MPU6050_InitializeData(&previous_accgyrotemp);

    /* initialize the HMC5883L data structure */
    hmc5883l_data_t compass, previous_compass;
    HMC5883L_InitializeData(&compass);
    HMC5883L_InitializeData(&previous_compass);

    /* initialize HMC5883L reading */
    uint32_t lastHMCRead = 0;
    const uint32_t readHMCEvery = 1000 / 75; /* at 75Hz, data come every (1000/75Hz) ms. */

    /************************************************************************/
    /* Fetch scaler values                                                  */
    /************************************************************************/

#if DATA_FUSE_MODE

    const fix16_t mpu6050_accelerometer_scaler = mpu6050_accelerometer_get_scaler();
    const fix16_t mpu6050_gyroscope_scaler = mpu6050_gyroscope_get_scaler();
    const fix16_t hmc5883l_magnetometer_scaler = hmc5883l_magnetometer_get_scaler();

#endif // DATA_FUSE_MODE

    /************************************************************************/
    /* Prepare data fusion                                                  */
    /************************************************************************/

#if DATA_FUSE_MODE

    uint32_t last_transmit_time = 0;
    uint32_t last_fusion_time = systemTime();

    fusion_initialize();

#endif // DATA_FUSE_MODE

    /************************************************************************/
    /* Main loop                                                            */
    /************************************************************************/

    for(;;)
    {
        /* helper variables to track data freshness */
        uint_fast8_t have_gyro_data = 0;
        uint_fast8_t have_acc_data = 0;
        uint_fast8_t have_mag_data = 0;

        /************************************************************************/
        /* Determine if sensor data fetching is required                        */
        /************************************************************************/

        /* helper variables for event processing */
        int eventsProcessed = 0;
        int readMPU, readHMC;
#if ENABLE_MMA8451Q
        int readMMA;
#endif

        /* atomic detection of fresh data */
        __disable_irq();
#if ENABLE_MMA8451Q
        readMMA = poll_mma8451q;
#endif
        readMPU = poll_mpu6050;
        poll_mma8451q = 0;
        poll_mpu6050 = 0;
        __enable_irq();

        /* detection of HMC read */
        /*
         * TODO: read synchronized with MPU
         */
        readHMC = 0;
        uint32_t time = systemTime();
        if ((time - lastHMCRead) >= readHMCEvery)
        {
            readHMC = 1;
            lastHMCRead = time;
        }

        /************************************************************************/
        /* Fetching MPU6050 sensor data if required                             */
        /************************************************************************/

        /* read accelerometer/gyro */
        if (readMPU)
        {
            LED_BlueOff();

            I2CArbiter_Select(MPU6050_I2CADDR);
            MPU6050_ReadData(&accgyrotemp);

            /* mark event as detected */
            eventsProcessed = 1;

            /* check for data freshness */
            have_acc_data = (accgyrotemp.accel.x != previous_accgyrotemp.accel.x)
                            || (accgyrotemp.accel.y != previous_accgyrotemp.accel.y)
                            || (accgyrotemp.accel.z != previous_accgyrotemp.accel.z);

            have_gyro_data = (accgyrotemp.gyro.x != previous_accgyrotemp.gyro.x)
                             || (accgyrotemp.gyro.y != previous_accgyrotemp.gyro.y)
                             || (accgyrotemp.gyro.z != previous_accgyrotemp.gyro.z);

            /* loop current data --> previous data */
            previous_accgyrotemp = accgyrotemp;
        }

        /************************************************************************/
        /* Fetching HMC5883L sensor data if required                            */
        /************************************************************************/

        /* read compass data */
        if (readHMC)
        {
            I2CArbiter_Select(HMC5883L_I2CADDR);
            HMC5883L_ReadData(&compass);

            /* mark event as detected */
            eventsProcessed = 1;

            /* check for data freshness */
            have_mag_data = (compass.x != previous_compass.x)
                            || (compass.y != previous_compass.y)
                            || (compass.z != previous_compass.z);

            /* loop current data --> previous data */
            previous_compass = compass;
        }

        /************************************************************************/
        /* Fetching MMA8451Q sensor data if required                            */
        /************************************************************************/

#if ENABLE_MMA8451Q
        /* read accelerometer */
        if (readMMA)
        {
            LED_RedOff();

            I2CArbiter_Select(MMA8451Q_I2CADDR);
            MMA8451Q_ReadAcceleration14bitNoFifo(&acc);

            /* mark event as detected */
            eventsProcessed = 1;
        }
#endif

        /************************************************************************/
        /* Raw sensor data output over serial                                   */
        /************************************************************************/

#if DATA_FETCH_MODE

        /* data availability + sanity check
         * This sent me on a long bug hunt: Sometimes the interrupt would be raised
         * even if not all data registers were written. This always resulted in a
         * z data register not being fully written which, in turn, resulted in
         * extremely jumpy measurements.
         */
        if (readMPU && accgyrotemp.status != 0)
        {
            /* write data */
            uint8_t type = 0x02;
            P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)accgyrotemp.data, sizeof(accgyrotemp.data), IO_SendByte);
        }

        /* data availability + sanity check */
        if (readHMC && (compass.status & HMC5883L_SR_RDY_MASK) != 0) /* TODO: check if not in lock state */
        {
            uint8_t type = 0x03;
            P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)compass.xyz, sizeof(compass.xyz), IO_SendByte);
        }

#if ENABLE_MMA8451Q
        /* data availability + sanity check */
        if (readMMA && acc.status != 0)
        {
            uint8_t type = 0x01;
            P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)acc.xyz, sizeof(acc.xyz), IO_SendByte);
        }
#endif

#endif // DATA_FETCH_MODE

        /************************************************************************/
        /* Sensor data fusion                                                   */
        /************************************************************************/

#if DATA_FUSE_MODE

        // if there were sensor data ...
        if (eventsProcessed)
        {
            v3d gyro, acc, mag;

            // convert, calibrate and store gyroscope data
            if (have_gyro_data)
            {
                sensor_prepare_mpu6050_gyroscope_data(&gyro, accgyrotemp.gyro.x, accgyrotemp.gyro.y, accgyrotemp.gyro.z, mpu6050_gyroscope_scaler);
                fusion_set_gyroscope_v3d(&gyro);
            }

            // convert, calibrate and store accelerometer data
            if (have_acc_data)
            {
                sensor_prepare_mpu6050_accelerometer_data(&acc, accgyrotemp.accel.x, accgyrotemp.accel.y, accgyrotemp.accel.z, mpu6050_accelerometer_scaler);
                fusion_set_accelerometer_v3d(&acc);
            }

            // convert, calibrate and store magnetometer data
            if (have_mag_data)
            {
                sensor_prepare_hmc5883l_data(&mag, compass.x, compass.y, compass.z, hmc5883l_magnetometer_scaler);
                fusion_set_magnetometer_v3d(&mag);
            }

            // get the time differential
            const uint32_t current_time = systemTime();
            const fix16_t deltaT_ms = fix16_from_int(current_time - last_fusion_time);
            const fix16_t deltaT = fix16_mul(deltaT_ms, F16(0.001));

            last_fusion_time = current_time;

            FusionSignal_Predict();

            // predict the current measurements
            fusion_predict(deltaT);

            FusionSignal_Update();

            // correct the measurements
            fusion_update(deltaT);


            FusionSignal_Clear();

#if 0

            fix16_t yaw, pitch, roll;
            fusion_fetch_angles(&roll, &pitch, &yaw);

#if 0
            float yawf = fix16_to_float(yaw),
                  pitchf = fix16_to_float(pitch),
                  rollf = fix16_to_float(roll);

            IO_SendInt16((int16_t)yawf);
            IO_SendInt16((int16_t)pitchf);
            IO_SendInt16((int16_t)rollf);

            IO_SendByteUncommited('\r');
            IO_SendByte('\n');
#else
            if (current_time - last_transmit_time >= 100)
            {
                /* write data */
                uint8_t type = 42;
                fix16_t buffer[3] = { roll, pitch, yaw };
                P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);

                last_transmit_time = current_time;
            }
#endif
#else
            if (current_time - last_transmit_time >= 100)
            {
                /* write data */
                switch (output_mode)
                {
                case RPY:
                {
                    fix16_t roll, pitch, yaw;
                    fusion_fetch_angles(&roll, &pitch, &yaw);

                    /* write data */
                    uint8_t type = 42;
                    fix16_t buffer[3] = { roll, pitch, yaw };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                case QUATERNION:
                {
                    qf16 orientation;
                    fusion_fetch_quaternion(&orientation);

                    uint8_t type = 43;
                    fix16_t buffer[4] = { orientation.a, orientation.b, orientation.c, orientation.d };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                case QUATERNION_RPY:
                {
                    fix16_t roll, pitch, yaw;
                    fusion_fetch_angles(&roll, &pitch, &yaw);

                    qf16 orientation;
                    fusion_fetch_quaternion(&orientation);

                    uint8_t type = 44;
                    fix16_t buffer[7] = { orientation.a, orientation.b, orientation.c, orientation.d, roll, pitch, yaw };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                case SENSORS_RAW:
                {
                    uint8_t type = 0;
                    fix16_t buffer[6] = { acc.x, acc.y, acc.z, mag.x, mag.y, mag.z };
                    P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
                    break;
                }
                }

                last_transmit_time = current_time;
            }
#endif

        }

#endif // DATA_FUSE_MODE

        /************************************************************************/
        /* Read user data input                                                 */
        /************************************************************************/

        /* as long as there is data in the buffer */
        while(!RingBuffer_Empty(&uartInputFifo))
        {
            /* light one led */
            LED_RedOn();

            /* fetch byte */
            uint8_t data = IO_ReadByte();

            output_mode = (output_mode_t)data;

            LED_RedOff();
#if 0
            /* echo to output */
            IO_SendByte(data);

            /* mark event as detected */
            eventsProcessed = 1;
#endif
        }

        /************************************************************************/
        /* Save energy if you like to                                           */
        /************************************************************************/

        /* in case of no events, allow a sleep */
        if (!eventsProcessed)
        {
            /*
             * Care must be taken with this instruction here, as it can lead
             * to a condition where after being woken up (e.g. by the SysTick)
             * and looping through, immediately before entering WFI again
             * an interrupt would yield a true condition for the branches below.
             * In this case this loop would be blocked until the next IRQ,
             * which, in case of a 1ms SysTick timer, could be too late.
             *
             * To counter this behaviour, SysTick has been speed up by factor
             * four (0.25ms).
             */
#if 0
            __WFI();
#endif
        }
    }

    return 0;
}