add UltraSonic and MultiWireLess

QuakeGod

2024-11-25 9aed5d7e7b3c7bf09da712e9c272ece401a7acc9

 Radio_LLCC68/Radio/KWireLess.c

@@ -113,7 +113,7 @@
 */
#define MASTER_RX_TIMEOUT_VALUE                 80         //mS
#define SLAVE_RX_TIMEOUT_VALUE                  400         //mS
#define CYCLE_TIME                                             120         //mS
#define CYCLE_TIME                                             80         //mS

const stWLConfig defaultWLConfig =
{
@@ -159,8 +159,8 @@



#define WL_RX_BUFFER_SIZE                                       256                   // Define the payload size here
#define WL_TX_BUFFER_SIZE                                       128                   // Define the payload size here
#define WL_RX_BUFFER_SIZE                                       160                   // Define the payload size here
#define WL_TX_BUFFER_SIZE                                       160                   // Define the payload size here

uint16_t nTimeOnAir;
/*
@@ -169,9 +169,10 @@
*/

//uint16_t BufferSize = BUFFER_SIZE;
uint8_t RX_Buffer[WL_RX_BUFFER_SIZE];
uint8_t TX_Buffer[WL_TX_BUFFER_SIZE];
uint8_t KwRx_Buffer[WL_RX_BUFFER_SIZE];
uint8_t KwTx_Buffer[WL_TX_BUFFER_SIZE];

#define WL_TT_EACH_SIZE                                                64                  // transparent transmission byte size for each send


States_t State = LOWPOWER;
@@ -385,7 +386,7 @@
{
      KMem.WDT[44] = size;
      if (size <= 120) {
         memcpy( RX_Buffer, payload, size );	
         memcpy( KwRx_Buffer, payload, size );	
         memcpy(&KMem.WDT[64],payload,size);
      }else
      {
@@ -535,12 +536,13 @@
   }
   return;
}

int KWLMasterParsePkt(int nChn,int size)
{
   bool CRC_OK =1;
///*      
   crc_value=RadioComputeCRC(RX_Buffer,size-2,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
   if (RX_Buffer[size-2] != (crc_value&0xff) && RX_Buffer[size-1] != (crc_value >> 8))
   crc_value=RadioComputeCRC(KwRx_Buffer,size-2,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
   if (KwRx_Buffer[size-2] != (crc_value&0xff) && KwRx_Buffer[size-1] != (crc_value >> 8))
   {
      KwRunStat.CRCErr++;
      KwRunStat.ErrStat=500;
@@ -549,7 +551,7 @@
      CRC_OK = 1;
   }   
//*/   
   pstKLPacket p1 = (pstKLPacket) RX_Buffer;
   pstKwPacket p1 = (pstKwPacket) KwRx_Buffer;
      if(CRC_OK && p1->STSign == enRplySign ) //memcmp(RX_Buffer,PongMsg,4)==0 
      {
            if (p1->DstAddr == MixAddr(WLCfg.nChannel,WLCfg.nRadioAddr))
@@ -561,8 +563,8 @@
                  KwRunStat.CtnLstPkts=0;                  
                  
                  if (p1->Stat==0) {
                     KwRunStat.tRSSI = RX_Buffer[10];
                     KwRunStat.tSNR = RX_Buffer[11];
                     KwRunStat.tRSSI = KwRx_Buffer[10];
                     KwRunStat.tSNR = KwRx_Buffer[11];
                     if (KwRunStat.tRSSI > -50) {
                        if (KwRunStat.Tx_Power > 0) {KwRunStat.Tx_Power--; SX126xSetRfTxPower(KwRunStat.Tx_Power);}
                     }else if (KwRunStat.tRSSI < -60) {
@@ -572,18 +574,94 @@
                     }
                     
                  }else if (p1->Stat==1){
                     KwRunStat.targetSentCount = RX_Buffer[8] + (RX_Buffer[9]<<8) + (RX_Buffer[10]<<16) + (RX_Buffer[11]<<24);
                     KwRunStat.targetSentCount = KwRx_Buffer[8] + (KwRx_Buffer[9]<<8) + (KwRx_Buffer[10]<<16) + (KwRx_Buffer[11]<<24);
                  }else if (p1->Stat==2) {
                     KwRunStat.targetRecvdCount = RX_Buffer[8] + (RX_Buffer[9]<<8) + (RX_Buffer[10]<<16) + (RX_Buffer[11]<<24);
                     KwRunStat.targetRecvdCount = KwRx_Buffer[8] + (KwRx_Buffer[9]<<8) + (KwRx_Buffer[10]<<16) + (KwRx_Buffer[11]<<24);
                  }else if (p1->Stat==3) {
                  }

                     KMem.WDT[41]=(-KwRunStat.tRSSI) + ((-KwRunStat.tSNR)<<8);

                     KMem.WFX[1]=(RX_Buffer[4]) + (RX_Buffer[5]<<8);
                     KMem.WFX[1]=(KwRx_Buffer[4]) + (KwRx_Buffer[5]<<8);
               //      KMem.WFX[1]=(RX_Buffer[4]) + (indicator3<<8);
                  
                     KMem.WFX[2]=(RX_Buffer[6]) + (RX_Buffer[7]<<8);
                     KMem.WFX[2]=(KwRx_Buffer[6]) + (KwRx_Buffer[7]<<8);
						
// 数据透传  数据的发送成功与否，需要在对方的应答包中确认
                  if (KwRunStat.sizesending > 0) {			
                     //  确认正在发送的，发送成功
                     KwRunStat.sentsize += KwRunStat.sizesending;
                     KwRunStat.sizesending = 0;
							
                     if (KwRunStat.sentsize >= KwRunStat.sizetosend) {
                        //整体 发送完成
                        KwRunStat.sizetosend =0;
                        KwRunStat.sentsize = 0;
                        KwRunStat.ttAirSize = 0;
                        // 发送完成
                     }
                  }	
// 接受到对方发来的数据

                  if (p1->bHasData && size >= 14+3 ) {
//                     int nFulllen1 = KwRx_Buffer[12];
                     int nthislen2 = KwRx_Buffer[13];
                     if (size >= 14 + nthislen2) {
                        if (p1->bHead) {
                           // 数据包的起始， 一个新的数据包的开始
                           KwRunStat.ttRxSize = 0;
//                           SendPacket(1, "H", 1);
									
                        }
                        memcpy (KwRunStat.ttRxBuf1 + KwRunStat.ttRxSize , KwRx_Buffer + 14 , nthislen2);
                        KwRunStat.ttRxSize += nthislen2;
                        if (p1->bTail) 
                           {
                           // 已经收到完整的一个数据包
                           KwRunStat.ttRxRecved =1;
                           KwRunStat.ErrStat=500;						
//                           SendPacket(1, "T", 1);
									
                           // 后续数据包处理流程 ;
                         if (Uart1Mode == 1) {      // 处于透传模式， 数据发回去。
                              SendPacket(1, KwRunStat.ttRxBuf1, KwRunStat.ttRxSize);
                         }
                           KwRunStat.ttRxSize = 0;
                           KwRunStat.ttRxRecved = 0;									 
								 
                           // 暂时流程，全部发回去。
//                           memcpy (KwRunStat.ttTxBuf1, KwRunStat.ttRxBuf1, KwRunStat.ttRxSize);
//                           KwRunStat.sizetosend = KwRunStat.ttRxSize;
//                           KwRunStat.sentsize = 0;
//                           KwRunStat.sizesending = 0;
                        }

                     }
                  }

/*


						
                  if (KwRunStat.ttAirSize > 0)
                  {
                     if (KwRunStat.bttAirHeader) {   KwRunStat.ttRxRecved=0;   }
                     memcpy(KwRunStat.ttRxBuf1 + KwRunStat.ttRxRecved,KwRunStat.ttAirBuf, KwRunStat.ttAirSize);
                     KwRunStat.ttRxRecved += KwRunStat.ttAirSize;
							
                     if (KwRunStat.bttAirTail) {
                        // 收到   一个数据包的最后部分	
                           KwRunStat.ttRxSize = KwRunStat.ttRxRecved;
                         if (Uart1Mode == 1) {      // 处于透传模式， 数据发回去。
                              SendPacket(1, KwRunStat.ttRxBuf1, KwRunStat.ttRxSize);
                         }
                        KwRunStat.ttRxSize = 0;
                        KwRunStat.ttRxRecved = 0;
                     }
                     KwRunStat.ttAirSize = 0;
                  }
*/						
						
               }
      }
   
@@ -592,10 +670,10 @@
int KWLSlaveParsePkt(int nChn,int size)
{
      bool CRC_OK =1;
   pstKLPacket p1 = (pstKLPacket) RX_Buffer;	
   pstKwPacket p1 = (pstKwPacket) KwRx_Buffer;	
///*      
   crc_value=RadioComputeCRC(RX_Buffer,size-2,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
   if (RX_Buffer[size-2] != (crc_value&0xff) && RX_Buffer[size-1] != (crc_value >> 8))
   crc_value=RadioComputeCRC(KwRx_Buffer,size-2,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
   if (KwRx_Buffer[size-2] != (crc_value&0xff) && KwRx_Buffer[size-1] != (crc_value >> 8))
   {
      KwRunStat.CRCErr++;
//      KwRunStat.ErrStat=500;
@@ -620,10 +698,10 @@
            KwRunStat.lastRecvdtime=GetTick();
            KwRunStat.lastAckTime = GetTick();            
            KwRunStat.CtnLstPkts=0;
				
            if (p1->Stat==0) {
               KwRunStat.tRSSI = RX_Buffer[10];
               KwRunStat.tSNR = RX_Buffer[11];
            int nSeq = p1->nSeq;
            if (nSeq==0) {
               KwRunStat.tRSSI = KwRx_Buffer[10];
               KwRunStat.tSNR = KwRx_Buffer[11];
                     if (KwRunStat.tRSSI > -50) {
                        if (KwRunStat.Tx_Power > 0) {KwRunStat.Tx_Power--; SX126xSetRfTxPower(KwRunStat.Tx_Power);}
                     }else if (KwRunStat.tRSSI < -60) {
@@ -631,17 +709,69 @@
                        if (KwRunStat.Tx_Power > WLCfg.Tx_Power) {KwRunStat.Tx_Power= WLCfg.Tx_Power; }
                        SX126xSetRfTxPower(KwRunStat.Tx_Power);
                     }
            }else if (p1->Stat==1){
               KwRunStat.targetSentCount = RX_Buffer[8] + (RX_Buffer[9]<<8) + (RX_Buffer[10]<<16) + (RX_Buffer[11]<<24);
            }else if (p1->Stat==2) {
               KwRunStat.targetRecvdCount = RX_Buffer[8] + (RX_Buffer[9]<<8) + (RX_Buffer[10]<<16) + (RX_Buffer[11]<<24);
            }else if (p1->Stat==3) {
            }else if (nSeq==1){
               KwRunStat.targetSentCount = KwRx_Buffer[8] + (KwRx_Buffer[9]<<8) + (KwRx_Buffer[10]<<16) + (KwRx_Buffer[11]<<24);
            }else if (nSeq==2) {
               KwRunStat.targetRecvdCount = KwRx_Buffer[8] + (KwRx_Buffer[9]<<8) + (KwRx_Buffer[10]<<16) + (KwRx_Buffer[11]<<24);
            }else if (nSeq==3) {
            }
            KMem.WFX[1]=(RX_Buffer[4]) + (RX_Buffer[5]<<8);
            KMem.WFX[1]=(KwRx_Buffer[4]) + (KwRx_Buffer[5]<<8);
         //   KMem.WFX[1]=(RX_Buffer[4]) + (indicator3<<8);
            
            KMem.WFX[2]=(RX_Buffer[6]) + (RX_Buffer[7]<<8);				
            KMem.WFX[2]=(KwRx_Buffer[6]) + (KwRx_Buffer[7]<<8);				

// 数据透传  数据的发送成功与否，需要在对方的应答包中确认
                  if (KwRunStat.sizesending > 0) {			
                     //  确认正在发送的，发送成功
                     KwRunStat.sentsize += KwRunStat.sizesending;
                     KwRunStat.sizesending = 0;
							
                     if (KwRunStat.sentsize >= KwRunStat.sizetosend) {
                        //整体 发送完成
                        KwRunStat.sizetosend =0;
                        KwRunStat.sentsize = 0;
                        KwRunStat.ttAirSize = 0;
                        // 发送完成
                     }
                  }	
				
            if (p1->bHasData && size >= 14+3 ) {
//               int nFulllen1 = KwRx_Buffer[12];
               int nthislen2 = KwRx_Buffer[13];
               if (size >= 14 + nthislen2) {
                  // 收到一个合格的携带传统数据的 包。
                  if (p1->bHead) {
                     // 数据段的起始， 一个新的数据段的开始
                     KwRunStat.ttRxSize = 0;
                  }
                  memcpy (KwRunStat.ttRxBuf1 + KwRunStat.ttRxSize, KwRx_Buffer + 14 , nthislen2);
                  KwRunStat.ttRxSize += nthislen2;
                  if (p1->bTail) {
                     // 已经收到完整的一个数据段
                     KwRunStat.ttRxRecved =1;
                     KwRunStat.ErrStat=500;						
							
                     // 后续数据段处理流程 ;
                         if (Uart1Mode == 1) {      // 处于透传模式， 数据发回去。
                              SendPacket(1, KwRunStat.ttRxBuf1, KwRunStat.ttRxSize);
                         }
                           KwRunStat.ttRxSize = 0;
                           KwRunStat.ttRxRecved = 0;									 							
							
                     // 暂时流程，全部发回去。
//                     memcpy (KwRunStat.ttTxBuf1, KwRunStat.ttRxBuf1, KwRunStat.ttRxSize);
//                     KwRunStat.sizetosend = KwRunStat.ttRxSize;
//                     KwRunStat.sentsize = 0;
//                     KwRunStat.sizesending = 0;
//                     KwRunStat.ttRxRecved = 0;
//                     KwRunStat.ttRxSize = 0; 
							
                  }

               }
					
				
            }
        KWLSlaveSendRplyPkt(1);
         //   Radio.StartCad();
//            KMem.WFY[0]=(RX_Buffer[4]<<8) + RX_Buffer[5];
@@ -662,48 +792,77 @@
int KWLMasterSendReqPkt(int nChn)
{
   int len1=12;
   pstKLPacket p1 = (pstKLPacket) TX_Buffer;
   pstKwPacket p1 = (pstKwPacket) KwTx_Buffer;
   p1->STSign = enReqSign;
   p1->DstAddr = MixAddr(WLCfg.nChannel,WLCfg.nRadioAddr);
   p1->Func = 0x3;
   p1->Stat = (p1->Stat + 1) &0x03;   //0x00;
   KwRunStat.nSeq = (KwRunStat.nSeq + 1)&0x03;
   p1->Stat = KwRunStat.nSeq;   //0x00;
   p1->Data[0]=KMem.WFY[1]; 
   p1->Data[1]=KMem.WFY[1]>>8; 
   p1->Data[2]=KMem.WFY[2]; 
   p1->Data[3]=KMem.WFY[2]>>8; 
   
/*	
         TX_Buffer[0] = 'P';
         TX_Buffer[1] = 'I';
         TX_Buffer[2] = 'N';
         TX_Buffer[3] = 'G'; 
         TX_Buffer[4] = KMem.WFY[1]; 
         TX_Buffer[5] = KMem.WFY[1]>>8; 
         TX_Buffer[6] = KMem.WFY[2]; 
         TX_Buffer[7] = KMem.WFY[2]>>8; 
*/

   if (p1->Stat == 0)
   {
         TX_Buffer[10] = RssiValue;
         TX_Buffer[11] = SnrValue;
         KwTx_Buffer[10] = RssiValue;
         KwTx_Buffer[11] = SnrValue;
   }else if (p1->Stat == 1){
      memcpy(TX_Buffer+8,&KwRunStat.sentCount,4);
      memcpy(KwTx_Buffer+8,&KwRunStat.sentCount,4);
   }else if (p1->Stat == 2) {
      memcpy(TX_Buffer+8,&KwRunStat.recvCount,4);		
      memcpy(KwTx_Buffer+8,&KwRunStat.recvCount,4);		
   }else if (p1->Stat == 3) {
      
   }
         crc_value=RadioComputeCRC(TX_Buffer,len1,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
         TX_Buffer[len1]=crc_value;
         TX_Buffer[len1+1]=crc_value>>8;
	
// 受限于空中包大小的限制， 透传数据可能需要分几次发送。
// 发送方负责拆包， 接收方负责 合包，  发送方发送第一个包，有起始标志， 发送方发送最后一个包，有结束标志。
// 如果只有一个包的数据，则同时具有起始和结束标志。
// 中间的数据包，不具有起始标志，也不具有结束标志。
// 利用包序列号来保证 中间 的数据包的 完整性 和顺序正确性。
// 先实现1个包的转发。	
         if (KwRunStat.sizetosend > 0) {
            uchar bStart=0,bEnd=1;
            int16_t thisToSend = KwRunStat.sizetosend - KwRunStat.sentsize;
            if (thisToSend > WL_TT_EACH_SIZE) { thisToSend = WL_TT_EACH_SIZE; bEnd=0;}
				
            if (KwRunStat.sentsize == 0) {      // 第一次发送
               bStart = 1;
            }
				
            memcpy(KwRunStat.ttAirBuf, KwRunStat.ttTxBuf1 + KwRunStat.sentsize , thisToSend);         // 发送数据， 如果上次有，继续上次的发。
				
            KwRunStat.sizesending = thisToSend;
            KwRunStat.ttAirSize = thisToSend;

//            KwRunStat.sentsize = 0;
            KwRunStat.bttAirHeader = bStart;
            KwRunStat.bttAirTail = bEnd;
				
            p1->bHasData=1;
            p1->bHead = bStart;
            p1->bTail = bEnd;
				
            KwTx_Buffer[len1] = KwRunStat.sizetosend;
            KwTx_Buffer[len1+1] = thisToSend;
				
            memcpy( KwTx_Buffer + len1 + 2 , KwRunStat.ttTxBuf1 + KwRunStat.sentsize, thisToSend);
            len1 += 2 + thisToSend;
				
         }
			

         crc_value=RadioComputeCRC(KwTx_Buffer,len1,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
         KwTx_Buffer[len1]=crc_value;
         KwTx_Buffer[len1+1]=crc_value>>8;
			
			
         KMem.WDT[56]=crc_value;
	

         KwRunStat.sentCount++;
         KwRunStat.cycleTime = GetTick()- KwRunStat.lastSendtime ;   
         KwRunStat.lastSendtime = GetTick();   
         Radio.Send( TX_Buffer, len1+2);
         Radio.Send( KwTx_Buffer, len1+2);
         KwRunStat.runStep=RS_SENDING;
         KwRunStat.lastActTime = GetTick();
   
@@ -713,7 +872,7 @@
int KWLSlaveSendRplyPkt(int nChn)
{
   int len1=12;
   pstKLPacket p1 = (pstKLPacket) TX_Buffer;
   pstKwPacket p1 = (pstKwPacket) KwTx_Buffer;
   p1->STSign = enRplySign;
   p1->DstAddr = MixAddr(WLCfg.nChannel,WLCfg.nRadioAddr);
   p1->Func = 0x3;
@@ -722,32 +881,60 @@
   p1->Data[1]=KMem.WFY[1]>>8; 
   p1->Data[2]=KMem.WFY[2]; 
   p1->Data[3]=KMem.WFY[2]>>8; 
/*	
         TX_Buffer[0] = 'P';
         TX_Buffer[1] = 'O';
         TX_Buffer[2] = 'N';
         TX_Buffer[3] = 'G'; 
         TX_Buffer[4] = KMem.WFY[1]; 
         TX_Buffer[5] = KMem.WFY[1]>>8; 
         TX_Buffer[6] = KMem.WFY[2]; 
         TX_Buffer[7] = KMem.WFY[2]>>8; 
*/

   if (p1->Stat == 0)
   {
         TX_Buffer[10] = RssiValue;
         TX_Buffer[11] = SnrValue;
         KwTx_Buffer[10] = RssiValue;
         KwTx_Buffer[11] = SnrValue;
   }else if (p1->Stat == 1){
      memcpy(TX_Buffer+8,&KwRunStat.sentCount,4);
      memcpy(KwTx_Buffer+8,&KwRunStat.sentCount,4);
   }else if (p1->Stat == 2) {
      memcpy(TX_Buffer+8,&KwRunStat.recvCount,4);		
      memcpy(KwTx_Buffer+8,&KwRunStat.recvCount,4);		
   }else if (p1->Stat == 3) {
      
   }   

         crc_value=RadioComputeCRC(TX_Buffer,len1,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
         TX_Buffer[len1]=crc_value;
         TX_Buffer[len1+1]=crc_value>>8;
         Radio.Send( TX_Buffer, len1+2);	
// 受限于空中包大小的限制， 透传数据可能需要分几次发送。
// 发送方负责拆包， 接收方负责 合包，  发送方发送第一个包，有起始标志， 发送方发送最后一个包，有结束标志。
// 如果只有一个包的数据，则同时具有起始和结束标志。
// 中间的数据包，不具有起始标志，也不具有结束标志。
// 利用包序列号来保证 中间 的数据包的 完整性 和顺序正确性。
// 先实现1个包的转发。	
	
         if (KwRunStat.sizetosend > 0) {
            uchar bStart=0,bEnd=1;
            int16_t thisToSend = KwRunStat.sizetosend - KwRunStat.sentsize;
            if (thisToSend > WL_TT_EACH_SIZE) { thisToSend = WL_TT_EACH_SIZE; bEnd=0;}
				
            if (KwRunStat.sentsize == 0) {      // 第一次发送
               bStart = 1;
            }
				
            memcpy(KwRunStat.ttAirBuf, KwRunStat.ttTxBuf1 + KwRunStat.sentsize , thisToSend);         // 发送数据， 如果上次有，继续上次的发。
				
            KwRunStat.sizesending = thisToSend;
            KwRunStat.ttAirSize = thisToSend;

//            KwRunStat.sentsize = 0;
            KwRunStat.bttAirHeader = bStart;
            KwRunStat.bttAirTail = bEnd;
				
            p1->bHasData=1;
            p1->bHead = bStart;
            p1->bTail = bEnd;
				
            KwTx_Buffer[len1] = KwRunStat.sizetosend;
            KwTx_Buffer[len1+1] = thisToSend;
				
            memcpy( KwTx_Buffer + len1 + 2 , KwRunStat.ttTxBuf1 + KwRunStat.sentsize, thisToSend);
            len1 += 2 + thisToSend;
				
         }
	
         crc_value=RadioComputeCRC(KwTx_Buffer,len1,CRC_TYPE_IBM);//计算得出要发送数据包CRC值
         KwTx_Buffer[len1]=crc_value;
         KwTx_Buffer[len1+1]=crc_value>>8;
         Radio.Send( KwTx_Buffer, len1+2);	
         KwRunStat.runStep=RS_SENDING;
   
         KwRunStat.lastActTime = GetTick();
@@ -795,7 +982,7 @@
   
         switch (KwRunStat.runStep){
            case RS_IDLE:
               if (GetTick() - KwRunStat.lastSendtime > (WLCfg.nCycleTime) *10){
               if (GetTick() - KwRunStat.lastSendtime > (WLCfg.nCycleTime) *10 ||KwRunStat.sizetosend > 0 ){
                  Radio.StartCad();
                  KwRunStat.runStep = RS_MASTER_CAD;
                  KwRunStat.lastActTime = GetTick();
@@ -815,7 +1002,7 @@
            case RS_SENT:
               break;
            case RS_RECVING:
               if (GetTick() - KwRunStat.lastSendtime > (WLCfg.nCycleTime) *10){
               if (GetTick() - KwRunStat.lastSendtime > (WLCfg.nCycleTime) *10 || KwRunStat.bMasterRecved){
                  KwRunStat.ErrStat=5000; 
                  KwRunStat.LostPackets++;
                  KwRunStat.CtnLstPkts++;
@@ -844,7 +1031,8 @@
                  
                  
                  KwRunStat.runStep = RS_IDLE;         
               }				
               }
					
               break;
            case RS_RECVED:
                  KwRunStat.runStep = RS_IDLE;