HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Devices/Libraries/Ros/modbus/README.md Executable file
View File

@@ -0,0 +1 @@
# modbus

38
Devices/Libraries/Ros/modbus/modbus_rtu/CMakeLists.txt Executable file
View File

@@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 3.0.2)
project(modbus_rtu)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
libserial
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES modbus_rtu
CATKIN_DEPENDS roscpp std_msgs libserial
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME} src/modbus_rtu.cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${Boost_INCLUDE_DIRS} ${catkin_LIBRARIES})
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)

40
Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/define.h Executable file
View File

@@ -0,0 +1,40 @@
#pragma once
/*
* Exception Codes
*/
#define EX_PARAM_NOT_EXIT 0x00 /* The parameter number does not exits */
#define EX_ACCESS_TO_PARAM 0x01 /* There is no write access to the parameter */
#define EX_PARAM_LIMITS 0x02 /* The data value exceeds the parameter limits */
#define EX_SUB_INDEX_NO_EXIT 0x03 /* The sub-index in use does not exits */
#define EX_PARAM_NOT_TYPE 0x04 /* The parameter is not of the array type */
#define EX_TYPE_NO_MATCH 0x05 /* The data type does not match the parameter called */
#define EX_ONLY_RESET 0x06 /* Only reset */
#define EX_NOT_CHANGEABLE 0x07 /* Not changeable */
#define EX_NO_WRITE_ACCESS 0x0B /* No write address */
#define EX_NO_POSSIBLE_PRE_MODE 0x11 /* Data change in the parameter called is not possible in the present mode */
#define EX_OTHER_ERROR 0x12 /* Other error */
#define EX_INVALID_DATA_ADDR 0x40 /* Invalid data address */
#define EX_INVALID_MSGS_LENGTH 0x41 /* Invalid message length */
#define EX_INVALID_LENGTH_VALUE 0x42 /* Invalid data length or value */
#define EX_INVALID_FUNC_CODE 0x43 /* Invalid function code */
#define EX_NO_BUS_ACCESS 0x82 /* There is no bus access */
#define EX_DATA_NOT_POSSIBLE 0x83 /* Data change is not possible because factory set-up is selected */
#define EX_BAD_DATA 0XFF /* Bad Data lenght or Address */
#define BAD_CON -1
#define BAD_CONFIG -2
#define WORD_BITS 65535
#define DATA_BITS 16
/*
* Commonly use function codes
*/
#define READ_COIL_BITS 0x01 /* Read coils status */
#define READ_INPUT_BITS 0x02 /* Read input status */
#define READ_HOLDING_REGS 0x03 /* Read holding registers */
#define READ_INPUT_REGS 0x04 /* Read input register */
#define WRITE_SINGLE_COIL 0x05 /* Write single coil status */
#define WRITE_SINGLE_HOLDING_REG 0x06 /* Write single register */
#define WRITE_MULTIPLE_COILS 0X0F /* Multiple coil write */
#define WRITE_MULTIPLE_HOLDING_REGS 0X10 /* Multiple register write*/

152
Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/modbus_rtu.h Executable file
View File

