git commit -m "first commit for v2"
This commit is contained in:
501
Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp
Executable file
501
Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp
Executable file
@@ -0,0 +1,501 @@
|
||||
#include "modbus_tcp/modbus.h"
|
||||
|
||||
|
||||
|
||||
modbus::modbus(std::string ip_address, int port)
|
||||
{
|
||||
this->HOST = ip_address;
|
||||
ROS_INFO("modbus - HOST = %s", this->HOST.c_str());
|
||||
this->PORT = (uint16_t)port;
|
||||
ROS_INFO("modbus - PORT = %d", this->PORT);
|
||||
this->_slaveid = 1;
|
||||
this->_msg_id = 1;
|
||||
this->_connected = false;
|
||||
this->err = false;
|
||||
this->err_no = 0;
|
||||
this->error_msg = "";
|
||||
this->timeout.tv_sec = 10; // after 10 seconds connect() will timeout
|
||||
this->timeout.tv_usec = 0;
|
||||
}
|
||||
|
||||
modbus::~modbus(void)
|
||||
{
|
||||
modbus_close();
|
||||
}
|
||||
|
||||
void modbus::modbus_set_slave_id(int id)
|
||||
{
|
||||
_slaveid = id;
|
||||
}
|
||||
|
||||
bool modbus::modbus_connect()
|
||||
{
|
||||
if(this->HOST.empty() || this->PORT == 0) {
|
||||
ROS_ERROR_STREAM("Modbus TCP: Missing Host and Port");
|
||||
return false;
|
||||
} else {
|
||||
ROS_INFO_STREAM("Modbus TCP: Found Proper Host "<< this->HOST << " and Port " <<this->PORT);
|
||||
}
|
||||
|
||||
this->_socket = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if(this->_socket == -1) {
|
||||
ROS_ERROR_STREAM("Modbus TCP: Error Opening Socket");
|
||||
return false;
|
||||
} else {
|
||||
ROS_INFO_STREAM("Modbus TCP: Socket Opened Successfully");
|
||||
//ROS_INFO_STREAM("Device is connecting......");
|
||||
}
|
||||
|
||||
setsockopt(_socket, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout));
|
||||
setsockopt(_socket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
|
||||
|
||||
_server.sin_family = AF_INET;
|
||||
_server.sin_addr.s_addr = inet_addr(HOST.c_str());
|
||||
_server.sin_port = htons(PORT);
|
||||
|
||||
if (connect(_socket, (struct sockaddr*)&_server, sizeof(_server)) < 0) {
|
||||
ROS_ERROR_STREAM("Modbus TCP: Host "<< this->HOST << " and Port " <<this->PORT << " Connection Error");
|
||||
return false;
|
||||
}
|
||||
|
||||
// ROS_INFO_STREAM("Modbus TCP: Host "<< this->HOST << " and Port " <<this->PORT << "is Connected");
|
||||
_connected = true;
|
||||
err = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
void modbus::modbus_close() const {
|
||||
close(_socket);
|
||||
ROS_INFO_STREAM("Modbus TCP: Socket Closed");
|
||||
}
|
||||
|
||||
void modbus::modbus_build_request(uint8_t *to_send, uint address, int func) const {
|
||||
to_send[0] = (uint8_t) _msg_id >> 8u;
|
||||
to_send[1] = (uint8_t) (_msg_id & 0x00FFu);
|
||||
to_send[2] = 0;
|
||||
to_send[3] = 0;
|
||||
to_send[4] = 0;
|
||||
to_send[6] = (uint8_t) _slaveid;
|
||||
to_send[7] = (uint8_t) func;
|
||||
to_send[8] = (uint8_t) (address >> 8u);
|
||||
to_send[9] = (uint8_t) (address & 0x00FFu);
|
||||
}
|
||||
|
||||
int modbus::modbus_write(int address, uint amount, int func, const uint16_t *value) {
|
||||
int status = 0;
|
||||
if(func == WRITE_COIL || func == WRITE_REG) {
|
||||
uint8_t to_send[12];
|
||||
modbus_build_request(to_send, address, func);
|
||||
to_send[5] = 6;
|
||||
to_send[10] = (uint8_t) (value[0] >> 8u);
|
||||
to_send[11] = (uint8_t) (value[0] & 0x00FFu);
|
||||
status = modbus_send(to_send, 12);
|
||||
} else if(func == WRITE_REGS){
|
||||
uint8_t to_send[13 + 2 * amount];
|
||||
modbus_build_request(to_send, address, func);
|
||||
to_send[5] = (uint8_t) (7 + 2 * amount);
|
||||
to_send[10] = (uint8_t) (amount >> 8u);
|
||||
to_send[11] = (uint8_t) (amount & 0x00FFu);
|
||||
to_send[12] = (uint8_t) (2 * amount);
|
||||
for(int i = 0; i < amount; i++) {
|
||||
to_send[13 + 2 * i] = (uint8_t) (value[i] >> 8u);
|
||||
to_send[14 + 2 * i] = (uint8_t) (value[i] & 0x00FFu);
|
||||
}
|
||||
status = modbus_send(to_send, 13 + 2 * amount);
|
||||
} else if(func == WRITE_COILS) {
|
||||
uint8_t to_send[14 + (amount -1) / 8 ];
|
||||
modbus_build_request(to_send, address, func);
|
||||
to_send[5] = (uint8_t) (7 + (amount + 7) / 8);
|
||||
to_send[10] = (uint8_t) (amount >> 8u);
|
||||
to_send[11] = (uint8_t) (amount & 0x00FFu);
|
||||
to_send[12] = (uint8_t) ((amount + 7) / 8);
|
||||
for(int i = 0; i < (amount+7)/8; i++)
|
||||
to_send[13 + i] = 0; // init needed before summing!
|
||||
for(int i = 0; i < amount; i++) {
|
||||
to_send[13 + i/8] += (uint8_t) (value[i] << (i % 8u));
|
||||
}
|
||||
status = modbus_send(to_send, 14 + (amount - 1) / 8);
|
||||
}
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
int modbus::modbus_read(int address, uint amount, int func){
|
||||
uint8_t to_send[12];
|
||||
modbus_build_request(to_send, address, func);
|
||||
to_send[5] = 6;
|
||||
to_send[10] = (uint8_t) (amount >> 8u);
|
||||
to_send[11] = (uint8_t) (amount & 0x00FFu);
|
||||
return modbus_send(to_send, 12);
|
||||
}
|
||||
|
||||
int modbus::modbus_read_holding_register(int address, uint16_t &buffer) {
|
||||
if(_connected) {
|
||||
if(address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, 1, READ_REGS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
if(k < 11) return EX_BAD_DATA;
|
||||
modbuserror_handle(to_rec, READ_REGS);
|
||||
if(err) return err_no;
|
||||
buffer = ((uint16_t)to_rec[9u]) << 8u;
|
||||
buffer += (uint16_t) to_rec[10u];
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_read_holding_registers(int address, int amount, uint16_t *buffer) {
|
||||
if(_connected) {
|
||||
if(amount > 65535 || address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, amount, READ_REGS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
if(k < 2 * amount + 9) return EX_BAD_DATA;
|
||||
modbuserror_handle(to_rec, READ_REGS);
|
||||
if(err) return err_no;
|
||||
for(uint i = 0; i < amount; i++) {
|
||||
buffer[i] = ((uint16_t)to_rec[9u + 2u * i]) << 8u;
|
||||
buffer[i] += (uint16_t) to_rec[10u + 2u * i];
|
||||
}
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_read_input_registers(int address, int amount, uint16_t *buffer) {
|
||||
if(_connected){
|
||||
if(amount > 65535 || address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, amount, READ_INPUT_REGS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
// ROS_INFO("modbus_read_input_registers: k = %d, amount = %d", (int)k , amount);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, READ_INPUT_REGS);
|
||||
if(err) return err_no;
|
||||
for(uint i = 0; i < amount; i++) {
|
||||
buffer[i] = ((uint16_t)to_rec[9u + 2u * i]) << 8u;
|
||||
buffer[i] += (uint16_t) to_rec[10u + 2u * i];
|
||||
}
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_read_coil(int address, bool &buffer) {
|
||||
if(_connected) {
|
||||
if(address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, 1, READ_COILS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
// ROS_INFO("modbus_read_coil: k = %d, amount = %d", (int)k , 1);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, READ_COILS);
|
||||
if(err) return err_no;
|
||||
buffer = (bool) ((to_rec[9u]) & 1u);
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_read_coils(int address, int amount, bool *buffer) {
|
||||
if(_connected) {
|
||||
if(amount > 2040 || address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, amount, READ_COILS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, READ_COILS);
|
||||
// ROS_INFO("modbus_read_coils: k = %d, amount = %d", (int)k , amount);
|
||||
if(err) return err_no;
|
||||
for(uint i = 0; i < amount; i++) {
|
||||
buffer[i] = (bool) ((to_rec[9u + i / 8u] >> (i % 8u)) & 1u);
|
||||
}
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_read_input_bit(int address, bool &buffer) {
|
||||
if(_connected) {
|
||||
if(address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, 1, READ_INPUT_BITS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
// ROS_INFO("modbus_read_input_bit: k = %d, amount = %d", (int)k , 1);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
if(err) return err_no;
|
||||
buffer = (bool)(to_rec[9u] & 1u);
|
||||
modbuserror_handle(to_rec, READ_INPUT_BITS);
|
||||
return 0;
|
||||
} else {
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_read_input_bits(int address, int amount, bool* buffer) {
|
||||
if(_connected) {
|
||||
if(amount > 2040 || address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_read(address, amount, READ_INPUT_BITS);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
// ROS_INFO("modbus_read_input_bits: k = %d, amount = %d", (int)k , amount);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
if(err) return err_no;
|
||||
for(uint i = 0; i < amount; i++) {
|
||||
buffer[i] = (bool) ((to_rec[9u + i / 8u] >> (i % 8u)) & 1u);
|
||||
}
|
||||
modbuserror_handle(to_rec, READ_INPUT_BITS);
|
||||
return 0;
|
||||
} else {
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_write_coil(int address, const bool& to_write) {
|
||||
if(_connected) {
|
||||
if(address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
int value = to_write * 0xFF00;
|
||||
modbus_write(address, 1, WRITE_COIL, (uint16_t *)&value);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, WRITE_COIL);
|
||||
if(err) return err_no;
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_write_register(int address, const uint16_t& value) {
|
||||
if(_connected) {
|
||||
if(address > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_write(address, 1, WRITE_REG, &value);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, WRITE_REG);
|
||||
if(err) return err_no;
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_write_coils(int address, int amount, const bool *value) {
|
||||
if(_connected) {
|
||||
if(address > 65535 || amount > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
uint16_t temp[amount];
|
||||
for(int i = 0; i < amount; i++) {
|
||||
temp[i] = (uint16_t)value[i];
|
||||
}
|
||||
modbus_write(address, amount, WRITE_COILS, temp);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
// ROS_INFO("modbus_write_coils: k = %d, amount = %d", (int)k , amount);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, WRITE_COILS);
|
||||
if(err) return err_no;
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
int modbus::modbus_write_registers(int address, int amount, const uint16_t *value) {
|
||||
if(_connected) {
|
||||
if(address > 65535 || amount > 65535) {
|
||||
set_bad_input();
|
||||
return EX_BAD_DATA;
|
||||
}
|
||||
modbus_write(address, amount, WRITE_REGS, value);
|
||||
uint8_t to_rec[MAX_MSG_LENGTH];
|
||||
ssize_t k = modbus_receive(to_rec);
|
||||
if (k == -1) {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
modbuserror_handle(to_rec, WRITE_REGS);
|
||||
if(err) return err_no;
|
||||
return 0;
|
||||
} else {
|
||||
set_bad_con();
|
||||
return BAD_CON;
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t modbus::modbus_send(uint8_t *to_send, int length) {
|
||||
_msg_id++;
|
||||
return send(_socket, to_send, (size_t)length, 0);
|
||||
}
|
||||
|
||||
|
||||
ssize_t modbus::modbus_receive(uint8_t *buffer) const {
|
||||
|
||||
timeval t_out;
|
||||
fd_set set;
|
||||
t_out.tv_sec = 1;
|
||||
t_out.tv_usec = 0;
|
||||
FD_ZERO(&set); /* clear the set */
|
||||
FD_SET(_socket, &set); /* add our file descriptor to the set */
|
||||
int rv = select(_socket +1, &set, NULL, NULL, &t_out);
|
||||
if(rv == -1)
|
||||
{
|
||||
ROS_ERROR_STREAM("socket error accured");
|
||||
return -1;
|
||||
}
|
||||
else if(rv == 0)
|
||||
{
|
||||
ROS_ERROR_STREAM("socket timeout occured");
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
return recv(_socket, (char *) buffer, 1024, 0);
|
||||
}
|
||||
|
||||
void modbus::set_bad_con() {
|
||||
err = true;
|
||||
_connected = false;
|
||||
error_msg = "BAD CONNECTION";
|
||||
}
|
||||
|
||||
|
||||
void modbus::set_bad_input() {
|
||||
err = true;
|
||||
error_msg = "BAD FUNCTION INPUT";
|
||||
}
|
||||
|
||||
/**
|
||||
* Error Code Handler
|
||||
* @param msg Message Received from the Server
|
||||
* @param func Modbus Functional Code
|
||||
*/
|
||||
void modbus::modbuserror_handle(const uint8_t *msg, int func) {
|
||||
if(msg[7] == func + 0x80) {
|
||||
err = true;
|
||||
switch(msg[8]){
|
||||
case EX_ILLEGAL_FUNCTION:
|
||||
error_msg = "1 Illegal Function";
|
||||
err_no = EX_ILLEGAL_FUNCTION;
|
||||
break;
|
||||
case EX_ILLEGAL_ADDRESS:
|
||||
error_msg = "2 Illegal Address";
|
||||
err_no = EX_ILLEGAL_ADDRESS;
|
||||
break;
|
||||
case EX_ILLEGAL_VALUE:
|
||||
error_msg = "3 Illegal Value";
|
||||
err_no = EX_ILLEGAL_VALUE;
|
||||
break;
|
||||
case EX_SERVER_FAILURE:
|
||||
error_msg = "4 Server Failure";
|
||||
err_no = EX_SERVER_FAILURE;
|
||||
break;
|
||||
case EX_ACKNOWLEDGE:
|
||||
error_msg = "5 Acknowledge";
|
||||
err_no = EX_ACKNOWLEDGE;
|
||||
break;
|
||||
case EX_SERVER_BUSY:
|
||||
error_msg = "6 Server Busy";
|
||||
err_no = EX_SERVER_BUSY;
|
||||
break;
|
||||
case EX_NEGATIVE_ACK:
|
||||
error_msg = "7 Negative Acknowledge";
|
||||
err_no = EX_NEGATIVE_ACK;
|
||||
break;
|
||||
case EX_MEM_PARITY_PROB:
|
||||
error_msg = "8 Memory Parity Problem";
|
||||
err_no = EX_MEM_PARITY_PROB;
|
||||
break;
|
||||
case EX_GATEWAY_PROBLEMP:
|
||||
error_msg = "10 Gateway Path Unavailable";
|
||||
err_no = EX_GATEWAY_PROBLEMP;
|
||||
break;
|
||||
case EX_GATEWYA_PROBLEMF:
|
||||
error_msg = "11 Gateway Target Device Failed to Respond";
|
||||
err_no = EX_GATEWYA_PROBLEMF;
|
||||
break;
|
||||
default:
|
||||
error_msg = "UNK";
|
||||
err_no = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
err = false;
|
||||
error_msg = "NO ERR";
|
||||
}
|
||||
Reference in New Issue
Block a user