modbus_linux/src/PLCModbus.cpp

#include "PLCModbus.h"

namespace plc_modbus
{
//********************** constructor **********************
CPLCModbus::CPLCModbus(void *pOwnerObject)
{
    this->ip = 0;
    this->port = 0;
}

CPLCModbus::~CPLCModbus()
{
}

//********************** init **********************
bool CPLCModbus::connect(const char *ip, int port)
{
    /* TCP */
    this->ip = ip;
    this->port = port;
    this->ctx = modbus_new_tcp(ip, port);
    modbus_set_debug(this->ctx, true);
    if (modbus_connect(this->ctx) == -1)
    {
        std::cout << "Connection failed: " << modbus_strerror(errno) << std::endl;
        modbus_free(this->ctx);
        return false;
    }
    else
    {
        std::cout << "connected" << std::endl;
        if(ctx==NULL){
            std::cout << "connected NULL" << std::endl;
            return false;
        }
        return true;
    }
}

void CPLCModbus::disconnect()
{
    /* Close the connection */
    modbus_close(this->ctx);
    modbus_free(this->ctx);
}

bool CPLCModbus::reconnect()
{
    CPLCModbus::disconnect();
    return CPLCModbus::connect(this->ip, this->port);
}

//********************** basic funcs **********************
uint16_t CPLCModbus::get_current_stage()
{
    uint16_t current_stage[] = {1};
    bool result = this->read_register(CURRENT_STAGE_ADDRESS, current_stage);
    if(result && current_stage)
    {
        return current_stage[0];
    }else{
        return (uint16_t)-1;
    }
}

uint16_t CPLCModbus::get_finish_status()
{
    uint16_t finish_status[] = {1};
    bool result = this->read_register(FINISH_TRAJECTORY_ADDRESS, finish_status);
    if (result && finish_status)
    {
        return finish_status[0];
    }
    else
    {
        return (uint16_t)-1;
    }
}

bool CPLCModbus::update_trajectory(uint16_t update_flag, uint16_t *dest)
{
    bool result = true;
    uint16_t updt_flag[1] = {update_flag};
    if (dest)
    {
        result &= this->write_registers(POSE_ADDRESS_START, POSE_COUNT * 2, dest);
        result &= this->write_register(UPDATE_FLAG_ADDRESS, updt_flag);
        return result;
    }else{
        return false;
    }
}

bool CPLCModbus::set_slave(int slave)
{
    int rc = modbus_set_slave(this->ctx, slave);
    if (rc != 1)
    {
        std::cout << "set slave failed: " << rc << "," << modbus_strerror(errno) << std::endl;
        return false;
    }
    else
    {
        return true;
    }
}

bool CPLCModbus::read_registers(int addr, int nb, uint16_t *dest)
{
    int rc = modbus_read_registers(this->ctx, addr, nb, dest);
    if (rc != nb)
    {
        std::cout << "read failed: " << rc << "," << modbus_strerror(errno) << std::endl;
        return false;
    }
    else
    {
        return true;
    }
}

bool CPLCModbus::write_registers(int addr, int nb, uint16_t *src)
{
    int rc = modbus_write_registers(this->ctx, addr, nb, src);
    if (rc != nb)
    {
        std::cout << "write failed: " << rc << "," << modbus_strerror(errno) << std::endl;
        return false;
    }
    else
    {
        return true;
    }
}

bool CPLCModbus::read_register(int addr, uint16_t *dest)
{
    int rc = modbus_read_registers(this->ctx, addr, 1, dest);
    if (rc != 1)
    {
        return false;
    }
    else
    {
        return true;
    }
}

bool CPLCModbus::write_register(int addr, uint16_t *src)
{
    int rc = modbus_write_register(this->ctx, addr, src[0]);
    if (rc != 1)
    {
        return false;
    }
    else
    {
        return true;
    }
}

} // namespace plc_modbus