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;
}
// record current state to telemU structure
void telemSaveSample(unsigned long sampIdx)
{ telemU data;
data.telemStruct.sampleIndex = sampIdx;
//Stopwatch was already started in the cmdSpecialTelemetry function
data.telemStruct.timeStamp = (long)swatchTic();
// since T1 has higher priority, these state readings might get interrupted
CRITICAL_SECTION_START // need coherent sample without T1 int updates
// save Hall encoder position instead of commanded thrust
data.telemStruct.posL = pidObjs[0].p_state;
data.telemStruct.posR = pidObjs[1].p_state;
// save output instead of reading PWM (sync issue?)
data.telemStruct.dcL = pidObjs[0].output; // left
data.telemStruct.dcR = pidObjs[1].output; // right
data.telemStruct.bemfL = bemf[0];
data.telemStruct.bemfR = bemf[1];
CRITICAL_SECTION_END
data.telemStruct.gyroX = gdata[0] - offsx;
data.telemStruct.gyroY = gdata[1] - offsy;
data.telemStruct.gyroZ = gdata[2] - offsz;
data.telemStruct.gyroAvg = gyroAvg;
data.telemStruct.accelX = xldata[0];
data.telemStruct.accelY = xldata[1];
data.telemStruct.accelZ = xldata[2];
data.telemStruct.Vbatt = (int) adcGetVbatt();
data.telemStruct.sOut = steeringPID.output;
// inside T5 interrupt, so don't need to DisableIntT5
telemFlashSample(&data);
}
// calibrate A/D offset, using PWM synchronized A/D reads inside
// timer 1 interrupt loop
// BATTERY CHANGED FOR IP2.5 ***** need to fix
void calibBatteryOffset(int spindown_ms){
long temp; // could be + or -
unsigned int battery_voltage;
// save current PWM config
int tempPDC1 = PDC1;
int tempPDC2 = PDC2;
PDC1 = 0; PDC2 = 0; /* SFR for PWM? */
// save current PID status, and turn off PID control
short tempPidObjsOnOff[NUM_PIDS];
tempPidObjsOnOff[0] = pidObjs[0].onoff;
tempPidObjsOnOff[1] = pidObjs[1].onoff;
pidObjs[0].onoff = 0; pidObjs[1].onoff = 0;
delay_ms(spindown_ms); //motor spin-down
LED_RED = 1;
offsetAccumulatorL = 0;
offsetAccumulatorR = 0;
offsetAccumulatorCounter = 0; // updated inside servo loop
calib_flag = 1; // enable calibration
while(offsetAccumulatorCounter < 100); // wait for 100 samples
calib_flag = 0; // turn off calibration
battery_voltage = adcGetVbatt();
//Left
temp = offsetAccumulatorL;
temp = temp/(long)offsetAccumulatorCounter;
pidObjs[0].inputOffset = (int) temp;
//Right
temp = offsetAccumulatorR;
temp = temp/(long)offsetAccumulatorCounter;
pidObjs[1].inputOffset = (int) temp;
LED_RED = 0;
// restore PID values
PDC1 = tempPDC1;
PDC2 = tempPDC2;
pidObjs[0].onoff = tempPidObjsOnOff[0];
pidObjs[1].onoff = tempPidObjsOnOff[1];
}