Advertisement
RuiViana

Slave que funfou

Feb 26th, 2016
240
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 21.02 KB | None | 0 0
  1. /*
  2. Basic_Slave
  3. Modbus serial - RTU Slave Arduino Sketch
  4. Este exemplo é de domínio público
  5. Testado na IDE 1.0.1
  6. Baseado na biblioteca de Juan Pablo Zometa : jpmzometa@gmail.com
  7. http://sites.google.com/site/jpmzometa/
  8. and Samuel Marco: sammarcoarmengol@gmail.com
  9. and Andras Tucsni.
  10. As funções do protocolo MODBUS implementadas neste código:
  11. 3 - Read holding registers;
  12. 6 - Preset single register;
  13. 16 - Preset multiple registers.
  14. */
  15.  
  16. /*
  17. * configure_mb_slave(baud, parity, tx_en_pin)
  18. *
  19. * configuração dos parametros da porta serial.
  20. *
  21. * baud: taxa de transmissão em bps (valores típicos entre 9600, 19200... 115200)
  22. * parity: seta o modo de paridade:
  23. * 'n' sem paridade (8N1); 'e' paridede impar (8E1), 'o' paridade par (8O1).
  24. * tx_en_pin: pino do arduino que controla a transmissão/recepção em uma linha RS485.
  25. * 0 or 1 desliga esta função (para rede RS232)
  26. * >2 para uma rede multiponto.
  27. */
  28. void configure_mb_slave(long baud, char parity, char txenpin);
  29. /*
  30. * update_mb_slave(slave_id, holding_regs_array, number_of_regs)
  31. *
  32. * verifica se há qualquer pedido válido do mestre modbus. Se houver,
  33. * executa a ação solicitada
  34. *
  35. * slave: endereço do escravo (arduino) (1 to 127)
  36. * regs: uma matriz com os holding registers. Eles começam no endereço 1 (mestre ponto de vista)
  37. * Regs_size: número total de holding registers.
  38. * Retorna: 0 se não houver pedido do mestre,
  39. * NO_REPLY (-1) se nenhuma resposta é enviada para o mestre
  40. * Caso um código de exceção (1 a 4) em algumas exceções de modbus
  41. * O número de bytes enviados como resposta (> 4) se OK.
  42. */
  43. int update_mb_slave(unsigned char slave, int *regs,
  44. unsigned int regs_size);
  45.  
  46. /* Aqui começa o código do exemplo */
  47. /* Parâmetros Modbus RTU de comunicação, o Mestre e os escravos devem usar os mesmos parâmetros */
  48. enum {
  49. COMM_BPS = 4800, /* baud rate */
  50. MB_SLAVE = 2, /* endereço do escravo modbus */
  51. /* cada escravo modbus deve ter um */
  52. /* endereço único na rede modbus */
  53. PARITY = 'n', /* paridade */
  54. TXEN = 3 /*Definir o pino usado para colocar o driver
  55. RS485 em modo de transmissão, utilizado
  56. somente em redes RS485 quando colocar em 0
  57. ou 1 para redes RS232 */
  58. };
  59.  
  60. /* registros do escravo (holding registers)
  61. *
  62. * Aqui ficam ordenados todos os registros de leitura e escrita
  63. * da comunicação entre o mestre e o escravo (SCADA e arduino)
  64. *
  65. */
  66. enum {
  67. MB_PINO_5, /* Controle do Led no pino 3 (desliga=0 liga=1) offset 0 (p/ data point no
  68. scadabr)*/
  69. MB_PINO_4, /* Leitura da chave no pino 4 (desliga=0 liga=1) offset 1 (p/ data point no
  70. scadabr)*/
  71. MB_A6, /* Leitura da chave no pino 4 (desliga=0 liga=1) offset 1 (p/ data point no
  72. scadabr)*/
  73. MB_A0, /* Leitura da entrada analógica 0 (0 a 1023) offset 2 (p/ data point no
  74. scadabr)*/
  75. MB_REGS /* número total de registros do escravo */
  76. };
  77. int regs[MB_REGS];
  78. int ledPin5 = 10;
  79. int ledPin13 = 13;
  80. int buttonPin4 = 11;
  81. int buttonState = 0; // variável para ler e estado do botão
  82. unsigned long wdog = 0; /* watchdog */
  83. unsigned long tprev = 0; /* tempo anterior do último comando*/
  84. unsigned long tanalogprev = 0; /* tempo anterior da leitura dos pinos analógicos*/
  85.  
  86. #include <LiquidCrystal.h> // Inclui a biblioteca LCD
  87. // LiquidCrystal lcd (12, 11, 9, 8, 7, 6); // Escolha dos pinos
  88. LiquidCrystal lcd (9, 8, 7, 6, 5, 4); // Escolha dos pinos
  89. /* incluir a library Emon e criar uma instance */
  90.  
  91. void setup()
  92. {
  93. lcd.begin(16, 2); // Inicia o lcd com 16 carateres e 2 linhas
  94. lcd.clear();
  95. lcd.setCursor(0, 0);
  96. lcd.print(" MODBUS ");
  97. /* configura cominicação modbus
  98. * 9600 bps, 8N1, RS485 network */
  99. configure_mb_slave(COMM_BPS, PARITY, TXEN);
  100. pinMode(ledPin5, OUTPUT);
  101. pinMode(ledPin13, OUTPUT);
  102. pinMode(buttonPin4, INPUT);
  103. }
  104. void loop()
  105. {
  106. /* verifica se há solicitações do mestre */
  107. if (update_mb_slave(MB_SLAVE, regs, MB_REGS))
  108. wdog = millis();
  109. if ((millis() - tanalogprev) > 1000) { /* atualiza as entradas analogica a cada 1 segundo */
  110. /* lê o valor da corrente no pino A0 e salva no REGISTER MB_A0 */
  111. // regs[MB_A0] = Irms; /* ler entrada analógica 0 e salva valor no registro modbus*/
  112. // regs[MB_A0] = 22; /* ler entrada analógica 0 e salva valor no registro modbus*/
  113. tanalogprev = millis();
  114. }
  115. buttonState = digitalRead(buttonPin4); // ler estado da chave no pino 4
  116. // regs[MB_PINO_4] = buttonState; // salva valor no registro modbus
  117. if (buttonState == HIGH) { // caso a chave esteja pressionada
  118. digitalWrite(ledPin13, HIGH); // liga o Led do pino 13
  119. }
  120. else { // caso a chave não esteja pressionada
  121. digitalWrite(ledPin13, LOW); // desliga o Led do pino 13
  122. }
  123. /* os valores do registro MB_PINO_5 é definido pelo mestre modbus (SCADA) */
  124. switch (regs[MB_PINO_5]) {
  125. case 1:
  126. digitalWrite(ledPin5, HIGH);
  127. break;
  128. default: /* apagado */
  129. digitalWrite(ledPin5, LOW);
  130. }
  131. lcd.setCursor(0, 1);
  132. lcd.print(regs[MB_PINO_5]);
  133. lcd.print(" ");
  134. lcd.print(regs[MB_PINO_4]);
  135. lcd.print(" ");
  136. lcd.print(regs[MB_A6]);
  137. lcd.print(" ");
  138. lcd.print(regs[MB_A0]);
  139. }
  140. /****************************************************************************
  141. * INICIO DAS FUNÇÕES ESCRAVO Modbus RTU
  142. ****************************************************************************/
  143. /* variaveis globais */
  144. unsigned int Txenpin = TXEN; /*Definir o pino usado para colocar o driver
  145. RS485 em modo de transmissão, utilizado
  146. somente em redes RS485 quando colocar em 0
  147. ou 1 para redes RS232 */
  148. /* Lista de códigos de função modbus suportados. Se você implementar um novo, colocar o seu
  149. código de função aqui! */
  150. enum {
  151. FC_READ_REGS = 0x03, //Read contiguous block of holding register (Ler um bloco contíguo de registos)
  152. FC_WRITE_REG = 0x06, //Write single holding register (Escrever em um único registro)
  153. FC_WRITE_REGS = 0x10 //Write block of contiguous registers (Escrever em um bloco contíguo de registos)
  154. };
  155. /* Funções suportadas. Se você implementar um novo, colocar seu código em função nessa
  156. matriz! */
  157. const unsigned char fsupported[] = {
  158. FC_READ_REGS, FC_WRITE_REG, FC_WRITE_REGS
  159. };
  160. /* constantes */
  161. enum {
  162. MAX_READ_REGS = 0x7D,
  163. MAX_WRITE_REGS = 0x7B,
  164. MAX_MESSAGE_LENGTH = 256
  165. };
  166. enum {
  167. RESPONSE_SIZE = 6,
  168. EXCEPTION_SIZE = 3,
  169. CHECKSUM_SIZE = 2
  170. };
  171. /* código de exceções */
  172. enum {
  173. NO_REPLY = -1,
  174. EXC_FUNC_CODE = 1,
  175. EXC_ADDR_RANGE = 2,
  176. EXC_REGS_QUANT = 3,
  177. EXC_EXECUTE = 4
  178. };
  179. /* posições dentro da matriz de consulta / resposta */
  180. enum {
  181. SLAVE = 0,
  182. FUNC,
  183. START_H,
  184. START_L,
  185. REGS_H,
  186. REGS_L,
  187. BYTE_CNT
  188. };
  189. /*
  190. CRC
  191. INPUTS:
  192. buf -> Matriz contendo a mensagem a ser enviada para o controlador mestre.
  193. start -> Início do loop no crc do contador, normalmente 0.
  194. cnt -> Quantidade de bytes na mensagem a ser enviada para o controlador mestre
  195. OUTPUTS:
  196. temp -> Retorna byte crc para a mensagem.
  197. COMMENTÁRIOS:
  198. Esta rotina calcula o byte crc alto e baixo de uma mensagem.
  199. Note que este CRC é usado somente para Modbus, não em Modbus PLUS ou TCP.
  200. ****************************************************************************/
  201. unsigned int crc(unsigned char *buf, unsigned char start,
  202. unsigned char cnt)
  203. {
  204. unsigned char i, j;
  205. unsigned temp, temp2, flag;
  206. temp = 0xFFFF;
  207. for (i = start; i < cnt; i++) {
  208. temp = temp ^ buf[i];
  209. for (j = 1; j <= 8; j++) {
  210. flag = temp & 0x0001;
  211. temp = temp >> 1;
  212. if (flag)
  213. temp = temp ^ 0xA001;
  214. }
  215. }
  216. /* Inverter a ordem dos bytes. */
  217. temp2 = temp >> 8;
  218. temp = (temp << 8) | temp2;
  219. temp &= 0xFFFF;
  220. return (temp);
  221. }
  222. /***********************************************************************
  223. *
  224. * As seguintes funções constroem o frame de
  225. * um pacote de consulta modbus.
  226. *
  227. ***********************************************************************/
  228. /*
  229. * Início do pacote de uma resposta read_holding_register
  230. */
  231. void build_read_packet(unsigned char slave, unsigned char function,
  232. unsigned char count, unsigned char *packet)
  233. {
  234. packet[SLAVE] = slave;
  235. packet[FUNC] = function;
  236. packet[2] = count * 2;
  237. }
  238. /*
  239. * Início do pacote de uma resposta preset_multiple_register
  240. */
  241. void build_write_packet(unsigned char slave, unsigned char function,
  242. unsigned int start_addr,
  243. unsigned char count,
  244. unsigned char *packet)
  245. {
  246. packet[SLAVE] = slave;
  247. packet[FUNC] = function;
  248. packet[START_H] = start_addr >> 8;
  249. packet[START_L] = start_addr & 0x00ff;
  250. packet[REGS_H] = 0x00;
  251. packet[REGS_L] = count;
  252. }
  253. /*
  254. * Início do pacote de uma resposta write_single_register
  255. */
  256. void build_write_single_packet(unsigned char slave, unsigned char function,
  257. unsigned int write_addr, unsigned int reg_val, unsigned char* packet)
  258. {
  259. packet[SLAVE] = slave;
  260. packet[FUNC] = function;
  261. packet[START_H] = write_addr >> 8;
  262. packet[START_L] = write_addr & 0x00ff;
  263. packet[REGS_H] = reg_val >> 8;
  264. packet[REGS_L] = reg_val & 0x00ff;
  265. }
  266. /*
  267. * Início do pacote de uma resposta excepção
  268. */
  269. void build_error_packet(unsigned char slave, unsigned char function,
  270. unsigned char exception, unsigned char *packet)
  271. {
  272. packet[SLAVE] = slave;
  273. packet[FUNC] = function + 0x80;
  274. packet[2] = exception;
  275. }
  276. /*************************************************************************
  277. *
  278. * modbus_query( packet, length)
  279. *
  280. * Função para adicionar uma soma de verificação para o fim de um pacote.
  281. * Por favor, note que a matriz pacote deve ser de pelo menos 2 campos mais do que
  282. * String_length.
  283. **************************************************************************/
  284. void modbus_reply(unsigned char *packet, unsigned char string_length)
  285. {
  286. int temp_crc;
  287. temp_crc = crc(packet, 0, string_length);
  288. packet[string_length] = temp_crc >> 8;
  289. string_length++;
  290. packet[string_length] = temp_crc & 0x00FF;
  291. }
  292. /***********************************************************************
  293. *
  294. * send_reply( query_string, query_length )
  295. *
  296. * Função para enviar uma resposta a um mestre Modbus.
  297. * Retorna: o número total de caracteres enviados
  298. ************************************************************************/
  299. int send_reply(unsigned char *query, unsigned char string_length)
  300. {
  301. unsigned char i;
  302. if (Txenpin > 1) { // coloca o MAX485 no modo de transmissão
  303. UCSR0A = UCSR0A | (1 << TXC0);
  304. digitalWrite( Txenpin, HIGH);
  305. delayMicroseconds(3640); // aguarda silencio de 3.5 caracteres em 9600bps
  306. }
  307. modbus_reply(query, string_length);
  308. string_length += 2;
  309. for (i = 0; i < string_length; i++) {
  310. Serial.write(byte(query[i]));
  311. }
  312. if (Txenpin > 1) {// coloca o MAX485 no modo de recepção
  313. while (!(UCSR0A & (1 << TXC0)));
  314. digitalWrite( Txenpin, LOW);
  315. }
  316. return i; /* isso não significa que a gravação foi bem sucedida */
  317. }
  318. /***********************************************************************
  319. *
  320. * receive_request( array_for_data )
  321. *
  322. * Função para monitorar um pedido do mestre modbus.
  323. *
  324. * Retorna: Número total de caracteres recebidos se OK
  325. * 0 se não houver nenhum pedido
  326. * Um código de erro negativo em caso de falha
  327. ***********************************************************************/
  328. int receive_request(unsigned char *received_string)
  329. {
  330. int bytes_received = 0;
  331. /* FIXME: não Serial.available esperar 1.5T ou 3.5T antes de sair do loop? */
  332. while (Serial.available()) {
  333. received_string[bytes_received] = Serial.read();
  334. bytes_received++;
  335. if (bytes_received >= MAX_MESSAGE_LENGTH)
  336. return NO_REPLY; /* erro de porta */
  337. }
  338. return (bytes_received);
  339. }
  340. /*********************************************************************
  341. *
  342. * modbus_request(slave_id, request_data_array)
  343. *
  344. * Função que é retornada quando o pedido está correto
  345. * e a soma de verificação está correto.
  346. * Retorna: string_length se OK
  347. * 0 se não
  348. * Menos de 0 para erros de exceção
  349. *
  350. * Nota: Todas as funções usadas para enviar ou receber dados via
  351. * Modbus devolver esses valores de retorno.
  352. *
  353. **********************************************************************/
  354. int modbus_request(unsigned char slave, unsigned char *data)
  355. {
  356. int response_length;
  357. unsigned int crc_calc = 0;
  358. unsigned int crc_received = 0;
  359. unsigned char recv_crc_hi;
  360. unsigned char recv_crc_lo;
  361. response_length = receive_request(data);
  362. if (response_length > 0) {
  363. crc_calc = crc(data, 0, response_length - 2);
  364. recv_crc_hi = (unsigned) data[response_length - 2];
  365. recv_crc_lo = (unsigned) data[response_length - 1];
  366. crc_received = data[response_length - 2];
  367. crc_received = (unsigned) crc_received << 8;
  368. crc_received =
  369. crc_received | (unsigned) data[response_length - 1];
  370. /*********** verificar CRC da resposta ************/
  371. if (crc_calc != crc_received) {
  372. return NO_REPLY;
  373. }
  374. /* verificar a ID do escravo */
  375. if (slave != data[SLAVE]) {
  376. return NO_REPLY;
  377. }
  378. }
  379. return (response_length);
  380. }
  381. /*********************************************************************
  382. *
  383. * validate_request(request_data_array, request_length, available_regs)
  384. *
  385. * Função para verificar se o pedido pode ser processado pelo escravo.
  386. *
  387. * Retorna: 0 se OK
  388. * Um código de exceção negativa em caso de erro
  389. *
  390. **********************************************************************/
  391. int validate_request(unsigned char *data, unsigned char length,
  392. unsigned int regs_size)
  393. {
  394. int i, fcnt = 0;
  395. unsigned int regs_num = 0;
  396. unsigned int start_addr = 0;
  397. unsigned char max_regs_num;
  398. /* verificar o código de função */
  399. for (i = 0; i < sizeof(fsupported); i++) {
  400. if (fsupported[i] == data[FUNC]) {
  401. fcnt = 1;
  402. break;
  403. }
  404. }
  405. if (0 == fcnt)
  406. return EXC_FUNC_CODE;
  407. if (FC_WRITE_REG == data[FUNC]) {
  408. /* Para a função de escrever um reg único, este é o registro alvo.*/
  409. regs_num = ((int) data[START_H] << 8) + (int) data[START_L];
  410. if (regs_num >= regs_size)
  411. return EXC_ADDR_RANGE;
  412. return 0;
  413. }
  414. /* Para as funções de leitura / escrita de registros, este é o intervalo. */
  415. regs_num = ((int) data[REGS_H] << 8) + (int) data[REGS_L];
  416. /* verifica a quantidade de registros */
  417. if (FC_READ_REGS == data[FUNC])
  418. max_regs_num = MAX_READ_REGS;
  419. else if (FC_WRITE_REGS == data[FUNC])
  420. max_regs_num = MAX_WRITE_REGS;
  421. if ((regs_num < 1) || (regs_num > max_regs_num))
  422. return EXC_REGS_QUANT;
  423. /* verificará a quantidade de registros, endereço inicial é 0 */
  424. start_addr = ((int) data[START_H] << 8) + (int) data[START_L];
  425. if ((start_addr + regs_num) > regs_size)
  426. return EXC_ADDR_RANGE;
  427. return 0; /* OK, sem exceção */
  428. }
  429. /************************************************************************
  430. *
  431. * write_regs(first_register, data_array, registers_array)
  432. *
  433. * escreve nos registradores do escravo os dados em consulta,
  434. * A partir de start_addr.
  435. *
  436. * Retorna: o número de registros escritos
  437. ************************************************************************/
  438. int write_regs(unsigned int start_addr, unsigned char *query, int *regs)
  439. {
  440. int temp;
  441. unsigned int i;
  442. for (i = 0; i < query[REGS_L]; i++) {
  443. /* mudar reg hi_byte para temp */
  444. temp = (int) query[(BYTE_CNT + 1) + i * 2] << 8;
  445. /* OR com lo_byte */
  446. temp = temp | (int) query[(BYTE_CNT + 2) + i * 2];
  447. regs[start_addr + i] = temp;
  448. }
  449. return i;
  450. }
  451. /************************************************************************
  452. *
  453. * preset_multiple_registers(slave_id, first_register, number_of_registers,
  454. * data_array, registers_array)
  455. *
  456. * Escreva os dados na matriz dos registos do escravo.
  457. *
  458. *************************************************************************/
  459. int preset_multiple_registers(unsigned char slave,
  460. unsigned int start_addr,
  461. unsigned char count,
  462. unsigned char *query,
  463. int *regs)
  464. {
  465. unsigned char function = FC_WRITE_REGS; /* Escrever em múltiplos registros */
  466. int status = 0;
  467. unsigned char packet[RESPONSE_SIZE + CHECKSUM_SIZE];
  468. build_write_packet(slave, function, start_addr, count, packet);
  469. if (write_regs(start_addr, query, regs)) {
  470. status = send_reply(packet, RESPONSE_SIZE);
  471. }
  472. return (status);
  473. }
  474. /************************************************************************
  475. *
  476. * write_single_register(slave_id, write_addr, data_array, registers_array)
  477. *
  478. * Escrever um único valor inteiro em um único registo do escravo.
  479. *
  480. *************************************************************************/
  481. int write_single_register(unsigned char slave,
  482. unsigned int write_addr, unsigned char *query, int *regs)
  483. {
  484. unsigned char function = FC_WRITE_REG; /* Função: Write Single Register */
  485. int status = 0;
  486. unsigned int reg_val;
  487. unsigned char packet[RESPONSE_SIZE + CHECKSUM_SIZE];
  488. reg_val = query[REGS_H] << 8 | query[REGS_L];
  489. build_write_single_packet(slave, function, write_addr, reg_val, packet);
  490. regs[write_addr] = (int) reg_val;
  491. /*
  492. written.start_addr=write_addr;
  493. written.num_regs=1;
  494. */
  495. status = send_reply(packet, RESPONSE_SIZE);
  496. return (status);
  497. }
  498. /************************************************************************
  499. *
  500. * read_holding_registers(slave_id, first_register, number_of_registers,
  501. * registers_array)
  502. *
  503. * lê os registros do escravo e envia para o mestre Modbus
  504. *
  505. *************************************************************************/
  506. int read_holding_registers(unsigned char slave, unsigned int start_addr,
  507. unsigned char reg_count, int *regs)
  508. {
  509. unsigned char function = 0x03; /* Função 03: Read Holding Registers */
  510. int packet_size = 3;
  511. int status;
  512. unsigned int i;
  513. unsigned char packet[MAX_MESSAGE_LENGTH];
  514. build_read_packet(slave, function, reg_count, packet);
  515. for (i = start_addr; i < (start_addr + (unsigned int) reg_count);
  516. i++) {
  517. packet[packet_size] = regs[i] >> 8;
  518. packet_size++;
  519. packet[packet_size] = regs[i] & 0x00FF;
  520. packet_size++;
  521. }
  522. status = send_reply(packet, packet_size);
  523. return (status);
  524. }
  525. void configure_mb_slave(long baud, char parity, char txenpin)
  526. {
  527. Serial.begin(baud);
  528. switch (parity) {
  529. case 'e': // 8E1
  530. UCSR0C |= ((1 << UPM01) | (1 << UCSZ01) | (1 << UCSZ00));
  531. // UCSR0C &= ~((1<<UPM00) | (1<<UCSZ02) | (1<<USBS0));
  532. break;
  533. case 'o': // 8O1
  534. UCSR0C |= ((1 << UPM01) | (1 << UPM00) | (1 << UCSZ01) | (1 << UCSZ00));
  535. // UCSR0C &= ~((1<<UCSZ02) | (1<<USBS0));
  536. break;
  537. case 'n': // 8N1
  538. UCSR0C |= ((1 << UCSZ01) | (1 << UCSZ00));
  539. // UCSR0C &= ~((1<<UPM01) | (1<<UPM00) | (1<<UCSZ02) | (1<<USBS0));
  540. break;
  541. default:
  542. break;
  543. }
  544. if (txenpin > 1) { // pino 0 & pino 1 são reservados para RX/TX
  545. Txenpin = txenpin; /* definir variável global */
  546. pinMode(Txenpin, OUTPUT);
  547. digitalWrite(Txenpin, LOW);
  548. }
  549. return;
  550. }
  551. /*
  552. * update_mb_slave(slave_id, holding_regs_array, number_of_regs)
  553. *
  554. * verifica se há qualquer pedido válido do mestre modbus. Se houver,
  555. * executa a ação solicitada
  556. */
  557. unsigned long Nowdt = 0;
  558. unsigned int lastBytesReceived;
  559. const unsigned long T35 = 5;
  560. int update_mb_slave(unsigned char slave, int *regs,
  561. unsigned int regs_size)
  562. {
  563. unsigned char query[MAX_MESSAGE_LENGTH];
  564. unsigned char errpacket[EXCEPTION_SIZE + CHECKSUM_SIZE];
  565. unsigned int start_addr;
  566. int exception;
  567. int length = Serial.available();
  568. unsigned long now = millis();
  569. if (length == 0) {
  570. lastBytesReceived = 0;
  571. return 0;
  572. }
  573. if (lastBytesReceived != length) {
  574. lastBytesReceived = length;
  575. Nowdt = now + T35;
  576. return 0;
  577. }
  578. if (now < Nowdt)
  579. return 0;
  580. lastBytesReceived = 0;
  581. length = modbus_request(slave, query);
  582. if (length < 1)
  583. return length;
  584. exception = validate_request(query, length, regs_size);
  585. if (exception) {
  586. build_error_packet(slave, query[FUNC], exception,
  587. errpacket);
  588. send_reply(errpacket, EXCEPTION_SIZE);
  589. return (exception);
  590. }
  591. start_addr = ((int) query[START_H] << 8) +
  592. (int) query[START_L];
  593. switch (query[FUNC]) {
  594. case FC_READ_REGS:
  595. return read_holding_registers(slave,
  596. start_addr,
  597. query[REGS_L],
  598. regs);
  599. break;
  600. case FC_WRITE_REGS:
  601. return preset_multiple_registers(slave,
  602. start_addr,
  603. query[REGS_L],
  604. query,
  605. regs);
  606. break;
  607. case FC_WRITE_REG:
  608. write_single_register(slave,
  609. start_addr,
  610. query,
  611. regs);
  612. break;
  613. }
  614. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement