Mirrors/VILLASnode
1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-16 00:00:02 +01:00
Code Issues Releases Wiki Activity

can: add function that converts arbitrary sized can data to union signal_data

Browse source
This commit is contained in:
Niklas Eiling 2020-07-08 16:07:57 +02:00 • committed by Steffen Vogel
parent 0235cc1190
commit e3e1ecda2d
2 changed files with 73 additions and 27 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
include/villas/nodes/can.hpp
View file

@ -50,7 +50,7 @@ struct can {
/* States */
int socket;
union signal_data *data;
union signal_data *sample_buf;
struct timespec start_time;
};

98
lib/nodes/can.cpp
View file

@ -21,6 +21,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*********************************************************************************/
#include "villas/signal_type.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
@ -66,6 +67,9 @@ int can_init(struct node *n)
c->interface_name = nullptr;
c->socket = 0;
c->sample_buf = nullptr;
c->in = nullptr;
c->out = nullptr;
return 0;
}
@ -78,6 +82,7 @@ int can_destroy(struct node *n)
if (c->socket != 0) {
close(c->socket);
}
free(c->sample_buf);
free(c->in);
free(c->out);
return 0;
@ -105,11 +110,6 @@ int can_parse_signal(json_t *json, struct vlist *node_signals, struct can_signal
goto out;
}
/*if (!name) {
error("No signale name specified for signal.");
goto out;
}*/
if (can_size > 8 || can_size <= 0) {
error("can_size of %d for signal \"%s\" is invalid. You must satisfy 0 < can_size <= 8.", can_size, name);
goto out;
@ -217,20 +217,16 @@ int can_check(struct node *n)
int can_prepare(struct node *n)
{
//struct can *c = (struct can *) n->_vd;
/* TODO: Add implementation here. The following is just an can */
//
// c->state1 = c->setting1;
//
// if (strcmp(c->setting2, "double") == 0)
// c->state1 *= 2;
return 0;
struct can *c = (struct can *) n->_vd;
c->sample_buf = (union signal_data*) calloc(
vlist_length(&n->in.signals),
sizeof(union signal_data));
return (c->sample_buf != 0 ? 0 : 1);
}
int can_start(struct node *n)
{
int ret = 1;
struct sockaddr_can addr = {0};
struct ifreq ifr;
@ -239,13 +235,13 @@ int can_start(struct node *n)
if((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
error("Error while opening CAN socket");
return 1;
goto out;
}
strcpy(ifr.ifr_name, c->interface_name);
if (ioctl(c->socket, SIOCGIFINDEX, &ifr) != 0) {
error("Could not find interface with name \"%s\".", c->interface_name);
return 1;
goto out;
}
addr.can_family = AF_CAN;
@ -253,10 +249,11 @@ int can_start(struct node *n)
if(bind(c->socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
error("Could not bind to interface with name \"%s\" (%d).", c->interface_name, ifr.ifr_ifindex);
return 1;
goto out;
}
return 0;
ret = 0;
out:
return ret;
}
int can_stop(struct node *n)
@ -290,6 +287,55 @@ int can_resume(struct node *n)
return 0;
}
int can_conv_from_raw(union signal_data* sig, void* from, int size, struct signal *to)
{
if (size <= 0 || size > 8) {
return 1;
}
switch(to->type) {
case SignalType::BOOLEAN:
sig->b = (bool)*(uint8_t*)from;
return 0;
case SignalType::INTEGER:
switch(size) {
case 1:
sig->i = (int64_t)*(int8_t*)from;
return 0;
case 2:
sig->i = (int64_t)*(int16_t*)from;
return 0;
case 3:
sig->i = (int64_t)*(int16_t*)from;
sig->i += ((int64_t)*((int8_t*)(from)+2)) << 16;
return 0;
case 4:
sig->i = (int64_t)*(int32_t*)from;
return 0;
case 8:
sig->i = *(uint64_t*)from;
return 0;
default:
error("unsupported conversion");
return 1;
}
case SignalType::FLOAT:
switch(size) {
case 4:
sig->f = (double)*(float*)from;
return 0;
case 8:
sig->f = *(double*)from;
return 0;
default:
error("unsupported conversion");
return 1;
}
default:
error("unsupported conversion");
return 1;
}
}
int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
int nbytes;
@ -327,12 +373,12 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele
for (size_t i=0; i < vlist_length(&(n->in.signals)); i++) {
if (c->in[i].id == frame.can_id) {
/* This is a bit ugly. But there is no clean way to
* clear the union. */
smps[nread]->data[i].i = 0;
memcpy(&smps[nread]->data[i],
((uint8_t*)&frame.data) + c->in[i].offset,
c->in[i].size);
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;
}
signal_num++;
found_id = true;
}