164 lines
3.7 KiB
C
164 lines
3.7 KiB
C
#include "modbus.h"
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/sys/util.h>
|
|
#include <zephyr/modbus/modbus.h>
|
|
#include <zephyr/logging/log.h>
|
|
|
|
LOG_MODULE_REGISTER(mbc, CONFIG_LOG_DEFAULT_LEVEL);
|
|
|
|
#define MODBUS_NODE DT_COMPAT_GET_ANY_STATUS_OKAY(zephyr_modbus_serial)
|
|
|
|
static int client_iface;
|
|
|
|
static struct
|
|
{
|
|
int level; // Water level value
|
|
int minimum; // Minimum value
|
|
int maximum; // Maximum value
|
|
int factor; // Factor for unit conversion
|
|
} measurement;
|
|
|
|
int mb_init_client(void)
|
|
{
|
|
const char iface_name[] = {DEVICE_DT_NAME(MODBUS_NODE)};
|
|
client_iface = modbus_iface_get_by_name(iface_name);
|
|
LOG_DBG("Modbus client interface: %d", client_iface);
|
|
if (client_iface < 0)
|
|
{
|
|
LOG_ERR("Failed to get Modbus interface by name: %s", iface_name);
|
|
return client_iface;
|
|
}
|
|
return modbus_init_client(client_iface, client_param);
|
|
}
|
|
|
|
int mb_read_holding_registers(int node, uint16_t reg_addr, uint16_t *data, size_t len)
|
|
{
|
|
return modbus_read_holding_regs(client_iface, node, reg_addr, data, len);
|
|
}
|
|
|
|
int mb_read()
|
|
{
|
|
int rc;
|
|
int16_t data[5] = {0};
|
|
rc = mb_read_holding_registers(1, 0x0002, data, sizeof(data) / sizeof(data[0]));
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read holding registers: %d", rc);
|
|
return rc;
|
|
}
|
|
LOG_HEXDUMP_DBG(data, sizeof(data), "Holding Registers Data");
|
|
|
|
int unit, decimals;
|
|
unit = data[0];
|
|
decimals = data[1];
|
|
int factor;
|
|
switch (unit)
|
|
{
|
|
case 1: // cm
|
|
factor = 10;
|
|
break;
|
|
case 2: // mm
|
|
factor = 1;
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown unit: %d", unit);
|
|
return -EINVAL;
|
|
}
|
|
switch(decimals)
|
|
{
|
|
case 0: // no decimals
|
|
factor /= 1;
|
|
break;
|
|
case 1: // one decimal
|
|
factor /= 10;
|
|
break;
|
|
case 2: // two decimals
|
|
factor /= 100;
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown decimals: %d", decimals);
|
|
return -EINVAL;
|
|
}
|
|
measurement.factor = factor;
|
|
measurement.level = data[2] * factor;
|
|
measurement.minimum = data[3] * factor;
|
|
measurement.maximum = data[4] * factor;
|
|
LOG_DBG("Water level: %dmm, Minimum: %dmm, Maximum: %dmm",
|
|
measurement.level, measurement.minimum, measurement.maximum);
|
|
return 0;
|
|
}
|
|
|
|
int mb_read_water_level(double *mb_read_water_level)
|
|
{
|
|
int rc = mb_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
return rc;
|
|
}
|
|
*mb_read_water_level = (double)measurement.level / 1000.0; // Convert to meters
|
|
|
|
return 0;
|
|
}
|
|
|
|
int mb_read_water_level_mm(int *mb_read_water_level)
|
|
{
|
|
int rc = mb_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
return rc;
|
|
}
|
|
*mb_read_water_level = measurement.level;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int mb_read_minimum_mm(int *mb_read_minimum)
|
|
{
|
|
int rc = mb_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
return rc;
|
|
}
|
|
*mb_read_minimum = measurement.minimum;
|
|
return 0;
|
|
}
|
|
|
|
int mb_read_maximum_mm(int *mb_read_maximum)
|
|
{
|
|
int rc = mb_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
return rc;
|
|
}
|
|
*mb_read_maximum = measurement.maximum;
|
|
return 0;
|
|
}
|
|
|
|
int mb_write_minimum_mm(int minimum)
|
|
{
|
|
int rc = mb_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
return rc;
|
|
}
|
|
modbus_write_holding_reg(client_iface, 1, 0x0005, minimum / measurement.factor);
|
|
return 0;
|
|
}
|
|
|
|
int mb_write_maximum_mm(int maximum)
|
|
{
|
|
int rc = mb_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
return rc;
|
|
}
|
|
modbus_write_holding_reg(client_iface, 1, 0x0006, maximum / measurement.factor);
|
|
return 0;
|
|
}
|