Чет работает
This commit is contained in:
467
AllLibs/Modbus/Src/modbus_slave.c
Normal file
467
AllLibs/Modbus/Src/modbus_slave.c
Normal file
@@ -0,0 +1,467 @@
|
||||
/**
|
||||
*******************************************************************************
|
||||
* @file modbus_slave.c
|
||||
* @brief Модуль для реализации слейв MODBUS.
|
||||
*******************************************************************************
|
||||
* @details
|
||||
Файл содержит реализацию функций для работы Modbus в режиме слейва.
|
||||
|
||||
@section slave Функции и макросы
|
||||
|
||||
- MB_Slave_Response() — Ответ на запрос
|
||||
- MB_Slave_Collect_Message() — Сбор сообщения в режиме слейва.
|
||||
- MB_Slave_Parse_Message() — Парс сообщения в режиме слейва.
|
||||
******************************************************************************/
|
||||
#include "modbus.h"
|
||||
|
||||
#ifdef MODBUS_ENABLE_SLAVE
|
||||
/**
|
||||
* @brief Ответ на сообщение в режиме слейва.
|
||||
* @param hmodbus Указатель на хендлер RS.
|
||||
* @param modbus_msg Указатель на структуру сообщения.
|
||||
* @return RS_RES Статус о результате ответа на комманду.
|
||||
*/
|
||||
RS_StatusTypeDef MB_Slave_Response(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
RS_StatusTypeDef MB_RES = 0;
|
||||
hmodbus->f.MessageHandled = 0;
|
||||
hmodbus->f.EchoResponse = 0;
|
||||
RS_Reset_TX_Flags(hmodbus); // reset flag for correct transmit
|
||||
|
||||
MB_Diagnostics_BusMessageCnt();
|
||||
if(hmodbus->ID == 0 || modbus_msg->DeviceAddr == 0)
|
||||
{
|
||||
MB_Diagnostics_SlaveNoResponseCnt(); // <-- Устройство не отвечает на широковещательные сообщения
|
||||
hmodbus->RS_STATUS = RS_SKIP;
|
||||
return RS_Handle_Receive_Start(hmodbus, modbus_msg);
|
||||
}
|
||||
MB_Diagnostics_SlaveMessageCnt();
|
||||
|
||||
if(modbus_msg->FuncCode < FC_ERR_VALUES_START)// if no errors after parsing
|
||||
{
|
||||
switch (modbus_msg->FuncCode)
|
||||
{
|
||||
// Read Coils
|
||||
case FC_R_COILS:
|
||||
hmodbus->f.MessageHandled = MB_Process_Read_Coils(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
// Read Hodling Registers
|
||||
case FC_R_HOLD_REGS:
|
||||
hmodbus->f.MessageHandled = MB_Process_Read_Hold_Regs(hmodbus->pMessagePtr);
|
||||
break;
|
||||
case FC_R_IN_REGS:
|
||||
hmodbus->f.MessageHandled = MB_Process_Read_Input_Regs(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
|
||||
// Write Single Coils
|
||||
case FC_W_COIL:
|
||||
hmodbus->f.MessageHandled = MB_Process_Write_Single_Coil(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.DataUpdated = 1;
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
case FC_W_HOLD_REG:
|
||||
hmodbus->f.MessageHandled = MB_Process_Write_Single_Reg(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.DataUpdated = 1;
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
// Write Multiple Coils
|
||||
case FC_W_COILS:
|
||||
hmodbus->f.MessageHandled = MB_Process_Write_Miltuple_Coils(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.DataUpdated = 1;
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
// Write Multiple Registers
|
||||
case FC_W_HOLD_REGS:
|
||||
hmodbus->f.MessageHandled = MB_Process_Write_Miltuple_Regs(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.DataUpdated = 1;
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
case FC_R_DEVICE_ID:
|
||||
hmodbus->f.MessageHandled = MB_Process_Read_Device_Identifications(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
// Добавить в switch-case после других case:
|
||||
case FC_R_DIAGNOSTICS:
|
||||
hmodbus->f.MessageHandled = MB_Process_Diagnostics(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
/* unknown func code */
|
||||
default:
|
||||
modbus_msg->Except_Code = 0x01; /* set exception code: illegal function */
|
||||
}
|
||||
|
||||
|
||||
// Проверяем режим устройства - если Listen Only, не обрабатываем команды
|
||||
if (MB_GetDeviceMode() == MODBUS_LISTEN_ONLY_MODE)
|
||||
{
|
||||
MB_Diagnostics_SlaveNoResponseCnt();
|
||||
hmodbus->RS_STATUS = RS_SKIP;
|
||||
return RS_Handle_Receive_Start(hmodbus, modbus_msg);;
|
||||
}
|
||||
|
||||
// Проверяем статус обработки запроса
|
||||
if(hmodbus->f.MessageHandled == 0)
|
||||
{
|
||||
MB_Diagnostics_ExceptionErrorCnt();
|
||||
TrackerCnt_Warn(hmodbus->rs_err);
|
||||
modbus_msg->FuncCode |= FC_ERR_VALUES_START;
|
||||
}
|
||||
else
|
||||
{
|
||||
TrackerCnt_Ok(hmodbus->rs_err);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// if we need response - check that transmit isnt busy
|
||||
if( RS_Is_TX_Busy(hmodbus) )
|
||||
RS_Abort(hmodbus, ABORT_TX); // if tx busy - set it free
|
||||
|
||||
// Transmit right there, or sets (fDeferredResponse) to transmit response in main code
|
||||
if(hmodbus->f.DeferredResponse == 0)
|
||||
{
|
||||
MB_RES = RS_Handle_Transmit_Start(hmodbus, modbus_msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
RS_Handle_Receive_Start(hmodbus, modbus_msg);
|
||||
hmodbus->f.DeferredResponse = 0;
|
||||
}
|
||||
|
||||
hmodbus->RS_STATUS = MB_RES;
|
||||
return MB_RES;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Сбор сообщения в буфер UART в режиме слейв (фрейм слейва из msg -> uart).
|
||||
* @param hmodbus Указатель на хендлер RS.
|
||||
* @param modbus_msg Указатель на структуру сообщения.
|
||||
* @param modbus_uart_buff Указатель на буффер UART.
|
||||
* @return RS_RES Статус о результате заполнения буфера.
|
||||
*/
|
||||
RS_StatusTypeDef MB_Slave_Collect_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
|
||||
{
|
||||
int ind = 0; // ind for modbus-uart buffer
|
||||
|
||||
if(hmodbus->f.EchoResponse && hmodbus->f.MessageHandled) // if echo response need
|
||||
ind = hmodbus->RS_Message_Size;
|
||||
else
|
||||
{
|
||||
//------INFO ABOUT DATA/MESSAGE------
|
||||
#ifdef MODBUS_PROTOCOL_TCP
|
||||
modbus_uart_buff[ind++] = modbus_msg->TransactionID >> 8;
|
||||
modbus_uart_buff[ind++] = modbus_msg->TransactionID& 0xFF;
|
||||
|
||||
modbus_uart_buff[ind++] = modbus_msg->ProtocolID >> 8;
|
||||
modbus_uart_buff[ind++] = modbus_msg->ProtocolID& 0xFF;
|
||||
|
||||
modbus_uart_buff[ind++] = modbus_msg->PDULength >> 8;
|
||||
modbus_uart_buff[ind++] = modbus_msg->PDULength& 0xFF;
|
||||
#endif
|
||||
//-----------[first bytes]-----------
|
||||
// set ID of message/user
|
||||
modbus_uart_buff[ind++] = modbus_msg->DeviceAddr;
|
||||
|
||||
// set dat or err response
|
||||
modbus_uart_buff[ind++] = modbus_msg->FuncCode;
|
||||
|
||||
if (modbus_msg->FuncCode < FC_ERR_VALUES_START) // if no error occur
|
||||
{
|
||||
// fill modbus header
|
||||
if(0) {}
|
||||
#ifdef MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
|
||||
else if(modbus_msg->FuncCode == FC_R_DEVICE_ID) // devide identifications header
|
||||
{
|
||||
modbus_uart_buff[ind++] = modbus_msg->DevId.MEI_Type;
|
||||
modbus_uart_buff[ind++] = modbus_msg->DevId.ReadDevId;
|
||||
modbus_uart_buff[ind++] = modbus_msg->DevId.Conformity;
|
||||
modbus_uart_buff[ind++] = modbus_msg->DevId.MoreFollows;
|
||||
modbus_uart_buff[ind++] = modbus_msg->DevId.NextObjId;
|
||||
modbus_uart_buff[ind++] = modbus_msg->DevId.NumbOfObj;
|
||||
|
||||
if (modbus_msg->ByteCnt > MbData_size*2) // if ByteCnt less than MbData_size
|
||||
{
|
||||
TrackerCnt_Err(hmodbus->rs_err);
|
||||
return RS_COLLECT_MSG_ERR;
|
||||
}
|
||||
|
||||
|
||||
//---------------DATA----------------
|
||||
//-----------[data bytes]------------
|
||||
uint8_t *tmp_data_addr = (uint8_t *)modbus_msg->MbData;
|
||||
for(int i = 0; i < modbus_msg->ByteCnt; i++) // filling buffer with data
|
||||
{ // set data
|
||||
modbus_uart_buff[ind++] = *tmp_data_addr;
|
||||
tmp_data_addr++;
|
||||
}
|
||||
|
||||
}
|
||||
#endif //MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
|
||||
#ifdef MODBUS_ENABLE_DIAGNOSTICS
|
||||
else if(modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
|
||||
{
|
||||
// Diagnostics special format: [SubFunc_HI][SubFunc_LO][Data_HI][Data_LO]
|
||||
modbus_uart_buff[ind++] = modbus_msg->MbData[0] >> 8; // Sub-function HI
|
||||
modbus_uart_buff[ind++] = modbus_msg->MbData[0] & 0xFF; // Sub-function LO
|
||||
modbus_uart_buff[ind++] = modbus_msg->MbData[1] >> 8; // Data HI
|
||||
modbus_uart_buff[ind++] = modbus_msg->MbData[1] & 0xFF; // Data LO
|
||||
}
|
||||
#endif //MODBUS_ENABLE_DIAGNOSTICS
|
||||
else // modbus data header
|
||||
{
|
||||
// set size of received data
|
||||
if (modbus_msg->ByteCnt <= MbData_size*2) // if ByteCnt less than MbData_size
|
||||
modbus_uart_buff[ind++] = modbus_msg->ByteCnt;
|
||||
else // otherwise return data_size err
|
||||
{
|
||||
TrackerCnt_Err(hmodbus->rs_err);
|
||||
return RS_COLLECT_MSG_ERR;
|
||||
}
|
||||
|
||||
//---------------DATA----------------
|
||||
//-----------[data bytes]------------
|
||||
uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->MbData;
|
||||
for(int i = 0; i < modbus_msg->ByteCnt; i++) // filling buffer with data
|
||||
{ // set data
|
||||
if (i%2 == 0) // HI byte
|
||||
modbus_uart_buff[ind++] = (*tmp_data_addr)>>8;
|
||||
else // LO byte
|
||||
{
|
||||
modbus_uart_buff[ind++] = *tmp_data_addr;
|
||||
tmp_data_addr++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
else // if some error occur
|
||||
{ // send expection code
|
||||
modbus_uart_buff[ind++] = modbus_msg->Except_Code;
|
||||
}
|
||||
}
|
||||
if(ind < 0)
|
||||
return RS_COLLECT_MSG_ERR;
|
||||
|
||||
//---------------CRC----------------
|
||||
//---------[last 16 bytes]----------
|
||||
#ifndef MODBUS_PROTOCOL_TCP
|
||||
// calc crc of received data
|
||||
uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
|
||||
// write crc to message structure and modbus-uart buffer
|
||||
modbus_msg->MbCRC = CRC_VALUE;
|
||||
modbus_uart_buff[ind++] = CRC_VALUE;
|
||||
modbus_uart_buff[ind++] = CRC_VALUE >> 8;
|
||||
#endif
|
||||
|
||||
hmodbus->RS_Message_Size = ind;
|
||||
|
||||
return RS_OK; // returns ok
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Определить размер модбас запроса (СЛЕЙВ версия).
|
||||
* @param hRS Указатель на хендлер RS.
|
||||
* @param rx_data_size Указатель на переменную для записи кол-ва байт для принятия.
|
||||
* @return RS_RES Статус о корректности рассчета кол-ва байт для принятия.
|
||||
* @details Определение сколько байтов надо принять по протоколу.
|
||||
*/
|
||||
static int MB_Define_Size_of_Function(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
RS_StatusTypeDef MB_RES = 0;
|
||||
int mb_func_size = 0;
|
||||
|
||||
if (modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
|
||||
{
|
||||
mb_func_size = 1;
|
||||
}
|
||||
else if(modbus_msg->FuncCode == FC_R_DEVICE_ID)
|
||||
{
|
||||
mb_func_size = 0;
|
||||
}
|
||||
else if ((modbus_msg->FuncCode & ~FC_ERR_VALUES_START) < 0x0F)
|
||||
{
|
||||
mb_func_size = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
mb_func_size = modbus_msg->ByteCnt + 2;
|
||||
}
|
||||
|
||||
|
||||
mb_func_size = RS_RX_FIRST_PART_SIZE + mb_func_size; // size of whole message
|
||||
return mb_func_size;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Парс сообщения в режиме слейв (фрейм мастера из uart -> msg).
|
||||
* @param hmodbus Указатель на хендлер RS.
|
||||
* @param modbus_msg Указатель на структуру сообщения.
|
||||
* @param modbus_uart_buff Указатель на буффер UART.
|
||||
* @return RS_RES Статус о результате заполнения структуры.
|
||||
* @details Заполнение структуры сообщения из буффера UART.
|
||||
*/
|
||||
RS_StatusTypeDef MB_Slave_Parse_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
|
||||
{
|
||||
uint32_t check_empty_buff;
|
||||
int ind = 0; // ind for modbus-uart buffer
|
||||
hmodbus->f.RX_Continue = 0;
|
||||
int expected_size = 0;
|
||||
//-----INFO ABOUT DATA/MESSAGE-------
|
||||
#ifdef MODBUS_PROTOCOL_TCP
|
||||
modbus_msg->TransactionID =modbus_uart_buff[ind++]<<8;
|
||||
modbus_msg->TransactionID |=modbus_uart_buff[ind++];
|
||||
|
||||
modbus_msg->ProtocolID =modbus_uart_buff[ind++]<<8;
|
||||
modbus_msg->ProtocolID |=modbus_uart_buff[ind++];
|
||||
|
||||
modbus_msg->PDULength =modbus_uart_buff[ind++]<<8;
|
||||
modbus_msg->PDULength |=modbus_uart_buff[ind++];
|
||||
#endif
|
||||
//-----------[first bits]------------
|
||||
// get ID of message/user
|
||||
if(modbus_uart_buff[ind] != hmodbus->ID)
|
||||
{
|
||||
modbus_msg->DeviceAddr = 0;
|
||||
ind++;
|
||||
}
|
||||
else
|
||||
{
|
||||
modbus_msg->DeviceAddr = modbus_uart_buff[ind++];
|
||||
}
|
||||
|
||||
// get func code
|
||||
modbus_msg->FuncCode = modbus_uart_buff[ind++];
|
||||
if(modbus_msg->FuncCode & FC_ERR_VALUES_START) // явная херня
|
||||
{
|
||||
MB_Diagnostics_SlaveNAKCnt();
|
||||
modbus_msg->DeviceAddr = 0;
|
||||
return RS_SKIP;
|
||||
}
|
||||
|
||||
if(0) {}
|
||||
#ifdef MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
|
||||
else if(modbus_msg->FuncCode == FC_R_DEVICE_ID) // if it device identifications request
|
||||
{
|
||||
modbus_msg->DevId.MEI_Type = modbus_uart_buff[ind++];
|
||||
modbus_msg->DevId.ReadDevId = modbus_uart_buff[ind++];
|
||||
modbus_msg->DevId.NextObjId = modbus_uart_buff[ind++];
|
||||
modbus_msg->ByteCnt = 0;
|
||||
}
|
||||
#endif //MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
|
||||
#ifdef MODBUS_ENABLE_DIAGNOSTICS
|
||||
else if(modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
|
||||
{
|
||||
// Diagnostics: читаем 4 байта в MbData[0] и MbData[1]
|
||||
// Sub-function
|
||||
modbus_msg->MbData[0] = modbus_uart_buff[ind++] << 8;
|
||||
modbus_msg->MbData[0] |= modbus_uart_buff[ind++];
|
||||
// Data
|
||||
modbus_msg->MbData[1] = modbus_uart_buff[ind++] << 8;
|
||||
modbus_msg->MbData[1] |= modbus_uart_buff[ind++];
|
||||
modbus_msg->Addr = 0; // не использует Addr
|
||||
modbus_msg->Qnt = 0; // не использует Qnt
|
||||
}
|
||||
#endif //MODBUS_ENABLE_DIAGNOSTICS
|
||||
else // if its classic modbus request
|
||||
{
|
||||
// get address from CMD
|
||||
modbus_msg->Addr = modbus_uart_buff[ind++] << 8;
|
||||
modbus_msg->Addr |= modbus_uart_buff[ind++];
|
||||
|
||||
// get address from CMD
|
||||
modbus_msg->Qnt = modbus_uart_buff[ind++] << 8;
|
||||
modbus_msg->Qnt |= modbus_uart_buff[ind++];
|
||||
}
|
||||
|
||||
if((hmodbus->pMessagePtr->FuncCode == 0x0F) || (hmodbus->pMessagePtr->FuncCode == 0x10))
|
||||
hmodbus->pMessagePtr->ByteCnt = modbus_uart_buff[ind++];
|
||||
else
|
||||
hmodbus->pMessagePtr->ByteCnt = 0;
|
||||
|
||||
expected_size = MB_Define_Size_of_Function(hmodbus, modbus_msg);
|
||||
// если размер меньше ожидаемого - продолжаем принимать
|
||||
if(hmodbus->RS_Message_Size < expected_size)
|
||||
{
|
||||
hmodbus->f.RX_Continue = 1;
|
||||
return RS_SKIP;
|
||||
}
|
||||
// если больше Ошибка
|
||||
else if (hmodbus->RS_Message_Size > expected_size)
|
||||
{
|
||||
MB_Diagnostics_CommunicationErrorCnt();
|
||||
return RS_PARSE_MSG_ERR;
|
||||
}
|
||||
|
||||
//---------------DATA----------------
|
||||
// (optional)
|
||||
if (modbus_msg->ByteCnt != 0)
|
||||
{
|
||||
//check that data size is correct
|
||||
if (modbus_msg->ByteCnt > MbData_size*2)
|
||||
{
|
||||
TrackerCnt_Err(hmodbus->rs_err);
|
||||
modbus_msg->FuncCode |= FC_ERR_VALUES_START;
|
||||
MB_Diagnostics_CommunicationErrorCnt();
|
||||
return RS_PARSE_MSG_ERR;
|
||||
}
|
||||
uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->MbData;
|
||||
for(int i = 0; i < modbus_msg->ByteCnt; i++)
|
||||
{ // set data
|
||||
if (i%2 == 0)
|
||||
*tmp_data_addr = ((uint16_t)modbus_uart_buff[ind++] << 8);
|
||||
else
|
||||
{
|
||||
*tmp_data_addr |= modbus_uart_buff[ind++];
|
||||
tmp_data_addr++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//---------------CRC----------------
|
||||
//----------[last 16 bits]----------
|
||||
#ifndef MODBUS_PROTOCOL_TCP
|
||||
// calc crc of received data
|
||||
uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
|
||||
// get crc of received data
|
||||
modbus_msg->MbCRC = modbus_uart_buff[ind++];
|
||||
modbus_msg->MbCRC |= modbus_uart_buff[ind++] << 8;
|
||||
// compare crc
|
||||
if (modbus_msg->MbCRC != CRC_VALUE)
|
||||
{
|
||||
MB_Diagnostics_CommunicationErrorCnt();
|
||||
TrackerCnt_Err(hmodbus->rs_err);
|
||||
modbus_msg->FuncCode |= FC_ERR_VALUES_START;
|
||||
}
|
||||
#endif
|
||||
|
||||
return RS_OK;
|
||||
}
|
||||
|
||||
#endif //MODBUS_ENABLE_SLAVE
|
||||
Reference in New Issue
Block a user