first working version of modbus-fronius

modbus-fronius.c

@@ -0,0 +1,390 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <modbus.h>
+#include <string.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <math.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+
+#define NB_REGS 120
+#define NB_CONNECTION 5
+#define IEM3155_ADDRESS 1 // Modbus RTU slave address for iEM3155
+#define METER_OR_GATEWAY_IP_ADDRESS "10.0.1.100"
+#define METER_OR_GATEWAY_PORT "502"
+#define TRANSLATOR_IP_ADDRESS "10.0.1.2"
+
+#define MODBUS_SET_INT32_TO_INT16_FRONIUS(tab_int16, index, value)              \
+    do {                                                                \
+        ((int16_t *) (tab_int16))[(index) + 1] = (int16_t) ((value) >> 16); \
+        ((int16_t *) (tab_int16))[(index)] = (int16_t) (value);     \
+    } while (0)
+
+int stringModbusCopy(char * destination,char * source, int n){ // Custom method to write string in the right format for Modbus
+    // Copy even characters
+    for(int i = 0; i < n; i+=2){
+        destination[i + 1] = source[i];
+    }
+    // Copy odd characters
+    for(int i = 1; i < n; i+=2){
+        destination[i - 1] = source[i];
+    }
+}
+
+static int server_socket = -1;
+static modbus_mapping_t *mb_mapping_sunspec;
+static modbus_t *ctx2; // Context for requests from GEN24
+
+static void close_sigint(int dummy)
+{
+    if (server_socket != -1) {
+        close(server_socket);
+    }
+    modbus_free(ctx2);
+    modbus_mapping_free(mb_mapping_sunspec);
+
+    exit(dummy);
+}
+
+int main()
+{
+    printf("Begin\n");
+    int ret;
+    int rc;
+    int header_length;
+    uint8_t *query;
+    uint8_t nb_retry = 0;
+    modbus_t *ctx; // Context for requests to iEM3155
+    uint16_t dest [NB_REGS];
+    uint16_t reg_address;
+    uint16_t nb_registers;
+    uint16_t tempTab [2];
+    float current_ph1, current_ph2, current_ph3, current_total, apower_ph1, apower_ph2, apower_ph3, apower_total, apower_max, apPower_total, rpower_total, pf;
+    int64_t activeEnergyImport, activeEnergyExport;
+    fd_set refset;
+    fd_set rdset;
+    /* Maximum file descriptor number */
+    int fdmax;
+    int master_socket;
+
+
+
+    //Reader preparation
+    ctx = modbus_new_tcp_pi(METER_OR_GATEWAY_IP_ADDRESS, METER_OR_GATEWAY_PORT);
+    if (ctx == NULL) {
+        perror("Unable to create the libmodbus context\n");
+        return -1;
+    }
+
+    modbus_set_response_timeout(ctx, 1, 0);
+
+    ret = modbus_set_slave(ctx, IEM3155_ADDRESS);
+    if(ret < 0){
+        perror("modbus_set_slave error\n");
+        return -1;
+    }
+    ret = modbus_connect(ctx);
+    if(ret < 0){
+        perror("modbus_connect error\n");
+        return -1;
+    }
+
+    // Receiver preparation
+    ctx2 = modbus_new_tcp_pi(TRANSLATOR_IP_ADDRESS, "502"); // You have to use port TCP/502 here, because the GEN24 doesn't appreciate otherwise
+    if (ctx2 == NULL) {
+        perror("Unable to create the libmodbus context\n");
+        return -1;
+    }
+    query = malloc(MODBUS_TCP_MAX_ADU_LENGTH);
+
+    ret = modbus_set_slave(ctx2, 1);
+    if(ret < 0){
+        perror("modbus_set_slave error\n");
+        return -1;
+    }
+
+    server_socket = modbus_tcp_pi_listen(ctx2, NB_CONNECTION);
+    if (server_socket  == -1) {
+        perror("Unable to listen\n");
+        return -1;
+    }
+    signal(SIGINT, close_sigint);
+
+    /* Clear the reference set of socket */
+    FD_ZERO(&refset);
+    /* Add the server socket */
+    FD_SET(server_socket, &refset);
+
+    /* Keep track of the max file descriptor */
+    fdmax = server_socket;
+
+    //mb_mapping = modbus_mapping_new_start_address(0,0,0,0,0,50002,0,0); //
+    mb_mapping_sunspec = modbus_mapping_new_start_address(0,0,0,0,40000,178,0,0);
+
+    // Static Sunspec Values
+    mb_mapping_sunspec->tab_registers[0] = 0x5375; // Sunspec ID
+    mb_mapping_sunspec->tab_registers[1] = 0x6e53; // Sunspec ID
+    mb_mapping_sunspec->tab_registers[2] = 1; // Sunspec ID
+    mb_mapping_sunspec->tab_registers[3] = 65; // Length of Sunspec model meter common
+    stringModbusCopy((char *) &mb_mapping_sunspec->tab_registers[4], "Schneider Electric", 18); // Manufacturer name
+    stringModbusCopy((char *) &mb_mapping_sunspec->tab_registers[20], "iEM3155", 7); // Meter product name
+    stringModbusCopy((char *) &mb_mapping_sunspec->tab_registers[36], "Utility", 7); // Meter name (test)
+    stringModbusCopy((char *) &mb_mapping_sunspec->tab_registers[44], "1.3.007", 7); // SW version
+    stringModbusCopy((char *) &mb_mapping_sunspec->tab_registers[52], "0123456789", 10); // Serial number
+    mb_mapping_sunspec->tab_registers[68] = 1; // Modbus address
+    mb_mapping_sunspec->tab_registers[69] = 203; // Meter type (203 = Three phases + Neutral)
+    mb_mapping_sunspec->tab_registers[70] = 105; // Length of Sunspec model ac_meter
+
+    // Static power factor
+    mb_mapping_sunspec->tab_registers[75] = -3; // Current scale factor (dynamic)
+    mb_mapping_sunspec->tab_registers[84] = -1; // Voltage scale factor
+    mb_mapping_sunspec->tab_registers[86] = -2; // Frequency scale factor
+    mb_mapping_sunspec->tab_registers[91] = 0; // Active power scale factor (dynamic)
+    mb_mapping_sunspec->tab_registers[96] = 0; // Apparent power scale factor (dynamic)
+    mb_mapping_sunspec->tab_registers[101] = 0; // Reactive power scale factor (dynamic)
+    mb_mapping_sunspec->tab_registers[106] = -1; // Power factor scale factor
+    mb_mapping_sunspec->tab_registers[123] = 0; // Active energy scale factor
+
+    mb_mapping_sunspec->tab_registers[70] = 105; // Length of Sunspec model ac_meter
+
+    mb_mapping_sunspec->tab_registers[176] = 0xFFFF; // End of model ac_meter
+    mb_mapping_sunspec->tab_registers[177] = 0; // End of model ac_meter
+
+    // Not implemented registers
+    mb_mapping_sunspec->tab_registers[93] = 0xFFFF; // Apparent power phase 1
+    mb_mapping_sunspec->tab_registers[94] = 0xFFFF; // Apparent power phase 2
+    mb_mapping_sunspec->tab_registers[95] = 0xFFFF; // Apparent power phase 3
+    mb_mapping_sunspec->tab_registers[98] = 0xFFFF; // Reactive power phase 1
+    mb_mapping_sunspec->tab_registers[99] = 0xFFFF; // Reactive power phase 2
+    mb_mapping_sunspec->tab_registers[100] = 0xFFFF; // Reactive power phase 3
+    mb_mapping_sunspec->tab_registers[103] = 0xFFFF; // Power factor phase 1
+    mb_mapping_sunspec->tab_registers[104] = 0xFFFF; // Power factor phase 2
+    mb_mapping_sunspec->tab_registers[105] = 0xFFFF; // Power factor phase 3
+    mb_mapping_sunspec->tab_registers[140] = 0x8000; // Apparent energy scale factor (not implemented)
+    mb_mapping_sunspec->tab_registers[173] = 0x8000; // Reactive energy scale factor (not implemented)
+
+    header_length = modbus_get_header_length(ctx2);
+
+    for (;;) {
+        rdset = refset;
+        if (select(fdmax + 1, &rdset, NULL, NULL, NULL) == -1) {
+            perror("Server select() failure.");
+            close_sigint(1);
+        }
+
+        /* Run through the existing connections looking for data to be
+         * read */
+        for (master_socket = 0; master_socket <= fdmax; master_socket++) {
+            if (!FD_ISSET(master_socket, &rdset)) {
+                continue;
+            }
+            if (master_socket == server_socket) {
+                /* A client is asking a new connection */
+                socklen_t addrlen;
+                struct sockaddr_in clientaddr;
+                int newfd;
+
+                /* Handle new connections */
+                addrlen = sizeof(clientaddr);
+                memset(&clientaddr, 0, sizeof(clientaddr));
+                newfd = accept(server_socket, (struct sockaddr *) &clientaddr, &addrlen);
+                if (newfd == -1) {
+                    perror("Server accept() error");
+                } else {
+                    FD_SET(newfd, &refset);
+
+                    if (newfd > fdmax) {
+                        /* Keep track of the maximum */
+                        fdmax = newfd;
+                    }
+                    printf("New connection from %s:%d on socket %d\n",
+                           inet_ntoa(clientaddr.sin_addr),
+                           clientaddr.sin_port,
+                           newfd);
+                }
+            } else {
+                modbus_set_socket(ctx2, master_socket);
+                rc = modbus_receive(ctx2, query);
+                if (rc > 0) {
+                    if (query[header_length] == 0x03) {
+                        //Read holding registers
+                        reg_address = (uint16_t) MODBUS_GET_INT16_FROM_INT8(query, header_length + 1);
+                        nb_registers = (uint16_t) MODBUS_GET_INT16_FROM_INT8(query, header_length + 3);
+                        //printf("Request received. Address : %d, nb registers : %d\n", reg_address, nb_registers);
+                        if((reg_address >= 40071 && reg_address < 40107) || ((reg_address + nb_registers) >= 40071 && (reg_address + nb_registers) < 40108) || (reg_address < 40071 && (reg_address + nb_registers) >= 40108)) { // Request dynamic registers (realtime data (V, I, PF, W...)), so we have to update registers
+                        //if(reg_address >= 40071 && (reg_address + nb_registers) < 40179) { // Request dynamic registers, so we have to update registers
+                            //printf("Realtime data update\n");
+                            ret = modbus_read_registers(ctx, 2999, 112, dest); // Request realtime data (V, I, PF, W...)
+                            if(ret < 0){
+                                if(nb_retry < 10){
+                                    printf("Error number : %d\n", errno);
+                                    perror("modbus_read_regs error (Realtime registers)\n");
+                                    nb_retry++;
+                                } else {
+                                    printf("Error number : %d\n", errno);
+                                    perror("modbus_read_regs error (Realtime registers)\n");
+                                     return -1;
+                                 }
+                            } else {
+                                 nb_retry = 0;
+                            }                
+                            // Current
+                            current_ph1 = modbus_get_float_abcd(&dest[0]);
+                            current_ph2 = modbus_get_float_abcd(&dest[2]);
+                            current_ph3 = modbus_get_float_abcd(&dest[4]);
+                            current_total = current_ph1 + current_ph2 + current_ph3;
+                            //printf("Phase 1 current : %f\n", current_ph1);
+                            if(current_ph1 > 32.5 || current_ph1 < -32.5 || current_ph2 > 32.5 || current_ph2 < -32.5 || current_ph3 > 32.5 || current_ph3 < -32.5 || current_total > 32.5 || current_total < -32.5){
+                                mb_mapping_sunspec->tab_registers[75] = -2; // Current scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[71] = (int16_t) (100 * current_total);
+                                mb_mapping_sunspec->tab_registers[72] = (int16_t) (100 * current_ph1);
+                                mb_mapping_sunspec->tab_registers[73] = (int16_t) (100 * current_ph2);
+                                mb_mapping_sunspec->tab_registers[74] = (int16_t) (100 * current_ph3);
+                            } else {
+                                mb_mapping_sunspec->tab_registers[75] = -3; // Current scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[71] = (int16_t) (1000 * current_total);
+                                mb_mapping_sunspec->tab_registers[72] = (int16_t) (1000 * current_ph1);
+                                mb_mapping_sunspec->tab_registers[73] = (int16_t) (1000 * current_ph2);
+                                mb_mapping_sunspec->tab_registers[74] = (int16_t) (1000 * current_ph3);
+                            }
+                            // L-N Voltage
+                            mb_mapping_sunspec->tab_registers[76] = (int16_t) (10 * modbus_get_float_abcd(&dest[36])); // L-N Voltage (avg)
+                            //printf("L-N Voltage : %d\n", (int16_t) (10 * modbus_get_float_abcd(&dest[36])));
+                            mb_mapping_sunspec->tab_registers[77] = (int16_t) (10 * modbus_get_float_abcd(&dest[28])); // L-N Voltage ph1
+                            mb_mapping_sunspec->tab_registers[78] = (int16_t) (10 * modbus_get_float_abcd(&dest[30])); // L-N Voltage ph2
+                            mb_mapping_sunspec->tab_registers[79] = (int16_t) (10 * modbus_get_float_abcd(&dest[32])); // L-N Voltage ph3
+
+                            // L-L Voltage
+                            mb_mapping_sunspec->tab_registers[80] = (int16_t) (10 * modbus_get_float_abcd(&dest[26])); // L-L Voltage (avg)
+                            //printf("L-L Voltage : %d\n", (int16_t) (10 * modbus_get_float_abcd(&dest[26])));
+                            mb_mapping_sunspec->tab_registers[81] = (int16_t) (10 * modbus_get_float_abcd(&dest[20])); // L1-L2 Voltage
+                            mb_mapping_sunspec->tab_registers[82] = (int16_t) (10 * modbus_get_float_abcd(&dest[22])); // L2-L3 Voltage
+                            mb_mapping_sunspec->tab_registers[83] = (int16_t) (10 * modbus_get_float_abcd(&dest[24])); // L3-L1 Voltage
+
+                            // Frequency
+                            mb_mapping_sunspec->tab_registers[85] = (int16_t) (100 * modbus_get_float_abcd(&dest[110]));
+                            //printf("Frequency : %d\n", (int16_t) (100 * modbus_get_float_abcd(&dest[110])));
+
+                            // Active power
+                            apower_ph1 = modbus_get_float_abcd(&dest[54]);
+                            apower_ph2 = modbus_get_float_abcd(&dest[56]);
+                            apower_ph3 = modbus_get_float_abcd(&dest[58]);
+                            apower_total = modbus_get_float_abcd(&dest[60]);
+                            apower_max = fmaxf(fabsf(apower_ph1),fmaxf(fabsf(apower_ph2),fmaxf(fabsf(apower_ph3),fabsf(apower_total))));
+                            //printf("Active Power : %f\n", apower_total);
+
+                            if(apower_max > 32.5){
+                                mb_mapping_sunspec->tab_registers[91] = 1; // Active power scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[87] = (int16_t) (100 * apower_total);
+                                mb_mapping_sunspec->tab_registers[88] = (int16_t) (100 * apower_ph1);
+                                mb_mapping_sunspec->tab_registers[89] = (int16_t) (100 * apower_ph2);
+                                mb_mapping_sunspec->tab_registers[90] = (int16_t) (100 * apower_ph3);
+                            } else {
+                                mb_mapping_sunspec->tab_registers[91] = 0; // Active power scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[87] = (int16_t) (1000 * apower_total);
+                                mb_mapping_sunspec->tab_registers[88] = (int16_t) (1000 * apower_ph1);
+                                mb_mapping_sunspec->tab_registers[89] = (int16_t) (1000 * apower_ph2);
+                                mb_mapping_sunspec->tab_registers[90] = (int16_t) (1000 * apower_ph3);
+                            }
+
+                            // Apparent power (sum)
+                            apPower_total = modbus_get_float_abcd(&dest[76]);
+                            //printf("Apparent Power : %f\n", modbus_get_float_abcd(&dest[76]));
+                            if(fabsf(apPower_total) > 32.6){
+                                mb_mapping_sunspec->tab_registers[96] = 1; // Apparent power scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[92] = (int16_t) (100 * apPower_total);
+                            } else {
+                                mb_mapping_sunspec->tab_registers[96] = 0; // Apparent power scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[92] = (int16_t) (1000 * apPower_total);
+                            }
+                            // Reactive power (sum)
+                            rpower_total = modbus_get_float_abcd(&dest[68]);
+                            //printf("Reactive Power : %f\n", modbus_get_float_abcd(&dest[68]));
+                            if(fabsf(rpower_total) > 32.6){
+                                mb_mapping_sunspec->tab_registers[101] = 1; // Reactive power scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[97] = (int16_t) (100 * rpower_total);
+                            } else {
+                                mb_mapping_sunspec->tab_registers[101] = 0; // Reactive power scale factor (dynamic)
+                                mb_mapping_sunspec->tab_registers[97] = (int16_t) (1000 * rpower_total);
+                            }
+                            // Power factor (sum)
+                            pf = modbus_get_float_abcd(&dest[84]);
+                            if(pf<-1){ // Calculation according to iEM3155 manual and Sunspec specification. It might be erronous.
+                                pf = - 2 - pf;
+                            } else if(pf < 0){
+                                pf = -pf;
+                            } else if(pf < 1){
+                                pf = -pf;
+                            } else {
+                                pf = 2 - pf;
+                            }
+                            mb_mapping_sunspec->tab_registers[102] = (int16_t) (1000 * pf);
+                            //printf("Power factor : %d\n", (int16_t) (1000 * pf));
+                            //printf("Power factor raw : %f\n",pf);
+                        }
+                        if((reg_address >= 40107 && reg_address < 40142) || ((reg_address + nb_registers) > 40107 && (reg_address + nb_registers) < 40142) || (reg_address < 40107 && (reg_address + nb_registers) >= 40142)) { // Request dynamic registers (energy registers (Wh)), so we have to update registers
+                            //printf("Energy data update\n");
+                            ret = modbus_read_registers(ctx, 3203, 8, dest); // Request energy registers (Wh)
+                            if(ret < 0){
+                                if(nb_retry < 10){
+                                    printf("Error number : %d\n", errno);
+                                    perror("modbus_read_regs error (Energy registers)\n");
+                                    nb_retry++;
+                                } else {
+                                    printf("Error number : %d\n", errno);
+                                    perror("modbus_read_regs error (Energy registers)\n");
+                                    return -1;
+                                }
+                            } else {
+                                nb_retry = 0;
+                            }
+
+                            // Active energy exported
+                            activeEnergyExport = MODBUS_GET_INT64_FROM_INT16(dest, 4);
+                            //printf("Active Energy Export : %d\n", activeEnergyExport);
+                            // Active energy imported
+                            activeEnergyImport = MODBUS_GET_INT64_FROM_INT16(dest, 0);
+                            //printf("Active Energy Import : %d\n", activeEnergyImport);
+                            if(fmax((double) (activeEnergyExport),(double) (activeEnergyImport)) > 4294967295) {
+                                mb_mapping_sunspec->tab_registers[123] = 1; // Active energy scale factor
+                                MODBUS_SET_INT32_TO_INT16(mb_mapping_sunspec->tab_registers, 107, (uint32_t) (activeEnergyExport / 10));
+                                MODBUS_SET_INT32_TO_INT16(mb_mapping_sunspec->tab_registers, 115, (uint32_t) (activeEnergyImport / 10));
+                            } else {
+                                mb_mapping_sunspec->tab_registers[123] = 0; // Active energy scale factor
+                                MODBUS_SET_INT32_TO_INT16(mb_mapping_sunspec->tab_registers, 107, (uint32_t) (activeEnergyExport));
+                                MODBUS_SET_INT32_TO_INT16(mb_mapping_sunspec->tab_registers, 115, (uint32_t) (activeEnergyImport));
+                            }
+                        }
+                        rc = modbus_reply(ctx2, query, rc, mb_mapping_sunspec);
+                        if (rc == -1) {
+                            break;
+                        }
+                    }
+                } else if (rc == -1) {
+                    /* This example server in ended on connection closing or
+                     * any errors. */
+                    printf("Connection closed on socket %d\n", master_socket);
+                    close(master_socket);
+
+                    /* Remove from reference set */
+                    FD_CLR(master_socket, &refset);
+
+                    if (master_socket == fdmax) {
+                        fdmax--;
+                    }
+                }
+            }
+        }
+
+    }
+
+    //modbus_close(ctx);
+    //modbus_free(ctx);
+    free(query);
+    modbus_close(ctx2);
+    modbus_free(ctx2);
+
+}