void FPGA_read(void)
{
Uint16_t countnum, *FpgaRxaddr, Rx[32];
if (g_commflagcount.FpgaReadFlag)
{
g_commflagcount.FpgaReadFlag = 0;
//讀操作
FpgaRxaddr = (Uint16_t *) FPGA_READ_ADDR;
for(countnum=0; countnum<32; countnum++)
{
Rx[countnum] = *(FpgaRxaddr);
FpgaRxaddr = FpgaRxaddr + 2;
}
if ((Rx[0]&0x00FF) == 0x7E)
{
stCommErrCode.nFuse = 0;
FpgaReadBuf = *((FPGA_READ_BUF *)(&Rx[0]));
}
else
{
stCommErrCode.nFuse =1;
}
//------------------------------
// volatile Uint32_t temp;
// temp = (FpgaReadBuf.head & 0xFF00)>>8;
// extern Uint32_t addr1;
// RamWriteFourByte(addr1, (void*)&temp); //for test80300000
// addr1 += 2;
//------------------------------
stEncodeSolveData.nAi_200hz = stEncodeSolveData.nAi;
stEncodeSolveData.nEi_200hz = stEncodeSolveData.nEi;
stCommSolveData.nMissAi = FpgaReadBuf.nAi - stEncodeSolveData.nAi;
stCommSolveData.nMissEi = FpgaReadBuf.nEi - stEncodeSolveData.nEi;
if (stCommSolveData.nMissAi > 9320675)
{
stCommSolveData.nMissAi -= 0x1000000;
}
else if(stCommSolveData.nMissAi < -9320675)
{
stCommSolveData.nMissAi += 0x1000000;
}
if (stCommSolveData.nMissEi > 9320675)
{
stCommSolveData.nMissEi -= 0x1000000;
}
else if(stCommSolveData.nMissEi < -9320675)
{
stCommSolveData.nMissEi += 0x1000000;
}
// if (g_commflagcount.ButtonState == SLAVEMODE && g_configPM.g_state.Astart)
// {
// RamWriteFourByte(addr1, (void*)&FpgaReadBuf.nAi); //for test80300000
// addr1 += 2;
// RamWriteFourByte(addr3, (void*)&stCommSolveData.nMissAi); //for test80500000
// addr3 += 2;
// RamWriteFourByte(addr4, (void*)&stEncodeSolveData.nAi); //for test80600000
// addr4 += 2;
// }
}
}
//
// FPGA_write: return DSP data to fuse PC through FPGA -by:fyb
//
void FPGA_write(void)
{
Uint16_t button, FpgaTx[32], countnumWR, *FpgaTxaddr;
Int32_t StickVelA, StickVelE;
static Uint16_t CycleCode;
if (g_commflagcount.FpgaWriteFlag)
{
g_commflagcount.FpgaWriteFlag = 0;
CycleCode ++;
if (CycleCode > 199)
{
CycleCode = 0;
}
switch (g_commflagcount.ButtonState)
{
case STICKMODE://單桿
button = 0x0080;
break;
case SLAVEMODE://數引
button = 0x0040;
break;
case INFRAREDMODE://中波紅外
button = 0x0020;
break;
case LONGFOCUSMODE://可見
button = 0x0010;
break;
case FUSE://融合
button = 0x00A0;
break;
case COLORLIVEMODE://彩色實況
button = 0x00C0;
break;
case SHORTNFRAREDMODE://短波
button = 0x00E0;
break;
case SPACETARGETMODE: //空間目標 后加0409
button = 0x00B0;
break;
case SPACESEARCHMODE: //空間搜索 后加0409
button = 0x00D0;
break;
default:
break;
}
if (g_configPM.g_state.Astart)
{
button = button | 0x0002;
}
else
{
button = button & 0xFFFD;
}
if (g_configPM.g_state.Estart)
{
button = button | 0x0001;
}
else
{
button = button & 0xFFFE;
}
if (g_configPM.PWMwarning_A)
{
button = button | 0x0004;
}
else
{
button = button & 0xFFFB;
}
if (g_configPM.PWMwarning_E)
{
button = button | 0x0008;
}
else
{
button = button & 0xFFF7;
}
if (g_configPM.Elimit_0)//第4個字節第1位，1=限位，0=正常
{
button = button | 0x0100;
}
else
{
button = button & 0xFEFF;
}
if (g_configPM.Elimit_180)
{
button = button | 0x0200;
}
else
{
button = button & 0xFDFF;
}
if (g_configPM.manual)
{
button = button | 0x0400;
}
else
{
button = button & 0xFBFF;
}
if (stCommErrCount.nEncode > 5) // if the count of err-code occur over 5 times, disable PWM
{
button = button | 0x0800; // tell FPGA its not safe to output PWM
}
else
{
button = button & 0xF7FF; // tell FPGA its safe to output PWM
}
StickVelA = ((Int32_t)stStickValue.dbStick1A - stEncodeSolveData.nAa) * g_configPM.Encode_SampleHz;
StickVelE = ((Int32_t)stStickValue.dbStick1E - stEncodeSolveData.nEe) * g_configPM.Encode_SampleHz;
FpgaTx[0] = (CycleCode << 8) | 0x007E;
FpgaTx[1] = button;
FpgaTx[2] = (Uint16_t)((Int32_t)stStickValue.dbStick1A & 0x0000FFFF);
FpgaTx[3] = (Uint16_t)((Int32_t)stStickValue.dbStick1A >> 16);
FpgaTx[4] = (Uint16_t)((Int32_t)stStickValue.dbStick1E & 0x0000FFFF);
FpgaTx[5] = (Uint16_t)((Int32_t)stStickValue.dbStick1E >> 16);
FpgaTx[6] = (Uint16_t)(stCommSolveData.nMissAi & 0x0000FFFF);
FpgaTx[7] = (Uint16_t)(stCommSolveData.nMissAi >> 16);
FpgaTx[8] = (Uint16_t)(stCommSolveData.nMissEi & 0x0000FFFF);
FpgaTx[9] = (Uint16_t)(stCommSolveData.nMissEi >> 16);
FpgaTx[10] = (Uint16_t)(StickVelA & 0x0000FFFF);
FpgaTx[11] = (Uint16_t)(StickVelA >> 16);
FpgaTx[12] = (Uint16_t)(StickVelE & 0x0000FFFF);
FpgaTx[13] = (Uint16_t)(StickVelE >> 16);
FpgaTx[14] = (Uint16_t)(stEncodeSolveData.nAi_3 & 0x0000FFFF);
FpgaTx[15] = (Uint16_t)(stEncodeSolveData.nAi_3 >> 16);
FpgaTx[16] = (Uint16_t)(stEncodeSolveData.nEi_3 & 0x0000FFFF);
FpgaTx[17] = (Uint16_t)(stEncodeSolveData.nEi_3 >> 16);
FpgaTx[18] = (Uint16_t)(stEncodeSolveData.nAi_2 & 0x0000FFFF);
FpgaTx[19] = (Uint16_t)(stEncodeSolveData.nAi_2 >> 16);
FpgaTx[20] = (Uint16_t)(encoderSyncInterval[4]);
FpgaTx[21] = (Uint16_t)(encoderSyncInterval[3]);
FpgaTx[22] = (Uint16_t)(encoderSyncInterval[2]);
FpgaTx[23] = (Uint16_t)(encoderSyncInterval[1]);
FpgaTx[24] = (Uint16_t)(encoderSyncInterval[0]);
FpgaTx[25] = (Uint16_t)(fuseRxSyncInterval);
FpgaTx[26] = (Uint16_t)(stEncodeSolveData.nAi & 0x0000FFFF);
FpgaTx[27] = (Uint16_t)(stEncodeSolveData.nAi >> 16);
FpgaTx[28] = (Uint16_t)(stEncodeSolveData.nEi & 0x0000FFFF);
FpgaTx[29] = (Uint16_t)(stEncodeSolveData.nEi >> 16);
FpgaTx[30] = FpgaReadBuf.head & 0xFF00;
FpgaTx[31] = 0xE700;
//------------------------------
// extern Uint32_t addr2;
// RamWriteFourByte(addr2, (void*)&button); //for test
// addr2 = addr2 + 2;
//------------------------------
FpgaTxaddr = (Uint16_t *) FPGA_WRITE_ADDR;
//寫操作，寫之后給個上升沿
GPIO_WritePin(19, 0);//寫之前拉低
for(countnumWR=0; countnumWR<32; countnumWR++)
{
*FpgaTxaddr = FpgaTx[countnumWR];
FpgaTxaddr = FpgaTxaddr + 2;
}
GPIO_WritePin(19, 1);//寫之后拉高
GPIO_WritePin(19, 0);//寫之前拉低
*((Uint16_t *)FPGA_HANDLE_ADDR) = 0x0066;
*((Uint16_t *)(FPGA_HANDLE_ADDR + 2)) = 0x0077;
GPIO_WritePin(19, 1);//寫之前拉低8
}
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
