提供一个SENT协议解码的Demo

只看该作者 · 2021-11-1 17:09

自己写了个SENT 编解码的demo，不知道咋上传附件，先上传个解码，太长了，回头再把编码传上来
#define SYNC_LENGTH_MAX  680    // (56 ticks * 3us /tick / 0.25us Timer resolution) + 25%
#define SYNC_LENGTH_MIN  660    // (56 ticks * 3us /tick / 0.25us Timer resolution) - 25%

char TempSerialDatabit3[SLOW_CHANNEL_DATA]={0};
char TempSerialDatabit2[SLOW_CHANNEL_DATA]={0};

char slow_channel[SLOW_CHANNEL_DATA];
char slow_channel_bit2[SLOW_CHANNEL_DATA];

u8 index_bit3=0;
u8 index_bit2=0;
char slow_data_index=0;
u8  slow_index=0;

__asm void nop(void)
{
NOP
}

#define SYNC_LENGTH_NOM  ((SYNC_LENGTH_MAX+SYNC_LENGTH_MIN)/2)

unsigned short int CRC4Table[256]={
0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,
13,12,15,14,9,8,11,10,5,4,7,6,1,0,3,2,
7,6,5,4,3,2,1,0,15,14,13,12,11,10,9,8,
10,11,8,9,14,15,12,13,2,3,0,1,6,7,4,5,
14,15,12,13,10,11,8,9,6,7,4,5,2,3,0,1,
3,2,1,0,7,6,5,4,11,10,9,8,15,14,13,12,
9,8,11,10,13,12,15,14,1,0,3,2,5,4,7,6,
4,5,6,7,0,1,2,3,12,13,14,15,8,9,10,11,
1,0,3,2,5,4,7,6,9,8,11,10,13,12,15,14,
12,13,14,15,8,9,10,11,4,5,6,7,0,1,2,3,
6,7,4,5,2,3,0,1,14,15,12,13,10,11,8,9,
11,10,9,8,15,14,13,12,3,2,1,0,7,6,5,4,
15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0,
2,3,0,1,6,7,4,5,10,11,8,9,14,15,12,13,
8,9,10,11,12,13,14,15,0,1,2,3,4,5,6,7,
5,4,7,6,1,0,3,2,13,12,15,14,9,8,11,10,
};

unsigned char CRC4(unsigned char *data,unsigned int len )
{
unsigned int i;
unsigned int tempCS;
unsigned int CheckSum=5;
for(i=0;i<len;i++)
   {
   tempCS =data[i]+ CheckSum*16;
   if(tempCS >256)
   return 0xFF;    // 严重错误返回
CheckSum = CRC4Table[tempCS];
   }
tempCS = CheckSum*16;
      if(tempCS >256)
   return 0xFF;    // 严重错误返回
   CheckSum = CRC4Table[tempCS];
return CheckSum;
}

/*多项式为：x6+x4+x3+1;这里主要对6位数据块进行校验，不会大于63，因此crc6表就只有64个值*/
uint8_t CRC6_Table[64]= { \
0, 25, 50, 43, 61, 36, 15, 22, 35, 58, 17, 8, 30, 7, 44 ,53,
31, 6, 45, 52, 34, 59, 16, 9, 60, 37, 14, 23, 1, 24, 51, 42,
62, 39, 12, 21, 3, 26, 49, 40, 29, 4, 47, 54, 32, 57, 18, 11,
33, 56, 19, 10, 28, 5, 46, 55, 2, 27, 48, 41, 63, 38, 13, 20};

uint8_t crc6_cal(uint8_t *data,uint8_t len)/*data位6位nibble块的值，len为数据块的数量*/
{
/*crc初始值*/
uint8_t result = 0x15;
uint8_t tableNo = 0;
int i = 0;

/*对额外添加的6个0进行查表计算crc*/
  tableNo = result ^ 0;
  result = CRC6_Table[tableNo];

for(i = 0; i < len; i++)
{
tableNo = result ^ data[i];
result= CRC6_Table[tableNo];
}

return result;
}

u8  Find_slow_data_start_flag(char *p,u8 len)

{

unsigned short int num=1 ,i;

for(i=0; i<len; i++)
{

      if ((p[i] == p[i+1])&&(p[i]==0x08)) {

         ++num;
      }
else{

   num=1;
}

      if(num>5)  // 这里6表示，找到连续6个1，即找到数据头
      {

         return i-num+2 ;             //返回数据头所在的位置

      }

   }
   return num;                   // 没有找到有效的数据头，因为暂存数组长度等于一帧数据的长度，所以实际上取数据的位置应该在头的2-6位。
}

/***********************************************************
解析半字节数据

输入：ticks *3us 对应的数值
输出：对应的0-15的数值
无法解析数据，或者是暂停位
************************************************************/

