Diod_Test/diode_tester/Core/Interfaces/modbus.c

/** 
**************************************************************************
* @file 	modbus.c
* @brief 	Модуль для реализации MODBUS.
**************************************************************************	
* @par Функции и дефайны
*
* Defines: data access
* 	- MB_Set_Coil_Local()						 			- Выставление коила по локальному адресу относительно начала массива
* 	- MB_Reset_Coil_Local()					 			- Сброс коила по локальному адресу относительно начала массива
* 	- MB_Toogle_Coil_Local()							- Переключение коила по локальному адресу относительно начала массива
* 	- MB_Read_Coil_Local()					 			- Чтение коила по локальному адресу относительно начала массива
*
* Functions: data access
* 	- MB_Write_Coil_Global()					 		- Запись 0/1 в один коил по глобальному адресу
* 	- MB_Read_Coil_Global()						 		- Чтение одного коила по глобальному адресу
* 
* Functions: process message
* 	- MB_DefineRegistersAddress() 		 		- Определение "начального" адреса регистров
* 	- MB_DefineCoilsAddress()					 		- Определение "начального" адреса коилов
* 	- MB_Check_Address_For_Arr() 			 		- Определение	принадлежит ли адресс Addr конкретному массиву
* 	- Обработка команд модбас
* 		- MB_Read_Coils(), 
*			- MB_Read_Hold_Regs(),
*			- MB_Write_Single_Coil()
*			- MB_Write_Miltuple_Coils()
*			- MB_Write_Miltuple_Regs()					
* 
* Functions: RS functions
* 	-	RS_Parse_Message() / RS_Collect_Message()	- Заполнение структуры сообщения и буфера
* 	-	RS_Response()  									 		- Ответ на комманду
* 	- RS_Define_Size_of_RX_Message()	 		- Определение размера принимаемых данных
* 	-	RS_Init()												 		- Инициализация периферии и modbus handler
* 
* Functions: initialization
* 	- MODBUS_FirstInit()									- Инициализация modbus
* 	
**************************************************************************	
* @par Данные для модбас
*
* Holding/Input Registers
* 	- Регистры представляют собой 16-битные числа (слова). В обработке комманд 
* 	находится адресс "начального" регистра и записывается в указатель. Доступ к 
* 	остальным регистрам осуществляется через указатель.	Таким образом, сами 
* 	регистры могут представлять собой как массив так и структуру.
* 
* Coils
*		- Коилы представляют собой биты, упакованные в 16-битные регистры. В обработке 
* 	комманд находится адресс "начального" регистра запрашиваемого коила. Доступ к 
* 	остальным коилам осуществляется через маску и указатель. Таким образом, сами 
* 	коилы могут представлять собой как массив так и структуру.
*
@verbatim
EXAMPLE: INIT SLAVE RECEIVE
  //--------------Настройка модбас--------------//		
  // set up UART for modbus
  modbus1_suart.huart.Instance = USED_MODBUS_UART;
  modbus1_suart.huart.Init.BaudRate = PROJSET.MB_SPEED;	
  modbus1_suart.GPIOx = GPIOB;
  modbus1_suart.GPIO_PIN_RX = GPIO_PIN_11;
  modbus1_suart.GPIO_PIN_TX = GPIO_PIN_10;
  
  // set up timeout TIM for modbus
  modbus1_stim.htim.Instance = TIM7;
  modbus1_stim.sTimAHBFreqMHz = 84;
  modbus1_stim.sTimMode = TIM_IT_CONF;
  
  // set up modbus: MB_RX_Size_NotConst and Timeout enable
  hmodbus1.ID = 1;
  hmodbus1.sRS_Timeout = 5000;
  hmodbus1.sRS_Mode = SLAVE_ALWAYS_WAIT;
  hmodbus1.sRS_RX_Size_Mode = RS_RX_Size_NotConst;
  
  // INIT
  hmodbus1.RS_STATUS = RS_Init(&hmodbus1, &modbus1_suart, &modbus1_stim, 0);	
  
  //----------------Прием модбас----------------//
  RS_MsgTypeDef MODBUS_MSG;
  RS_Receive_IT(&hmodbus1, &MODBUS_MSG);
@endverbatim
*************************************************************************/

#include "rs_message.h"
uint32_t dbg_temp, dbg_temp2, dbg_temp3; // for debug
/* MODBUS HANDLES */
#ifdef INCLUDE_GENERAL_PERIPH_LIBS
UART_SettingsTypeDef modbus1_suart;
TIM_SettingsTypeDef modbus1_stim;
#else
extern UART_HandleTypeDef rs_huart;
extern TIM_HandleTypeDef rs_htim;
#endif
RS_HandleTypeDef hmodbus1;

