Esempio n. 1
0
static void imuISRHandler() {

    LED_GREEN = 1;

    int gyroData[3];
    int xlData[3];

#if defined (__IMAGEPROC24)
            /////// Get Gyro data and calc average via filter
            gyroReadXYZ(); //bad design of gyro module; todo: humhu
    gyroGetIntXYZ(gyroData);
#elif defined (__IMAGEPROC25)
    mpuBeginUpdate();
    mpuGetGyro(gyroData);
    mpuGetXl(xlData);
#endif


    lastGyro[0] = gyroData[0];
    lastGyro[1] = gyroData[1];
    lastGyro[2] = gyroData[2];

    lastXL[0] = xlData[0];
    lastXL[1] = xlData[1];
    lastXL[2] = xlData[2];


    int i;
    for (i = 0; i < 3; i++) {
        //Threshold:
        if (ABS(lastGyro[i]) < GYRO_DRIFT_THRESH) {
            lastGyro[0] = lastGyro[i] >> 1; //fast divide by 2
        }
    }
Esempio n. 2
0
void telemGetPID(){

    telemPIDdata.posL = pidObjs[0].p_state;
    telemPIDdata.posR = pidObjs[1].p_state;
    telemPIDdata.composL = pidObjs[0].p_input + pidObjs[0].interpolate;
    telemPIDdata.composR = pidObjs[1].p_input + pidObjs[1].interpolate;
    telemPIDdata.dcL = pidObjs[0].output; // left
    telemPIDdata.dcR = pidObjs[1].output; // right
    telemPIDdata.bemfL = bemf[0];
    telemPIDdata.bemfR = bemf[1];

    mpuGetGyro(gdata);
    mpuGetXl(xldata);

    telemPIDdata.gyroX = gdata[0];
    telemPIDdata.gyroY = gdata[1];
    telemPIDdata.gyroZ = gdata[2];
    telemPIDdata.accelX = xldata[0];
    telemPIDdata.accelY = xldata[1];
    telemPIDdata.accelZ = xldata[2];
    telemPIDdata.Vbatt = (int) adcGetVbatt();

    // Save Data to flash
    if (samplesToSave > 0) {
        telemPIDdata.timestamp = sclockGetTime() - telemStartTime;
        telemPIDdata.sampleIndex = sampIdx;

        telemSaveData(&telemPIDdata);
        sampIdx++;
    }

    return;
}