Collect/Core/Src/rkg.c

#include "rkg.h"
#include "config.h"
#include "main.h"
#include "cang.h"




uint8_t RKG_Max = 0;
uint16_t RefrenceLast = 0;
uint16_t DgLast[16];
uint16_t XgLast[16];







S_ANGLEDATA gs_AngleData[SENSOR_DEEP];
RKG_Inf rkg_inf[SENSOR_DEEP];
Cang_Inf cang_inf;



void LIB_MemClr(uint8_t* pucDst, uint16_t uiLen)
{
    while(uiLen > 0)
    {
        *pucDst++ = 0;
        uiLen--;
    }
}

/*******************************************************************************************************
**函数名称	int32 AGL_CalcDelta(uint32 ulAngle1, uint32 ulAngle2)
**输入:	ulAngle1, 角度1
**      ulAngle2,角度2
**输出: 实际角差
**说明: 计算当前时间的角差
**------------------------------------------------------------------------------------------------------
** 作　者: pylon_zlq
** 日　期:    2019/6/26 17:20:59
********************************************************************************************************/
static int32_t AGL_CalcDelta(int32_t ulAngle1, int32_t ulAngle2, int32_t srDir,int32_t srNo)
{
	  S_ANGLEDATA* psAD = gs_AngleData;
    if(ulAngle1 == 0xFFFF || ulAngle2 == 0xFFFF)
    {
        return 0xFFFF;
    }

    int jz_angle = 0;
    int rkg_angle = 0;
    if(ulAngle1<18000)
    jz_angle = ulAngle1;
    else
    jz_angle = 36000 - ulAngle1;
/*
    if(jz_angle>MxMax)
        MxMax = jz_angle;
    else if(jz_angle<MxMin)
        MxMin = jz_angle;
*/
    if(jz_angle > 3300)           //基准误差处理
    {
      jz_angle = 3300;
    }

    if(jz_angle - RefrenceLast > 500)
    jz_angle = RefrenceLast + 500;
    else if(RefrenceLast - jz_angle > 500)
    jz_angle = RefrenceLast - 500;
    RefrenceLast = jz_angle;
    ulAngle1 = jz_angle;


    if(ulAngle2<18000)
     rkg_angle = ulAngle2;
    else
     rkg_angle = 36000 - ulAngle2;


    if(srDir > 1)
    {
 //       if(ulAngle2<18000&&ulAngle2>3300)           //小盖误差处理
 //           ulAngle2 = 3000;
 //       else if(ulAngle2> 18000&&ulAngle2<20000)
 //           ulAngle2 = 20000;
        if(rkg_angle- XgLast[srNo] > 500)
        rkg_angle = XgLast[srNo] + 500;
        else if(XgLast[srNo] - rkg_angle > 500)
        rkg_angle = XgLast[srNo] - 500;
        XgLast[srNo] = rkg_angle;
        ulAngle2 = rkg_angle;
    }
    else                                                    //小盖误差处理
    {
 //       if(ulAngle2<32700 && ulAngle2>27000)           //大盖误差处理
 //           ulAngle2 = 32700;
 //       else if(ulAngle2>20000 &&ulAngle2<27000)
 //           ulAngle2 = 20000;
        if(rkg_angle- DgLast[srNo] > 500)
        rkg_angle = DgLast[srNo] + 500;
        else if(DgLast[srNo] - rkg_angle > 500)
        rkg_angle = DgLast[srNo] - 500;
        DgLast[srNo] = rkg_angle;
        ulAngle2 = rkg_angle;
				if(ulAngle2>=ulAngle1)
				{
				 ulAngle2 -= ulAngle1; 
				 psAD[srNo].uiDG = ulAngle2;
				}
				else
				{
				 ulAngle1 -= ulAngle2; 	 
         psAD[srNo].uiDG = ulAngle1;
				}
    }


    int32_t lDelta;
    if(srDir == 0)         //两个角度传感器同方向
    {
        lDelta = ulAngle2 - ulAngle1;                   //同向角差不变

        if(lDelta < -18000)                             //这种情况是20 - 35900
        {
            lDelta += 36000;
        }
    }
    else
    {
        lDelta = ulAngle2 + ulAngle1;                   //反方向两角的和不变
        if(lDelta >= 36000)                             //这种情况是35900 + 35900
        {
            lDelta -= 36000;
        }
    }

    if(lDelta > 27000)
    {
        lDelta -= 36000;
    }

    return lDelta;
}