/* DEFINE REGISTERS/COILS */
MB_DataStructureTypeDef MB_DATA;
RS_MsgTypeDef MODBUS_MSG;
MB_DeviceIdentificationTypeDef MB_INFO;

uint32_t  delay_scide = 1;
uint32_t  numb_scide = 10;
//-------------------------------------------------------------------
//-----------------------------FOR USER------------------------------
/** 
	* @brief 		First set up of MODBUS.
	* @details 	Первый инит модбас. Заполняет структуры и инициализирует таймер и юарт для общения по модбас.
	* 					Скважность ШИМ меняется по закону синусоиды, каждый канал генерирует свой полупериод синуса (от -1 до 0 И от 0 до 1)
	* 					ШИМ генерируется на одном канале.
	* @note			This called from main
	*/
void MODBUS_FirstInit(void)
{
  MB_DevoceInentificationInit();
	//-----------SETUP MODBUS-------------
	// set up UART for modbus
#ifdef INCLUDE_GENERAL_PERIPH_LIBS
	modbus1_suart.huart.Instance = USED_MODBUS_UART;
	modbus1_suart.huart.Init.BaudRate = MODBUS_SPEED;	
	modbus1_suart.GPIOx = MODBUS_GPIOX;
	modbus1_suart.GPIO_PIN_RX = MODBUS_GPIO_PIN_RX;
	modbus1_suart.GPIO_PIN_TX = MODBUS_GPIO_PIN_TX;
	
	// set up timeout TIM for modbus
	modbus1_stim.htim.Instance = USED_MODBUS_TIM;
	modbus1_stim.sTimAHBFreqMHz = MODBUS_TIM_AHB_FREQ;
	modbus1_stim.sTimMode = TIM_IT_CONF;
	
#endif
	// set up modbus: MB_RX_Size_NotConst and Timeout enable
	hmodbus1.ID = MODBUS_DEVICE_ID;
	hmodbus1.sRS_Mode = SLAVE_ALWAYS_WAIT;
	hmodbus1.sRS_RX_Size_Mode = RS_RX_Size_NotConst;
	
	// INIT
#ifdef INCLUDE_GENERAL_PERIPH_LIBS
	hmodbus1.RS_STATUS = RS_Init(&hmodbus1, &modbus1_suart, &modbus1_stim, 0);	
#else
  hmodbus1.RS_STATUS = RS_Init(&hmodbus1, &rs_huart, &rs_htim, 0);
#endif

	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)
	{
		// Устаки для тестера
		if(MB_Check_Address_For_Arr(Addr, Qnt, R_SETTINGS_ADDR, R_SETTINGS_QNT) == NO_ERRORS)
		{
			*pRegs = MB_Set_Register_Ptr(&MB_DATA.HoldRegs, Addr);	// начало регистров хранения/входных
		}
		// if address doesnt match any array - return illegal data address response
		else	
		{
			return ILLEGAL_DATA_ADDRESS;
		}
	}
	else if(RegisterType == RegisterType_Input)
	{
		// Измеренные параметры диода
		if(MB_Check_Address_For_Arr(Addr, Qnt, R_MEASURED_ADDR, R_MEASURED_QNT) == NO_ERRORS)
		{
			*pRegs = MB_Set_Register_Ptr(&MB_DATA.InRegs, 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
	}
		
		// tester settings coils
	if(MB_Check_Address_For_Arr(Addr, Qnt, C_SETTINGS_ADDR, C_SETTINGS_QNT) == NO_ERRORS)
	{
		*pCoils = MB_Set_Coil_Reg_Ptr(&MB_DATA.Coils, Addr);
	}// tester control coils
	else 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);
	}
	// 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 = 3;
		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 = 0;
	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 = 0x01; /* set exception code: illegal function */
		}		
				
		if(hmodbus->f.MessageHandled == 0)
    {
      TrackerCnt_Err(hmodbus->rs_err);
			modbus_msg->Func_Code += 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	
	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
        {
          TrackerCnt_Err(hmodbus->rs_err);
          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
        {
          TrackerCnt_Err(hmodbus->rs_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 = modbus_uart_buff[ind++];
	if(modbus_msg->Func_Code == MB_R_DEVICE_INFO) // if it device identification 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;
  }
  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)
			{
        TrackerCnt_Err(hmodbus->rs_err);
				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)
    {
      TrackerCnt_Err(hmodbus->rs_err);
			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 = 0;
	
	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);
}