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 __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) {
int j;
LED_3 = 1;
interrupt_count++;
if(interrupt_count == 4) {
mpuBeginUpdate();
amsEncoderStartAsyncRead();
} else if(interrupt_count == 5) {
interrupt_count = 0;
if (t1_ticks == T1_MAX) t1_ticks = 0;
t1_ticks++;
pidGetState(); // always update state, even if motor is coasting
for (j = 0; j< NUM_PIDS; j++) {
// only update tracking setpoint if time has not yet expired
if (pidObjs[j].onoff) {
if (pidObjs[j].timeFlag){
if (pidObjs[j].start_time + pidObjs[j].run_time >= t1_ticks){
pidGetSetpoint(j);
}
if(t1_ticks > lastMoveTime){ // turn off if done running all legs
pidObjs[0].onoff = 0;
pidObjs[1].onoff = 0;
}
}
else {
pidGetSetpoint(j);
}
}
}
pidSetControl();
if(pidObjs[0].onoff) {
telemGetPID();
// uart_tx_packet = ppoolRequestFullPacket(sizeof(telemStruct_t));
// if(uart_tx_packet != NULL) {
// //time|Left pstate|Right pstate|Commanded Left pstate| Commanded Right pstate|DCR|DCL|RBEMF|LBEMF|Gyrox|Gyroy|Gyroz|Ax|Ay|Az
// //bytes: 4,4,4,4,4,2,2,2,2,2,2,2,2,2,2
// paySetType(uart_tx_packet->payload, CMD_PID_TELEMETRY);
// paySetStatus(uart_tx_packet->payload, 0);
// paySetData(uart_tx_packet->payload, sizeof(telemStruct_t), (unsigned char *) &telemPIDdata);
// uart_tx_flag = 1;
}
}
LED_3 = 0;
_T1IF = 0;
}