/*******************************************************************************************************
**函数名称	void AGL_CalcDeltaAll(void)
**输入:	void
**输出:
**说明:
**------------------------------------------------------------------------------------------------------
** 作　者: pylon_zlq
** 日　期:    2019/12/10 8:46:58
********************************************************************************************************/
void AGL_CalcDeltaAll(int32_t rkg_addr, uint16_t uiDGXGType)
{
		S_ANGLEDATA* psAD = gs_AngleData;
		int32_t srNo = 0;
		if(rkg_addr > 0x3F)
		{
			srNo +=(rkg_addr - 0x40);
		}
		else
		{
			srNo +=(rkg_addr - 0x30);
		}

		
    S_DELTA* p = psAD[srNo].sDelta + psAD[srNo].uiPos;

    p->iXG = p->iDG = -37000;
    if(psAD->uiRefrence != 0xFFFF)
    {
        if(psAD[0].uiXG != 0xFFFFFFFF)
        {
            p->iXG = AGL_CalcDelta(psAD[0].uiRefrence, psAD[srNo].uiXG, (uiDGXGType & BIT7)+2,srNo);
        }
        if(psAD[rkg_addr - 0x30].uiDG != 0xFFFFFFFF)
        {
            p->iDG = AGL_CalcDelta(psAD[0].uiRefrence, psAD[srNo].uiDG, uiDGXGType & BIT6,srNo);
        }
    }


    if(++psAD[srNo].uiPos >= ANGLE_DATA_CNT)
    {
        psAD[srNo].uiPos = 0;
    }
  //  psAD[srNo].uiXG = psAD[srNo].uiDG = 0xFFFF;
}

/*******************************************************************************************************
**函数名称	void AGL_Init(void)
**输入:
**输出:
**说明:
** 全局变量:
** 调用模块:
**------------------------------------------------------------------------------------------------------
** 作　者: pylon_zlq
** 日　期:    2019/5/18 10:33:47
********************************************************************************************************/
void RKG_Init(void)
{
		Cang_Inf* pcang = &cang_inf;
		RKG_Inf* prkg = rkg_inf;
    S_ANGLEDATA* p = gs_AngleData;
		uint16_t Flash_buf[20];
		uint8_t i = 0;
		
		Flash_ReadBytes(Flash_buf,ADDR_RKG_NUM,8);
		for(i = 0;i < pcang->Cang_Num;i++)
		{
			if(Flash_buf[i] != 0xFFFF)
				pcang->RKG_Num[i] = Flash_buf[i];

		}
		
		Flash_ReadBytes(Flash_buf,ADDR_RKGSENSOR_TYPE,1);
		if(Flash_buf[0] != 0xFFFF)
			pcang->RKG_Type = Flash_buf[0];
		Flash_ReadBytes(Flash_buf,ADDR_RKG_THRESHOLD,1);
		if(Flash_buf[0] == 0xFFFF)
		{
			prkg[0].RKG_Threshold = 600;//slm
			prkg[1].RKG_Threshold = 600;//slm
			prkg[2].RKG_Threshold = 600;//slm		
			prkg[3].RKG_Threshold = 600;//slm		
			prkg[4].RKG_Threshold = 600;//slm					
		}
		else
		{
			prkg[0].RKG_Threshold = Flash_buf[0];//slm
			prkg[1].RKG_Threshold = Flash_buf[0];//slm
			prkg[2].RKG_Threshold = Flash_buf[0];//slm		
			prkg[3].RKG_Threshold = Flash_buf[0];//slm
			prkg[4].RKG_Threshold = Flash_buf[0];//slm					
		
		}
			
    for(i=0; i<ANGLE_DEVICE_CNT; i++, p++)
    {
        p->uiPos = 0;
        p->uiRefrence = p->uiXG = p->uiDG = 0xFFFF;
        LIB_MemClr((void*)&p->sDelta, sizeof(S_DELTA)*ANGLE_DATA_CNT);
    }
		
		for(i = 0;i < pcang->Cang_Num;i++)
			RKG_Max=2 ;//+= pcang->RKG_Num[i]
}

/*******************************************************************************************************
**函数名称	void AGL_Calibration(uint16 uiAngle1, uint16 uiAngle2)
**输入:	uiAngle1, 角度1
**      uiAngle2, 角度2
**输出: none
**说明: 标定
**------------------------------------------------------------------------------------------------------
** 作　者: pylon_zlq
** 日　期:    2019/6/26 16:57:15
********************************************************************************************************/
void AGL_Calibration(int32_t ulAngle1, int32_t ulAngle2)
{
    //DP_InnerWrite(&ulAngle1, DP_CID_ANGLE_STD_OFFSET);
    //DP_InnerWrite(&ulAngle2, DP_CID_ANGLE_XG_OFFSET);

    //gs_AngleFsm.iInitDelta = AGL_CalcDelta(ulAngle1, ulAngle2);
}

/*******************************************************************************************************
**函数名称	void AddNewAngleData(int16 iAngle1, int16 iAngle2, int16 iDelta)
**输入:	iAngle1,基准的倾角值
**      iAngle2,小盖上的倾角值
**      iDelta, 实际的两个角度值的差
**输出: true / false
**说明: 将获取的两个角度值进行队列存储,能存进来的必定是有效数据
** 全局变量:
** 调用模块:
**------------------------------------------------------------------------------------------------------
** 作　者: pylon_zlq
** 日　期:    2019/5/18 10:13:29
********************************************************************************************************/
void AGL_AddNewData(uint16_t uiAngle,int32_t rkg_addr)
{
		S_ANGLEDATA* psAD = gs_AngleData;
		
    if(rkg_addr == 0x30)																		//基准
			psAD[rkg_addr - 0x30].uiRefrence = uiAngle;  
		else if(rkg_addr >= 0x31 || rkg_addr <= 0x3f)						//大盖
			psAD[rkg_addr - 0x30].uiDG = uiAngle;
		else if(rkg_addr >= 0x41 || rkg_addr <= 0x4f)						//小盖
			psAD[rkg_addr - 0x30].uiXG = uiAngle;	
}

