453 lines
18 KiB
C
453 lines
18 KiB
C
#include "modbus.h"
|
||
|
||
uint8_t state;
|
||
uint8_t packet[SIZE_MODBUS_PAKET];
|
||
uint8_t size_of_packet;
|
||
uint16_t crc_modbus;
|
||
uint16_t first_register_device;
|
||
uint16_t number_of_registers_required;
|
||
uint16_t crc_answ;
|
||
uint8_t modbus_answ[SIZE_MODBUS_PAKET];
|
||
uint16_t expected_crc;
|
||
unsigned long time;
|
||
uint8_t packet_accepted;
|
||
uint16_t last_possible_register;
|
||
|
||
uint16_t crc_chk(uint8_t *data, uint8_t length);
|
||
uint16_t bytes_unification(uint8_t high, uint8_t low);
|
||
uint8_t read_coils(uint8_t address, uint8_t value, uint8_t *modbus_answ);
|
||
uint8_t read_discrete_inputs(uint8_t address, uint8_t value, uint8_t *modbus_answ);
|
||
uint8_t read_holding_registers(uint8_t address, uint8_t value, uint8_t *modbus_answ);
|
||
uint8_t read_input_registers(uint8_t address, uint8_t value, uint8_t *modbus_answ);
|
||
uint8_t write_single_coil(uint8_t address, uint16_t coil_addr, int on, uint8_t *modbus_answ);
|
||
uint8_t write_single_register(uint8_t address, uint16_t register_addr, uint16_t new_value, uint8_t *modbus_answ);
|
||
uint8_t write_multiple_coils(uint8_t address, uint16_t first_coil_addr, uint16_t recorded_num, uint8_t *modbus_answ);
|
||
uint8_t write_multiple_registers(uint8_t address, uint16_t first_register_addr, uint16_t recorded_num, uint8_t *modbus_answ);
|
||
uint8_t modbus_err_answ(uint8_t address, uint8_t func_code, uint8_t err_code, uint8_t *modbus_answ);
|
||
void modbus_rtu_send(uint8_t answ_len);
|
||
|
||
void modbus_init(void)
|
||
{
|
||
size_of_packet = 0;
|
||
crc_modbus = 0xFFFF;
|
||
state = 0;
|
||
}
|
||
|
||
void range_in_range(uint8_t a, uint8_t b, uint8_t x, uint8_t y)
|
||
{
|
||
uint8_t c = a + b - 1;
|
||
if (a >= x && a <= y) || (c >= x && c <= y)
|
||
{
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
// Функция приема modbus-запроса RTU
|
||
void modbus_rtu(void)
|
||
{
|
||
// состояние приема
|
||
if (state == 0)
|
||
{
|
||
uint8_t data;
|
||
// Размер пакета 0
|
||
size_of_packet = 0;
|
||
// Записываем время начала приема пакета
|
||
time = get_millis();
|
||
// Пакет не принят
|
||
packet_accepted = 0;
|
||
|
||
// Цикл приема символов
|
||
while (!packet_accepted)
|
||
{
|
||
// Читаем символ из входящего кольцевого буфера
|
||
// Кольцевой буфер не пуст
|
||
if (count_elements(get_rb_receive()) > 0)
|
||
{
|
||
// Читаем символ из кольцевого буфера
|
||
rb_get(get_rb_receive(), &data);
|
||
if (size_of_packet == 0)
|
||
{
|
||
// Пришел новый пакет
|
||
packet[0] = data;
|
||
size_of_packet = 1;
|
||
time = get_millis();
|
||
}
|
||
|
||
else
|
||
{
|
||
// Проверяем линейный буфер на переполнение
|
||
if (size_of_packet == SIZE_MODBUS_PAKET)
|
||
{
|
||
// Буфер переполнился
|
||
// Ждем новый пакет
|
||
size_of_packet = 0;
|
||
continue;
|
||
}
|
||
else
|
||
{
|
||
time = get_millis();
|
||
packet[size_of_packet] = data;
|
||
size_of_packet++;
|
||
}
|
||
}
|
||
}
|
||
// Проверка временного интервала между символами
|
||
else
|
||
{
|
||
if ((get_millis() - time) > MAX_PAUSE)
|
||
{
|
||
// Превышен таймаут
|
||
// Пакет закончился
|
||
packet_accepted = 1;
|
||
}
|
||
}
|
||
}
|
||
// Проверяем размер пакета
|
||
if (size_of_packet < 5)
|
||
{
|
||
size_of_packet = 0;
|
||
crc_modbus = 0xFFFF;
|
||
return;
|
||
}
|
||
|
||
// считаем контрольную сумму CRC и сравниваем с CRC в пакете
|
||
expected_crc = bytes_unification(size_of_packet - 2, size_of_packet - 1);
|
||
crc_modbus = crc_chk(packet, size_of_packet - 3);
|
||
// проверка контрольной суммы CRC
|
||
if (crc_modbus != expected_crc)
|
||
{
|
||
size_of_packet = 0;
|
||
crc_modbus = 0xFFFF;
|
||
// формируем ошибку произошла невосполнимая ошибка
|
||
return;
|
||
}
|
||
// проверка адреса устройства
|
||
else if (get_device_address() != packet[0])
|
||
{
|
||
size_of_packet = 0;
|
||
crc_modbus = 0xFFFF;
|
||
// формируем ошибку адреса
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
// Расчленяем пакет
|
||
state = 1;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Функция отправки modbus-запроса в кольцевой буфер
|
||
void modbus_rtu_send(uint8_t answ_len)
|
||
{
|
||
size_of_packet = 0;
|
||
// состояние отправки
|
||
if (state == 1)
|
||
{
|
||
// цикл отправки символов
|
||
while (state == 1)
|
||
{
|
||
// Отправка всех байт в кольцевой буфер
|
||
rb_put(get_rb_transmit(), modbus_answ[size_of_packet]);
|
||
|
||
// смещаем указатель
|
||
size_of_packet++;
|
||
if (size_of_packet == answ_len)
|
||
{
|
||
state = 0;
|
||
size_of_packet = 0;
|
||
crc_modbus = 0xFFFF;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// Функция формирования modbus-ответа RTU
|
||
void modbus_answer()
|
||
{
|
||
switch (packet[1])
|
||
{
|
||
case READ_COILS:
|
||
first_register_device = bytes_unification(2, 3);
|
||
number_of_registers_required = bytes_unification(4, 5);
|
||
|
||
if (range_in_range(first_register_device, number_of_registers_required, COIL_1, COIL_4) && number_of_registers_required > 0)
|
||
{
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
|
||
}
|
||
else
|
||
{
|
||
uint8_t value = 0;
|
||
while (number_of_registers_required > 0)
|
||
{
|
||
switch (first_register_device)
|
||
{
|
||
case COIL_1:
|
||
value |= (gpio_read(PIN_COIL1));
|
||
break;
|
||
case COIL_2:
|
||
value |= (gpio_read(PIN_COIL2));
|
||
break;
|
||
case COIL_3:
|
||
value |= (gpio_read(PIN_COIL3));
|
||
break;
|
||
case COIL_4:
|
||
value |= (gpio_read(PIN_COIL4));
|
||
break;
|
||
}
|
||
|
||
first_register_device++;
|
||
number_of_registers_required--;
|
||
}
|
||
modbus_rtu_send(read_coils(get_device_address(), value, &modbus_answ));
|
||
}
|
||
|
||
break;
|
||
case READ_DISCRETE_INPUTS:
|
||
first_register_device = bytes_unification(2, 3);
|
||
number_of_registers_required = bytes_unification(4, 5);
|
||
|
||
if (range_in_range(first_register_device, number_of_registers_required, DISCRETE_INPUTS_1, DISCRETE_INPUTS_4) && number_of_registers_required > 0)
|
||
{
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
|
||
}
|
||
else
|
||
{
|
||
uint8_t value = 0;
|
||
while (number_of_registers_required > 0)
|
||
{
|
||
switch (first_register_device)
|
||
{
|
||
case DISCRETE_INPUTS_1:
|
||
value |= (gpio_read(PIN_DISCRETE_INPUT_1));
|
||
break;
|
||
case DISCRETE_INPUTS_2:
|
||
value |= (gpio_read(PIN_DISCRETE_INPUT_2));
|
||
break;
|
||
case DISCRETE_INPUTS_3:
|
||
value |= (gpio_read(PIN_DISCRETE_INPUT_3));
|
||
break;
|
||
case DISCRETE_INPUTS_4:
|
||
value |= (gpio_read(PIN_DISCRETE_INPUT_4));
|
||
break;
|
||
}
|
||
|
||
first_register_device ++;
|
||
number_of_registers_required--;
|
||
}
|
||
modbus_rtu_send(read_discrete_inputs(get_device_address(), value, &modbus_answ));
|
||
}
|
||
|
||
break;
|
||
case READ_HOLDING_REGISTERS:
|
||
first_register_device = bytes_unification(2, 3);
|
||
number_of_registers_required = bytes_unification(4, 5);
|
||
|
||
if (first_register_device != HOLDING_REGISTER_SLAVE_ADDRESS || number_of_registers_required != 1)
|
||
{
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
|
||
}
|
||
else
|
||
{
|
||
modbus_rtu_send(read_holding_registers(get_device_address(), get_device_address(), &modbus_answ));
|
||
}
|
||
|
||
break;
|
||
case READ_INPUT_REGISTERS:
|
||
first_register_device = bytes_unification(2, 3);
|
||
number_of_registers_required = bytes_unification(4, 5);
|
||
|
||
if (first_register_device != INPUT_REGISTERS || number_of_registers_required != 1)
|
||
{
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
|
||
}
|
||
else
|
||
{
|
||
modbus_rtu_send(read_input_registers(get_device_address(), get_adc_value(), &modbus_answ));
|
||
}
|
||
|
||
break;
|
||
case WRITE_SINGLE_COIL:
|
||
first_register_device = bytes_unification(2, 3);
|
||
|
||
if (range_in_range(first_register_device, 1, COIL_1, COIL_4))
|
||
{
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
|
||
}
|
||
else
|
||
{
|
||
uint8_t value = 0;
|
||
switch (first_register_device)
|
||
{
|
||
case COIL_1:
|
||
value = gpio_write(PIN_COIL1, packet[4]);
|
||
break;
|
||
case COIL_2:
|
||
value = gpio_write(PIN_COIL2, packet[4]);
|
||
break;
|
||
case COIL_3:
|
||
value = gpio_write(PIN_COIL3, packet[4]);
|
||
break;
|
||
case COIL_4:
|
||
value = gpio_write(PIN_COIL4, packet[4]);
|
||
break;
|
||
}
|
||
|
||
modbus_rtu_send(write_single_coil(get_device_address(), first_register_device, GPIORead(first_register_device), &modbus_answ));
|
||
}
|
||
|
||
break;
|
||
case WRITE_SINGLE_REGISTER:
|
||
first_register_device = bytes_unification(2, 3);
|
||
|
||
if (first_register_device != HOLDING_REGISTER_SLAVE_ADDRESS)
|
||
{
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_DATA_ADDRESS, &modbus_answ));
|
||
}
|
||
else
|
||
{
|
||
write_device_address(bytes_unification(4, 5));
|
||
|
||
modbus_rtu_send(write_single_register(get_device_address(), first_register_device, get_device_address(), &modbus_answ));
|
||
}
|
||
|
||
break;
|
||
default:
|
||
modbus_rtu_send(modbus_err_answ(get_device_address(), packet[1], ILLEGAL_FUNCTION, &modbus_answ));
|
||
break;
|
||
}
|
||
}
|
||
|
||
// Функция вычисления контрольной суммы CRC
|
||
uint16_t crc_chk(uint8_t *data, uint8_t length)
|
||
{
|
||
register int16_t j;
|
||
register uint16_t reg_crc = 0xFFFF;
|
||
while (length--)
|
||
{
|
||
reg_crc ^= *data++;
|
||
for (j = 0; j < 8; j++)
|
||
{
|
||
if (reg_crc & 0x01)
|
||
{
|
||
reg_crc = (reg_crc >> 1) ^ 0xA001;
|
||
}
|
||
else
|
||
{
|
||
reg_crc = reg_crc >> 1;
|
||
}
|
||
}
|
||
}
|
||
return reg_crc;
|
||
}
|
||
|
||
// Функция объединения двух байт (старшего и младшего)
|
||
uint16_t bytes_unification(uint8_t high, uint8_t low)
|
||
{
|
||
return (packet[high] << 8) | packet[low];
|
||
}
|
||
|
||
// Коды функций (для формирования ответа)
|
||
|
||
// Чтение данных с регистров флагов (функция с кодом 1)
|
||
uint8_t read_coils(uint8_t address, uint8_t value, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x01; // Код функции
|
||
modbus_answ[2] = 0x01; // Кол-во передаваемых байт
|
||
modbus_answ[3] = value; // Данные
|
||
crc_answ = crc_chk(modbus_answ, 4); // Подсчет контрольной суммы
|
||
modbus_answ[4] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[5] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 6;
|
||
}
|
||
|
||
// Чтение данных с дискретных входов (функция с кодом 2)
|
||
uint8_t read_discrete_inputs(uint8_t address, uint8_t value, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x02; // Код функции
|
||
modbus_answ[2] = 0x01; // Кол-во передаваемых байт
|
||
modbus_answ[3] = value; // Данные
|
||
crc_answ = crc_chk(modbus_answ, 4); // Подсчет контрольной суммы
|
||
modbus_answ[4] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[5] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 6;
|
||
}
|
||
|
||
// Чтение данных с регистров хранения (функция с кодом 3)
|
||
uint8_t read_holding_registers(uint8_t address, uint16_t value, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x03; // Код функции
|
||
modbus_answ[2] = 0x01; // Кол-во передаваемых байт
|
||
modbus_answ[3] = value; // Первый байт данных
|
||
modbus_answ[4] = value >> 8; // Второй байт данных
|
||
crc_answ = crc_chk(modbus_answ, 5); // Подсчет контрольной суммы
|
||
modbus_answ[5] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[6] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 7;
|
||
}
|
||
|
||
// Чтение данных с регистров ввода (функция с кодом 4)
|
||
uint8_t read_input_registers(uint8_t address, uint16_t value, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x04; // Код функции
|
||
modbus_answ[2] = 0x01; // Кол-во передаваемых байт
|
||
modbus_answ[3] = value; // Первый байт данных
|
||
modbus_answ[4] = value >> 8; // Второй байт данных
|
||
crc_answ = crc_chk(modbus_answ, 5); // Подсчет контрольной суммы
|
||
modbus_answ[5] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[6] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 7;
|
||
}
|
||
|
||
// Запись данных в регистр флагов (функция с кодом 5)
|
||
uint8_t write_single_coil(uint8_t address, uint16_t coil_addr, int on, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x05; // Код функции
|
||
modbus_answ[2] = (coil_addr >> 8) & 0xFF; // Старший байт адреса
|
||
modbus_answ[3] = coil_addr & 0xFF; // Младший байт адреса
|
||
modbus_answ[4] = on ? 0xFF : 0x00; // Значение, которое нужно записать
|
||
modbus_answ[5] = 0x00;
|
||
crc_answ = crc_chk(modbus_answ, 6); // Подсчет контрольной суммы
|
||
modbus_answ[6] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[7] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 8;
|
||
}
|
||
|
||
// Запись данных в регистр (функция с кодом 6)
|
||
uint8_t write_single_register(uint8_t address, uint16_t register_addr, uint16_t new_value, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x06; // Код функции
|
||
modbus_answ[2] = (register_addr >> 8) & 0xFF; // Старший байт адреса
|
||
modbus_answ[3] = register_addr & 0xFF; // Младший байт адреса
|
||
modbus_answ[4] = (new_value >> 8) & 0xFF; // Старший байт значения
|
||
modbus_answ[5] = new_value & 0xFF; // Младший байт значения
|
||
crc_answ = crc_chk(modbus_answ, 6); // Подсчет контрольной суммы
|
||
modbus_answ[6] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[7] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 8;
|
||
}
|
||
|
||
// Сообщение об ошибке
|
||
uint8_t modbus_err_answ(uint8_t address, uint8_t func_code, uint8_t err_code, uint8_t *modbus_answ)
|
||
{
|
||
modbus_answ[0] = address; // Адрес
|
||
modbus_answ[1] = 0x80 + func_code; // Код функции + ошибка
|
||
modbus_answ[2] = err_code; // Код ошибки
|
||
crc_answ = crc_chk(modbus_answ, 3); // Подсчет контрольной суммы
|
||
modbus_answ[3] = crc_answ & 0xFF; // Первый байт контрольной суммы
|
||
modbus_answ[4] = (crc_answ >> 8) & 0xFF; // Второй байт контрольной суммы
|
||
return 5;
|
||
}
|