@@ -0,0 +1,152 @@
#ifndef __MOSBUS_RTU_H_INCLUDE_
#define __MOSBUS_RTU_H_INCLUDE_
#include <iostream>
#include <stdio.h>
#include <cstdint> /* uin8_t */
#include "modbus_rtu/define.h"
#include "libserial/rs485.h"
class modbus_rtu : public rs485{
public:
/*
* @brief Constructor
* @param[in] poly The poly number to caculartion CRC16
* @param[in] devfile Example /dev/tty*
* @param[in] baud Number of transmitted bits per a second
* @param[in] parity The parity check bit {Even, None , Old }
* @param[in] data_bit Number of bits in a transmission frame
* @param[in] stop_bit End bit
*/
modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit);
/*
* Destructor.
*/
virtual ~modbus_rtu();
/*
* Read Holding Registers
* @brief MODBUS FUNCTION 0x03
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Registers to Read
* @param[in] buffer Buffer to Store Data Read from Registers
*/
virtual int modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer);
/*
* Read Input Registers
* @brief MODBUS FUNCTION 0x04
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Registers to Read
* @param[in] buffer Buffer to Store Data Read from Registers
*/
virtual int modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer);
/*
* Write Single Register
* @brief FUCTION 0x06
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] value Value to Be Written to Register
*/
virtual int modbus_write_register(uint8_t slave_id, int address, const uint16_t& value);
/*
* Write Multiple Registers
* @brief MODBUS FUNCION 0x10
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Value to Write
* @param[in] value Values to Be Written to the Registers
*/
int modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value);
/*
* @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
* @param[in] msg Message to send
* @param[in] length The length of message to send
* @param[in] poly The poly number to caculartion CRC16
* @return CRC Result is CRC value 16 bit
*/
virtual uint16_t getCRC16(uint8_t *to_send, uint16_t length);
/*
* @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
* @param[in] msg Message to send
* @param[in] length The length of message to send
* @param[in] poly The poly number to caculartion CRC16
* @return bool Result is CRC value true/false
*/
virtual bool checkCRC16(uint8_t *to_send, uint16_t length);
/*
* Modbus Request Builder
* @param[in] slave_id The id on modbus device
* @param[in] to_send Message Buffer to Be Sent
* @param[in] address Reference Address
* @param[im] func Modbus Functional Code
*/
inline void modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const;
/*
* Read Request Builder and Sender
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Data to Read
* @param[in] func Modbus Functional Code
*/
ssize_t modbus_read(uint8_t slave_id, int address, uint amount, int func);
/*
* Write Request Builder and Sender
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of data to be Written
* @param[in] func Modbus Functional Code
* @param[in] value Data to Be Written
*/
ssize_t modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value);
/*
* Data Sender
* @param[in] to_send Request to Be Sent to Server
* @param[in] length Length of the Request
* @param[in] Size of the request
*/
inline ssize_t modbus_send(uint8_t *to_send, uint16_t length);
/*
* Data Receiver
* @param[in] buffer Buffer to Store the Data Retrieved
* @return Size of Incoming Data
*/
inline ssize_t modbus_receive(uint8_t *buffer);
/*
* Error Code Handler
* @param[in] msg Message Received from the Server
* @param[in] func Modbus Functional Code
*/
virtual void modbuserror_handle(const uint8_t *msg, int func);
/*
* Set Bad Data lenght or Address
*/
inline void set_bad_input(void);
/*
* Set Bad connection
*/
virtual void set_bad_con(void);
/**
* Set config faild
*/
virtual void setConfigFaild(void);
private:
/* Properties */
bool err{};
int err_no{};
uint16_t _poly; /* POLY is const number to cacurla CRC16 */
};
#endif

67
Devices/Libraries/Ros/modbus/modbus_rtu/package.xml Executable file
View File

@@ -0,0 +1,67 @@
<?xml version="1.0"?>
<package format="2">
<name>modbus_rtu</name>
<version>0.0.0</version>
<description>The modbus_rtu package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/modbus_rtu</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<build_depend>libserial</build_depend>
<exec_depend>libserial</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

450
Devices/Libraries/Ros/modbus/modbus_rtu/src/modbus_rtu.cpp Executable file
View File