/*******************************************************************************************************
**函数名称	CPU_SR AGL_JudgeState(CPU_SR srNo, uint16 uiThreshold, CPU_SR srCnt)
**输入:	CPU_SR srNo, uint16 uiThreshold, CPU_SR srCnt
**输出:
**说明:
**------------------------------------------------------------------------------------------------------
** 作　者: pylon_zlq
** 日　期:    2019/12/11 9:38:37
********************************************************************************************************/
#define rkg_erro_uplimit    10
void AGL_JudgeState(void)
{
    S_ANGLEDATA* psAD = gs_AngleData;
    Cang_Inf* pcang = &cang_inf;
		int32_t i=1,j = 0;
		RKG_Inf* prkg = rkg_inf;
		
	

		for(i=1; i <= RKG_Max; i++)                       //SENSOR_DEEP:人孔大盖传感器数量，00可能做为基准传感器，所有从1开始循环
    {
			prkg[i].RKDG_Error = 0;

			prkg[i].RKXG_Error = 0;

//			prkg[i].RKDG_State = 0;
//			prkg[i].RKDG_StateCnt = 0;	
			prkg[i].RKXG_State = 0;   
			prkg[i].RKXG_StateCnt = 0;	                      //状态清零
			
			
			for(j = 1;j < 2;j++)//RKG_BUF_DEP
			{
				if(pcang->RKG_XG == 1)														//小盖
				{
//					if(psAD[i].sDelta[j].iXG <= -37000)
//					{
//							prkg[i].RKDG_ErrorCnt++;
//					}
//					else if(psAD[i].sDelta[j].iXG > prkg->RKG_Threshold)
//					{
//							prkg[i].RKXG_StateCnt++;
//					}
				}
				if((pcang->RKG_DG == 1)&&(pcang->RKG_Type == 0))			//倾角大盖，传感器数据处理后再进行判断
				{
					if(psAD[i].sDelta[j].iDG  <= -37000)
					{
							prkg[i].RKDG_ErrorCnt++;
					}
					if( - psAD[i].sDelta[j].iDG > prkg->RKG_Threshold || psAD[i].sDelta[j].iDG >prkg->RKG_Threshold )//prkg->RKG_Threshold
							prkg[i].RKDG_StateCnt++;
					else
							__NOP();//prkg[i].RKDG_StateCnt = 0;
				}
				else if((pcang->RKG_DG == 1)&&(pcang->RKG_Type == 1))   //磁编码大盖,直接用传感器数据判断
				{
//					if(psAD[i].uiDG < 0 ||psAD[i].uiDG >= 27000)//角度
//					{
//							prkg[i].RKDG_ErrorCnt++;  
//					}
//					if( - psAD[i].uiDG > prkg->RKG_Threshold || psAD[i].uiDG > prkg->RKG_Threshold)
//							prkg[i].RKDG_StateCnt++;
//					else
//							prkg[i].RKDG_StateCnt = 0;
		/*			if(psAD[i].uiDG >= 0 ||psAD[i].uiDG <= 27000)//角度
					{

					  if( - psAD[i].uiDG > prkg->RKG_Threshold || psAD[i].uiDG > prkg->RKG_Threshold)
							prkg[i].RKDG_StateCnt++;
					  else
							prkg[i].RKDG_StateCnt = 0;
			
				    if(prkg[i].RKDG_StateCnt >= 30)//prkg[i].RKG_StateKeepNum
				    {
              //RisingEdge++;	
							prkg[i].RKDG_StateCnt = 30;
						  prkg[i].RKDG_State = 1;
			    	}
				    else
				    {
						prkg[i].RKDG_State = 0;				
				    } 
					}*/
					
					__NOP();
				 }
			}

			if(prkg[i].RKXG_ErrorCnt >= prkg[i].RKG_StateKeepNum)				//判断人孔小盖
			{
					prkg[i].RKXG_Error = 0;
			}
			else if(prkg[i].RKXG_StateCnt >= prkg[i].RKG_StateKeepNum)
			{
					prkg[i].RKXG_State = 0;
			}
			
			
			if(prkg[i].RKDG_ErrorCnt >= rkg_erro_uplimit)							//判断人口大盖prkg[i].RKG_StateKeepNum
			{
				//	prkg[i].RKDG_Error = 1;
			}
			
//			if(prkg[i].RKDG_StateCnt >= prkg[i].RKG_StateKeepNum)
//			{

//					prkg[i].RKDG_State = 1;
//			}
		}
}