iEMforGEN24/modbus-fronius.c

#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);

}