static void getOnePieceOfRoomInfo(uint8_t index, uint8_t* buff)
{
uint32_t addr = getFlashAddress(index);
uint16_t tmpCRC[2] = {0, 0};
stRoomInfo tmpInfo[2];
memset((uint8_t *)&tmpInfo[0], 0, sizeof(stRoomInfo));
memset((uint8_t *)&tmpInfo[1], 0, sizeof(stRoomInfo));
Flash_Read((uint8_t *)&tmpInfo[0], addr, sizeof(stRoomInfo));
Flash_Read((uint8_t *)&tmpInfo[1], addr + FLASH_SECTOR_SIZE, sizeof(stRoomInfo));
tmpCRC[0] = CRC16((uint8_t *)&tmpInfo[0], sizeof(stRoomInfo) - sizeof(uint16_t));
tmpCRC[1] = CRC16((uint8_t *)&tmpInfo[1], sizeof(stRoomInfo) - sizeof(uint16_t));
if ((tmpCRC[0] == tmpInfo[0].crc) && (tmpCRC[1]== tmpInfo[1].crc))
{
memcpy(buff, (uint8_t *)&tmpInfo[0], sizeof(stRoomInfo));
}
else if(tmpCRC[0] == tmpInfo[0].crc)
{
memcpy(buff, (uint8_t *)&tmpInfo[0], sizeof(stRoomInfo));
}
else if(tmpCRC[1]== tmpInfo[1].crc)
{
memcpy(buff, (uint8_t *)&tmpInfo[1], sizeof(stRoomInfo));
}
else
{
bitMapFlashDateError |= (1 << index);
}
}
/*
* Compute string hash code.
*/
WORD HashKeyword( LPCTSTR pszKeyword, int nLength, BOOL bCaseOn )
{
register WORD wHash = ( WORD )-1;
register int i;
/*
* Compute the hash, or 16 bit CRC,
* of the given string.
*/
if ( bCaseOn )
{
/*
* Case sensitive.
*/
for ( i = 0; i < nLength; i++ )
wHash = ( WORD )CRC16( *pszKeyword++, wHash );
}
else
{
/*
* Case insensitive.
*/
for ( i = 0; i < nLength; i++ )
wHash = ( WORD )CRC16( _totlower( *pszKeyword++ ), wHash );
}
/*
* Return hash code.
*/
return wHash;
}
//Every block has the following header:
// 0 - compressed size includeing header (word)
// 2 - CRC-16 (word)
// 4 - block type
// 6 - uncompressed size (word)
// 8 .. (compressed size + 5) - compressed data (byte array)
dword CompressTFD (byte *pSrc, dword SourceBufferSize, byte *pDest, word TFDType, word SysID, void *pPercentFinishedCallback)
{
word OrigSize, CompSize;
dword OutBufferSize, NrBlocks = 0, FullSize = SourceBufferSize;
byte *FileHeader;
//PercentFinishedCallback is called for every block. PercentFinished contains a number between 0 and 100
void (*PercentFinishedCallback) (dword PercentFinished) = pPercentFinishedCallback;
//Build the tfd file header
FileHeader = pDest;
if (pDest)
{
STORE_WORD (pDest , 0x0008);
STORE_WORD (pDest + 4, SysID);
STORE_WORD (pDest + 6, 0x0001);
pDest += 10;
}
OutBufferSize = 10;
while (SourceBufferSize)
{
if (PercentFinishedCallback) PercentFinishedCallback ((FullSize - SourceBufferSize) * 100 / FullSize);
NrBlocks++;
OrigSize = (SourceBufferSize > 0x7ffa) ? 0x7ffa : SourceBufferSize;
if (pDest)
{
CompSize = CompressBlock (pSrc, OrigSize, pDest + 8);
//Build the block header
STORE_WORD (pDest , CompSize + 6);
STORE_WORD (pDest + 4, TFDType);
STORE_WORD (pDest + 6, OrigSize);
STORE_WORD (pDest + 2, CRC16 (0, pDest + 4, 4 + CompSize));
pDest += CompSize + 8;
}
else CompSize = CompressBlock (pSrc, OrigSize, NULL);
OutBufferSize += CompSize + 8;
SourceBufferSize -= OrigSize;
pSrc += OrigSize;
}
if (FileHeader)
{
STORE_WORD (FileHeader + 8, NrBlocks);
STORE_WORD (FileHeader + 2, CRC16 (0, FileHeader + 4, 6));
}
if (PercentFinishedCallback) PercentFinishedCallback (100);
return OutBufferSize;
}
bool cSmartCardDataNagra::Parse(const char *line)
{
bool isSK=false;
int dlen=64;
BN_set_word(exp,3);
line=skipspace(line);
unsigned char id[4];
if(GetHex(line,id,sizeof(id))!=sizeof(id)) {
PRINTF(L_CORE_LOAD,"smartcarddatanagra: format error: card id");
return false;
}
cardid=UINT32_BE(id);
line=skipspace(line);
if(!strncasecmp(line,"SK",2)) { isSK=true; dlen=96; line+=2; }
else {
if(GetHex(line,bk,sizeof(bk))!=sizeof(bk)) {
PRINTF(L_CORE_LOAD,"smartcarddatanagra: format error: boxkey");
return false;
}
line=skipspace(line);
if(!strncasecmp(line,"IRDMOD",6)) { isIrdMod=true; line+=6; }
else if(!strncasecmp(line,"CARDMOD",7)) { isIrdMod=false; line+=7; }
else {
PRINTF(L_CORE_LOAD,"smartcarddatanagra: format error: IRDMOD/CARDMOD");
return false;
}
}
line=skipspace(line);
unsigned char *buff=AUTOMEM(dlen);
if(GetHex(line,buff,dlen)!=dlen) {
PRINTF(L_CORE_LOAD,"smartcarddatanagra: format error: data block");
return false;
}
if(!isSK) {
mod.Get(buff,64);
}
else {
struct SecondaryKey *sk=(struct SecondaryKey *)buff;
if(UINT16_BE(sk->cs)!=CRC16(buff,sizeof(*sk)-sizeof(sk->cs),false)) {
PRINTF(L_CORE_LOAD,"smartcarddatanagra: secondary key CRC failed");
return false;
}
unsigned short e=UINT16_BE(sk->exp);
if(e!=0x0003 && e!=CRC16(buff,12,false)) BN_set_word(exp,e);
xxor(bk,sizeof(bk),sk->y1,sk->y2);
mod.Get(sk->mod,sizeof(sk->mod));
}
return true;
}
QByteArray Modbus_RTU::RequestHoldingRegisters(qint8 dev_addr,qint16 first_reg_addr,qint16 num_reg)
{
QByteArray mb_request;
quint16 CRC;
mb_request.append(dev_addr);
mb_request.append(this->ReadHld);
mb_request.append(((char*)(&first_reg_addr))[1]);
mb_request.append(((char*)(&first_reg_addr))[0]);
mb_request.append(((char*)(&num_reg))[1]);
mb_request.append(((char*)(&num_reg))[0]);
CRC=CRC16(mb_request,mb_request.length());
mb_request.append(((char*)(&CRC))[0]);
mb_request.append(((char*)(&CRC))[1]);
//port->write(mb_request);
emit this->WriteToOut_Thread(mb_request);
last_request=mb_request;
qDebug( "SEND DATA: %X",mb_request.data()) ;
timer->start(100);
return mb_request;
}
QByteArray Modbus_RTU::CalibrateRequest(quint8 dev_addr,quint8 point ,float density)
{
QByteArray mb_request;
quint16 CRC;
union
{
quint8 dnst[4];
float dnst_2;
};
mb_request.append(dev_addr);
mb_request.append(this->Calibrate);
mb_request.append(point);
dnst_2=density;
mb_request.append(dnst[0]);//передача float
mb_request.append(dnst[1]);
mb_request.append(dnst[2]);
mb_request.append(dnst[3]);
CRC=CRC16(mb_request,mb_request.length());
mb_request.append(((char*)(&CRC))[0]);
mb_request.append(((char*)(&CRC))[1]);
emit this->WriteToOut_Thread(mb_request);
last_request=mb_request;
qDebug( "SEND DATA: %X",mb_request.data()) ;
return mb_request;
}
//-----------------------------------------------------------------------------
void SetSingleRegister(void)//установить значение одного регистра
{
unsigned char i=0;
unsigned int CRC=0x0;
unsigned int addr;//адрес регистра
unsigned int num;//значение регистра
//-----------установка нужного регистра------------
addr=(((unsigned int)RecieveBuf[2])|((unsigned int)RecieveBuf[3]<<8));
num=(((unsigned int)RecieveBuf[4]<<8)|((unsigned int)RecieveBuf[5]));
if(addr>=REG_ADDR_MIN && addr<=REG_ADDR_MAX)
{
controller_reg[addr]=num;
for (i=0;i<(buf_count-CRC_LEN);i++)
{
TransferBuf[i]=RecieveBuf[i];
}
buf_len=buf_count;
CRC=CRC16(&TransferBuf,buf_len-CRC_LEN);
TransferBuf[buf_len-2]=HI(CRC);
TransferBuf[buf_len-1]=LO(CRC);
}
else
{
//обработка ошибки
MD_STATE=MD_ERR_HANDLING;
}
//-------------------------------------------------
}
// Receive ISR callback
static void sumdDataReceive(uint16_t c)
{
uint32_t sumdTime;
static uint32_t sumdTimeLast;
static uint8_t sumdIndex;
sumdTime = micros();
if ((sumdTime - sumdTimeLast) > 4000)
sumdIndex = 0;
sumdTimeLast = sumdTime;
if (sumdIndex == 0) {
if (c != SUMD_SYNCBYTE)
return;
else
{
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
crc = 0;
}
}
if (sumdIndex == 2)
sumdChannelCount = (uint8_t)c;
if (sumdIndex < SUMD_BUFFSIZE)
sumd[sumdIndex] = (uint8_t)c;
sumdIndex++;
if (sumdIndex < sumdChannelCount * 2 + 4)
CRC16((uint8_t)c);
else
if (sumdIndex == sumdChannelCount * 2 + 5) {
sumdIndex = 0;
sumdFrameDone = true;
}
}
int main(void)
{
unsigned int CrcResult = 0;
char *Str = "a";
CrcResult = CRC16(Str, strlen(Str));
printf("R:%x\n",CrcResult);
return 0;
}
u16 uffs_crc16update(const void *data, int length, u16 crc)
{
int i;
const u8 *p = (const u8 *)data;
for (i = 0; i < length; i++, p++) {
CRC16(crc, *p);
}
return crc;
}
/* @overload append_crc16(buf)
* Appends a CRC16 checksum to a string
*
* @param buf [String] the starting string
* @return [String] the original string plus its CRC16
*/
static VALUE mm_gps_add_CRC16(VALUE klass, VALUE str)
{
union {
ushort u;
char s[2];
} crc;
Check_Type(str, T_STRING);
crc.u = CRC16(RSTRING_PTR(str), RSTRING_LEN(str));
return rb_str_buf_cat(str, crc.s, 2);
}
int cmd_check(uint8_t *data)//OK 1; failure 0
{
BITMAIN_TASK_P bt = (BITMAIN_TASK_P)data;
uint16_t r_crc = 0;
uint16_t crc = bt->crc;
uint16_t length = 0;
if(bt->token_type == BM_TX_TASK)
{
#if HAVE_NUM
uint16_t len = 4 + bt->work_num*sizeof(struct ASIC_TASK);
#else
uint16_t len = le16_to_cpu(bt->length)+ 2;
#endif
//crc = data[bt->length] | (data[bt->length+1]<<8);
crc = data[len] | (data[len+1]<<8);
length = le16_to_cpu(bt->length) + 2;
}
else if(bt->token_type == BM_TX_CONF)
{
BITMAIN_CONFIGURE_P btc = (BITMAIN_CONFIGURE_P)data;
length = btc->length;
crc = cpu_to_le16(btc->crc);
}
else if(bt->token_type == BM_GET_STATUS)
{
BITMAIN_GET_STATUS_P bgs = (BITMAIN_GET_STATUS_P)data;
length = bgs->length;
crc = cpu_to_le16(bgs->crc);
}
else
{
printk("Tx token err {%#x}\n", bt->token_type);
return 0;
}
if(crc == (r_crc=CRC16(data, length)))//length 去除了type和length
{
//rintf("OK: crc{%#x}r_crc{%#x}\n", crc, r_crc);
return 1;
}
else
{
printk("Err:token{%#x} crc{%#x}r_crc{%#x}len{%#x}\n",
bt->token_type, crc, r_crc,length);
if(bt->token_type == BM_TX_TASK)
{
#if HAVE_NUM
printk("work_num {%d}\n", bt->work_num);
#else
printk("work_num {%d}\n", (le16_to_cpu(bt->length) - 3)/sizeof(struct ASIC_TASK));
#endif
}
return 0;
}
}
unsigned short doCRC16( unsigned char *mstr,unsigned char len)
{
unsigned char ch;
crc = 0;
do{
ch = *mstr++;
CRC16(ch);
}while(--len);
return(crc);
}
int32_t nReadCommandHandler(uint16_t* pCOM_Buff)
{
if (COMM_GET_DATA(pCOM_Buff[0]) < COMM_READ_MAX)
{
SPI_WRITE_TX(SPI, tMotor.unValue[COMM_GET_DATA(pCOM_Buff[0])]);
SPI_WRITE_TX(SPI, CRC16((uint8_t*)(&(tMotor.unValue[COMM_GET_DATA(pCOM_Buff[0])])), 1));
return 0;
}
else
{
return -1;
}
}
void saveRoomInfo(uint8_t index)
{
uint32_t addr = getFlashAddress(index);
roomInfo[index].crc = CRC16((uint8_t *)&roomInfo[index], sizeof(stRoomInfo) - sizeof(uint16_t));
Flash_EraseSector(addr);
Flash_EraseSector(addr + FLASH_SECTOR_SIZE);
Flash_PageWrite((uint8_t *)&roomInfo[index], addr, sizeof(stRoomInfo));
Flash_PageWrite((uint8_t *)&roomInfo[index], addr + FLASH_SECTOR_SIZE, sizeof(stRoomInfo));
bitMapFlashDateError &= ~(1 << index);
}
/**
* @brief Generates a message with no data part.
*
* @param[in] command Command to generate message for.
* @param[in] Cbuff Pointer to the circular buffer to put the data in.
* @return HAL_FAILED if the message didn't fit or HAL_SUCCESS
* if it did fit.
*/
static bool GenerateHeaderOnlyCommand(kfly_command_t command,
circular_buffer_t *Cbuff)
{
uint16_t crc16;
uint8_t header[2] = {(uint8_t) command, 0};
/* Calculate the CRC. */
crc16 = CRC16(header, 2);
/* Apply SLIP encoding on the fly and return the result. */
return GenerateSLIP_HBT(header, 2, NULL, 0, (uint8_t *)&crc16, 2,
Cbuff);
}
// Communicating with mast via SPI
void COMM_Manager(void)
{
static uint16_t unCOM_Buff[COMM_FIFO_LENGTH];
static uint32_t unLastFrameCNT = 0;
static uint32_t unLastCheckTime = 0;
// All transactions are handled in interrupt
if (tMotor.structMotor.MSR.bNewComFrameReceived == TRUE)
{
memcpy(unCOM_Buff, unCOM_SPI_ReadData, COMM_FIFO_LENGTH);
tMotor.structMotor.MSR.bNewComFrameReceived = FALSE;
if (CRC16((uint8_t *)unCOM_Buff, (IS_COMM_RD_CMD(unCOM_Buff[0]) ? ((COMM_RD_CMD_CNT - 1) << 1) : ((COMM_WR_CMD_CNT - 1) << 1))) ==
(IS_COMM_RD_CMD(unCOM_Buff[0]) ? unCOM_Buff[COMM_RD_CMD_CNT - 1] : unCOM_Buff[COMM_WR_CMD_CNT - 1]))
{
unValidFrameCNT++;
// safe zone
if (IS_COMM_RD_CMD(unCOM_Buff[0]))
{
nReadCommandHandler(unCOM_Buff);
}
else
{
nWriteCommandHandler(unCOM_Buff);
}
}
else
{
unCOM_SPI_TransErrCNT++;
}
}
// Comm protection 1: If have NOT received any frame in 500ms, error
if ((uint32_t)(unSystemTick - unLastCheckTime) > 500)
{
unLastCheckTime = unSystemTick;
if ((uint32_t)(unValidFrameCNT - unLastFrameCNT) < 1)
{
BLDC_stopMotor();
setError(ERR_COMMUNICATION_FAIL);
}
unLastFrameCNT = unValidFrameCNT;
}
// Comm protection 2: If received error frame exceed some threshold, error
if (unCOM_SPI_TransErrCNT > COM_SPI_TRANS_ERR_THRESHOLD)
{
BLDC_stopMotor();
setError(ERR_COMMUNICATION_FAIL);
}
}
//---------------------------------------------------------------------------------------------
void stub(void)//заглушка для проверки
{
unsigned int CRC=0;
TransferBuf[0]=ADRESS_DEV;
TransferBuf[1]=0x3;//read reg
TransferBuf[2]=0x2;
TransferBuf[3]=0x1;
TransferBuf[4]=0x1;
//TransferBuf[5]=0x1;
CRC=CRC16(&TransferBuf,5);
TransferBuf[5]=HI(CRC);
TransferBuf[6]=LO(CRC);
buf_len=0x7;
}
//Every block has the following header:
// 0 - compressed size includeing header (word)
// 2 - CRC-16 (word)
// 4 - block type
// 6 - uncompressed size (word)
// 8 .. (compressed size + 5) - compressed data (byte array)
dword UncompressTFD (byte *pSrc, byte *pDest, void *pPercentFinishedCallback)
{
word compSize = 0, uncompSize = 0, NrBlocks = 0;
dword outSize = 0, i;
//PercentFinishedCallback is called for every block. PercentFinished contains a number between 0 and 100
void (*PercentFinishedCallback) (dword PercentFinished) = pPercentFinishedCallback;
if (LOAD_WORD(pSrc) != 8) return 0; //Invalid header?
if (CRC16 (0, pSrc + 4, 6) != LOAD_WORD(pSrc + 2)) return 0; //Invalid header CRC?
if (LOAD_WORD(pSrc + 6) != 1) return 0; //Invalid file version?
NrBlocks = LOAD_WORD(pSrc + 8);
pSrc += 10;
for (i = 0; i < NrBlocks; i++)
{
if (PercentFinishedCallback) PercentFinishedCallback (i * 100 / NrBlocks);
compSize = LOAD_WORD(pSrc) - 6;
uncompSize = LOAD_WORD(pSrc + 6);
if (uncompSize > 0x7ffa) return 0;
pSrc += 8;
if(compSize == uncompSize)
{
// not compressed data, copy it directly
if (pDest) memcpy(pDest, pSrc, uncompSize);
}
else
{
// compressed data, uncompress it
if (!UncompressBlock (pSrc, compSize, pDest, uncompSize)) return 0;
}
if (pDest) pDest += uncompSize;
pSrc += compSize;
outSize += uncompSize;
}
if (PercentFinishedCallback) PercentFinishedCallback (100);
return outSize;
}
// поиск БВК при включении контроллера
void FindBPS(uint8_t addr)
{
u8 i;
uint16_t crc;
TxBufModbus[0] = addr;// Адрес на шине Modbus.
TxBufModbus[1] = 0x02;// Код команды.
crc = CRC16(TxBufModbus, 2); // Подсчет CRC16.
TxBufModbus[3] = (0xff00&crc)>>8;
TxBufModbus[2] = 0x00ff&crc;
for (i=0; i < 8; i++) RxBufModbus[i] = 0; // Чистим буфер приемника.
// Заранее заряжаем количество байт на передачу в DMA.
SetRS485_Transmit;
DMA_SetCurrDataCounter(DMA1_Channel2,BPSfindLen); // Количество передаваемых байт.
Device.AnswerLength = 8; // Длина ожидаемого ответа
SetTimeoutRS485(TimeoutWaitAnswer); // Таймаут ожидания ответа
DMA_Cmd(DMA1_Channel2, ENABLE); // Включаем DMA передатчи
}
unsigned int MakePacket(unsigned char func, unsigned short param, unsigned char gid, unsigned short nid, const unsigned char *pData, unsigned int len, unsigned char *pPacket)
{
UsnSendPacketHeader_t send;
send.stx = STX;
send.len = len + sizeof(UsnSendPacketHeader_t) - 3/*STX+Len*/ + 4/*CRC16+ETX*/;
send.func = func;
send.param = param;
send.gid = gid;
send.nid = nid;
unsigned char *pByte = pPacket;
//========================================================================================
// 2009.5.8 - ���� ����
//========================================================================================
memcpy(pByte, &send, sizeof(send));
pByte += sizeof(send);
//========================================================================================
// 2009.5.8 - ������ ����
//========================================================================================
if (NULL != pData)
{
memcpy(pByte, pData, len);
pByte += len;
}
//========================================================================================
// 2009.5.8 - CRC16 �� ETX ����
//========================================================================================
unsigned short wCRC16;
unsigned char temp;
wCRC16 = CRC16(pPacket, send.len -4/*CRC16+ETX*/ +3/*STX+Len*/);
memcpy(pByte, &wCRC16, sizeof(wCRC16));
pByte += sizeof(wCRC16);
temp = 0x0a;
memcpy(pByte, &temp,1);
pByte += 1;
*pByte = (CPU_INT08U)ETX;
return send.len + 4;/*STX+Len*/
}
bool EQProtocolPacket::ValidateCRC(const unsigned char *buffer, int length, uint32 Key)
{
bool valid=false;
// OP_SessionRequest, OP_SessionResponse, OP_OutOfSession are not CRC'd
if (buffer[0]==0x00 && (buffer[1]==OP_SessionRequest || buffer[1]==OP_SessionResponse || buffer[1]==OP_OutOfSession)) {
valid=true;
} else {
uint16 comp_crc=CRC16(buffer,length-2,Key);
uint16 packet_crc=ntohs(*(const uint16 *)(buffer+length-2));
#ifdef EQN_DEBUG
if (packet_crc && comp_crc != packet_crc) {
std::cout << "CRC mismatch: comp=" << std::hex << comp_crc << ", packet=" << packet_crc << std::dec << std::endl;
}
#endif
valid = (!packet_crc || comp_crc == packet_crc);
}
return valid;
}
/**
Send a single block of data.
A zero sized block terminates the transfer.
@param data The data to transfer.
@param size Size of data.
@return The number of transfered, or an error code on failure.
The number of bytes may be less than \a size.
@pre SendInitialise() must have been successful.
*/
int QymodemTx::SendBlock(const quint8* data, size_t size)
{
quint8 block[1+2+1024+2]; // buffer to hold data in the block
int retryCount = 10; // number of attempts to send the block
bool waitForBlockACK = WaitForBlockACK;
change_mode:
size_t blockSize = (Use1KBlocks && size>=1024) ? 1024 : 128;
size_t dataSize = size<blockSize ? size : blockSize; // size of data to send in block
if(!dataSize)
{
// all bytes sent, so end transfer by sending a single EOT...
block[0] = EOT;
blockSize = 1;
waitForBlockACK = true;
}
else
{
// make block header...
block[0] = blockSize==1024 ? STX : SOH;
block[1] = BlockNumber&0xffu;
block[2] = (~BlockNumber)&0xffu;
// copy data for block (padding with EOF)...
memcpy(block+3,data,dataSize);
memset(block+3+dataSize,26,blockSize-dataSize);
// append checksum/crc...
if(SendCRC)
{
quint16 crc = CRC16(block+3,blockSize);
blockSize += 3;
block[blockSize++] = (quint8)(crc>>8);
block[blockSize++] = (quint8)crc;
}
else
{
quint8 sum = Checksum(block+3,blockSize);
blockSize += 3;
block[blockSize++] = sum;
}
}
char LeddarOne::sendRequest()
{
unsigned char dataBuffer[19] = {0};
unsigned int i = 0;
unsigned long startTime = millis();
// clear serial buffer
while (SerialPort.available())
{
SerialPort.read();
if (++i > 250) return ERR_LEDDAR_SERIAL_PORT;
}
// Enable TX if necessary
if (TxEn_Pin)
{
digitalWrite(TxEn_Pin, TxEn_Action);
}
// Send message on UART:
dataBuffer[0] = SlaveAddress;
dataBuffer[1] = 0x04;
dataBuffer[2] = 0;
dataBuffer[3] = 20;
dataBuffer[4] = 0;
dataBuffer[5] = 10;
CRC16(dataBuffer, 6, false);
for (i = 0; i<8; i++)
{
SerialPort.write(dataBuffer[i]);
}
SerialPort.flush();
// Disable TX if necessary
if (TxEn_Pin)
{
digitalWrite(TxEn_Pin, 1-TxEn_Action);
}
}
dword TFDSize(byte *pTFD)
{
TRACEENTER();
word NrBlocks = 0;
dword i;
byte *p = pTFD;
if(!pTFD)
{
TRACEEXIT();
return 0;
}
if(LOAD_WORD(p) != 8)
{
//Invalid header
TRACEEXIT();
return 0;
}
if(CRC16 (0, p + 4, 6) != LOAD_WORD(p + 2))
{
//Invalid header CRC
TRACEEXIT();
return 0;
}
if(LOAD_WORD(p + 6) != 1)
{
//Invalid file version
TRACEEXIT();
return 0;
}
NrBlocks = LOAD_WORD(p + 8);
p += 10;
for(i = 0; i < NrBlocks; i++)
p += LOAD_WORD(p) + 2;
TRACEEXIT();
return p - pTFD;
}
//-----------------------------------------------------------------------------
void ReadHoldingReg(void)//чтение регистров хранения
{
#define HEAD_LEN 3
//
//[addr|func|n_regs|....|crc|crc]-кадр ответа
//[ head |............]
unsigned char i=0,count=0;
unsigned int CRC=0x0;
unsigned int addr;//адрес начального регистра
unsigned int len;//количество считываемых регистров
TransferBuf[0]=ADRESS_DEV;
TransferBuf[1]=READ_HOLDING_REG;//прочитать регистры хранения
//-----------чтение начального адреса и количества регистров------------
addr=(((unsigned int)RecieveBuf[2]<<8)|(unsigned int)RecieveBuf[3]);
len= (((unsigned int)RecieveBuf[4]<<8)|(unsigned int)RecieveBuf[5]);
if(addr>=REG_ADDR_MIN && addr<=REG_ADDR_MAX && (len+addr)<=REG_ADDR_MAX)
{
for(i=addr;i<(addr+len);i++)
{
TransferBuf[count+HEAD_LEN]=HI(controller_reg[i]);
TransferBuf[count+HEAD_LEN+1]=LO(controller_reg[i]);
count+=2;
}
TransferBuf[2]=count; //счетчик байт
buf_len=HEAD_LEN+count+CRC_LEN; //длина передаваемого буфера
CRC=CRC16(&TransferBuf,buf_len-CRC_LEN);
TransferBuf[buf_len-2]=HI(CRC);
TransferBuf[buf_len-1]=LO(CRC);
}
else
{
//обработка ошибки
MD_STATE=MD_ERR_HANDLING;
}
//-------------------------------------------------
}
/* 与A8通信 */
void Task_A8CONNECT(void *p_arg)
{
(void)p_arg; //'p_arg'没有用到，防止编译器警告
while(1)
{
unsigned char errA8;
// unsigned int i=0;
// printf("A8_wait");
// for(i=0;i<10;i++)
// {
// printf("%c",RxBuffer1[i]);
// }
OSSemPend(A8_SEM,0,&errA8);
// printf("A8_in");
//
// if(RxBuffer1[0]!=0x01)
// {
// for(i=0;i<10;i++)
// RxBuffer1[i]=0;
// RxCounter1=0;
// }
CRC16((unsigned char *)RxBuffer1,5);
// printf("16H:%c,16L:%c",crc16H,crc16L);
//
// for(i=0;i<10;i++)
// printf("%c",RxBuffer1[i]);
if(crc16L==RxBuffer1[6]&&crc16H==RxBuffer1[5])
{
//将EEPROM读出数据顺序保持到I2c_Buf_Read中
I2C_EE_BufferRead(I2c_Buf_Read, EEP_Firstpage, 256);
Receiver_Control=RxBuffer1[2];
Receiver_Spare=RxBuffer1[3];
// printf("OK!886");
replyA8();
}
OSTimeDlyHMSM(0,0,0,500);
}
}
//-----------------------------------------------------------------------------
void Send_Info(void)//информация об устройстве
{
unsigned int CRC=0x0;
TransferBuf[0]=ADRESS_DEV;
TransferBuf[1]=0x11;//получить информацию
TransferBuf[2]=0x4; //счетчик байт
TransferBuf[3]=HI(DEV_ID);//id
TransferBuf[4]=LO(DEV_ID);//id
TransferBuf[5]=HI(DEV_SN);//SN
TransferBuf[6]=LO(DEV_SN);//SN
CRC=CRC16(&TransferBuf,7);
TransferBuf[7]=HI(CRC);
TransferBuf[8]=LO(CRC);
buf_len=0x9;
}
void PackageTxBuffer(Meter *mt)
{
if (mt->Command == CommandType_ReadRegister)
{
mt->PackageBuffer.TxLength = 8;
mt->PackageBuffer.TxBuffer = malloc(mt->PackageBuffer.TxLength);
mt->PackageBuffer.RxLength = 200;
mt->PackageBuffer.RxBuffer = malloc(mt->PackageBuffer.RxLength);
//mt->PackageBuffer.RxBufferValid = FALSE;
mt->PackageBuffer.TxBuffer[0] = mt->NodeAddress;
mt->PackageBuffer.TxBuffer[1] = mt->Command;
mt->PackageBuffer.TxBuffer[2] = INT16HighByte(mt->StartAddress);
mt->PackageBuffer.TxBuffer[3] = INT16LowByte(mt->StartAddress);
mt->PackageBuffer.TxBuffer[4] = INT16HighByte(mt->RegisterCount);
mt->PackageBuffer.TxBuffer[5] = INT16LowByte(mt->RegisterCount);
INT16U crc16 = CRC16(mt->PackageBuffer.TxBuffer, 6);
crc16 = crc_cal16(mt->PackageBuffer.TxBuffer, 6);
mt->PackageBuffer.TxBuffer[6] = INT16LowByte(crc16);
mt->PackageBuffer.TxBuffer[7] = INT16HighByte(crc16);
}
}
QByteArray Modbus_RTU::SetSingleRegister(qint8 dev_addr,qint16 reg_addr,quint16 value)
{
QByteArray mb_request;
quint16 CRC;
mb_request.append(dev_addr);
mb_request.append(this->WriteReg);
mb_request.append(((char*)(®_addr))[1]);
mb_request.append(((char*)(®_addr))[0]);
mb_request.append(((char*)(&value))[1]);
mb_request.append(((char*)(&value))[0]);
CRC=CRC16(mb_request,mb_request.length());
mb_request.append(((char*)(&CRC))[0]);
mb_request.append(((char*)(&CRC))[1]);
//port->write(mb_request);
emit this->WriteToOut_Thread(mb_request);
last_request=mb_request;
qDebug( "SEND DATA: %X",mb_request.data()) ;
return mb_request;
}