u8 Sent_decode(SentData *dat, float data)
{
      unsigned char nibbles;

      nibbles = (unsigned char)(data/dat->ui_TickRatio+0.5-12);// +-25%  偏差

      if((nibbles>=0)&&(nibbles<=15)){
         return nibbles;
      }
else if((nibbles>=43)&&(nibbles<=45)){
      return 0xa8;    // 同步半字节信号
      }
else
   {
      return 0xee;    // sent 2010  加入的暂停半字节位
   }


}

/*****************************************************************
*
*从接受收队列取出数据，并找到SYNC，然后处理，接收sent 半字节数据
*
****************************************************************/

void Sent_Data_Rev(Queue *q, SentData *data)
{
float cur_temp;

   data->RDataCounter=0;

   Get_queue(q,&data->ui_Result[data->RDataCounter]);
   nop();
   cur_temp=data->ui_Result[data->RDataCounter++];
nop();

if((cur_temp < SYNC_LENGTH_MAX)&&(cur_temp > SYNC_LENGTH_MIN)){

   data->ui_TickRatio=(cur_temp/56);

do{

      Get_queue(q,&data->ui_Result[data->RDataCounter++]);

      }

     while(data->RDataCounter<SENT_NIBBLES_MAX);

Sent_Data_Store(data);

}
else
{

   Clear_queue(q);  // 清洗队列如果出错，可能会丢到一些数据  main.c 中判断队列长度再去处理的阈值不能太大，要不丢失数据比较多

}
}
/********************************************************************
*
*SENT数据进一步处理
*
********************************************************************/

void Sent_Data_Store(SentData *data)
{
  unsigned char temp_data[SENT_NIBBLES_MAX];
unsigned char channel_data[6];
unsigned char crc_value;
char index=0;

if(data->ui_Result[index]!=0){
data->Q_data_raw.sync_ =    Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.status_ = Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.data1_high = Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.data1_mid =  Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.data1_low =  Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.data2_high = Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.data2_mid =  Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.data2_low =  Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.short_crc =  Sent_decode(data,data->ui_Result[index++]);
data->Q_data_raw.pause =    Sent_decode(data,data->ui_Result[index++]);

memcpy(channel_data,&data->Q_data_raw.data1_high,sizeof(channel_data));

// 将sent数据通道数据合并，预备使用
//  data->Q_data.sync = data->Q_data_raw.sync_;
// data->Q_data.status = data->Q_data_raw.status_;
// data->Q_data.channel_1=((data->Q_data_raw.data1_high)<<8)|((data->Q_data_raw.data1_mid)<<4)|(data->Q_data_raw.data1_low);
// data->Q_data.channel_2=((data->Q_data_raw.data2_high)<<8)|((data->Q_data_raw.data2_mid)<<4)|(data->Q_data_raw.data2_low);
// data->Q_data.short_crc=data->Q_data_raw.short_crc;
// data->Q_data.pause=data->Q_data_raw.pause;

   crc_value=CRC4(channel_data,sizeof(channel_data)); //CRC计算

   if(crc_value== data->Q_data_raw.short_crc)          //CRC校验，如果正确，打印到串口
   {
      memcpy(temp_data,&data->Q_data_raw.sync_,10);
   Usart_SendString(USART1,temp_data,10);          //通过串口将数据打印出来
      memset(data->ui_Result,0,sizeof(data->ui_Result));
   memset(channel_data,0,sizeof(channel_data));
         }

   else
   {

      if(DEBUG_EN){ //  调试开关，0是关闭，1是打开
         memcpy(temp_data,&data->Q_data_raw.sync_,10);
            Usart_SendString(USART1,temp_data,10);          //通过串口将数据打印出来 for debug
         memset(data->ui_Result,0,sizeof(data->ui_Result));
         }
   }
slow_channel_decode(data);
   }

}





