avixDeclareISR(_IC2Interrupt, no_auto_psv){
// M_ToggleIO(LED3);
// LED3 = C_ON;
is_complete = 1;
ReadCapture2(&edge_buffer_down[temp_edge_index_2++]);
if (temp_edge_index_2 >= 200)temp_edge_index_2 = 0;
// IC2_Clear_Intr_Status_Bit;
IFS0bits.IC2IF = 0;
}
void __ISR(_INPUT_CAPTURE_2_VECTOR) INT_IC2_Handler(void)
{
mIC2ClearIntFlag();
//Read captured timer values
if(mIC2CaptureReady())
{
IC2_CT_Rise = mIC2ReadCapture();
IC2_CT_Fall = mIC2ReadCapture();
}
if(mIC2CaptureReady())
ReadCapture2(garbage); //clear any remaining capture values
//Determine period
if(IC2_CT_Rise > IC2_CT_Fall)
period2 = (IC2_CT_Fall + PER_REG) - IC2_CT_Rise;
else
period2 = IC2_CT_Fall - IC2_CT_Rise;
//Correct period if out of range
if(period2 > YAW_H - 3)
period2 = YAW_H;
else if(period2 < YAW_L + 3)
period2 = YAW_L;
//Turn period into a percentage 0.0 thru 100.0% and
//a Yaw angle -180.0 thru 180.0 degrees
if(zero_mode == 0)
{
if(period2 > ym + 5)
{
yaw_pc = ((YAW_H + period2 - (2.0 * ym)) / (YAW_H - ym)) * 50.0;
IC_YAW = 2 * PI * ((yaw_pc - 50.0) / 100.0);
}
else if(period2 < ym - 5)
{
yaw_pc = ((period2 - YAW_L) / (ym - YAW_L)) * 50.0;
IC_YAW = -2.0 * PI * ((50.0 - yaw_pc) / 100.0);
}
else
{
yaw_pc = 50.0;
IC_YAW = 0.0;
}
}
}
