mini7_V2/tshell/van_com_algorithm.c

/******************************************************************
*  Copyright (C) 2023 - 2023 OneChip, Inc. All rights reserved.
* File name: van_com_algorithm.c
* Author: fanyidong 	 Date:2024.1.21
* Description:		常用算法定义
* Others:
* History:
        1. Date:
                 Author:
                 Modification:

*****************************************************************/

#include "van_com_algorithm.h"
#include <stdio.h>
#include <string.h>
#define g_uMsgCmdFrameLen (6)

/****************************************************************
* 函数名:    checkSum
* 创建时间:  2024/02/18
* 创建人:    范义东
* 函数说明：求和
* 输入参数: 无

* 输出参数:
* 返回值 :   void
****************************************************************/
unsigned int checkSum(const unsigned char *pInBuffer, unsigned int uLen) {
  unsigned int sum = 0;

  for (int i = 0; i < uLen; i++) {
    sum += pInBuffer[i];
  }

  return (sum);
}

///****************************************************************
//* 函数名:    CRC16
//* 创建时间:  2024/02/18
//* 创建人:    范义东
//* 函数说明：求和
//* 输入参数: 无
//
//* 输出参数:
//* 返回值 :   void
//****************************************************************/
// unsigned short CRC16(unsigned char *bpMsgStartAddress, unsigned int bDataLen)
// {
//  unsigned char i, n;
//  unsigned short crc = 0xffff;
//  unsigned char u8tmp;
//  unsigned short u16tmp;
//  for (i = 0; i < bDataLen; i++) {
//    crc ^= *bpMsgStartAddress++;
//    for (n = 0; n < 8; n++) {
//      u8tmp = crc & 1;
//      crc >>= 1;
//      crc &= 0x7fff;
//      if (u8tmp == 1) {
//        crc ^= 0xa001;
//      }
//    }
//  }
//  return crc;
//}

static unsigned char auchCRCHi[] = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40};

static char auchCRCLo[] = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
    0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
    0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
    0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
    0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
    0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
    0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
    0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
    0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
    0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
    0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
    0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
    0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
    0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
    0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
    0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
    0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
    0x41, 0x81, 0x80, 0x40};

unsigned short CRC16_modebus(unsigned char *puchMsg, unsigned short usDataLen) {
  unsigned char uchCRCHi = 0xFF; /* 高字节初始化值   */
  unsigned char uchCRCLo = 0xFF; /* 低字节初始化值   */
  unsigned uIndex;

  while (usDataLen--) {
    uIndex = uchCRCLo ^ *puchMsg++;
    uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex];
    uchCRCHi = auchCRCLo[uIndex];
  }
  return (uchCRCHi << 8 | uchCRCLo);
}

unsigned short CRC16_RTU(unsigned char *bpMsgStartAddress,
                         unsigned short bDataLen) {
  unsigned char i, n;
  unsigned short crc = 0xffff;
  unsigned char u8tmp;
  for (i = 0; i < bDataLen; i++) {
    crc ^= *bpMsgStartAddress++;
    for (n = 0; n < 8; n++) {
      u8tmp = crc & 1;
      crc >>= 1;
      crc &= 0x7fff;
      if (u8tmp == 1) {
        crc ^= 0xa001;
      }
    }
  }
  return crc;
}

unsigned int CRC16_loop(const unsigned char *puchMsg, unsigned int usDataLen,
                        unsigned int loopLen, unsigned int curheader) {
  unsigned char uchCRCHi = 0xFF; /* 高字节初始化值   */
  unsigned char uchCRCLo = 0xFF; /* 低字节初始化值   */
  unsigned int uIndex;
  unsigned int loopindex = 0;
  unsigned int clcCrc = 0;
  unsigned int recCrc = 0;

  while (usDataLen--) {
    uIndex = uchCRCLo ^ puchMsg[(loopindex + curheader) % loopLen];
    uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex];
    uchCRCHi = auchCRCLo[uIndex];
    loopindex++;
  }

  clcCrc = (uchCRCHi << 8 | uchCRCLo);
  recCrc = puchMsg[(loopindex + curheader) % loopLen];

  loopindex++;
  recCrc = recCrc << 8;
  recCrc += puchMsg[(loopindex + curheader) % loopLen];

  if (clcCrc == recCrc) {
    return 1;
  } else {
    return 0;
  }
}