@@ -0,0 +1,450 @@
#include "modbus_rtu/modbus_rtu.h"
/**
* @brief Constructor
* @param[in] poly The poly number to caculartion CRC16
* @param[in] devfile Example /dev/tty*
* @param[in] baud Number of transmitted bits per a second
* @param[in] parity The parity check bit {Even, None , Old }
* @param[in] data_bit Number of bits in a transmission frame
* @param[in] stop_bit End bit
*/
modbus_rtu::modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit) : _poly(poly), rs485(devfile, baud, parity, data_bit, stop_bit){
err = false;
err_no = 0;
error_msg = "";
ROS_INFO("Set POLY value is 0x%x", _poly);
if(setCharacterNumToRead(5) < 0) {setConfigFaild();}
}
/**
* Destructor.
*/
modbus_rtu::~modbus_rtu(){}
/**
* Read Holding Registers
* MODBUS FUNCTION 0x03
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Registers to Read
* @param[in] buffer Buffer to Store Data Read from Registers
*/
int modbus_rtu::modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){
if(_connected){
if(address > WORD_BITS || amount > DATA_BITS){
set_bad_input();
return EX_BAD_DATA;
}
modbus_read(slave_id, address, amount, READ_HOLDING_REGS);
//if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;}
uint8_t to_rec[MAX_MSG_LENGTH];
memset(to_rec,'\0',MAX_MSG_LENGTH);
ssize_t k = 0;
k = modbus_receive(to_rec);
if (k == -1 || k == 65535) {return BAD_CON;}
if(k == 5 + amount * 2) {
if(!checkCRC16(to_rec,k)){
ROS_ERROR("modbus_read_holding_registers");
set_bad_input();
return EX_BAD_DATA;
}
modbuserror_handle(to_rec, READ_HOLDING_REGS);
if(err) return err_no;
for(int i = 0; i < to_rec[2u]/2; i++){
buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u;
buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu);
}
return 0;
} else {
ROS_ERROR("receive message is not correct %d ", (int)k);
for(int i = 0; i < k; i++) printf("0x%x ",to_rec[i]);
printf("\n");
set_bad_input();
return EX_BAD_DATA;
}
} else {
return BAD_CON;
}
}
/**
* Read Input Registers
* MODBUS FUNCTION 0x04
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Registers to Read
* @param[in] buffer Buffer to Store Data Read from Registers
*/
int modbus_rtu::modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){
if(_connected){
if(amount > DATA_BITS || address > WORD_BITS){
set_bad_input();
return EX_BAD_DATA;
}
modbus_read(slave_id, address, amount, READ_INPUT_REGS);
//if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;}
uint8_t to_rec[MAX_MSG_LENGTH];
memset(to_rec,'\0',MAX_MSG_LENGTH);
ssize_t k = 0;
k = modbus_receive(to_rec);
if (k == -1 || k == 65535) {return BAD_CON;}
if(k > 0){
if(!checkCRC16(to_rec,k)) {
ROS_ERROR("modbus_read_input_registers");
set_bad_input();
return EX_BAD_DATA;
}
modbuserror_handle(to_rec, READ_INPUT_REGS);
if(err) return err_no;
for(int i = 0; i < to_rec[2u]/2; i++){
buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u;
buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu);
}
return 0;
} else {
ROS_ERROR("Slave no response !!");
set_bad_input();
return EX_BAD_DATA;
}
} else {
return BAD_CON;
}
}
/**
* Write Single Register
* @brief FUCTION 0x06
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] value Value to Be Written to Register
*/
int modbus_rtu::modbus_write_register(uint8_t slave_id, int address, const uint16_t& value) {
if(_connected){
if(address > WORD_BITS){
set_bad_input();
return EX_BAD_DATA;
}
modbus_write(slave_id, address, 1, WRITE_SINGLE_HOLDING_REG, &value);
//if(setCharacterNumToRead(8) < 0 ) {setConfigFaild(); return BAD_CONFIG;}
uint8_t to_rec[MAX_MSG_LENGTH];
memset(to_rec,'\0',MAX_MSG_LENGTH);
ssize_t k = 0;
k = modbus_receive(to_rec);
if (k == -1 || k == 65535) {return BAD_CON;}
if(k == 8){
// for(int i = 0; i < k; i++) ROS_INFO("0x%x",to_rec[i]);
if(!checkCRC16(to_rec, k)) {
ROS_ERROR("modbus_write_register");
set_bad_input();
return EX_BAD_DATA;
}
modbuserror_handle(to_rec, WRITE_SINGLE_HOLDING_REG);
if(err) return err_no;
return 0;
} else {
ROS_ERROR("Slave no response !!");
set_bad_input();
return EX_BAD_DATA;
}
} else {
return BAD_CON;
}
}
/**
* Write Multiple Registers
* @brief MODBUS FUNCION 0x10
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Value to Write
* @param[in] value Values to Be Written to the Registers
*/
int modbus_rtu::modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value){
if(_connected){
if(amount > DATA_BITS || address > WORD_BITS){
set_bad_input();
return EX_BAD_DATA;
}
modbus_write(slave_id, address, amount, WRITE_MULTIPLE_HOLDING_REGS, value);
//if(setCharacterNumToRead(8) < 0) {setConfigFaild(); return BAD_CONFIG;}
uint8_t to_rec[MAX_MSG_LENGTH];
memset(to_rec,'\0',MAX_MSG_LENGTH);
ssize_t k = 0;
k = modbus_receive(to_rec);
if (k == -1 || k == 65535) {return BAD_CON;}
if(k == 8) {
if(!checkCRC16(to_rec, k)){
ROS_ERROR("modbus_write_registers");
set_bad_input();
return EX_BAD_DATA;
}
modbuserror_handle(to_rec, WRITE_MULTIPLE_HOLDING_REGS);
if(err) return err_no;
return 0;
} else {
ROS_ERROR("Slave no response !!");
set_bad_input();
return EX_BAD_DATA;
}
} else {
return BAD_CON;
}
}
/**
* @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
* @param[in] msg Message to send
* @param[in] length The length of message to send
* @param[in] poly The poly number to caculartion CRC16
* @return CRC Result is CRC value 16 bit
*/
uint16_t modbus_rtu::getCRC16(uint8_t *to_send, uint16_t length){
/*
EX: Ban đầu CRC = 1111 1111 1111 1111 chuyển sang Hex là FFFF
Chọn data_p là 54 hay 0101 0100(1 byte) là số cần tính.
Chọn số poly = A001h hay 1010 000 000 0001
(Poly là một số mà bạn sử dụng làm phép tính số CRC cơ sở của mình.)
+ Bước 1: Dịch CRC và data_p sang phải 1 bit
data_p = 54, là 0101 0100 trở thành 0010 1010
CRC = 1111 1111 1111 1111 trở thành 0111 1111 1111 1111
+ Bước 2: Kiểm tra BIT ngoài cùng bên phải của Dữ liệu và so sánh nó với một trong các CRC
NẾU chúng bằng nhau, dịch chuyển CRC sang phải 1 bit
NẾU chúng không phải, dịch chuyển CRC sang phải 1 bit VÀ cộng thêm số Poly một lần nữa.
Thực hiện bước 2 đúng 8 lần vì 1 byte có 8 bit.
+Bước 3: Bước 1 và 2 sẽ được lăp lại theo số lượng data_p.
*/
unsigned char i;
unsigned int data;
unsigned int crc = 0xffff;
do {
for (i=0, data=(unsigned int)0xff & *to_send++; i < 8; i++, data >>= 1) {
if ((crc & 0x0001) ^ (data & 0x0001))
crc = (crc >> 1) ^ _poly;
else crc >>= 1;
}
} while (--length);
return (crc);
}
/**
* @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
* @param[in] msg Message to send
* @param[in] length The length of message to send
* @param[in] poly The poly number to caculartion CRC16
* @return bool Result is CRC value true/false
*/
bool modbus_rtu::checkCRC16(uint8_t *to_send, uint16_t length){
uint16_t to_check;
to_check = getCRC16(to_send,length - 2);
for(int i = 0; i < length; i++) printf("0x%x ",to_send[i]);
printf("length(%d) crc16(0x%x 0x%x) 0x%x 0x%x ", (int)length, to_check & 0x00FFu, to_check >> 8u, to_send[length - 2], to_send[length - 1]);
printf("\n");
return to_send[length - 2] == (uint8_t)(to_check & 0x00FFu) && to_send[length - 1] == (uint8_t)(to_check >> 8u);
}
/*
* Modbus Request Builder
* @param[in] slave_id The id on modbus device
* @param[in] to_send Message Buffer to Be Sent
* @param[in] address Reference Address
* @param[in] func Modbus Functional Code
*/
void modbus_rtu::modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const {
to_send[0] = slave_id;
to_send[1] = func;
to_send[2] = (uint8_t) (address >> 8u);
to_send[3] = (uint8_t) (address & 0x00FFu);
}
/*
* Write Request Builder and Sender
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of data to be Written
* @param[in] func Modbus Functional Code
* @param[in] value Data to Be Written
*/
ssize_t modbus_rtu::modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value) {
ssize_t result = 0;
if(func == WRITE_SINGLE_COIL || func == WRITE_SINGLE_HOLDING_REG){
uint8_t to_send[8];
modbus_build_request(slave_id, to_send, address, func);
to_send[4] = (uint8_t) (value[0] >> 8u);
to_send[5] = (uint8_t) (value[0] & 0x00FFu);
uint16_t crc = getCRC16(to_send,6);
to_send[6] = (uint8_t) (crc & 0x00FFu);
to_send[7] = (uint8_t) (crc >> 8u);
// for(int i = 0; i < 8; i++) ROS_INFO("0x%x", to_send[i]);
result = modbus_send(to_send,8);
} else if(func == WRITE_MULTIPLE_HOLDING_REGS){
uint8_t to_send[9 + 2 * amount];
modbus_build_request(slave_id,to_send, address, func);
to_send[4] = (uint8_t)(amount >> 8u);
to_send[5] = (uint8_t)(amount & 0x00FFu);
to_send[6] = (uint8_t)(amount * 2);
for(int i = 0; i < amount; i++){
to_send[7 + 2 * i] = (uint8_t)(value[i] >> 8u);
to_send[8 + 2 * i] = (uint8_t)(value[i] & 0x00FFu);
}
uint16_t crc = getCRC16(to_send, 9 + 2 * amount - 2);
to_send[9 + 2 * amount - 2] = (uint8_t)(crc & 0x00FFu);
to_send[9 + 2 * amount - 1] = (uint8_t)(crc >> 8u);
//for(int i = 0; i < 9 + 2 * amount; i++) ROS_INFO("0x%x", to_send[i]);
result = modbus_send(to_send, 9 + 2 * amount);
}
return result;
}
/*
* Read Request Builder and Sender
* @param[in] slave_id The id on modbus device
* @param[in] address Reference Address
* @param[in] amount Amount of Data to Read
* @param[in] func Modbus Functional Code
*/
ssize_t modbus_rtu::modbus_read(uint8_t slave_id, int address, uint amount, int func){
uint8_t to_send[8];
modbus_build_request(slave_id, to_send, address, func);
to_send[4] = (uint8_t) (amount >> 8u);
to_send[5] = (uint8_t) (amount & 0x00FFu);
uint16_t crc = getCRC16(to_send,6);
to_send[6] = (uint8_t) (crc & 0x00FFu);
to_send[7] = (uint8_t) (crc >> 8u);
return modbus_send(to_send, 8);
}
/*
* Data Sender
* @param[in] to_send Request to Be Sent to Server
* @param[in] length Length of the Request
* @return Size of the request
*/
ssize_t modbus_rtu::modbus_send(uint8_t *to_send, uint16_t length){
struct stat sb;
if(stat(ctx._port, &sb) < 0) reconnect();
std::cout << std::endl;
return sendMsgs(to_send, length);
}
/*
* Data Receiver
* @param[in] buffer Buffer to Store the Data Retrieved
* @return Size of Incoming Data
*/
ssize_t modbus_rtu::modbus_receive(uint8_t *buffer) {
struct timeval timeout;
timeout.tv_sec = 2;
timeout.tv_usec = 0;
fd_set set;
FD_ZERO(&set); /* clear the set */
FD_SET(ctx.s, &set); /* add our file descriptor to the set */
int rv = select(ctx.s + 1, &set, NULL, NULL, &timeout);
if(rv == -1){
err = true;
_connected = false;
error_msg = "Bad connection select";
return BAD_CON;
} else if(rv == 0) {
err = true;
_connected = false;
error_msg = "Bad connection timeout"; /* an timeout occured */
return BAD_CON;
} else
return receiveMsgs(buffer);
}
/*
* Error Code Handler
* @param[in] msg Message Received from the Server
* @param[in] func Modbus Functional Code
*/
void modbus_rtu::modbuserror_handle(const uint8_t *msg, int func){
if(msg[1] == func + 0x80){
err = true;
err_no = 1;
switch(msg[2]){
case EX_PARAM_NOT_EXIT:
error_msg = "The parameter number does not exits";
break;
case EX_ACCESS_TO_PARAM:
error_msg = "There is no write access to the parameter";
break;
case EX_PARAM_LIMITS:
error_msg = "The data value exceeds the parameter limits";
break;
case EX_SUB_INDEX_NO_EXIT:
error_msg = "The sub-index in use does not exits";
break;
case EX_PARAM_NOT_TYPE:
error_msg = "The parameter is not of the array type";
break;
case EX_TYPE_NO_MATCH:
error_msg = "The data type does not match the parameter called";
break;
case EX_ONLY_RESET:
error_msg = "Only reset";
break;
case EX_NOT_CHANGEABLE:
error_msg = "Not changeable";
break;
case EX_NO_WRITE_ACCESS:
error_msg = "No write address";
break;
case EX_NO_POSSIBLE_PRE_MODE:
error_msg = "Data change in the parameter called is not possible in the present mode";
break;
case EX_OTHER_ERROR:
error_msg = "Other error";
break;
case EX_INVALID_DATA_ADDR:
error_msg = "Invalid data address";
break;
case EX_INVALID_MSGS_LENGTH:
error_msg = "Invalid message length";
break;
case EX_INVALID_LENGTH_VALUE:
error_msg = "Invalid data length or value";
break;
case EX_INVALID_FUNC_CODE:
error_msg = "Invalid function code";
break;
case EX_NO_BUS_ACCESS:
error_msg = "There is no bus access";
break;
case EX_DATA_NOT_POSSIBLE:
error_msg = "Data change is not possible because factory set-up is selected";
break;
}
}
err = false;
error_msg = "NO ERR";
}
/*
* Set Bad Data lenght or Address
*/
void modbus_rtu::set_bad_input(void) {
err = true;
error_msg = "Bad Data lenght or Address";
}
/*
* Set Bad connection
*/
void modbus_rtu::set_bad_con(void) {
err = true;
_connected = false;
error_msg = "Bad connection";
}
/**
* Set config faild
*/
void modbus_rtu::setConfigFaild(void){
err = true;
_connected = false;
error_msg = "Driver config failded";
}

