Пока не рабоатет
This commit is contained in:
954
modbus.cpp
Normal file
954
modbus.cpp
Normal file
@@ -0,0 +1,954 @@
|
||||
/**
|
||||
**************************************************************************
|
||||
* @file modbus.c
|
||||
* @brief Модуль для реализации MODBUS.
|
||||
**************************************************************************
|
||||
* @details Файл содержит реализацию функций работы с Modbus, включая:
|
||||
* - доступ к coils и registers;
|
||||
* - обработку команд протокола;
|
||||
* - взаимодействие с RS (UART);
|
||||
* - инициализацию.
|
||||
*
|
||||
* @section Функции и макросы
|
||||
*
|
||||
* ### Доступ к coils:
|
||||
* - MB_Set_Coil_Local() — Установить coil по локальному адресу.
|
||||
* - MB_Reset_Coil_Local() — Сбросить coil по локальному адресу.
|
||||
* - MB_Toogle_Coil_Local() — Инвертировать coil по локальному адресу.
|
||||
* - MB_Read_Coil_Local() — Прочитать coil по локальному адресу.
|
||||
* - MB_Write_Coil_Global() — Установить/сбросить coil по глобальному адресу.
|
||||
* - MB_Read_Coil_Global() — Прочитать coil по глобальному адресу.
|
||||
*
|
||||
* ### Обработка команд Modbus:
|
||||
* - MB_DefineRegistersAddress() — Определить начальный адрес регистра.
|
||||
* - MB_DefineCoilsAddress() — Определить начальный адрес coils.
|
||||
* - MB_Check_Address_For_Arr() — Проверить, принадлежит ли адрес массиву.
|
||||
* - Основные команды Modbus:
|
||||
* - MB_Read_Coils()
|
||||
* - MB_Read_Hold_Regs()
|
||||
* - MB_Write_Single_Coil()
|
||||
* - MB_Write_Miltuple_Coils()
|
||||
* - MB_Write_Miltuple_Regs()
|
||||
*
|
||||
* ### Функции для работы с RS (UART):
|
||||
* - RS_Parse_Message() / RS_Collect_Message() — Парсинг и сборка сообщения.
|
||||
* - RS_Response() — Отправка ответа.
|
||||
* - RS_Define_Size_of_RX_Message() — Определение размера принимаемого сообщения.
|
||||
* - RS_Init() — Инициализация UART.
|
||||
*
|
||||
* ### Инициализация:
|
||||
* - MODBUS_FirstInit() — Инициализация модуля Modbus.
|
||||
*
|
||||
* @section Структура данных Modbus
|
||||
*
|
||||
* #### Holding/Input Registers:
|
||||
* - Регистры — 16-битные слова. Доступ к регистрам осуществляется через указатель.
|
||||
* Таким образом, сами регистры могут представлять собой как массив так и структуру.
|
||||
*
|
||||
* #### Coils:
|
||||
* - Coils — это биты, упакованные в 16-битные слова. Доступ к коилам осуществляется через указатель.
|
||||
* Таким образом, сами коилы могут представлять собой как массив так и структуру.
|
||||
*
|
||||
* @section Инструкция по подключению
|
||||
* Для корректной работы надо подключить обработчики RS_UART_Handler(), RS_TIM_Handler(),
|
||||
* в соответствубщие низкоуровневые прерывания UART_IRQHandler, TIM_IRQHandler. После HAL'овского обработчика
|
||||
*
|
||||
* Также необходимо в modbus_config.h настроить дефайны для нужной работы UART
|
||||
* После для запуска Modbus:
|
||||
* @verbatim
|
||||
//----------------Прием модбас----------------//
|
||||
#include "rs_message.h"
|
||||
MODBUS_FirstInit();
|
||||
RS_Receive_IT(&hmodbus1, &MODBUS_MSG);
|
||||
* @endverbatim
|
||||
*
|
||||
******************************************************************************/
|
||||
#include "crc_algs.h"
|
||||
#include "rs_message.h"
|
||||
uint32_t dbg_temp, dbg_temp2, dbg_temp3; // for debug
|
||||
RS_HandleTypeDef hmodbus1;
|
||||
|
||||
/* DEFINE REGISTERS/COILS */
|
||||
MB_DeviceIdentificationTypeDef MB_INFO;
|
||||
MB_DataStructureTypeDef MB_DATA;
|
||||
RS_MsgTypeDef MODBUS_MSG;
|
||||
|
||||
//-------------------------------------------------------------------
|
||||
//-----------------------------FOR USER------------------------------
|
||||
/**
|
||||
* @brief First set up of MODBUS.
|
||||
* @details Первый инит модбас. Заполняет структуры и инициализирует таймер и юарт для общения по модбас.
|
||||
* @note This called from main
|
||||
*/
|
||||
void MODBUS_FirstInit(void)
|
||||
{
|
||||
MB_DevoceInentificationInit();
|
||||
//-----------SETUP MODBUS-------------
|
||||
// set up modbus: MB_RX_Size_NotConst and Timeout enable
|
||||
hmodbus1.ID = MODBUS_DEVICE_ID;
|
||||
hmodbus1.sRS_Timeout = MODBUS_TIMEOUT;
|
||||
hmodbus1.sRS_Mode = SLAVE_ALWAYS_WAIT;
|
||||
hmodbus1.sRS_RX_Size_Mode = RS_RX_Size_NotConst;
|
||||
|
||||
// INIT
|
||||
hmodbus1.RS_STATUS = RS_Init(&hmodbus1, &rs_huart, 0);
|
||||
|
||||
RS_EnableReceive();
|
||||
}
|
||||
/**
|
||||
* @brief Set or Reset Coil at its global address.
|
||||
* @param Addr - адрес коила.
|
||||
* @param WriteVal - Что записать в коил: 0 или 1.
|
||||
* @return ExceptionCode - Код исключения если коила по адресу не существует, и NO_ERRORS если все ок.
|
||||
*
|
||||
* @details Позволяет обратиться к любому коилу по его глобальному адрессу.
|
||||
Вне зависимости от того как коилы размещены в памяти.
|
||||
*/
|
||||
MB_ExceptionTypeDef MB_Write_Coil_Global(uint16_t Addr, MB_CoilsOpTypeDef WriteVal)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
MB_ExceptionTypeDef Exception = NO_ERRORS;
|
||||
uint16_t *coils;
|
||||
uint16_t start_shift = 0; // shift in coils register
|
||||
|
||||
//------------WRITE COIL-------------
|
||||
Exception = MB_DefineCoilsAddress(&coils, Addr, 1, &start_shift, 1);
|
||||
if(Exception == NO_ERRORS)
|
||||
{
|
||||
switch(WriteVal)
|
||||
{
|
||||
case SET_COIL:
|
||||
*coils |= (1<<start_shift);
|
||||
break;
|
||||
|
||||
case RESET_COIL:
|
||||
*coils &= ~(1<<start_shift);
|
||||
break;
|
||||
|
||||
case TOOGLE_COIL:
|
||||
*coils ^= (1<<start_shift);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
return Exception;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Read Coil at its global address.
|
||||
* @param Addr - адрес коила.
|
||||
* @param Exception - Указатель на переменную для кода исключения, в случа неудачи при чтении.
|
||||
* @return uint16_t - Возвращает весь регистр с маской на запрошенном коиле.
|
||||
*
|
||||
* @details Позволяет обратиться к любому коилу по его глобальному адрессу.
|
||||
Вне зависимости от того как коилы размещены в памяти.
|
||||
*/
|
||||
uint16_t MB_Read_Coil_Global(uint16_t Addr, MB_ExceptionTypeDef *Exception)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
MB_ExceptionTypeDef Exception_tmp;
|
||||
if(Exception == NULL) // if exception is not given to func fill it
|
||||
Exception = &Exception_tmp;
|
||||
|
||||
uint16_t *coils;
|
||||
uint16_t start_shift = 0; // shift in coils register
|
||||
|
||||
//------------READ COIL--------------
|
||||
*Exception = MB_DefineCoilsAddress(&coils, Addr, 1, &start_shift, 0);
|
||||
if(*Exception == NO_ERRORS)
|
||||
{
|
||||
return ((*coils)&(1<<start_shift));
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------
|
||||
//----------------FUNCTIONS FOR PROCESSING MESSAGE-------------------
|
||||
/**
|
||||
* @brief Check is address valid for certain array.
|
||||
* @param Addr - начальный адресс.
|
||||
* @param Qnt - количество запрашиваемых элементов.
|
||||
* @param R_ARR_ADDR - начальный адресс массива R_ARR.
|
||||
* @param R_ARR_NUMB - количество элементов в массиве R_ARR.
|
||||
* @return ExceptionCode - ILLEGAL DATA ADRESS если адресс недействителен, и NO_ERRORS если все ок.
|
||||
*
|
||||
* @details Позволяет определить, принадлежит ли адресс Addr массиву R_ARR:
|
||||
* Если адресс Addr находится в диапазоне адрессов массива R_ARR, то возвращаем NO_ERROR.
|
||||
* Если адресс Addr находится за пределами адрессов массива R_ARR - ILLEGAL_DATA_ADDRESSю.
|
||||
*/
|
||||
MB_ExceptionTypeDef MB_Check_Address_For_Arr(uint16_t Addr, uint16_t Qnt, uint16_t R_ARR_ADDR, uint16_t R_ARR_NUMB)
|
||||
{
|
||||
// if address from this array
|
||||
if(Addr >= R_ARR_ADDR)
|
||||
{
|
||||
// if quantity too big return error
|
||||
if ((Addr - R_ARR_ADDR) + Qnt > R_ARR_NUMB)
|
||||
{
|
||||
return ILLEGAL_DATA_ADDRESS; // return exception code
|
||||
}
|
||||
// if all ok - return no errors
|
||||
return NO_ERRORS;
|
||||
}
|
||||
// if address isnt from this array return error
|
||||
else
|
||||
return ILLEGAL_DATA_ADDRESS; // return exception code
|
||||
}
|
||||
/**
|
||||
* @brief Define Address Origin for Input/Holding Registers
|
||||
* @param pRegs - указатель на указатель регистров.
|
||||
* @param Addr - адрес начального регистра.
|
||||
* @param Qnt - количество запрашиваемых регистров.
|
||||
* @param WriteFlag - флаг регистр нужны для чтения или записи.
|
||||
* @return ExceptionCode - Код исключения если есть, и NO_ERRORS если нет.
|
||||
*
|
||||
* @details Определение адреса начального регистра.
|
||||
* @note WriteFlag пока не используется.
|
||||
*/
|
||||
MB_ExceptionTypeDef MB_DefineRegistersAddress(uint16_t **pRegs, uint16_t Addr, uint16_t Qnt, uint8_t RegisterType)
|
||||
{
|
||||
/* check quantity error */
|
||||
if (Qnt > 125)
|
||||
{
|
||||
return ILLEGAL_DATA_VALUE; // return exception code
|
||||
}
|
||||
|
||||
if(RegisterType == RegisterType_Holding)
|
||||
{
|
||||
// Default holding registers
|
||||
if(MB_Check_Address_For_Arr(Addr, Qnt, R_HOLDING_ADDR, R_HOLDING_QNT) == NO_ERRORS)
|
||||
{
|
||||
*pRegs = MB_Set_Register_Ptr(&MB_DATA.HoldRegs, Addr); // указатель на выбранный по Addr регистр
|
||||
}
|
||||
// if address doesnt match any array - return illegal data address response
|
||||
else
|
||||
{
|
||||
return ILLEGAL_DATA_ADDRESS;
|
||||
}
|
||||
}
|
||||
else if(RegisterType == RegisterType_Input)
|
||||
{
|
||||
// Default input registers
|
||||
if(MB_Check_Address_For_Arr(Addr, Qnt, R_INPUT_ADDR, R_INPUT_QNT) == NO_ERRORS)
|
||||
{
|
||||
*pRegs = MB_Set_Register_Ptr(&MB_DATA.InRegs, Addr); // указатель на выбранный по Addr регистр
|
||||
}
|
||||
// if address doesnt match any array - return illegal data address response
|
||||
else
|
||||
{
|
||||
return ILLEGAL_DATA_ADDRESS;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return ILLEGAL_FUNCTION;
|
||||
}
|
||||
// if found requeried array return no err
|
||||
return NO_ERRORS; // return no errors
|
||||
}
|
||||
/**
|
||||
* @brief Define Address Origin for coils
|
||||
* @param pCoils - указатель на указатель коилов.
|
||||
* @param Addr - адресс начального коила.
|
||||
* @param Qnt - количество запрашиваемых коилов.
|
||||
* @param start_shift - указатель на переменную содержащую сдвиг внутри регистра для начального коила.
|
||||
* @param WriteFlag - флаг коилы нужны для чтения или записи.
|
||||
* @return ExceptionCode - Код исключения если есть, и NO_ERRORS если нет.
|
||||
*
|
||||
* @details Определение адреса начального регистра запрашиваемых коилов.
|
||||
* @note WriteFlag используется для определния регистров GPIO: ODR или IDR.
|
||||
*/
|
||||
MB_ExceptionTypeDef MB_DefineCoilsAddress(uint16_t **pCoils, uint16_t Addr, uint16_t Qnt, uint16_t *start_shift, uint8_t WriteFlag)
|
||||
{
|
||||
/* check quantity error */
|
||||
if (Qnt > 2000)
|
||||
{
|
||||
return ILLEGAL_DATA_VALUE; // return exception code
|
||||
}
|
||||
|
||||
// Default coils
|
||||
if(MB_Check_Address_For_Arr(Addr, Qnt, C_CONTROL_ADDR, C_CONTROL_QNT) == NO_ERRORS)
|
||||
{
|
||||
*pCoils = MB_Set_Coil_Reg_Ptr(&MB_DATA.Coils, Addr); // указатель на выбранный по Addr массив коилов
|
||||
}
|
||||
// if address doesnt match any array - return illegal data address response
|
||||
else
|
||||
{
|
||||
return ILLEGAL_DATA_ADDRESS;
|
||||
}
|
||||
|
||||
*start_shift = Addr % 16; // set shift to requested coil
|
||||
// if found requeried array return no err
|
||||
return NO_ERRORS; // return no errors
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Proccess command Read Coils (01 - 0x01).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Read Coils.
|
||||
*/
|
||||
uint8_t MB_Read_Coils(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
uint16_t *coils;
|
||||
uint16_t start_shift = 0; // shift in coils register
|
||||
|
||||
modbus_msg->Except_Code = MB_DefineCoilsAddress(&coils, modbus_msg->Addr, modbus_msg->Qnt, &start_shift, 0);
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
//-----------READING COIL------------
|
||||
// setup output message data size
|
||||
modbus_msg->ByteCnt = Divide_Up(modbus_msg->Qnt, 8);
|
||||
// create mask for coils
|
||||
uint16_t mask_for_coils = 0; // mask for coils that've been chosen
|
||||
uint16_t setted_coils = 0; // value of setted coils
|
||||
uint16_t temp_reg = 0; // temp register for saving coils that hasnt been chosen
|
||||
uint16_t coil_cnt = 0; // counter for processed coils
|
||||
|
||||
// cycle until all registers with requered coils would be processed
|
||||
int shift = start_shift; // set shift to first coil in first register
|
||||
int ind = 0; // index for coils registers and data
|
||||
for(; ind <= Divide_Up(start_shift + modbus_msg->Qnt, 16); ind++)
|
||||
{
|
||||
//----SET MASK FOR COILS REGISTER----
|
||||
mask_for_coils = 0;
|
||||
for(; shift < 0x10; shift++)
|
||||
{
|
||||
mask_for_coils |= 1<<(shift); // choose certain coil
|
||||
if(++coil_cnt >= modbus_msg->Qnt)
|
||||
break;
|
||||
}
|
||||
shift = 0; // set shift to zero for the next step
|
||||
|
||||
//-----------READ COILS--------------
|
||||
modbus_msg->DATA[ind] = (*(coils+ind)&mask_for_coils) >> start_shift;
|
||||
if(ind > 0)
|
||||
modbus_msg->DATA[ind-1] |= ((*(coils+ind)&mask_for_coils) << 16) >> start_shift;
|
||||
|
||||
}
|
||||
// т.к. DATA 16-битная, для 8-битной передачи, надо поменять местами верхний и нижний байты
|
||||
for(; ind >= 0; --ind)
|
||||
modbus_msg->DATA[ind] = ByteSwap16(modbus_msg->DATA[ind]);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Proccess command Read Holding Registers (03 - 0x03).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Read Holding Registers.
|
||||
*/
|
||||
uint8_t MB_Read_Hold_Regs(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
// get origin address for data
|
||||
uint16_t *pHoldRegs;
|
||||
modbus_msg->Except_Code = MB_DefineRegistersAddress(&pHoldRegs, modbus_msg->Addr, modbus_msg->Qnt, RegisterType_Holding); // определение адреса регистров
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
|
||||
//-----------READING REGS------------
|
||||
// setup output message data size
|
||||
modbus_msg->ByteCnt = modbus_msg->Qnt*2; // *2 because we transmit 8 bits, not 16 bits
|
||||
// read data
|
||||
int i;
|
||||
for (i = 0; i<modbus_msg->Qnt; i++)
|
||||
{
|
||||
modbus_msg->DATA[i] = *(pHoldRegs++);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Proccess command Read Input Registers (04 - 0x04).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Read Input Registers.
|
||||
*/
|
||||
uint8_t MB_Read_Input_Regs(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
// get origin address for data
|
||||
uint16_t *pInRegs;
|
||||
modbus_msg->Except_Code = MB_DefineRegistersAddress(&pInRegs, modbus_msg->Addr, modbus_msg->Qnt, RegisterType_Input); // определение адреса регистров
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
|
||||
//-----------READING REGS------------
|
||||
// setup output message data size
|
||||
modbus_msg->ByteCnt = modbus_msg->Qnt*2; // *2 because we transmit 8 bits, not 16 bits
|
||||
// read data
|
||||
int i;
|
||||
for (i = 0; i<modbus_msg->Qnt; i++)
|
||||
{
|
||||
if(*((int16_t *)pInRegs) > 0)
|
||||
modbus_msg->DATA[i] = (*pInRegs++);
|
||||
else
|
||||
modbus_msg->DATA[i] = (*pInRegs++);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/**
|
||||
* @brief Proccess command Write Single Coils (05 - 0x05).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Write Single Coils.
|
||||
*/
|
||||
uint8_t MB_Write_Single_Coil(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
if ((modbus_msg->Qnt != 0x0000) && (modbus_msg->Qnt != 0xFF00))
|
||||
{
|
||||
modbus_msg->Except_Code = ILLEGAL_DATA_VALUE;
|
||||
return 0;
|
||||
}
|
||||
// define position of coil
|
||||
uint16_t *coils;
|
||||
uint16_t start_shift = 0; // shift in coils register
|
||||
modbus_msg->Except_Code = MB_DefineCoilsAddress(&coils, modbus_msg->Addr, 0, &start_shift, 1);
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
|
||||
//----------WRITTING COIL------------
|
||||
if(modbus_msg->Qnt == 0xFF00)
|
||||
*(coils) |= 1<<start_shift; // write flags corresponding to received data
|
||||
else
|
||||
*(coils) &= ~(1<<start_shift); // write flags corresponding to received data
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Proccess command Write Single Register (06 - 0x06).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Write Single Register.
|
||||
*/
|
||||
uint8_t MB_Write_Single_Reg(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
// get origin address for data
|
||||
uint16_t *pHoldRegs;
|
||||
modbus_msg->Except_Code = MB_DefineRegistersAddress(&pHoldRegs, modbus_msg->Addr, 1, RegisterType_Holding); // определение адреса регистров
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
//-----------WRITTING REG------------
|
||||
*(pHoldRegs) = modbus_msg->Qnt;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Proccess command Write Multiple Coils (15 - 0x0F).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Write Multiple Coils.
|
||||
*/
|
||||
uint8_t MB_Write_Miltuple_Coils(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
if (modbus_msg->ByteCnt != Divide_Up(modbus_msg->Qnt, 8))
|
||||
{ // if quantity too large OR if quantity and bytes count arent match
|
||||
modbus_msg->Except_Code = ILLEGAL_DATA_VALUE;
|
||||
return 0;
|
||||
}
|
||||
// define position of coil
|
||||
uint16_t *coils; // pointer to coils
|
||||
uint16_t start_shift = 0; // shift in coils register
|
||||
modbus_msg->Except_Code = MB_DefineCoilsAddress(&coils, modbus_msg->Addr, modbus_msg->Qnt, &start_shift, 1);
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
//----------WRITTING COILS-----------
|
||||
// create mask for coils
|
||||
uint16_t mask_for_coils = 0; // mask for coils that've been chosen
|
||||
uint32_t setted_coils = 0; // value of setted coils
|
||||
uint16_t temp_reg = 0; // temp register for saving coils that hasnt been chosen
|
||||
uint16_t coil_cnt = 0; // counter for processed coils
|
||||
|
||||
// cycle until all registers with requered coils would be processed
|
||||
int shift = start_shift; // set shift to first coil in first register
|
||||
for(int ind = 0; ind <= Divide_Up(start_shift + modbus_msg->Qnt, 16); ind++)
|
||||
{
|
||||
//----SET MASK FOR COILS REGISTER----
|
||||
mask_for_coils = 0;
|
||||
for(; shift < 0x10; shift++)
|
||||
{
|
||||
mask_for_coils |= 1<<(shift); // choose certain coil
|
||||
if(++coil_cnt >= modbus_msg->Qnt)
|
||||
break;
|
||||
}
|
||||
shift = 0; // set shift to zero for the next step
|
||||
|
||||
|
||||
|
||||
//-----------WRITE COILS-------------
|
||||
// get current coils
|
||||
temp_reg = *(coils+ind);
|
||||
// set coils
|
||||
setted_coils = ByteSwap16(modbus_msg->DATA[ind]) << start_shift;
|
||||
if(ind > 0)
|
||||
{
|
||||
setted_coils |= ((ByteSwap16(modbus_msg->DATA[ind-1]) << start_shift) >> 16);
|
||||
}
|
||||
// write coils
|
||||
|
||||
*(coils+ind) = setted_coils & mask_for_coils;
|
||||
// restore untouched coils
|
||||
*(coils+ind) |= temp_reg&(~mask_for_coils);
|
||||
|
||||
|
||||
if(coil_cnt >= modbus_msg->Qnt) // if all coils written - break cycle
|
||||
break; // *kind of unnecessary
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Proccess command Write Multiple Registers (16 - 0x10).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Write Multiple Registers.
|
||||
*/
|
||||
uint8_t MB_Write_Miltuple_Regs(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
//---------CHECK FOR ERRORS----------
|
||||
if (modbus_msg->Qnt*2 != modbus_msg->ByteCnt)
|
||||
{ // if quantity and bytes count arent match
|
||||
modbus_msg->Except_Code = ILLEGAL_DATA_VALUE;
|
||||
return 0;
|
||||
}
|
||||
// get origin address for data
|
||||
uint16_t *pHoldRegs;
|
||||
modbus_msg->Except_Code = MB_DefineRegistersAddress(&pHoldRegs, modbus_msg->Addr, modbus_msg->Qnt, RegisterType_Holding); // определение адреса регистров
|
||||
if(modbus_msg->Except_Code != NO_ERRORS)
|
||||
return 0;
|
||||
|
||||
//-----------WRITTING REGS-----------
|
||||
for (int i = 0; i<modbus_msg->Qnt; i++)
|
||||
{
|
||||
*(pHoldRegs++) = modbus_msg->DATA[i];
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void MB_WriteObjectToMessage(char *mbdata, unsigned *ind, MB_DeviceObjectTypeDef *obj)
|
||||
{
|
||||
mbdata[(*ind)++] = obj->length;
|
||||
for (int i = 0; i < obj->length; i++)
|
||||
{
|
||||
mbdata[(*ind)++] = obj->name[i];
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief Proccess command Read Device Identification (43/14 - 0x2B/0E).
|
||||
* @param modbus_msg - указатель на структуру собщения modbus.
|
||||
* @return fMessageHandled - статус о результате обработки комманды.
|
||||
* @details Обработка команды Write Single Register.
|
||||
*/
|
||||
uint8_t MB_Read_Device_Identification(RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
char *mbdata = (char *)modbus_msg->DATA;
|
||||
unsigned ind = 0;
|
||||
switch(modbus_msg->DevId.ReadDevId)
|
||||
{
|
||||
case MB_BASIC_IDENTIFICATION:
|
||||
mbdata[ind++] = 0x00;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.VendorName);
|
||||
mbdata[ind++] = 0x01;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.ProductCode);
|
||||
mbdata[ind++] = 0x02;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.Revision);
|
||||
modbus_msg->DevId.NumbOfObj = 3;
|
||||
break;
|
||||
case MB_REGULAR_IDENTIFICATION:
|
||||
mbdata[ind++] = 0x03;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.VendorUrl);
|
||||
mbdata[ind++] = 0x04;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.ProductName);
|
||||
mbdata[ind++] = 0x05;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.ModelName);
|
||||
mbdata[ind++] = 0x06;
|
||||
MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.UserApplicationName);
|
||||
modbus_msg->DevId.NumbOfObj = 4;
|
||||
break;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
|
||||
modbus_msg->ByteCnt = ind;
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Respond accord to received message.
|
||||
* @param hRS - указатель на хендлер RS.
|
||||
* @param RS_msg - указатель на структуру сообщения.
|
||||
* @return RS_RES - статус о результате ответа на комманду.
|
||||
* @details Обработка принятой комманды и ответ на неё.
|
||||
*/
|
||||
RS_StatusTypeDef RS_Response(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
|
||||
{
|
||||
RS_StatusTypeDef MB_RES = RS_OK;
|
||||
hmodbus->f.MessageHandled = 0;
|
||||
hmodbus->f.EchoResponse = 0;
|
||||
RS_Reset_TX_Flags(hmodbus); // reset flag for correct transmit
|
||||
|
||||
if(modbus_msg->Func_Code < ERR_VALUES_START)// if no errors after parsing
|
||||
{
|
||||
switch (modbus_msg->Func_Code)
|
||||
{
|
||||
// Read Coils
|
||||
case MB_R_COILS:
|
||||
hmodbus->f.MessageHandled = MB_Read_Coils(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
// Read Hodling Registers
|
||||
case MB_R_HOLD_REGS:
|
||||
hmodbus->f.MessageHandled = MB_Read_Hold_Regs(hmodbus->pMessagePtr);
|
||||
break;
|
||||
case MB_R_IN_REGS:
|
||||
hmodbus->f.MessageHandled = MB_Read_Input_Regs(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
|
||||
// Write Single Coils
|
||||
case MB_W_COIL:
|
||||
hmodbus->f.MessageHandled = MB_Write_Single_Coil(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
case MB_W_HOLD_REG:
|
||||
hmodbus->f.MessageHandled = MB_Write_Single_Reg(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
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 MB_W_COILS:
|
||||
hmodbus->f.MessageHandled = MB_Write_Miltuple_Coils(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
// Write Multiple Registers
|
||||
case MB_W_HOLD_REGS:
|
||||
hmodbus->f.MessageHandled = MB_Write_Miltuple_Regs(hmodbus->pMessagePtr);
|
||||
if(hmodbus->f.MessageHandled)
|
||||
{
|
||||
hmodbus->f.EchoResponse = 1;
|
||||
hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
|
||||
}
|
||||
break;
|
||||
|
||||
case MB_R_DEVICE_INFO:
|
||||
hmodbus->f.MessageHandled = MB_Read_Device_Identification(hmodbus->pMessagePtr);
|
||||
break;
|
||||
|
||||
|
||||
/* unknown func code */
|
||||
default: modbus_msg->Except_Code = ILLEGAL_FUNCTION; /* set exception code: illegal function */
|
||||
}
|
||||
|
||||
if(hmodbus->f.MessageHandled == 0)
|
||||
{
|
||||
modbus_msg->Func_Code = static_cast<MB_FunctonTypeDef>(
|
||||
static_cast<int>(modbus_msg->Func_Code) + ERR_VALUES_START
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// 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
|
||||
MB_RES = RS_Handle_Transmit_Start(hmodbus, modbus_msg);
|
||||
|
||||
hmodbus->RS_STATUS = MB_RES;
|
||||
return MB_RES;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Collect message in buffer to transmit it.
|
||||
* @param hRS - указатель на хендлер RS.
|
||||
* @param RS_msg - указатель на структуру сообщения.
|
||||
* @param msg_uart_buff - указатель на буффер UART.
|
||||
* @return RS_RES - статус о результате заполнения буфера.
|
||||
* @details Заполнение буффера UART из структуры сообщения.
|
||||
*/
|
||||
RS_StatusTypeDef RS_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------
|
||||
//-----------[first bytes]-----------
|
||||
// set ID of message/user
|
||||
modbus_uart_buff[ind++] = modbus_msg->MbAddr;
|
||||
|
||||
// set dat or err response
|
||||
modbus_uart_buff[ind++] = modbus_msg->Func_Code;
|
||||
|
||||
if (modbus_msg->Func_Code < ERR_VALUES_START) // if no error occur
|
||||
{
|
||||
// fill modbus header
|
||||
if(modbus_msg->Func_Code == MB_R_DEVICE_INFO) // devide identification 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 > DATA_SIZE*2) // if ByteCnt less than DATA_SIZE
|
||||
{
|
||||
return RS_COLLECT_MSG_ERR;
|
||||
}
|
||||
|
||||
|
||||
//---------------DATA----------------
|
||||
//-----------[data bytes]------------
|
||||
uint8_t *tmp_data_addr = (uint8_t *)modbus_msg->DATA;
|
||||
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++;
|
||||
}
|
||||
|
||||
}
|
||||
else // modbus data header
|
||||
{
|
||||
// set size of received data
|
||||
if (modbus_msg->ByteCnt <= DATA_SIZE*2) // if ByteCnt less than DATA_SIZE
|
||||
modbus_uart_buff[ind++] = modbus_msg->ByteCnt;
|
||||
else // otherwise return data_size err
|
||||
{
|
||||
return RS_COLLECT_MSG_ERR;
|
||||
}
|
||||
|
||||
//---------------DATA----------------
|
||||
//-----------[data bytes]------------
|
||||
uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->DATA;
|
||||
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;
|
||||
}
|
||||
}
|
||||
//---------------CRC----------------
|
||||
//---------[last 16 bytes]----------
|
||||
// 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->MB_CRC = CRC_VALUE;
|
||||
modbus_uart_buff[ind++] = CRC_VALUE;
|
||||
modbus_uart_buff[ind++] = CRC_VALUE >> 8;
|
||||
|
||||
hmodbus->RS_Message_Size = ind;
|
||||
|
||||
return RS_OK; // returns ok
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parse message from buffer to process it.
|
||||
* @param hRS - указатель на хендлер RS.
|
||||
* @param RS_msg - указатель на структуру сообщения.
|
||||
* @param msg_uart_buff - указатель на буффер UART.
|
||||
* @return RS_RES - статус о результате заполнения структуры.
|
||||
* @details Заполнение структуры сообщения из буффера UART.
|
||||
*/
|
||||
RS_StatusTypeDef RS_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
|
||||
//-----INFO ABOUT DATA/MESSAGE-------
|
||||
//-----------[first bits]------------
|
||||
// get ID of message/user
|
||||
modbus_msg->MbAddr = modbus_uart_buff[ind++];
|
||||
if(modbus_msg->MbAddr != hmodbus->ID)
|
||||
return RS_SKIP;
|
||||
|
||||
// get func code
|
||||
modbus_msg->Func_Code = static_cast<MB_FunctonTypeDef>(modbus_uart_buff[ind++]);
|
||||
if(modbus_msg->Func_Code == MB_R_DEVICE_INFO) // if it device identification request
|
||||
{
|
||||
modbus_msg->DevId.MEI_Type = static_cast<MB_MEITypeDef>(modbus_uart_buff[ind++]);
|
||||
modbus_msg->DevId.ReadDevId = static_cast<MB_ConformityTypeDef>(modbus_uart_buff[ind++]);
|
||||
modbus_msg->DevId.NextObjId = modbus_uart_buff[ind++];
|
||||
modbus_msg->ByteCnt = 0;
|
||||
}
|
||||
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->f.RX_Half == 0) // if all message received
|
||||
{
|
||||
//---------------DATA----------------
|
||||
// (optional)
|
||||
if (modbus_msg->ByteCnt != 0)
|
||||
{
|
||||
ind++; // increment ind for data_size byte
|
||||
//check that data size is correct
|
||||
if (modbus_msg->ByteCnt > DATA_SIZE*2)
|
||||
{
|
||||
modbus_msg->Func_Code = static_cast<MB_FunctonTypeDef>(
|
||||
static_cast<int>(modbus_msg->Func_Code) + ERR_VALUES_START
|
||||
);
|
||||
return RS_PARSE_MSG_ERR;
|
||||
}
|
||||
uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->DATA;
|
||||
for(int i = 0; i < modbus_msg->ByteCnt; i++) // /2 because we transmit 8 bits, not 16 bits
|
||||
{ // 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]----------
|
||||
// calc crc of received data
|
||||
uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
|
||||
// get crc of received data
|
||||
modbus_msg->MB_CRC = modbus_uart_buff[ind++];
|
||||
modbus_msg->MB_CRC |= modbus_uart_buff[ind++] << 8;
|
||||
// compare crc
|
||||
if (modbus_msg->MB_CRC != CRC_VALUE)
|
||||
{
|
||||
modbus_msg->Func_Code = static_cast<MB_FunctonTypeDef>(
|
||||
static_cast<int>(modbus_msg->Func_Code) + ERR_VALUES_START
|
||||
);
|
||||
}
|
||||
// hmodbus->MB_RESPONSE = MB_CRC_ERR; // set func code - error about wrong crc
|
||||
|
||||
// check is buffer empty
|
||||
check_empty_buff = 0;
|
||||
for(int i=0; i<ind;i++)
|
||||
check_empty_buff += modbus_uart_buff[i];
|
||||
// if(check_empty_buff == 0)
|
||||
// hmodbus->MB_RESPONSE = MB_EMPTY_MSG; //
|
||||
}
|
||||
|
||||
return RS_OK;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Define size of RX Message that need to be received.
|
||||
* @param hRS - указатель на хендлер RS.
|
||||
* @param rx_data_size - указатель на переменную для записи кол-ва байт для принятия.
|
||||
* @return RS_RES - статус о корректности рассчета кол-ва байт для принятия.
|
||||
* @details Определение сколько байтов надо принять по протоколу.
|
||||
*/
|
||||
RS_StatusTypeDef RS_Define_Size_of_RX_Message(RS_HandleTypeDef *hmodbus, uint32_t *rx_data_size)
|
||||
{
|
||||
RS_StatusTypeDef MB_RES = RS_OK;
|
||||
|
||||
MB_RES = RS_Parse_Message(hmodbus, hmodbus->pMessagePtr, hmodbus->pBufferPtr);
|
||||
if(MB_RES == RS_SKIP) // if message not for us
|
||||
return MB_RES; // return
|
||||
|
||||
|
||||
if ((hmodbus->pMessagePtr->Func_Code & ~ERR_VALUES_START) < 0x0F)
|
||||
{
|
||||
hmodbus->pMessagePtr->ByteCnt = 0;
|
||||
*rx_data_size = 1;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
hmodbus->pMessagePtr->ByteCnt = hmodbus->pBufferPtr[RX_FIRST_PART_SIZE-1]; // get numb of data in command
|
||||
// +1 because that defines is size, not ind.
|
||||
*rx_data_size = hmodbus->pMessagePtr->ByteCnt + 2;
|
||||
}
|
||||
|
||||
|
||||
if(hmodbus->pMessagePtr->Func_Code == MB_R_DEVICE_INFO)
|
||||
{
|
||||
*rx_data_size = 0;
|
||||
}
|
||||
|
||||
hmodbus->RS_Message_Size = RX_FIRST_PART_SIZE + *rx_data_size; // size of whole message
|
||||
return RS_OK;
|
||||
}
|
||||
|
||||
//-----------------------------FOR USER------------------------------
|
||||
//-------------------------------------------------------------------
|
||||
|
||||
|
||||
void MB_DevoceInentificationInit(void)
|
||||
{
|
||||
MB_INFO.VendorName.name = MODBUS_VENDOR_NAME;
|
||||
MB_INFO.ProductCode.name = MODBUS_PRODUCT_CODE;
|
||||
MB_INFO.Revision.name = MODBUS_REVISION;
|
||||
MB_INFO.VendorUrl.name = MODBUS_VENDOR_URL;
|
||||
MB_INFO.ProductName.name = MODBUS_PRODUCT_NAME;
|
||||
MB_INFO.ModelName.name = MODBUS_MODEL_NAME;
|
||||
MB_INFO.UserApplicationName.name = MODBUS_USER_APPLICATION_NAME;
|
||||
|
||||
|
||||
MB_INFO.VendorName.length = sizeof(MODBUS_VENDOR_NAME);
|
||||
MB_INFO.ProductCode.length = sizeof(MODBUS_PRODUCT_CODE);
|
||||
MB_INFO.Revision.length = sizeof(MODBUS_REVISION);
|
||||
MB_INFO.VendorUrl.length = sizeof(MODBUS_VENDOR_URL);
|
||||
MB_INFO.ProductName.length = sizeof(MODBUS_PRODUCT_NAME);
|
||||
MB_INFO.ModelName.length = sizeof(MODBUS_MODEL_NAME);
|
||||
MB_INFO.UserApplicationName.length = sizeof(MODBUS_USER_APPLICATION_NAME);
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user