unsigned int CRC16_loop_RTU(const unsigned char *puchMsg, unsigned int usDataLen,
                        unsigned int loopLen, unsigned int curheader) {
  unsigned char uchCRCHi = 0xFF; /* 高字节初始化值	*/
  unsigned char uchCRCLo = 0xFF; /* 低字节初始化值	*/
  unsigned int uIndex;
  unsigned int loopindex = 0;
  unsigned int clcCrc = 0;
  unsigned int recCrc = 0;

  while (usDataLen--) {
    uIndex = uchCRCLo ^ puchMsg[(loopindex + curheader) % loopLen];
    uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex];
    uchCRCHi = auchCRCLo[uIndex];
    loopindex++;
  }

  clcCrc = (uchCRCLo << 8 | uchCRCHi);
  recCrc = puchMsg[(loopindex + curheader) % loopLen];
  loopindex++;
  recCrc = recCrc << 8;
  recCrc += puchMsg[(loopindex + curheader) % loopLen]; 
//  osDelay(10U);
  if (clcCrc == recCrc) {
//  	Van_Device_Printf(4, "len:%d head:%d CRC[%x %x]\n\r",usDataLentemp,curheader, clcCrc, recCrc); 
    return 1;
  } else {
    return 0;
  }
}

/********************************************************************
 *                         int Decode(const char *pInBuffer, int uLen)
 *
 *描述：对串口数据进行解码成结构体
 *参数：pInBuffer    接收缓冲区
        *uheader 当前头位置
        ulast 当前头尾位置
        maxlen 缓冲区长度
        pMessage  命令结构体
        pdata 命令数据
 *返回：1解码成功，0解码失败
 *其他：无
 *--------------------------------------------------------------------
 *记录：
 ********************************************************************/
int DecodeNew(CmdMessage *pMessage, unsigned char *pdata,
              const unsigned char *pInBuffer, unsigned int *pheader,
              unsigned int ulast, unsigned int maxlen) {
  unsigned int bufRealLen;
  unsigned int sizeofMessage = SIZEOF_MSG_ST;
  unsigned int header = *pheader;
  unsigned int i = 0;
  if (NULL == pInBuffer || NULL == pMessage || NULL == pdata ||
      NULL == pheader) {
    /*空指针判断*/
    return 0;
  }
  /*缓冲区有效数据长度，缓冲区循环处理，会出现头大于未情况*/
  bufRealLen = (ulast > header) ? (ulast - header) : (ulast + maxlen - header);
	if(bufRealLen > maxlen){		      
					return 0;
	}
  if (bufRealLen < sizeofMessage) {
    /*长度过段，跳过*/
    return 0;
  }
  bufRealLen -= sizeofMessage; /*最大有效数据长度*/
  for (; i <= bufRealLen;) {
    unsigned int headerflag = pInBuffer[(header + i) % maxlen];
    headerflag += pInBuffer[(header + i + 1) % maxlen] << 8;
    if (headerflag != VAN_COM_HEADER_FALG) {
      i += 1;
      //			header = (header + 1)%maxlen;
      continue;
    }
    unsigned int messagelen = pInBuffer[(header + i + 4) % maxlen];
    messagelen += pInBuffer[(header + i + 5) % maxlen] << 8;
    if (messagelen > (maxlen - sizeofMessage)) {
      /*长度超长 跳过2个字节*/
      i += 1;
      //			header = (header + 1)%maxlen;
      continue;
    }
    if (messagelen > bufRealLen) {
      /*当前起始点可能存在数据，跳出循环，*/
      i++; /*for采样小于等于判断，循环次数多1，此处加1便于后续处理*/
      break;
    }

    unsigned short checkesum = 0;
    unsigned short recsum = 0;
    if ((header + i + 6 + messagelen) < maxlen) {
      checkesum = checkSum(pInBuffer + header + i + 2, messagelen + 4);
    } else if (header + i + 2 < maxlen) //校验开始位
    {
      checkesum =
          checkSum(pInBuffer + header + i + 2, (maxlen - (header + i + 2)));
      checkesum += checkSum(pInBuffer, (header + i + 6 + messagelen) % maxlen);
    } else {
      checkesum +=
          checkSum(pInBuffer + (header + i + 2 - maxlen), messagelen + 4);
    }
    recsum = *(pInBuffer + (header + i + 6 + messagelen) % maxlen);
    recsum += *(pInBuffer + (header + i + 7 + messagelen) % maxlen) << 8;
    /*固定结尾预留做调试用*/
    if (checkesum != recsum && recsum != VAN_COM_TAIL_FALG) {
      i += 1;
      //			header = (header + 1)%maxlen;
    } else {
      /*解析成功,拷贝消息结构体*/
      unsigned char *pCharMessage = (unsigned char *)pMessage;
      for (unsigned copyi = 0; copyi < (sizeofMessage - 2); copyi++) {
        *pCharMessage++ = pInBuffer[(header + i + copyi) % maxlen];
      }
      /*解析成功,拷贝数据结构体*/
      for (unsigned copyi = 0; copyi < messagelen; copyi++) {
        *pdata++ = pInBuffer[(header + i + 6 + copyi) % maxlen];
      }
      *pheader = (header + messagelen + i + sizeofMessage - 2) % maxlen;
      return 1;
    }
  }
  *pheader = (header + i - 1) % maxlen;
  return 0;
}


