mirror of
https://github.com/eddyem/eddys_snippets.git
synced 2026-03-21 09:11:03 +03:00
pre-alpha
This commit is contained in:
409
modbus_params/modbus.c
Normal file
409
modbus_params/modbus.c
Normal file
@@ -0,0 +1,409 @@
|
||||
/*
|
||||
* This file is part of the modbus_param project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <modbus/modbus.h>
|
||||
#include <stdatomic.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "modbus.h"
|
||||
#include "verbose.h"
|
||||
|
||||
static FILE *dumpfile = NULL;
|
||||
static dicentry_t *dumppars = NULL;
|
||||
static int dumpsize = 0;
|
||||
|
||||
static dicentry_t *dictionary = NULL;
|
||||
static int dictsize = 0;
|
||||
|
||||
static modbus_t *modbus_ctx = NULL;
|
||||
static pthread_mutex_t modbus_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
|
||||
static atomic_int stopdump = FALSE, isstopped = TRUE;
|
||||
|
||||
// open dictionary file and check it; return TRUE if all OK
|
||||
// all after "#" is comment;
|
||||
// dictionary format: "'code' 'register' 'value' 'readonly flag'\n", e.g.
|
||||
// "F00.09 61444 5000 1"
|
||||
int opendict(const char *dic){
|
||||
FILE *f = fopen(dic, "r");
|
||||
if(!f){
|
||||
WARN("Can't open %s", dic);
|
||||
return FALSE;
|
||||
}
|
||||
int dicsz = 0;
|
||||
size_t linesz = BUFSIZ;
|
||||
char *line = MALLOC(char, linesz);
|
||||
dicentry_t curentry;
|
||||
curentry.code = MALLOC(char, BUFSIZ);
|
||||
int retcode = TRUE;
|
||||
while(1){
|
||||
if(getline(&line, &linesz, f) < 0) break;
|
||||
// DBG("Original LINE: '%s'", line);
|
||||
char *comment = strchr(line, '#');
|
||||
if(comment) *comment = 0;
|
||||
char *newline = strchr(line, '\n');
|
||||
if(newline) *newline = 0;
|
||||
// DBG("LINE: '%s'", line);
|
||||
if(*line == 0) continue;
|
||||
if(4 != sscanf(line, "%s %" SCNu16 " %" SCNu16 " %" SCNu8, curentry.code, &curentry.reg, &curentry.value, &curentry.readonly)){
|
||||
WARNX("Can't understand this line: '%s'", line);
|
||||
continue;
|
||||
}
|
||||
// DBG("Got line: '%s %" PRIu16 " %" PRIu16 " %" PRIu8, curentry.code, curentry.reg, curentry.value, curentry.readonly);
|
||||
if(++dictsize >= dicsz){
|
||||
dicsz += 50;
|
||||
dictionary = realloc(dictionary, sizeof(dicentry_t) * dicsz);
|
||||
if(!dictionary){
|
||||
WARN("Can't allocate memory for dictionary");
|
||||
retcode = FALSE;
|
||||
goto ret;
|
||||
}
|
||||
}
|
||||
dicentry_t *entry = &dictionary[dictsize-1];
|
||||
entry->code = strdup(curentry.code);
|
||||
entry->reg = curentry.reg;
|
||||
entry->value = curentry.value;
|
||||
entry->readonly = curentry.readonly;
|
||||
//DBG("Add entry; now dictsize is %d", dictsize);
|
||||
}
|
||||
ret:
|
||||
fclose(f);
|
||||
FREE(curentry.code);
|
||||
FREE(line);
|
||||
return retcode;
|
||||
}
|
||||
|
||||
static int chkdict(){
|
||||
if(dictsize < 1){
|
||||
WARNX("Open dictionary first");
|
||||
return FALSE;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// find dictionary entry
|
||||
dicentry_t *findentry_by_code(const char *code){
|
||||
if(!chkdict()) return NULL;
|
||||
for(int i = 0; i < dictsize; ++i){
|
||||
if(strcmp(code, dictionary[i].code)) continue;
|
||||
return &dictionary[i];
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
dicentry_t *findentry_by_reg(uint16_t reg){
|
||||
if(!chkdict()) return NULL;
|
||||
for(int i = 0; i < dictsize; ++i){
|
||||
if(reg != dictionary[i].reg) continue;
|
||||
return &dictionary[i];
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// prepare a list with dump parameters
|
||||
int setdumppars(char **pars){
|
||||
if(!pars || !*pars) return FALSE;
|
||||
if(!chkdict()) return FALSE;
|
||||
FNAME();
|
||||
char **p = pars;
|
||||
static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static int cursz = -1; // current allocated size
|
||||
int N = 0;
|
||||
pthread_mutex_lock(&mutex);
|
||||
while(*p){ // count parameters and check them
|
||||
dicentry_t *e = findentry_by_code(*p);
|
||||
if(!e){
|
||||
WARNX("Can't find entry with code %s", *p);
|
||||
pthread_mutex_unlock(&mutex);
|
||||
return FALSE;
|
||||
}
|
||||
DBG("found entry do dump, reg=%d", e->reg);
|
||||
if(cursz <= N){
|
||||
cursz += 50;
|
||||
DBG("realloc list to %d", cursz);
|
||||
dumppars = realloc(dumppars, sizeof(dicentry_t) * (cursz));
|
||||
DBG("zero mem");
|
||||
bzero(&dumppars[N], sizeof(dicentry_t)*(cursz-N));
|
||||
}
|
||||
FREE(dumppars[N].code);
|
||||
dumppars[N] = *e;
|
||||
dumppars[N].code = strdup(e->code);
|
||||
DBG("Add %s", e->code);
|
||||
++N; ++p;
|
||||
}
|
||||
dumpsize = N;
|
||||
pthread_mutex_unlock(&mutex);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// open dump file and add header; return FALSE if failed
|
||||
int opendumpfile(const char *name){
|
||||
if(!chkdict()) return FALSE;
|
||||
if(dumpsize < 1){
|
||||
WARNX("Set dump parameters first");
|
||||
return FALSE;
|
||||
}
|
||||
if(!name) return FALSE;
|
||||
closedumpfile();
|
||||
dumpfile = fopen(name, "w+");
|
||||
if(!dumpfile){
|
||||
WARN("Can't open %s", name);
|
||||
return FALSE;
|
||||
}
|
||||
fprintf(dumpfile, "# time,s ");
|
||||
for(int i = 0; i < dumpsize; ++i){
|
||||
fprintf(dumpfile, "%s ", dumppars[i].code);
|
||||
}
|
||||
fprintf(dumpfile, "\n");
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
void closedumpfile(){
|
||||
if(dumpfile && !isstopped){
|
||||
if(!isstopped){
|
||||
stopdump = TRUE;
|
||||
while(!isstopped);
|
||||
}
|
||||
fclose(dumpfile);
|
||||
}
|
||||
}
|
||||
|
||||
static void *dumpthread(void *p){
|
||||
isstopped = FALSE;
|
||||
stopdump = FALSE;
|
||||
double dT = *(double*)p;
|
||||
DBG("Dump thread started. Period: %gs", dT);
|
||||
double startT = sl_dtime();
|
||||
while(!stopdump){
|
||||
double t0 = sl_dtime();
|
||||
fprintf(dumpfile, "%6.3g", t0 - startT);
|
||||
for(int i = 0; i < dumpsize; ++i){
|
||||
if(!read_entry(&dumppars[i])) fprintf(dumpfile, "---- ");
|
||||
else fprintf(dumpfile, "%4d ", dumppars[i].value);
|
||||
}
|
||||
fprintf(dumpfile, "\n");
|
||||
while(sl_dtime() - t0 < dT) usleep(100);
|
||||
}
|
||||
isstopped = TRUE;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int rundump(double dT){
|
||||
if(!dumpfile){
|
||||
WARNX("Open dump file first");
|
||||
return FALSE;
|
||||
}
|
||||
if(dT < 0.){
|
||||
WARNX("Time interval should be > 0");
|
||||
return FALSE;
|
||||
}
|
||||
pthread_t thread;
|
||||
static double T = 0.;
|
||||
if(T != dT) T = dT;
|
||||
DBG("user give dT: %g", T);
|
||||
if(pthread_create(&thread, NULL, dumpthread, (void*)&T)){
|
||||
WARN("Can't create dumping thread");
|
||||
return FALSE;
|
||||
}
|
||||
DBG("Thread created, detach");
|
||||
pthread_detach(thread);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
void close_modbus(){
|
||||
if(modbus_ctx){
|
||||
closedumpfile();
|
||||
modbus_close(modbus_ctx);
|
||||
modbus_free(modbus_ctx);
|
||||
}
|
||||
}
|
||||
|
||||
// read register and modify entry->reg; return FALSE if failed
|
||||
int read_entry(dicentry_t *entry){
|
||||
if(!entry) return FALSE;
|
||||
int ret = TRUE;
|
||||
pthread_mutex_lock(&modbus_mutex);
|
||||
if(modbus_read_registers(modbus_ctx, entry->reg, 1, &entry->value) < 0) ret = FALSE;
|
||||
pthread_mutex_unlock(&modbus_mutex);
|
||||
return ret;
|
||||
}
|
||||
// write register value; FALSE - if failed or read-only
|
||||
int write_entry(dicentry_t *entry){
|
||||
if(!entry || entry->readonly) return FALSE;
|
||||
int ret = TRUE;
|
||||
pthread_mutex_lock(&modbus_mutex);
|
||||
if(modbus_write_register(modbus_ctx, entry->reg, entry->value) < 0) ret = FALSE;
|
||||
pthread_mutex_unlock(&modbus_mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// write multiply regs (without checking by dict) by NULL-terminated array "reg=val"; return amount of items written
|
||||
int write_regval(char **keyval){
|
||||
int written = 0;
|
||||
dicentry_t entry = {0};
|
||||
while(*keyval){
|
||||
DBG("Parse %s", *keyval);
|
||||
char key[SL_KEY_LEN], value[SL_VAL_LEN];
|
||||
if(2 != sl_get_keyval(*keyval, key, value)){
|
||||
WARNX("%s isn't in format reg=val", *keyval);
|
||||
}else{
|
||||
DBG("key: %s, val: %s", key, value);
|
||||
int r=-1, v=-1;
|
||||
if(!sl_str2i(&r, key) || !sl_str2i(&v, value) || r < 0 || v < 0 || r > UINT16_MAX || v > UINT16_MAX){
|
||||
WARNX("Wrong register number or value: %d=%d; should be uint16_t", r, v);
|
||||
}else{
|
||||
entry.value = (uint16_t)v;
|
||||
entry.reg = (uint16_t)r;
|
||||
if(write_entry(&entry)){
|
||||
++written;
|
||||
verbose(LOGLEVEL_MSG, "Written %u to register %u", entry.value, entry.reg);
|
||||
}else verbose(LOGLEVEL_WARN, "Can't write %u to register %u", entry.value, entry.reg);
|
||||
}
|
||||
}
|
||||
++keyval;
|
||||
}
|
||||
return written;
|
||||
}
|
||||
|
||||
// write multiply regs by NULL-terminated array "keycode=val" (checking `keycode`); return amount of items written
|
||||
int write_codeval(char **codeval){
|
||||
if(!chkdict()) return FALSE;
|
||||
int written = 0;
|
||||
dicentry_t entry = {0};
|
||||
while(*codeval){
|
||||
char key[SL_KEY_LEN], value[SL_VAL_LEN];
|
||||
if(2 != sl_get_keyval(*codeval, key, value)){
|
||||
WARNX("%s isn't in format keycode=val");
|
||||
}else{
|
||||
int v=-1;
|
||||
if(!sl_str2i(&v, value) || v < 0 || v > UINT16_MAX){
|
||||
WARNX("Wrong value: %d; should be uint16_t", v);
|
||||
}else{
|
||||
dicentry_t *de = findentry_by_code(key);
|
||||
if(!de){
|
||||
WARNX("Keycode %s not found in dictionary", key);
|
||||
}else{
|
||||
entry.readonly = de->readonly;
|
||||
entry.reg = de->reg;
|
||||
if(entry.readonly){
|
||||
verbose(LOGLEVEL_WARN, "Can't change readonly register %d", entry.reg);
|
||||
}else{
|
||||
entry.value = (uint16_t)v;
|
||||
if(write_entry(&entry)){
|
||||
++written;
|
||||
verbose(LOGLEVEL_MSG, "Written %u to register %u", entry.value, entry.reg);
|
||||
}else verbose(LOGLEVEL_WARN, "Can't write %u to register %u", entry.value, entry.reg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
++codeval;
|
||||
}
|
||||
return written;
|
||||
}
|
||||
|
||||
int open_modbus(const char *path, int baudrate){
|
||||
int ret = FALSE;
|
||||
if(modbus_ctx) close_modbus();
|
||||
pthread_mutex_lock(&modbus_mutex);
|
||||
modbus_ctx = modbus_new_rtu(path, baudrate, 'N', 8, 1);
|
||||
if(!modbus_ctx){
|
||||
WARNX("Can't open device %s @ %d", path, baudrate);
|
||||
goto rtn;
|
||||
}
|
||||
modbus_set_response_timeout(modbus_ctx, 0, 100000);
|
||||
if(modbus_connect(modbus_ctx) < 0){
|
||||
WARNX("Can't connect to device %s", path);
|
||||
modbus_free(modbus_ctx);
|
||||
modbus_ctx = NULL;
|
||||
goto rtn;
|
||||
}
|
||||
ret = TRUE;
|
||||
rtn:
|
||||
pthread_mutex_unlock(&modbus_mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int set_slave(int ID){
|
||||
int ret = TRUE;
|
||||
pthread_mutex_lock(&modbus_mutex);
|
||||
if(modbus_set_slave(modbus_ctx, ID)){
|
||||
WARNX("Can't set slave ID to %d", ID);
|
||||
ret = FALSE;
|
||||
}
|
||||
pthread_mutex_unlock(&modbus_mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// dump all registers by input dictionary into output; also modify values of registers in dictionary
|
||||
int dumpall(const char *outdic){
|
||||
if(!chkdict()) return -1;
|
||||
int got = 0;
|
||||
FILE *o = fopen(outdic, "w");
|
||||
if(!o){
|
||||
WARN("Can't open %s", outdic);
|
||||
return -1;
|
||||
}
|
||||
for(int i = 0; i < dictsize; ++i){
|
||||
if(read_entry(&dictionary[i])){
|
||||
verbose(LOGLEVEL_MSG, "Read register %d, value: %d\n", dictionary[i].reg, dictionary[i].value);
|
||||
++got;
|
||||
fprintf(o, "%s %4" PRIu16 " %4" PRIu16 " %" PRIu8 "\n", dictionary[i].code, dictionary[i].reg, dictionary[i].value, dictionary[i].readonly);
|
||||
}else verbose(LOGLEVEL_WARN, "Can't read value of register %d\n", dictionary[i].reg);
|
||||
}
|
||||
fclose(o);
|
||||
return got;
|
||||
}
|
||||
|
||||
// read dump register values and dump to stdout
|
||||
void dumpregs(int **addresses){
|
||||
dicentry_t entry = {0};
|
||||
green("Dump registers: (reg val)\n");
|
||||
while(*addresses){
|
||||
int addr = **addresses;
|
||||
if(addr < 0 || addr > UINT16_MAX){
|
||||
WARNX("Wrong register number: %d", addr);
|
||||
}else{
|
||||
entry.reg = (uint16_t) addr;
|
||||
if(!read_entry(&entry)) WARNX("Can't read register %d", addr);
|
||||
else printf("%4d %4d\n", entry.reg, entry.value);
|
||||
}
|
||||
++addresses;
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
// `dumpregs` but by keycodes (not modifying dictionary)
|
||||
void dumpcodes(char **keycodes){
|
||||
if(!chkdict()) return;
|
||||
dicentry_t entry = {0};
|
||||
green("Dump registers: (code (reg) val)\n");
|
||||
while(*keycodes){
|
||||
dicentry_t *de = findentry_by_code(*keycodes);
|
||||
if(!de){
|
||||
WARNX("Can't find keycode %s in dictionary", *keycodes);
|
||||
}else{
|
||||
entry.reg = de->reg;
|
||||
if(!read_entry(&entry)) WARNX("Can't read register %s (%d)", *keycodes, entry.reg);
|
||||
else printf("%s (%4d) %4d\n", de->code, entry.reg, entry.value);
|
||||
}
|
||||
++keycodes;
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
Reference in New Issue
Block a user