From c4b21e5f263439496b8de6c036e13a7a33fab73f Mon Sep 17 00:00:00 2001 From: Niklas Eiling Date: Wed, 8 Jul 2020 16:08:00 +0200 Subject: [PATCH] can: add to_raw conversion and fix data conversions in can_write --- etc/examples/nodes/can.conf | 26 ++++++- include/villas/nodes/can.hpp | 1 + lib/nodes/can.cpp | 137 ++++++++++++++++++++++++++++------- 3 files changed, 138 insertions(+), 26 deletions(-) diff --git a/etc/examples/nodes/can.conf b/etc/examples/nodes/can.conf index df12146fa..d0be21466 100644 --- a/etc/examples/nodes/can.conf +++ b/etc/examples/nodes/can.conf @@ -1,3 +1,6 @@ +logging = { + level = "debug" +}, nodes = { can_node1 = { type = "can", @@ -12,23 +15,44 @@ nodes = { can_id = 66, can_size = 8, can_offset = 0 + }, + { + name = "sigin2", + unit = "Volts", + type = "float", + enabled = true, + can_id = 67, + can_size = 4, + can_offset = 0 + }, + { + name = "sigin3", + unit = "Volts", + type = "integer", + enabled = true, + can_id = 66, + can_size = 4, + can_offset = 4 }) }, out = { signals = ( { + type = "float", can_id = 1, can_size = 4, can_offset = 0 }, { + type = "float", can_id = 1, can_size = 4, can_offset = 4 }, { + type = "float", can_id = 2, - can_size = 4, + can_size = 8, can_offset = 0 }), } diff --git a/include/villas/nodes/can.hpp b/include/villas/nodes/can.hpp index 5b721a9bb..c5788f3a6 100644 --- a/include/villas/nodes/can.hpp +++ b/include/villas/nodes/can.hpp @@ -51,6 +51,7 @@ struct can { /* States */ int socket; union signal_data *sample_buf; + size_t sample_buf_num; struct timespec start_time; }; diff --git a/lib/nodes/can.cpp b/lib/nodes/can.cpp index d2ff8bc21..6e959ae79 100644 --- a/lib/nodes/can.cpp +++ b/lib/nodes/can.cpp @@ -21,6 +21,8 @@ * along with this program. If not, see . *********************************************************************************/ +#include "villas/signal_data.h" +#include "villas/signal_list.h" #include "villas/signal_type.h" #include #include @@ -68,6 +70,7 @@ int can_init(struct node *n) c->interface_name = nullptr; c->socket = 0; c->sample_buf = nullptr; + c->sample_buf_num = 0; c->in = nullptr; c->out = nullptr; @@ -287,9 +290,70 @@ int can_resume(struct node *n) return 0; } +int can_conv_to_raw(union signal_data* sig, struct signal *from, void* to, int size) +{ + if (size <= 0 || size > 8) { + error("signal size cannot be larger than 8!"); + return 1; + } + switch(from->type) { + case SignalType::BOOLEAN: + *(uint8_t*)to = sig->b; + return 0; + case SignalType::INTEGER: + switch(size) { + case 1: + *(int8_t*)to = (int8_t)sig->i; + return 0; + case 2: + *(int16_t*)to = (int16_t)sig->i; + sig->i = (int64_t)*(int16_t*)from; + return 0; + case 3: + *(int16_t*)to = (int16_t)sig->i; + *((int8_t*)to+2) = (int8_t)(sig->i >> 16); + return 0; + case 4: + *(int32_t*)to = (int32_t)sig->i; + return 0; + case 8: + *(int64_t*)to = sig->i; + return 0; + default: + goto fail; + } + case SignalType::FLOAT: + switch(size) { + case 4: + assert(sizeof(float) == 4); + *(float*)to = (float)sig->f; + return 0; + case 8: + *(double*)to = sig->f; + return 0; + default: + goto fail; + } + case SignalType::COMPLEX: + if (size != 8) { + goto fail; + } + *(float*)to = sig->z.real(); + *((float*)to+1) = sig->z.imag(); + return 0; + default: + goto fail; + } + fail: + error("unsupported conversion to %s from raw (%p, %d)", + signal_type_to_str(from->type), to, size); + return 1; +} + int can_conv_from_raw(union signal_data* sig, void* from, int size, struct signal *to) { if (size <= 0 || size > 8) { + error("signal size cannot be larger than 8!"); return 1; } switch(to->type) { @@ -315,32 +379,40 @@ int can_conv_from_raw(union signal_data* sig, void* from, int size, struct signa sig->i = *(uint64_t*)from; return 0; default: - error("unsupported conversion"); - return 1; + goto fail; } case SignalType::FLOAT: switch(size) { case 4: + assert(sizeof(float) == 4); sig->f = (double)*(float*)from; return 0; case 8: sig->f = *(double*)from; return 0; default: - error("unsupported conversion"); - return 1; + goto fail; } + case SignalType::COMPLEX: + if (size != 8) { + goto fail; + } + sig->z = std::complex(*(float*)from, *((float*)from+1)); + return 0; default: - error("unsupported conversion"); - return 1; + goto fail; } + fail: + error("unsupported conversion from %s to raw (%p, %d)", + signal_type_to_str(to->type), from, size); + return 1; } int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) { + int ret = 0; int nbytes; unsigned nread = 0; - unsigned signal_num = 0; struct can_frame frame; struct timeval tv; bool found_id = false; @@ -352,12 +424,12 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele nbytes = read(c->socket, &frame, sizeof(struct can_frame)); if (nbytes == -1) { error("CAN read() returned -1. Is the CAN interface up?"); - return 0; + goto out; } if ((unsigned)nbytes != sizeof(struct can_frame)) { error("CAN read() error. read() returned %d bytes but expected %zu", nbytes, sizeof(struct can_frame)); - return 0; + goto out; } debug(0,"received can message: (id:%d, len:%u, data: 0x%x:0x%x)", @@ -376,10 +448,10 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele if (can_conv_from_raw(&c->sample_buf[i], ((uint8_t*)&frame.data) + c->in[i].offset, c->in[i].size, - (struct signal*) vlist_at(&n->in.signals, i)) == 0) { - return 1; + (struct signal*) vlist_at(&n->in.signals, i)) != 0) { + goto out; } - signal_num++; + c->sample_buf_num++; found_id = true; } } @@ -387,12 +459,22 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele error("did not find signal for can id %d\n", frame.can_id); return 0; } + debug(0, "received %zu signals\n", c->sample_buf_num); + /* Copy signal data to sample only when all signals have been received */ + if (c->sample_buf_num == vlist_length(&n->in.signals)) { + smps[nread]->length = c->sample_buf_num; + memcpy(smps[nread]->data, c->sample_buf, c->sample_buf_num*sizeof(union signal_data)); + c->sample_buf_num = 0; + smps[nread]->flags |= (int) SampleFlags::HAS_DATA; + ret = 1; + } else { + smps[nread]->length = 0; + ret = 0; + } + out: /* Set signals, because other VILLASnode parts expect us to */ smps[nread]->signals = &n->in.signals; - smps[nread]->length = signal_num; - smps[nread]->flags |= (int) SampleFlags::HAS_DATA; - printf("flags: %d\n", smps[nread]->flags); - return 1; + return ret; } int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release) @@ -416,9 +498,11 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel } frame[fsize].can_dlc = c->out[i].size; frame[fsize].can_id = c->out[i].id; - memcpy(((uint8_t*)&frame[fsize].data) + c->out[i].offset, - &smps[nwrite]->data[i], - c->out[i].size); + can_conv_to_raw( + &smps[nwrite]->data[i], + (struct signal*)vlist_at(&(n->out.signals), i), + &frame[fsize].data, + c->out[i].size); fsize++; } for (size_t i=0; i < vlist_length(&(n->out.signals)); i++) { @@ -426,13 +510,16 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel continue; } for (size_t j=0; j < fsize; j++) { - if (c->out[i].id == frame[j].can_id) { - frame[j].can_dlc += c->out[i].size; - memcpy(((uint8_t*)&frame[j].data) + c->out[i].offset, - &smps[nwrite]->data[i], - c->out[i].size); - break; + if (c->out[i].id != frame[j].can_id) { + continue; } + frame[j].can_dlc += c->out[i].size; + can_conv_to_raw( + &smps[nwrite]->data[i], + (struct signal*)vlist_at(&(n->out.signals), i), + (uint8_t*)&frame[j].data + c->out[i].offset, + c->out[i].size); + break; } } for (size_t j=0; j < fsize; j++) {