float PIDRealize(pid_def *pid)
{
	pid->err=pid->goal-pid->realtemp;
	pid->variate=pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);
	pid->err_last=pid->err;
	if(pid->variate>100)
	{
		pid->variate=100;
	}else{			
		pid->integral+=pid->err;
	}
	if(pid->variate<0)
	{
		pid->variate=0;
	}
    return pid->variate;
}



int hex_to_ascii(const void *hex, void *string, int len)
{
    int str_len = 0;
    const unsigned char *phex = (const unsigned char*)hex;
    unsigned char *pstr = (unsigned char*)string;
    unsigned char temp;

    if(NULL == hex ||  NULL == string || len < 0)
	{
		return -1;
	}

    for(int i = 0; i < len; i++)
    {
        temp = ((phex[i] >> 4) & 0x0F);
        if(temp > 9)
            pstr[str_len++] = temp + '7';
        else
            pstr[str_len++] = temp + '0';

        temp = (phex[i]) & 0x0F ;
        if(temp > 9)
            pstr[str_len++] = temp + '7';
        else
            pstr[str_len++] = temp + '0';
    }

    if(len > 0)
        pstr[str_len] = 0;
    
    return 0;
}

static int checkCmd(const unsigned char *pInBuffer, int uLen)
{
    int ret = -1;
    for (int i = 0; i < uLen; i++)
    {
        unsigned char temp = pInBuffer[i];
        if (temp == 0xaa)
        {
            unsigned char temp1 = pInBuffer[i + 1];
            if (temp1 == 0x55)
            {
                ret = i;
                break;
            }
        }
    }
    return ret;
}

/********************************************************************
 *                         int Decode(const char *pInBuffer, int uLen)
 *
 *描述：对串口数据进行解码成结构体
 *参数：无
 *返回：无
 *其他：无
 *--------------------------------------------------------------------
 *记录：
 ********************************************************************/
int Decode(CmdMessage *pMessage, const  unsigned char *pInBuffer, int uLen)
{
    if (NULL == pInBuffer || uLen < g_uMsgCmdFrameLen)
    {
        return -1;
    }

    int startPos = checkCmd(pInBuffer, uLen);
    if (startPos == -1)
    {
        return -3;
    }

    char *pTemp = (char *)pInBuffer + startPos;
    memcpy((void *)pMessage, (const void *)(pTemp), g_uMsgCmdFrameLen);

    int uMsgLen = g_uMsgCmdFrameLen + pMessage->len + 2;
    if (uLen < uMsgLen)
    {
        // 黏包
        return -2;
    }
    // data
    pTemp += g_uMsgCmdFrameLen;
    pMessage->data = pTemp;

    // checksum

    return uMsgLen;
}