void slow_channel_decode(SentData *data)
{

   u8 crc6_temp[4];
   u8 slow_data_high,slow_data_low;

      if(slow_index){

   slow_index--;

         }
   else{

            TempSerialDatabit3[index_bit3++]= data->Q_data_raw.status_&SLOW_CHANNEL_DATA_BIT3_MASK;
            nop();
      TempSerialDatabit2[index_bit2++]= data->Q_data_raw.status_&SLOW_CHANNEL_DATA_BIT2_MASK;

   if(index_bit3>SLOW_CHANNEL_DATA-1){

            index_bit3=0;
            index_bit2=0;

         slow_index =  Find_slow_data_start_flag(TempSerialDatabit3,SLOW_CHANNEL_DATA);  // 查找数据头所在位置

         if(slow_index<1){       // 恰好接收到完整一帧数据

            memcpy(slow_channel,&TempSerialDatabit3[slow_index],SLOW_CHANNEL_DATA);
         memcpy(slow_channel_bit2,&TempSerialDatabit2[slow_index],SLOW_CHANNEL_DATA);

      data->SerialData.frame_id= slow_channel[8]<<4|slow_channel[9]<<3\
      |slow_channel[10]<<2|slow_channel[11]<<1\
      |slow_channel[13]|slow_channel[14]>>1\
      |slow_channel[15]>>2|slow_channel[16]>>3;


            data->SerialData.crc_data=slow_channel_bit2[0]<<3|slow_channel_bit2[1]<<2|slow_channel_bit2[2]<<1\
            |slow_channel_bit2[3]|slow_channel_bit2[4]>>1|TempSerialDatabit2[5]>>2;

      data->SerialData.frame_data=slow_channel_bit2[6]<<9|slow_channel_bit2[7]<<8\
      |slow_channel_bit2[8]<<7|slow_channel_bit2[9]<<6\
      |slow_channel_bit2[10]<<5|slow_channel_bit2[11]<<4\
      |slow_channel_bit2[12]<<3|slow_channel_bit2[13]<<2\
      |slow_channel_bit2[14]<<1|slow_channel_bit2[15]\
      |slow_channel_bit2[16]>>1|slow_channel_bit2[17]>>2;


            crc6_temp[0]=slow_channel_bit2[6]<<3|slow_channel[6]<<1\
   |slow_channel_bit2[7]<<1|slow_channel[7]>>1\
|slow_channel_bit2[8]>>1|slow_channel[8]>>3;

crc6_temp[1]=slow_channel_bit2[9]<<3|slow_channel[9]<<1\
|slow_channel_bit2[10]<<1|slow_channel[10]>>1\
|slow_channel_bit2[11]>>1|slow_channel[11]>>3;

crc6_temp[2]=slow_channel_bit2[12]<<3|slow_channel[12]<<1\
   |slow_channel_bit2[13]<<1|slow_channel[13]>>1\
|slow_channel_bit2[14]>>1|slow_channel[14]>>3;

crc6_temp[3]=slow_channel_bit2[15]<<3|slow_channel[15]<<1\
   |slow_channel_bit2[16]<<1|slow_channel[16]>>1\
|slow_channel_bit2[17]>>1|slow_channel[17]>>3;

               slow_data_high =(data->SerialData.frame_data>>8)&0xff;
               slow_data_low = (data->SerialData.frame_data)&0xff;

// slow_data_high= slow_channel_bit2[6]<<9|slow_channel_bit2[7]<<8\
//       |slow_channel_bit2[8]<<7|slow_channel_bit2[9]<<6;
//
// slow_data_low= slow_channel_bit2[10]<<5|slow_channel_bit2[11]<<4\
//       |slow_channel_bit2[12]<<3|slow_channel_bit2[13]<<2\
//       |slow_channel_bit2[14]<<1|slow_channel_bit2[15]\
//       |slow_channel_bit2[16]>>1|slow_channel_bit2[17]>>2;


   if(data->SerialData.crc_data == crc6_cal(crc6_temp,sizeof(crc6_temp)))
{

               Usart_SendString(USART1,&data->SerialData.frame_id,1);
      Usart_SendString(USART1,&slow_data_high,1);
      Usart_SendString(USART1,&slow_data_low,1);

               }
               else {

                  if(DEBUG_EN){
//  调试开关，0是关闭，1是打
               Usart_SendString(USART1,&data->SerialData.frame_id,1);
   Usart_SendString(USART1,&slow_data_high,1);
      Usart_SendString(USART1,&slow_data_low,1);
                        }

               }

            }
else if((slow_index>1)&&(slow_index<14)){

         return;
      // 在一帧数据包里，能够找到数据头，但是不能解出完整的数据，需要调整接收起始的位置
            //index_bit3=0;
               //index_bit2=0;
         }
else{

               slow_index=SLOW_CHANNEL_DATA-slow_index; // 在一帧数据包里找不到数据头
         //index_bit3=0;
               //index_bit2=0;
      return;
         }
   }
   }
}

int main(void)
{
Hardware_Init();
Sent_temp =Init_queue();

StatusFlags=SENT_STATE_FIND_SYNCH;

while(1)
{

   if(StatusFlags==SENT_STATE_FIND_SYNCH){

      StatusFlags=SENT_STATE_GET_DATA;
      DelayMs(2);                                  //  延时便于电脑端  显示可调整
}
   else if(StatusFlags==SENT_STATE_GET_DATA){

   if( Queue_len(Sent_temp)>SENT_NIBBLES_MAX){    // 等队列数据存到一定数量后，再处理，可调整

         StatusFlags=SENT_STATE_ANALYZE_DATA;

      }
else{

      StatusFlags=SENT_STATE_FIND_SYNCH;
            DelayMs(2);
   }

   }
   else if(StatusFlags==SENT_STATE_ANALYZE_DATA){

         StatusFlags=SENT_STATE_FIND_SYNCH;
      Sent_Data_Rev(Sent_temp,&Sent_raw_data);
      DelayMs(2);

   }
   }

}

只看该作者 · 2022-5-25 21:30

用的什么型号的mcu,验证的结果怎么样？

提供一个SENT协议 解码的Demo

相关下载

相关帖子

提供一个SENT协议解码的Demo