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