78
Devices/Libraries/Ros/modbus/modbus_tcp/CMakeLists.txt Executable file
View File

@@ -0,0 +1,78 @@
cmake_minimum_required(VERSION 3.0.2)
project(modbus_tcp)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES modbus_tcp
CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME} src/modbus.cpp)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_modbus_tcp.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

240
Devices/Libraries/Ros/modbus/modbus_tcp/include/modbus_tcp/modbus.h Executable file
View File

@@ -0,0 +1,240 @@
#ifndef MODBUS_H
#define MODBUS_H
#pragma once
#include <ros/ros.h>
#include <cstring>
#include <stdint.h>
#include <iostream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#define MAX_MSG_LENGTH 260
///Function Code
#define READ_COILS 0x01
#define READ_INPUT_BITS 0x02
#define READ_REGS 0x03
#define READ_INPUT_REGS 0x04
#define WRITE_COIL 0x05
#define WRITE_REG 0x06
#define WRITE_COILS 0x0F
#define WRITE_REGS 0x10
///Exception Codes
#define EX_ILLEGAL_FUNCTION 0x01 // Function Code not Supported
#define EX_ILLEGAL_ADDRESS 0x02 // Output Address not exists
#define EX_ILLEGAL_VALUE 0x03 // Output Value not in Range
#define EX_SERVER_FAILURE 0x04 // Slave Deive Fails to process request
#define EX_ACKNOWLEDGE 0x05 // Service Need Long Time to Execute
#define EX_SERVER_BUSY 0x06 // Server Was Unable to Accept MB Request PDU
#define EX_NEGATIVE_ACK 0x07
#define EX_MEM_PARITY_PROB 0x08
#define EX_GATEWAY_PROBLEMP 0x0A // Gateway Path not Available
#define EX_GATEWYA_PROBLEMF 0x0B // Target Device Failed to Response
#define EX_BAD_DATA 0XFF // Bad Data lenght or Address
#define BAD_CON -1
/**
* @class Modbus Operator Class
* @brief Providing networking support and mobus operation support.
*/
class modbus {
public:
/**
* Main Constructor of Modbus Connector Object
* @param host IP Address of Host
* @param port Port for the TCP Connection
* @return A Modbus Connector Object
*/
modbus(std::string ip_address, int port);
/**
* Destructor of Modbus Connector Object
*/
virtual ~modbus();
/**
* Modbus Slave ID Setter
* @param id ID of the Modbus Server Slave
*/
void modbus_set_slave_id(int id);
/**
* @brief Build up a Modbus/TCP Connection
* @return If A Connection Is Successfully Built
*/
bool modbus_connect();
/**
* @brief Close the Modbus/TCP Connection
*/
void modbus_close() const;
/**
* @brief Read Coil
* MODBUS FUNCTION 0x01
* @param address Reference Address
* @param buffer Buffer to Store Data Read from Coils
*/
int modbus_read_coil(int address, bool &buffer);
/**
* @brief Read Coils
* MODBUS FUNCTION 0x01
* @param address Reference Address
* @param amount Amount of Coils to Read
* @param buffer Buffer to Store Data Read from Coils
*/
int modbus_read_coils(int address, int amount, bool* buffer);
/**
* @brief Read Input Bits(Discrete Data)
* MODBUS FUNCITON 0x02
* @param address Reference Address
* @param buffer Buffer to store Data Read from Input Bits
*/
int modbus_read_input_bit(int address, bool &buffer);
/**
* @brief Read Input Bits(Discrete Data)
* MODBUS FUNCITON 0x02
* @param address Reference Address
* @param amount Amount of Bits to Read
* @param buffer Buffer to store Data Read from Input Bits
*/
int modbus_read_input_bits(int address, int amount, bool* buffer);
/**
* @brief Read Holding Register
* MODBUS FUNCTION 0x03
* @param address Reference Address
* @param buffer Buffer to Store Data Read from Registers
*/
int modbus_read_holding_register(int address, uint16_t &buffer);
/**
* @brief Read Holding Registers
* MODBUS FUNCTION 0x03
* @param address Reference Address
* @param amount Amount of Registers to Read
* @param buffer Buffer to Store Data Read from Registers
*/
int modbus_read_holding_registers(int address, int amount, uint16_t *buffer);
/**
* @brief Read Input Registers
* MODBUS FUNCTION 0x04
* @param address Reference Address
* @param amount Amount of Registers to Read
* @param buffer Buffer to Store Data Read from Registers
*/
int modbus_read_input_registers(int address, int amount, uint16_t *buffer);
/**
* @brief Write Single Coils
* MODBUS FUNCTION 0x05
* @param address Reference Address
* @param to_write Value to be Written to Coil
*/
int modbus_write_coil(int address, const bool& to_write);
/**
* @brief Write Single Register
* FUCTION 0x06
* @param address Reference Address
* @param value Value to Be Written to Register
*/
int modbus_write_register(int address, const uint16_t& value);
/**
* @brief Write Multiple Coils
* MODBUS FUNCTION 0x0F
* @param address Reference Address
* @param amount Amount of Coils to Write
* @param value Values to Be Written to Coils
*/
int modbus_write_coils(int address, int amount, const bool *value);
/**
* @brief Write Multiple Registers
* MODBUS FUNCION 0x10
* @param address Reference Address
* @param amount Amount of Value to Write
* @param value Values to Be Written to the Registers
*/
int modbus_write_registers(int address, int amount, const uint16_t *value);
bool _connected{};
bool err{};
int err_no{};
std::string error_msg;
struct timeval timeout{};
private:
/**
* @brief Modbus Request Builder
* @param to_send Message Buffer to Be Sent
* @param address Reference Address
* @param func Modbus Functional Code
*/
void modbus_build_request(uint8_t *to_send, uint address, int func) const;
/**
* @brief Read Request Builder and Sender
* @param address Reference Address
* @param amount Amount of Data to Read
* @param func Modbus Functional Code
*/
int modbus_read(int address, uint amount, int func);
/**
* @brief Write Request Builder and Sender
* @param address Reference Address
* @param amount Amount of data to be Written
* @param func Modbus Functional Code
* @param value Data to Be Written
*/
int modbus_write(int address, uint amount, int func, const uint16_t *value);
/**
* @brief Data Sender
* @param to_send Request to Be Sent to Server
* @param length Length of the Request
* @return Size of the request
*/
ssize_t modbus_send(uint8_t *to_send, int length);
/**
* @brief Data Receiver
* @param buffer Buffer to Store the Data Retrieved
* @return Size of Incoming Data
*/
ssize_t modbus_receive(uint8_t *buffer) const;
/**
* @brief Error Code Handler
* @param msg Message Received from the Server
* @param func Modbus Functional Code
*/
void modbuserror_handle(const uint8_t *msg, int func);
void set_bad_con();
void set_bad_input();
// Properties
int _socket{};
uint _msg_id{};
int _slaveid{};
uint16_t PORT{};
std::string HOST;
struct sockaddr_in _server{};
};
#endif //MODBUS_H

65
Devices/Libraries/Ros/modbus/modbus_tcp/package.xml Executable file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>modbus_tcp</name>
<version>0.0.0</version>
<description>The modbus_tcp package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/modbus_tcp</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

501
Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp Executable file
View 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";
}