@knebb The relevant function is fieldbus_thread, that basically runs a polling loop every 20 ms. If you do not need threads, you can consider that function as main() and drop all the locking stuff (i.e. a good 50% of code in iteration).
#include "lappaseven.h"
#include "fieldbus.h"
#include <errno.h>
#include <string.h>
#define MODBUS_ID 0
#define AI1_ADDRESS 3
#define DI1_ADDRESS 0
#define DI2_ADDRESS 100
#define DI3_ADDRESS 200
#define DO1_ADDRESS 1
#define DO2_ADDRESS 101
#define DO3_ADDRESS 201
#define AO1_ADDRESS 2
#define VREFINT_ADDRESS 5
#define VREF_ADDRESS 1009
static IO image = { 0 };
void
fieldbus_free(modbus_t *modbus)
{
if (modbus != NULL) {
modbus_close(modbus);
modbus_free(modbus);
}
}
modbus_t *
fieldbus_new(void)
{
modbus_t *modbus;
gchar *host;
int port;
host = g_settings_get_string(gs.settings, "modbus-host");
port = g_settings_get_uint(gs.settings, "modbus-porta");
modbus = modbus_new_tcp(host, port);
if (modbus == NULL ||
modbus_connect(modbus) == -1 ||
modbus_set_slave(modbus, MODBUS_ID) == -1)
{
g_warning("Unable to initialize communication on %s:%d: %s",
host, port, modbus_strerror(errno));
fieldbus_free(modbus);
modbus = NULL;
}
g_free(host);
return modbus;
}
gboolean
fieldbus_get_ai(modbus_t *modbus)
{
uint16_t vrefint, vref, ai;
if (modbus_read_registers(modbus, VREFINT_ADDRESS, 1, &vrefint) == -1 ||
modbus_read_registers(modbus, VREF_ADDRESS, 1, &vref) == -1 ||
modbus_read_registers(modbus, AI1_ADDRESS, 1, &ai) == -1)
{
g_warning("fieldbus_get_ai error: %s", modbus_strerror(errno));
return FALSE;
}
/* Linearization to volts as described by Neuron manual, page 14.
* AI1vdev and AI1voffset are set to supposedly 0. */
image.AI1 = 3.3 * vref / vrefint * 3 * ai / 4096;
return TRUE;
}
gboolean
fieldbus_get_di(modbus_t *modbus)
{
uint16_t word1, word2, word3;
int n;
if (modbus_read_registers(modbus, DI1_ADDRESS, 1, &word1) == -1 ||
modbus_read_registers(modbus, DI2_ADDRESS, 1, &word2) == -1 ||
modbus_read_registers(modbus, DI3_ADDRESS, 1, &word3) == -1)
{
g_warning("fieldbus_get_di error: %s", modbus_strerror(errno));
return FALSE;
}
for (n = 0; n < G_N_ELEMENTS(image.DI1); ++n) {
image.DI1[n] = (word1 & (1 << n)) > 0;
}
for (n = 0; n < G_N_ELEMENTS(image.DI2); ++n) {
image.DI2[n] = (word2 & (1 << n)) > 0;
}
for (n = 0; n < G_N_ELEMENTS(image.DI3); ++n) {
image.DI3[n] = (word3 & (1 << n)) > 0;
}
return TRUE;
}
gboolean
fieldbus_set_do(modbus_t *modbus)
{
uint16_t word1, word2, word3;
int n;
word1 = 0;
word2 = 0;
word3 = 0;
for (n = 0; n < G_N_ELEMENTS(image.DO1); ++n) {
word1 |= image.DO1[n] << n;
}
for (n = 0; n < G_N_ELEMENTS(image.DO2); ++n) {
word2 |= image.DO2[n] << n;
}
for (n = 0; n < G_N_ELEMENTS(image.DO3); ++n) {
word3 |= image.DO3[n] << n;
}
if (modbus_write_register(modbus, DO1_ADDRESS, word1) == -1 ||
modbus_write_register(modbus, DO2_ADDRESS, word2) == -1 ||
modbus_write_register(modbus, DO3_ADDRESS, word3) == -1)
{
g_warning("fieldbus_set_do error: %s", modbus_strerror(errno));
return FALSE;
}
return TRUE;
}
gboolean
fieldbus_set_ao(modbus_t *modbus)
{
/* Analog output must be between 0 .. 5 V, hence the division */
if (modbus_write_register(modbus, AO1_ADDRESS, image.AO1 / 2) == -1) {
g_warning("fieldbus_set_ao error: %s", modbus_strerror(errno));
return FALSE;
}
return TRUE;
}
#if THREADING
static gboolean
iteration(modbus_t *modbus)
{
static gboolean workaround = FALSE;
gboolean AI, DI, AO, DO, quit;
AI = fieldbus_get_ai(modbus);
DI = fieldbus_get_di(modbus);
g_mutex_lock(&gs.fieldbus_lock);
AO = gs.hal.AO1 != gs.share.AO1;
DO = memcmp(gs.hal.DO1, gs.share.DO1, sizeof(gs.hal.DO1)) != 0 ||
memcmp(gs.hal.DO2, gs.share.DO2, sizeof(gs.hal.DO2)) != 0 ||
memcmp(gs.hal.DO3, gs.share.DO3, sizeof(gs.hal.DO3)) != 0;
if (DI) {
memcpy(gs.share.DI1, image.DI1, sizeof(image.DI1));
memcpy(gs.share.DI2, image.DI2, sizeof(image.DI2));
memcpy(gs.share.DI3, image.DI3, sizeof(image.DI3));
}
if (AI) {
gs.share.AI1 = image.AI1;
}
if (DO) {
memcpy(image.DO1, gs.share.DO1, sizeof(image.DO1));
memcpy(image.DO2, gs.share.DO2, sizeof(image.DO2));
memcpy(image.DO3, gs.share.DO3, sizeof(image.DO3));
}
if (AO) {
image.AO1 = gs.share.AO1;
}
quit = gs.quit;
g_mutex_unlock(&gs.fieldbus_lock);
if (DI) {
memcpy(gs.hal.DI1, image.DI1, sizeof(image.DI1));
memcpy(gs.hal.DI2, image.DI2, sizeof(image.DI2));
memcpy(gs.hal.DI3, image.DI3, sizeof(image.DI3));
}
if (DO && fieldbus_set_do(modbus)) {
memcpy(gs.hal.DO1, image.DO1, sizeof(image.DO1));
memcpy(gs.hal.DO2, image.DO2, sizeof(image.DO2));
memcpy(gs.hal.DO3, image.DO3, sizeof(image.DO3));
}
/* XXX Workaround for a unknown fieldbus error that seems to reset the
* analog output: if this happens, force a AO1 refresh cycle */
if (! DI || ! AI) {
workaround = TRUE;
}
AO = AO || workaround;
if (AO && fieldbus_set_ao(modbus)) {
g_debug("AO1 set to %d", image.AO1);
gs.hal.AO1 = image.AO1;
workaround = FALSE;
}
return ! quit;
}
gpointer
fieldbus_thread(gpointer user_data)
{
modbus_t *modbus;
gint64 period, scan, now, span, limit;
if (gs.dry_run) {
return NULL;
}
modbus = fieldbus_new();
if (modbus == NULL) {
return NULL;
}
period = g_settings_get_uint(gs.settings, "periodo");
if (period == 0) {
g_warning("Period set to 0: fallback to 20000");
period = 20000;
}
scan = g_get_monotonic_time();
limit = period;
do {
g_main_context_invoke_full(NULL, G_PRIORITY_HIGH,
cycle_iteration, NULL, NULL);
now = g_get_monotonic_time();
span = now - scan;
if (span > limit) {
limit = span;
g_warning("Period exceeded: %.3f ms > %.3f ms" ,
limit / 1000., period / 1000.);
}
scan += period;
if (now < scan) {
g_usleep(scan - now);
}
} while (iteration(modbus));
fieldbus_free(modbus);
return NULL;
}
#endif
The IO struct is just an image of the I/O status:
typedef struct {
uint8_t DI1[4];
uint8_t DI2[16];
uint8_t DI3[16];
uint8_t DO1[4];
uint8_t DO2[14];
uint8_t DO3[14];
double AI1;
uint16_t AO1;
} IO;
