Erick
/
ICE-F412
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: ICE-Application/src/Drivers/mod.cpp
- Revision:
- 0:61364762ee0e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ICE-Application/src/Drivers/mod.cpp Tue Jan 24 19:05:33 2017 +0000
@@ -0,0 +1,516 @@
+// modbus
+// ---------------------------------------------------------------------------------------------------------------------
+// includes
+
+#include "mbed.h"
+#include "global.h"
+#include "mod.h"
+
+// ---------------------------------------------------------------------------------------------------------------------
+// globals
+
+unsigned short int mod_data[MOD_DATA];
+unsigned short int mod_ireg[MOD_IREG];
+
+int mod_verbose = 0;
+
+// -------------------------------------------------------------------------------------------------
+// modbus buffers
+
+#define MOD_RXNUM 200
+#define MOD_TXNUM 200
+
+int mod_rxnum = 0;
+int mod_txnum = 0;
+
+unsigned char mod_rxbuf[MOD_RXNUM];
+unsigned char mod_txbuf[MOD_TXNUM];
+
+// -------------------------------------------------------------------------------------------------
+// print functions
+
+void print_uc(char hex, char bin, char dec, char* pre, unsigned char input, char* post)
+{
+ if(pre[0] != 0) printf("%s", pre);
+
+ if(hex)
+ {
+ printf("0x%02X", input);
+ }
+ if(bin)
+ {
+ if(hex) printf(" ");
+ for(int i=7; i>=0; i--)
+ {
+ if(i<7 && (i%4)==3) printf(" "); if(input & (1<<i)) printf("1"); else printf("0");
+ }
+ }
+ if(dec)
+ {
+ if(hex || bin) printf(" "); printf("%u", input);
+ }
+ if(post[0] != 0) printf("%s", post);
+}
+
+
+void print_us(char hex, char bin, char dec, char* pre, unsigned short int input, char* post)
+{
+ if(pre[0] != 0) printf("%s", pre);
+
+ if(hex)
+ {
+ printf("0x%04X", input);
+ }
+ if(bin)
+ {
+ if(hex) printf(" ");
+ for(int i=15; i>=0; i--)
+ {
+ if(i<15 && (i%4)==3) printf(" "); if(input & (1<<i)) printf("1"); else printf("0");
+ }
+ }
+ if(dec)
+ {
+ if(hex || bin) printf(" "); printf("%u", input);
+ }
+ if(post[0] != 0) printf("%s", post);
+}
+
+// -------------------------------------------------------------------------------------------------
+// initialization
+
+RawSerial mod(PC_6, PC_7);
+
+DigitalOut mod_de(PC_8);
+DigitalOut mod_re(PC_9);
+
+void mod_init(void)
+{
+ mod_de = 0;
+ mod_re = 0;
+
+ mod.baud(MOD_BAUD);
+ mod.attach(&mod_process, RawSerial::RxIrq);
+
+ for(int i=0; i<MOD_IREG; i++) mod_ireg[i] = 0;
+}
+
+// -------------------------------------------------------------------------------------------------
+// utilities
+
+void mod_clear(void)
+{
+ mod_rxnum = 0; for(int i=0; i<MOD_RXNUM; i++) mod_rxbuf[i] = 0;
+ mod_txnum = 0; for(int i=0; i<MOD_TXNUM; i++) mod_txbuf[i] = 0;
+}
+
+unsigned short int mod_calc_crc(unsigned char *buffer, int length)
+{
+ unsigned short int temp=0xFFFF, flag;
+
+ for(int i=0; i<length; i++)
+ {
+ temp = temp ^ buffer[i];
+
+ for(int j=1; j<=8; j++)
+ {
+ flag = temp & 0x0001;
+ temp = temp >> 1;
+ if(flag) temp = temp ^ 0xA001;
+ }
+ }
+
+ return(temp);
+}
+
+void mod_ireg_dump(void)
+{
+ for(int i=0; i<MOD_IREG; i++)
+ {
+ printf(" I%02d = ", i);
+ print_us(1,0,1, "", mod_ireg[i], "\r\n");
+ }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+// process received data
+
+void mod_process(void)
+{
+ if(mod.readable())
+ {
+ if(mod_rxnum < MOD_RXNUM)
+ {
+ mod_rxbuf[mod_rxnum++] = mod.getc();
+ }
+ else
+ {
+ if(mod_verbose) printf(" [mod_process] buffer full\r\n");
+ mod_clear();
+ }
+ }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+// MOD_FUNC_GET_IREG (NBYT = total bytes of data transmitted)
+// MOD_FUNC_GET_HREG
+//
+// byte: 0 1 2 3 4 5 6 7
+// received: ADDR FUNC SREG-H SREG-L NREG-H NREG-L RCRC-L RCRC-H
+// transmit: ADDR FUNC NBYT DATA-H DATA-L TCRC-L TCRC-H NULL
+
+int mod_read(int inp_addr, int inp_func, int inp_sreg, int inp_nreg, unsigned char *rd_buf)
+{
+ int mod_error = 0;
+
+ unsigned char tx_addr = inp_addr;
+ unsigned char tx_func = inp_func;
+ WORD2BYTE tx_sreg; tx_sreg.value = inp_sreg;
+ if(inp_nreg < 1) inp_nreg = 1;
+ WORD2BYTE tx_nreg; tx_nreg.value = inp_nreg;
+
+// fill transmit buffer
+
+ mod_clear();
+
+ mod_txbuf[mod_txnum++] = tx_addr;
+ mod_txbuf[mod_txnum++] = tx_func;
+ mod_txbuf[mod_txnum++] = tx_sreg.byte1;
+ mod_txbuf[mod_txnum++] = tx_sreg.byte0;
+ mod_txbuf[mod_txnum++] = tx_nreg.byte1;
+ mod_txbuf[mod_txnum++] = tx_nreg.byte0;
+
+ WORD2BYTE tx_crc; tx_crc.value = mod_calc_crc(mod_txbuf, mod_txnum);
+ mod_txbuf[mod_txnum++] = tx_crc.byte0;
+ mod_txbuf[mod_txnum++] = tx_crc.byte1;
+
+// transmit
+
+ mod_de = 1;
+ mod_re = 1;
+ wait_ms(1);
+ for(int i=0; i<mod_txnum; i++) mod.putc(mod_txbuf[i]);
+ wait_ms(2);
+ mod_de = 0;
+ mod_re = 0;
+
+ wait_ms(30);
+
+// output transmit buffer
+
+ if(mod_verbose)
+ {
+ printf(" mod_txbuf: %d\r\n\r\n", mod_txnum);
+ for(int i=0; i<mod_txnum; i++)
+ {
+ printf(" %02d", i); print_uc(1,0,1, " = ", mod_txbuf[i], "\r\n");
+ }
+ if(mod_txnum)
+ {
+ printf("\r\n");
+ print_uc(1,0,1, " addr = ", tx_addr, "\r\n");
+ print_uc(1,0,1, " func = ", tx_func, "\r\n");
+ print_us(1,0,1, " sreg = ", tx_sreg.value, "\r\n");
+ print_us(1,0,1, " nreg = ", tx_nreg.value, "\r\n");
+ print_us(1,0,1, " tcrc = ", tx_crc.value, "\r\n");
+ }
+ }
+
+// process received buffer
+
+ if(mod_verbose)
+ {
+ printf("\r\n mod_rxbuf: %d\r\n\r\n", mod_rxnum);
+ for(int i=0; i<mod_rxnum; i++)
+ {
+ printf(" %02d", i); print_uc(1,0,1, " = ", mod_rxbuf[i], "\r\n");
+ }
+ }
+ if(mod_rxnum)
+ {
+ int rxnum = 0;
+ unsigned char rx_addr = mod_rxbuf[rxnum++];
+ unsigned char rx_func = mod_rxbuf[rxnum++];
+
+ if(mod_verbose)
+ {
+ print_uc(1,0,1, "\r\n addr = ", rx_addr, "\r\n");
+ print_uc(1,0,1, " func = ", rx_func, "");
+ }
+ if(rx_func == tx_func)
+ {
+ int rd_buf_index = 0;
+ unsigned char rx_nbyt = mod_rxbuf[rxnum++];
+
+ if(mod_verbose) print_uc(1,0,1, "\r\n nbyt = ", rx_nbyt, "\r\n\r\n");
+
+ for(int i=0; i<rx_nbyt/2; i++)
+ {
+ WORD2BYTE rx_data;
+ rd_buf[rd_buf_index++] = rx_data.byte1 = mod_rxbuf[rxnum++];
+ rd_buf[rd_buf_index++] = rx_data.byte0 = mod_rxbuf[rxnum++];
+
+ int ireg = tx_sreg.value + i;
+
+ if(ireg < MOD_IREG) mod_ireg[ireg] = rx_data.value;
+
+ if(mod_verbose)
+ {
+ printf(" data(%03d)", ireg);
+ print_us(1,0,1, " = ", rx_data.value, "\r\n");
+ }
+ }
+ WORD2BYTE rx_crc;
+ rx_crc.byte0 = mod_rxbuf[rxnum++];
+ rx_crc.byte1 = mod_rxbuf[rxnum++];
+
+ if(mod_verbose) print_us(1,0,1, "\r\n rcrc = ", rx_crc.value, "\r\n");
+ }
+ else
+ {
+ unsigned char rx_nerr = mod_rxbuf[rxnum++]; mod_error = rx_nerr;
+
+ if(mod_verbose)
+ {
+ if(rx_func == tx_func+0x80) printf(" (func %d error)\r\n", tx_func);
+ else printf(" (unknown func error)\r\n");
+ print_uc(1,0,1, " nerr = ", rx_nerr, " (");
+
+ switch(rx_nerr)
+ {
+ case MOD_ERROR_ILL_FUNC: ; printf("slave received illegal function code") ; break;
+ case MOD_ERROR_ILL_ADDR: ; printf("slave received illegal register address") ; break;
+ case MOD_ERROR_ILL_DATA: ; printf("slave received illegal register value") ; break;
+ case MOD_ERROR_SLAVE_FAIL: ; printf("slave device-specific failure") ; break;
+ case MOD_ERROR_SLAVE_ACK: ; printf("slave ACK") ; break;
+ case MOD_ERROR_SLAVE_BUSY: ; printf("slave is busy") ; break;
+ case MOD_ERROR_SLAVE_NACK: ; printf("slave NACK") ; break;
+ case MOD_ERROR_SLAVE_PARITY: ; printf("slave memory parity error") ; break;
+ case MOD_ERROR_RX_TIMEOUT: ; printf("master receive timeout") ; break;
+ case MOD_ERROR_TX_TIMEOUT: ; printf("master transmit timeout") ; break;
+ case MOD_ERROR_CRC: ; printf("master CRC error") ; break;
+ default: ; printf("unknown error") ; break;
+ }
+ printf(")\r\n");
+ }
+
+ WORD2BYTE rx_crc;
+ rx_crc.byte0 = mod_rxbuf[rxnum++];
+ rx_crc.byte1 = mod_rxbuf[rxnum++];
+
+ if(mod_verbose) print_us(1,0,1, " rcrc = ", rx_crc.value, "\r\n");
+ }
+ }
+ else
+ {
+ mod_error = MOD_ERROR_RX_TIMEOUT; // no received data
+ }
+
+ if(mod_verbose) printf("\r\n mod_error: %d\r\n", mod_error);
+
+ return(mod_error);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+// MOD_FUNC_SET_HREG
+//
+// byte: 0 1 2 3 4 5 6 7 8
+// received: ADDR FUNC SREG-H SREG-L DATA-H DATA-L RCRC-L RCRC-H
+// transmit: ADDR FUNC SREG-H SREG-L DATA-H DATA-L TCRC-L TCRC-H NULL
+//
+// MOD_FUNC_SET_HREGS
+//
+// byte: 0 1 2 3 4 5 6 7 8 9 10
+// received: ADDR FUNC SREG-H SREG-L NREG-H NREG-L NBYT DATA-H DATA-L RCRC-L RCRC-H
+// transmit: ADDR FUNC SREG-H SREG-L NREG-H NREG-L TCRC-L TCRC-H NULL
+//
+int mod_write(int inp_addr, int inp_func, int inp_sreg, int inp_nreg, unsigned char *xmt_data)
+{
+ int mod_error = 0;
+
+ unsigned char tx_addr = inp_addr;
+ unsigned char tx_func = inp_func;
+ WORD2BYTE tx_sreg; tx_sreg.value = inp_sreg;
+ WORD2BYTE tx_nreg; tx_nreg.value = inp_nreg;
+ WORD2BYTE data;
+ unsigned char nbyt = 2*inp_nreg;
+
+// fill transmit buffer
+
+ mod_clear();
+
+ mod_txbuf[mod_txnum++] = tx_addr;
+ mod_txbuf[mod_txnum++] = tx_func;
+ mod_txbuf[mod_txnum++] = tx_sreg.byte1;
+ mod_txbuf[mod_txnum++] = tx_sreg.byte0;
+
+ if(tx_func == MOD_FUNC_SET_HREG)
+ {
+// data.value = xmt_data[0];
+// mod_txbuf[mod_txnum++] = data.byte1;
+// mod_txbuf[mod_txnum++] = data.byte0;
+ mod_txbuf[mod_txnum++] = xmt_data[0];
+ mod_txbuf[mod_txnum++] = xmt_data[1];
+ }
+ else
+ {
+ mod_txbuf[mod_txnum++] = tx_nreg.byte1;
+ mod_txbuf[mod_txnum++] = tx_nreg.byte0;
+ mod_txbuf[mod_txnum++] = nbyt;
+ for(int i=0,i2=0; i<inp_nreg; i++)
+ {
+ // data.value = xmt_data[i];
+ // mod_txbuf[mod_txnum++] = data.byte1;
+ // mod_txbuf[mod_txnum++] = data.byte0;
+ mod_txbuf[mod_txnum++] = xmt_data[i2];
+ mod_txbuf[mod_txnum++] = xmt_data[(i2+1)];
+ i2=i2+2;
+ }
+ }
+
+ WORD2BYTE tx_crc; tx_crc.value = mod_calc_crc(mod_txbuf, mod_txnum);
+ mod_txbuf[mod_txnum++] = tx_crc.byte0;
+ mod_txbuf[mod_txnum++] = tx_crc.byte1;
+
+// transmit
+
+ mod_de = 1;
+ mod_re = 1;
+ wait_ms(1);
+ for(int i=0; i<mod_txnum; i++) mod.putc(mod_txbuf[i]);
+ wait_ms(2);
+ mod_de = 0;
+ mod_re = 0;
+
+ wait_ms(30);
+
+// output transmit buffer
+
+ if(mod_verbose)
+ {
+ printf(" mod_txbuf: %d\r\n\r\n", mod_txnum);
+ for(int i=0; i<mod_txnum; i++)
+ {
+ printf(" %02d", i); print_uc(1,0,1, " = ", mod_txbuf[i], "\r\n");
+ }
+
+ if(mod_txnum)
+ {
+ print_uc(1,0,1, "\r\n addr = ", tx_addr, "\r\n");
+ print_uc(1,0,1, " func = ", tx_func, "\r\n");
+ print_us(1,0,1, " sreg = ", tx_sreg.value, "\r\n");
+
+ if(tx_func == MOD_FUNC_SET_HREG)
+ {
+ data.value = xmt_data[0];
+ print_us(1,0,1, " data = ", data.value, "\r\n");
+ }
+ else
+ {
+ print_us(1,0,1, " nreg = ", tx_nreg.value, "\r\n");
+ print_uc(1,0,1, " nbyt = ", nbyt, "\r\n\r\n");
+
+ for(int i=0; i<inp_nreg; i++)
+ {
+ printf(" data(%03d)", tx_sreg.value+i);
+ data.value = xmt_data[i];
+ print_us(1,0,1, " = ", data.value, "\r\n");
+ }
+ }
+ print_us(1,0,1, " tcrc = ", tx_crc.value, "\r\n");
+ }
+ }
+
+// process received buffer
+
+ if(mod_verbose)
+ {
+ printf("\r\n mod_rxbuf: %d\r\n\r\n", mod_rxnum);
+ for(int i=0; i<mod_rxnum; i++)
+ {
+ printf(" %02d", i); print_uc(1,0,1, " = ", mod_rxbuf[i], "\r\n");
+ }
+ }
+ if(mod_rxnum)
+ {
+ int rxnum = 0;
+ unsigned char rx_addr = mod_rxbuf[rxnum++];
+ unsigned char rx_func = mod_rxbuf[rxnum++];
+
+ if(mod_verbose)
+ {
+ print_uc(1,0,1, "\r\n addr = ", rx_addr, "\r\n");
+ print_uc(1,0,1, " func = ", rx_func, "");
+ }
+
+ if(rx_func == tx_func)
+ {
+ WORD2BYTE rx_sreg;
+ rx_sreg.byte1 = mod_rxbuf[rxnum++];
+ rx_sreg.byte0 = mod_rxbuf[rxnum++];
+
+ WORD2BYTE rx_nreg;
+ rx_nreg.byte1 = mod_rxbuf[rxnum++];
+ rx_nreg.byte0 = mod_rxbuf[rxnum++];
+
+ WORD2BYTE rx_crc;
+ rx_crc.byte0 = mod_rxbuf[rxnum++];
+ rx_crc.byte1 = mod_rxbuf[rxnum++];
+
+ if(mod_verbose)
+ {
+ print_us(1,0,1, "\r\n sreg = ", rx_sreg.value, "\r\n");
+ print_us(1,0,1, " nreg = ", rx_nreg.value, "\r\n");
+ print_us(1,0,1, " rcrc = ", rx_crc.value, "\r\n");
+ }
+ }
+ else
+ {
+ unsigned char rx_nerr = mod_rxbuf[rxnum++]; mod_error = rx_nerr;
+
+ if(mod_verbose)
+ {
+ if(rx_func == tx_func+0x80) printf(" (func %d error)\r\n", tx_func);
+ else printf(" (unknown func error)\r\n");
+
+ print_uc(1,0,1, " nerr = ", rx_nerr, " (");
+
+ switch(rx_nerr)
+ {
+ case MOD_ERROR_ILL_FUNC: ; printf("slave received illegal function code") ; break;
+ case MOD_ERROR_ILL_ADDR: ; printf("slave received illegal register address") ; break;
+ case MOD_ERROR_ILL_DATA: ; printf("slave received illegal register value") ; break;
+ case MOD_ERROR_SLAVE_FAIL: ; printf("slave device-specific failure") ; break;
+ case MOD_ERROR_SLAVE_ACK: ; printf("slave ACK") ; break;
+ case MOD_ERROR_SLAVE_BUSY: ; printf("slave is busy") ; break;
+ case MOD_ERROR_SLAVE_NACK: ; printf("slave NACK") ; break;
+ case MOD_ERROR_SLAVE_PARITY: ; printf("slave memory parity error") ; break;
+ case MOD_ERROR_RX_TIMEOUT: ; printf("master receive timeout") ; break;
+ case MOD_ERROR_TX_TIMEOUT: ; printf("master transmit timeout") ; break;
+ case MOD_ERROR_CRC: ; printf("master CRC error") ; break;
+ default: ; printf("unknown error") ; break;
+ }
+ printf(")\r\n");
+ }
+
+ WORD2BYTE rx_crc;
+ rx_crc.byte0 = mod_rxbuf[rxnum++];
+ rx_crc.byte1 = mod_rxbuf[rxnum++];
+
+ if(mod_verbose) print_us(1,0,1, " rcrc = ", rx_crc.value, "\r\n");
+ }
+ }
+ else
+ {
+ mod_error = MOD_ERROR_RX_TIMEOUT; // no received data
+ }
+
+ if(mod_verbose) printf("\r\n mod_error: %d\r\n", mod_error);
+
+ return(mod_error);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+
+