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 } }
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; }