radio add new device type

QuakeGod

2023-02-01 7a2ff321965f1f24f3047fedd44a83f451f1f793

 Src/ModbusRTU.c

@@ -66,7 +66,7 @@
   0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400 

};



uint16_t crc16tablefast(uint8_t *ptr, uint16_t len) 
uint16_t crc16tablefast(const uint8_t *ptr, uint16_t len) 
{

   uint16_t crc = 0xffff; 

   uint16_t i;

@@ -84,14 +84,14 @@
//**********************************************************************

//******************  Modbus slave  ************************************



int ModBusSlaveCheckPkg(void *ptr, uint16_t len1)

int ModBusSlaveCheckPkg(int nChn, void *ptr, uint16_t len1)

{

   if (len1 <=4) return -1;      //包长

   if (len1 <=4) return -1;      //包长

   pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr;

   if (pPkg->Dst >127) return -2;   //地址码

   if ((pPkg->Cmd&0x7f) > 0x1f) return -3;  //功能码

   uint16_t crc = crc16tablefast(ptr,len1);   //CRC 校验

   if (crc != 0 ) return 4;      //CRC 校验错误

   if (pPkg->Dst >127) return -2;   //地址码

   if ((pPkg->Cmd&0x7f) > 0x1f) return -3;  //功能码

   uint16_t crc = crc16tablefast(ptr,len1);   //CRC 校验

   if (crc != 0 ) return 4;      //CRC 校验错误

   return S_OK;

}



@@ -133,10 +133,10 @@


uint8_t Pkgbuf[128];



int ModBusSlaveParsePkg( void *ptr, uint16_t len)

int ModBusSlaveParsePkg(int nChn, void *ptr, uint16_t len)

{

   uchar * bptr = (uchar *) ptr;

   int err=ModBusSlaveCheckPkg(ptr,len);	
   int err=ModBusSlaveCheckPkg(nChn, ptr, len);	
   if (err) return -1;

   pModBusRTUReqPkg pPkg = (pModBusRTUReqPkg) ptr;

   uint8_t DstHost=pPkg->Dst;

@@ -170,7 +170,7 @@
            {

               Addr1=(Addr&0xff0)>>4;

               bitAddr=Addr&0xf;

               if (GetBitValue( KMem.WR[Addr1],bitAddr))

               if (GetBitValue( KMem.WY[Addr1],bitAddr))

               {

                  thisValue|=bitmask;

               }

@@ -181,7 +181,7 @@
         }

         len2+=1+nByteCount;

         break;

      case ReadInputs:   //2  bits

      case ReadInputs:   //2 , bits

      // Fetch Datas;

         nByteCount=(nCount+7)/8;

         Pkgbuf[2]=nByteCount;

@@ -229,12 +229,12 @@
      case WriteCoil:   //5  bit

      //Store Datas;

         //SetBitValue(&KMem.WR[Addr1],bitAddr,pPkg->nCount);   

         if (nCount == 0)

         if (nCount == 0)      // set to 0

         {

            ResetBit(&KMem.WR[Addr1],bitAddr);

         }else if (nCount == 0xFF00)

            ResetBit(&KMem.WY[Addr1],bitAddr);

         }else if (nCount == 0xFF00)   // set to 1

         {

            SetAddrBit(&KMem.WR[Addr1],bitAddr);

            SetAddrBit(&KMem.WY[Addr1],bitAddr);

         }else

         {

            //error

@@ -263,7 +263,7 @@
               Addr1=(Addr&0xff0)>>4;

               bitAddr=Addr&0xf;

               

               SetBitValue( &KMem.WR[Addr1],bitAddr,bitmask&thisValue);

               SetBitValue( &KMem.WY[Addr1],bitAddr,bitmask&thisValue);

               bitmask<<=1;

               Addr++;

            }

@@ -303,7 +303,7 @@
         len2+=2;

   }

         //SendPkg(Pkgbuf,len2);

         SendPacket1(Pkgbuf,len2);

         SendPacket(nChn, Pkgbuf, len2);

}   

   

   return 0;