385 lines
14 KiB
C
385 lines
14 KiB
C
#include "waterlevel_sensor.h"
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/logging/log.h>
|
|
#include <zephyr/modbus/modbus.h>
|
|
#include <stdlib.h>
|
|
#include "canbus.h"
|
|
|
|
LOG_MODULE_REGISTER(wls, CONFIG_LOG_WATERLEVELSENSOR_LEVEL);
|
|
|
|
#define MODBUS_NODE DT_COMPAT_GET_ANY_STATUS_OKAY(zephyr_modbus_serial)
|
|
|
|
struct k_thread waterlevel_sensor_thread_data;
|
|
K_THREAD_STACK_DEFINE(waterlevel_sensor_stack, WATERLEVEL_SENSOR_STACK_SIZE);
|
|
K_MSGQ_DEFINE(waterlevel_sensor_msgq, sizeof(waterlevel_command_t), WATERLEVEL_MESSAGE_QUEUE_SIZE, 4);
|
|
|
|
static int modbus_client_iface;
|
|
|
|
volatile static struct
|
|
{
|
|
int level; // Water level value
|
|
int zeropoint; // Minimum value
|
|
int maxpoint; // Maximum value
|
|
int factor; // Factor for unit conversion
|
|
bool factor_set; // Flag to indicate if factor is set
|
|
} waterlevel_measurement = {
|
|
.factor_set = false,
|
|
};
|
|
|
|
struct modbus_iface_param client_param = {
|
|
.mode = MODBUS_MODE_RTU,
|
|
.rx_timeout = 50000, // Timeout for receiving data in milliseconds
|
|
.serial = {
|
|
.baud = 9600,
|
|
.parity = UART_CFG_PARITY_NONE,
|
|
.stop_bits_client = UART_CFG_STOP_BITS_1, // 1 stop bit for Modbus RTU
|
|
},
|
|
};
|
|
|
|
static int waterlevel_modbus_init() {
|
|
const char iface_name[] = {DEVICE_DT_NAME(MODBUS_NODE)};
|
|
modbus_client_iface = modbus_iface_get_by_name(iface_name);
|
|
if (modbus_client_iface < 0)
|
|
{
|
|
LOG_ERR("Failed to get Modbus interface by name: %s", iface_name);
|
|
return modbus_client_iface;
|
|
}
|
|
LOG_DBG("Initializing modbus client interface: %s", iface_name);
|
|
return modbus_init_client(modbus_client_iface, client_param);
|
|
}
|
|
|
|
static int waterlevel_modbus_read(void) {
|
|
int rc;
|
|
union
|
|
{
|
|
struct
|
|
{
|
|
int16_t unit; // Unit of measurement (e.g., cm, mm)
|
|
int16_t decimals; // Number of decimal places for the measurement
|
|
int16_t level; // Water level value
|
|
int16_t zeropoint; // Zero point for the measurement
|
|
int16_t maxpoint; // Maximum point for the measurement
|
|
};
|
|
int16_t data[5]; // Data array for holding registers
|
|
} waterlevel_modbus_data;
|
|
rc = modbus_read_holding_regs(modbus_client_iface, WATERLEVEL_SENSOR_MODBUS_NODE_ID, 0x0002, waterlevel_modbus_data.data, sizeof(waterlevel_modbus_data.data) / sizeof(waterlevel_modbus_data.data[0]));
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read holding registers, node <%d>, returncode: %d", WATERLEVEL_SENSOR_MODBUS_NODE_ID, rc);
|
|
return rc;
|
|
}
|
|
|
|
LOG_DBG("Got values. Unit: %d, Decimals: %d, Level: %d, Zero Point: %d, Max Point: %d",
|
|
waterlevel_modbus_data.unit,
|
|
waterlevel_modbus_data.decimals,
|
|
waterlevel_modbus_data.level,
|
|
waterlevel_modbus_data.zeropoint,
|
|
waterlevel_modbus_data.maxpoint);
|
|
|
|
LOG_HEXDUMP_DBG(waterlevel_modbus_data.data, sizeof(waterlevel_modbus_data.data), "Waterlevel Sensor Holding Registers Data");
|
|
|
|
switch (waterlevel_modbus_data.unit)
|
|
{
|
|
case 1: // cm
|
|
waterlevel_measurement.factor = 10;
|
|
break;
|
|
case 2: // mm
|
|
waterlevel_measurement.factor = 1;
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown unit: %d", waterlevel_modbus_data.unit);
|
|
waterlevel_measurement.factor_set = false;
|
|
return -EINVAL;
|
|
}
|
|
switch (waterlevel_modbus_data.decimals)
|
|
{
|
|
case 0: // no decimals
|
|
waterlevel_measurement.factor /= 1;
|
|
break;
|
|
case 1: // one decimal
|
|
waterlevel_measurement.factor /= 10;
|
|
break;
|
|
case 2: // two decimals
|
|
waterlevel_measurement.factor /= 100;
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown decimals: %d", waterlevel_modbus_data.decimals);
|
|
waterlevel_measurement.factor_set = false;
|
|
return -EINVAL;
|
|
}
|
|
waterlevel_measurement.factor_set = true;
|
|
waterlevel_measurement.level = waterlevel_modbus_data.level * waterlevel_measurement.factor;
|
|
waterlevel_measurement.zeropoint = waterlevel_modbus_data.zeropoint * waterlevel_measurement.factor;
|
|
waterlevel_measurement.maxpoint = waterlevel_modbus_data.maxpoint * waterlevel_measurement.factor;
|
|
|
|
LOG_DBG("Water level: %dmm, zero point: %dmm, maximum point: %dmm",
|
|
waterlevel_measurement.level,
|
|
waterlevel_measurement.zeropoint,
|
|
waterlevel_measurement.maxpoint);
|
|
LOG_HEXDUMP_DBG(waterlevel_modbus_data.data, sizeof(waterlevel_modbus_data.data), "Waterlevel Sensor Holding Registers Data");
|
|
return 0;
|
|
}
|
|
|
|
static int waterlevel_send_level(void) {
|
|
if (!waterlevel_measurement.factor_set) {
|
|
LOG_ERR("Factor not set, cannot send water level");
|
|
return -EINVAL;
|
|
}
|
|
|
|
LOG_INF("Sending water level: %dmm", waterlevel_measurement.level);
|
|
canbus_send16(CANBUS_REG_WATERLEVEL_LEVEL, waterlevel_measurement.level);
|
|
return 0;
|
|
}
|
|
|
|
static int waterlevel_send_zero_point(void) {
|
|
if (!waterlevel_measurement.factor_set) {
|
|
LOG_ERR("Factor not set, cannot send zero point");
|
|
return -EINVAL;
|
|
}
|
|
|
|
LOG_INF("Sending water zero point: %dmm", waterlevel_measurement.zeropoint);
|
|
canbus_send16(CANBUS_REG_WATERLEVEL_ZERO_POINT, waterlevel_measurement.zeropoint);
|
|
return 0;
|
|
}
|
|
|
|
static int waterlevel_send_max_point(void) {
|
|
if (!waterlevel_measurement.factor_set) {
|
|
LOG_ERR("Factor not set, cannot send maximum point");
|
|
return -EINVAL;
|
|
}
|
|
|
|
LOG_INF("Sending water maximum point: %dmm", waterlevel_measurement.maxpoint);
|
|
canbus_send16(CANBUS_REG_WATERLEVEL_MAX_POINT, waterlevel_measurement.maxpoint);
|
|
return 0;
|
|
}
|
|
|
|
static int waterlevel_set_zero_point(int zeropoint) {
|
|
if (!waterlevel_measurement.factor_set) {
|
|
LOG_ERR("Factor not set, cannot set zero point");
|
|
return -EINVAL;
|
|
}
|
|
|
|
int16_t zeropoint_modbus = zeropoint / waterlevel_measurement.factor;
|
|
int rc = modbus_write_holding_regs(modbus_client_iface, WATERLEVEL_SENSOR_MODBUS_NODE_ID, 0x0005, &zeropoint_modbus, 1);
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to write zero point: %d", rc);
|
|
return rc;
|
|
}
|
|
|
|
waterlevel_measurement.zeropoint = zeropoint; // Update the local measurement structure
|
|
LOG_INF("Zero point set to: %dmm", waterlevel_measurement.zeropoint);
|
|
rc = waterlevel_send_zero_point();
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to send zero point: %d", rc);
|
|
return rc;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int waterlevel_set_max_point(int maxpoint) {
|
|
if (!waterlevel_measurement.factor_set) {
|
|
LOG_ERR("Factor not set, cannot set maximum point");
|
|
return -EINVAL;
|
|
}
|
|
|
|
int16_t maxpoint_modbus = maxpoint / waterlevel_measurement.factor;
|
|
int rc = modbus_write_holding_regs(modbus_client_iface, WATERLEVEL_SENSOR_MODBUS_NODE_ID, 0x0006, &maxpoint_modbus, 1);
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to write maximum point: %d", rc);
|
|
return rc;
|
|
}
|
|
|
|
waterlevel_measurement.maxpoint = maxpoint; // Update the local measurement structure
|
|
LOG_INF("Maximum point set to: %dmm", waterlevel_measurement.maxpoint);
|
|
rc = waterlevel_send_max_point();
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to send maximum point: %d", rc);
|
|
return rc;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void waterlevel_sensor_thread(void *arg1, void *arg2, void *arg3)
|
|
{
|
|
ARG_UNUSED(arg1);
|
|
ARG_UNUSED(arg2);
|
|
ARG_UNUSED(arg3);
|
|
|
|
// Initialize the Modbus client
|
|
int rc = waterlevel_modbus_init();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to initialize Modbus client: %d", rc);
|
|
return;
|
|
}
|
|
|
|
rc = waterlevel_modbus_read();
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to read initial water level: %d", rc);
|
|
return;
|
|
}
|
|
waterlevel_send_level();
|
|
waterlevel_send_zero_point();
|
|
waterlevel_send_max_point();
|
|
|
|
// Initialize the last transmission time and level
|
|
// Use k_uptime_get_32() to get the current uptime in milliseconds
|
|
// and store the initial water level measurement.
|
|
// This will be used to determine when to send updates.
|
|
|
|
uint32_t last_transmission_time_ms = k_uptime_get_32();
|
|
int32_t last_transmission_level = waterlevel_measurement.level;
|
|
|
|
while (1)
|
|
{
|
|
uint32_t current_time_ms = k_uptime_get_32();
|
|
uint32_t delta_time = current_time_ms-last_transmission_time_ms;
|
|
waterlevel_command_t command;
|
|
|
|
rc = waterlevel_modbus_read();
|
|
if (rc < 0)
|
|
{
|
|
LOG_ERR("Failed to read water level: %d", rc);
|
|
continue;
|
|
}
|
|
|
|
if (delta_time >= WATERLEVEL_SENSOR_MAX_UPDATE_INTERVAL_MS ||
|
|
abs(waterlevel_measurement.level - last_transmission_level) >= WATERLEVEL_SENSOR_MIN_DELTA) {
|
|
rc = waterlevel_send_level();
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to send water level: %d", rc);
|
|
} else {
|
|
last_transmission_time_ms = current_time_ms;
|
|
last_transmission_level = waterlevel_measurement.level;
|
|
}
|
|
}
|
|
|
|
while (k_msgq_get(&waterlevel_sensor_msgq, &command, K_NO_WAIT) == 0)
|
|
{
|
|
switch(command.cmd)
|
|
{
|
|
case WATERLEVEL_CMD_SET:
|
|
switch (command.reg)
|
|
{
|
|
case CANBUS_REG_WATERLEVEL_ZERO_POINT: // Set zero point
|
|
rc = waterlevel_set_zero_point(command.data);
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to set zero point: %d", rc);
|
|
}
|
|
break;
|
|
case CANBUS_REG_WATERLEVEL_MAX_POINT: // Set maximum point
|
|
rc = waterlevel_set_max_point(command.data);
|
|
if (rc < 0) {
|
|
LOG_ERR("Failed to set maximum point: %d", rc);
|
|
}
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown register for SET command: 0x%02X", command.reg);
|
|
break;
|
|
}
|
|
break;
|
|
case WATERLEVEL_CMD_GET:
|
|
switch (command.reg)
|
|
{
|
|
case CANBUS_REG_WATERLEVEL_LEVEL: // Get water level
|
|
waterlevel_send_level();
|
|
break;
|
|
case CANBUS_REG_WATERLEVEL_ZERO_POINT: // Get zero point
|
|
waterlevel_send_zero_point();
|
|
break;
|
|
case CANBUS_REG_WATERLEVEL_MAX_POINT: // Get maximum point
|
|
waterlevel_send_max_point();
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown register for GET command: 0x%02X", command.reg);
|
|
break;
|
|
}
|
|
break;
|
|
default:
|
|
LOG_ERR("Unknown command type: %d", command.cmd);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int waterlevel_sensor_start_thread(void)
|
|
{
|
|
k_tid_t waterlevel_sensor_thread_id;
|
|
|
|
// Start the thread
|
|
waterlevel_sensor_thread_id = k_thread_create(&waterlevel_sensor_thread_data, waterlevel_sensor_stack,
|
|
K_THREAD_STACK_SIZEOF(waterlevel_sensor_stack), waterlevel_sensor_thread,
|
|
NULL, NULL, NULL,
|
|
WATERLEVEL_SENSOR_THREAD_PRIORITY, 0, K_NO_WAIT);
|
|
|
|
if (waterlevel_sensor_thread_id == NULL)
|
|
{
|
|
LOG_ERR("Failed to create water level sensor thread");
|
|
return -ENOMEM;
|
|
}
|
|
k_thread_name_set(waterlevel_sensor_thread_id, "waterlevel_sensor");
|
|
|
|
LOG_INF("Water level sensor thread started successfully");
|
|
return 0;
|
|
}
|
|
|
|
#ifdef CONFIG_SHELL
|
|
#include <zephyr/shell/shell.h>
|
|
|
|
void waterlevel_set_zero_point_shell(const struct shell *shell, size_t argc, char **argv) {
|
|
if (argc != 2) {
|
|
shell_error(shell, "Usage: waterlevel_sensor set_zero_point <zeropoint>");
|
|
return;
|
|
}
|
|
|
|
int zeropoint = atoi(argv[1]);
|
|
int rc = waterlevel_set_zero_point(zeropoint);
|
|
if (rc < 0) {
|
|
shell_error(shell, "Failed to set zero point: %d", rc);
|
|
} else {
|
|
shell_print(shell, "Zero point set to: %dmm", zeropoint);
|
|
}
|
|
}
|
|
|
|
void waterlevel_set_max_point_shell(const struct shell *shell, size_t argc, char **argv) {
|
|
if (argc != 2) {
|
|
shell_error(shell, "Usage: waterlevel_sensor set_max_point <maxpoint>");
|
|
return;
|
|
}
|
|
|
|
int maxpoint = atoi(argv[1]);
|
|
int rc = waterlevel_set_max_point(maxpoint);
|
|
if (rc < 0) {
|
|
shell_error(shell, "Failed to set maximum point: %d", rc);
|
|
} else {
|
|
shell_print(shell, "Maximum point set to: %dmm", maxpoint);
|
|
}
|
|
}
|
|
|
|
void waterlevel_sensor_print_shell(const struct shell *shell, size_t argc, char **argv) {
|
|
ARG_UNUSED(argc);
|
|
ARG_UNUSED(argv);
|
|
|
|
waterlevel_modbus_read();
|
|
if (!waterlevel_measurement.factor_set) {
|
|
shell_error(shell, "Factor not set, cannot print water level");
|
|
return;
|
|
}
|
|
|
|
shell_print(shell, "Current water level: %4dmm", waterlevel_measurement.level);
|
|
shell_print(shell, "Zero point: %4dmm", waterlevel_measurement.zeropoint);
|
|
shell_print(shell, "Maximum point: %4dmm", waterlevel_measurement.maxpoint);
|
|
}
|
|
|
|
// Define the shell commands for the water level sensor
|
|
SHELL_STATIC_SUBCMD_SET_CREATE(
|
|
waterlevel_sensor_cmds,
|
|
SHELL_CMD(print, NULL, "Print the current water level, zero point, and maximum point", waterlevel_sensor_print_shell),
|
|
SHELL_CMD(setzero, NULL, "Set the zero point for the water level sensor", waterlevel_set_zero_point_shell),
|
|
SHELL_CMD(setmax, NULL, "Set the maximum point for the water level sensor", waterlevel_set_max_point_shell),
|
|
SHELL_SUBCMD_SET_END);
|
|
|
|
SHELL_CMD_REGISTER(wls, &waterlevel_sensor_cmds, "Water level sensor commands", NULL);
|
|
#endif // CONFIG_SHELL
|