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;
}
static void telemISRHandler() {
//skipcounter decrements to 0, triggering a telemetry save, and resets
// value of skicounter
if (samplesToSave > 0) {
telemBuffer.timestamp = sclockGetTime() - telemStartTime;
telemBuffer.sampleIndex = sampIdx;
//Write telemetry data into packet
//TELEMPACKFUNC((unsigned char*) &(telemBuffer.telemData));
TELEMPACKFUNC( &(telemBuffer.telemData) );
telemSaveData(&telemBuffer);
sampIdx++;
}
}
