mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
can: implement config parsing and reading from can bus
This commit is contained in:
Niklas Eiling
parent
707e0e5175
commit
bf337ec2a3
2 changed files with 183 additions and 33 deletions
|
|
@ -34,9 +34,18 @@
|
|||
#include <villas/io.h>
|
||||
#include <villas/timing.h>
|
||||
|
||||
struct can_signal {
|
||||
uint32_t id;
|
||||
int offset;
|
||||
int size;
|
||||
};
|
||||
|
||||
struct can {
|
||||
/* Settings */
|
||||
char *interface_name;
|
||||
double *sample_rate;
|
||||
struct can_signal *in;
|
||||
struct can_signal *out;
|
||||
|
||||
/* States */
|
||||
int socket;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
/** Node-type: CAN bus
|
||||
*
|
||||
* @author Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
|
||||
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
|
||||
* @copyright 2014-2020, Institute for Automation of Complex Power Systems, EONERC
|
||||
* @license GNU General Public License (version 3)
|
||||
|
|
@ -31,6 +32,7 @@
|
|||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <linux/sockios.h>
|
||||
|
||||
#include <villas/nodes/can.hpp>
|
||||
#include <villas/utils.hpp>
|
||||
|
|
@ -76,26 +78,136 @@ int can_destroy(struct node *n)
|
|||
if (c->socket != 0) {
|
||||
close(c->socket);
|
||||
}
|
||||
free(c->in);
|
||||
free(c->out);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int can_parse(struct node *n, json_t *cfg)
|
||||
{
|
||||
int ret;
|
||||
int ret = 1;
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
size_t i;
|
||||
json_t *json_in_signals;
|
||||
json_t *json_out_signals;
|
||||
json_t *json_signal;
|
||||
json_error_t err;
|
||||
|
||||
ret = json_unpack_ex(cfg, &err, 0, "{ s?: s }",
|
||||
"interface_name", &c->interface_name
|
||||
);
|
||||
if (ret)
|
||||
jerror(&err, "Failed to parse configuration of node %s", node_name(n));
|
||||
c->in = nullptr;
|
||||
c->out = nullptr;
|
||||
|
||||
return 0;
|
||||
ret = json_unpack_ex(cfg, &err, 0, "{ s: s, s: F, s: { s: o}, s: { s: o}}",
|
||||
"interface_name", &c->interface_name,
|
||||
"sample_rate", &c->sample_rate,
|
||||
"in",
|
||||
"signals", &json_in_signals,
|
||||
"out",
|
||||
"signals", &json_out_signals
|
||||
);
|
||||
if (ret) {
|
||||
jerror(&err, "Failed to parse configuration of node %s", node_name(n));
|
||||
goto out;
|
||||
}
|
||||
|
||||
if ((c->in = (struct can_signal*)calloc(
|
||||
json_array_size(json_in_signals),
|
||||
sizeof(struct can_signal))) == nullptr) {
|
||||
error("failed to allocate memory for input ids");
|
||||
goto out;
|
||||
}
|
||||
if ((c->out = (struct can_signal*)calloc(
|
||||
json_array_size(json_out_signals),
|
||||
sizeof(struct can_signal))) == nullptr) {
|
||||
error("failed to allocate memory for output ids");
|
||||
goto out;
|
||||
}
|
||||
|
||||
json_array_foreach(json_in_signals, i, json_signal) {
|
||||
const char *name = nullptr;
|
||||
uint64_t can_id = 0;
|
||||
int can_size = 8;
|
||||
int can_offset = 0;
|
||||
struct signal* sig = nullptr;
|
||||
|
||||
ret = json_unpack_ex(json_signal, &err, 0, "{ s: s, s?: i, s?: i, s?: i }",
|
||||
"name", &name,
|
||||
"can_id", &can_id,
|
||||
"can_size", &can_size,
|
||||
"can_offset", &can_offset
|
||||
);
|
||||
if (ret) {
|
||||
jerror(&err, "Failed to parse signal configuration of node %s", node_name(n));
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (!name) {
|
||||
error("No signale name specified for signal %zu of node %s.", i, node_name(n));
|
||||
goto out;
|
||||
}
|
||||
|
||||
for (size_t i=0; i < vlist_length(&(n->in.signals)); i++) {
|
||||
sig = (struct signal*)vlist_at(&n->in.signals, i);
|
||||
if (strcmp(name, sig->name) == 0) {
|
||||
c->in[i].id = can_id;
|
||||
c->in[i].size = can_size;
|
||||
c->in[i].offset = can_offset;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
json_array_foreach(json_out_signals, i, json_signal) {
|
||||
const char *name = nullptr;
|
||||
uint64_t can_id;
|
||||
int can_size = 8;
|
||||
int can_offset = 0;
|
||||
struct signal* sig = nullptr;
|
||||
|
||||
ret = json_unpack_ex(json_signal, &err, 0, "{ s: s, s?: i, s?: i, s?: i }",
|
||||
"name", &name,
|
||||
"can_id", &can_id,
|
||||
"can_size", &can_size,
|
||||
"can_offset", &can_offset
|
||||
);
|
||||
if (ret) {
|
||||
jerror(&err, "Failed to parse signal configuration of node %s", node_name(n));
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (!name) {
|
||||
error("No signale name specified for signal %zu of node %s.", i, node_name(n));
|
||||
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;
|
||||
}
|
||||
|
||||
if (can_offset > 8 || can_offset < 0) {
|
||||
error("can_offset of %d for signal \"%s\" is invalid. You must satisfy 0 <= can_offset <= 8.", can_offset, name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
for (size_t i=0; i < vlist_length(&n->in.signals); i++) {
|
||||
sig = (struct signal*)vlist_at(&n->in.signals, i);
|
||||
if (strcmp(name, sig->name) == 0) {
|
||||
c->in[i].id = can_id;
|
||||
c->in[i].size = can_size;
|
||||
c->in[i].offset = can_offset;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
ret = 0;
|
||||
out:
|
||||
if (ret != 0) {
|
||||
free(c->in);
|
||||
free(c->out);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
char * can_print(struct node *n)
|
||||
char *can_print(struct node *n)
|
||||
{
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
|
|
@ -106,15 +218,17 @@ int can_check(struct node *n)
|
|||
{
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
if (c->interface_name != nullptr && strlen(c->interface_name) > 0)
|
||||
return -1;
|
||||
if (c->interface_name == nullptr || strlen(c->interface_name) == 0) {
|
||||
error("interface_name is empty. Please specify the name of the CAN interface!");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int can_prepare(struct node *n)
|
||||
{
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
//struct can *c = (struct can *) n->_vd;
|
||||
|
||||
/* TODO: Add implementation here. The following is just an can */
|
||||
//
|
||||
|
|
@ -129,26 +243,28 @@ int can_prepare(struct node *n)
|
|||
int can_start(struct node *n)
|
||||
{
|
||||
struct sockaddr_can addr = {0};
|
||||
struct can_frame frame;
|
||||
struct ifreq ifr;
|
||||
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
c->start_time = time_now();
|
||||
|
||||
if((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
return strf("Error while opening CAN socket");
|
||||
error("Error while opening CAN socket");
|
||||
return 1;
|
||||
}
|
||||
|
||||
strcpy(ifr.ifr_name, c->interface_name);
|
||||
if (ioctl(c->socket, SIOCGIFINDEX, &ifr) != 0) {
|
||||
return strf("Could not find interface with name \"%s\".", c->interface_name);
|
||||
error("Could not find interface with name \"%s\".", c->interface_name);
|
||||
return 1;
|
||||
}
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
if(bind(c->socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
return strf("Could not bind to interface with name \"%s\" (%d).", c->interface_name, ifr.ifr_ifindex);
|
||||
error("Could not bind to interface with name \"%s\" (%d).", c->interface_name, ifr.ifr_ifindex);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
@ -188,30 +304,54 @@ int can_resume(struct node *n)
|
|||
int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
|
||||
{
|
||||
int nbytes;
|
||||
unsigned nread;
|
||||
struct can_frame frame;
|
||||
struct timespec now;
|
||||
struct timeval tv;
|
||||
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
/* TODO: Add implementation here. The following is just an can */
|
||||
|
||||
assert(cnt >= 1 && smps[0]->capacity >= 1);
|
||||
|
||||
nbytes = read(c->socket, &frame, sizeof(struct can_frame));
|
||||
if (nbytes != sizeof(struct can_frame)) {
|
||||
return strf("CAN read() error. read() returned %d bytes but expected %d",
|
||||
nbytes, sizeof(struct can_frame));
|
||||
for (nread=0; nread < cnt; nread++) {
|
||||
if ((nbytes = read(c->socket, &frame, sizeof(struct can_frame))) == -1) {
|
||||
error("CAN read() returned -1. Is the CAN interface up?");
|
||||
return 1;
|
||||
}
|
||||
if ((unsigned)nbytes != sizeof(struct can_frame)) {
|
||||
error("CAN read() error. read() returned %d bytes but expected %zu",
|
||||
nbytes, sizeof(struct can_frame));
|
||||
return 1;
|
||||
}
|
||||
|
||||
printf("id:%d, len:%u, data: 0x%x:0x%x\n", frame.can_id,
|
||||
frame.can_dlc,
|
||||
((uint32_t*)&frame.data)[0],
|
||||
((uint32_t*)&frame.data)[1]);
|
||||
|
||||
if (ioctl(c->socket, SIOCGSTAMP, &tv) != 0) {
|
||||
warning("Could not get timestamp from CAN driver on interface \"%s\".", c->interface_name);
|
||||
smps[nread]->ts.received = time_now();
|
||||
} else {
|
||||
TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
/* Set signals, because other VILLASnode parts expect us to */
|
||||
smps[nread]->signals = &n->in.signals;
|
||||
}
|
||||
|
||||
printf("id:%d, len:%d, data: 0x%x:0x%x\n", frame.can_id,
|
||||
frame.can_dlc,
|
||||
((uint32_t*)&frame.data)[0],
|
||||
((uint32_t*)&frame.data)[1]);
|
||||
|
||||
now = time_now();
|
||||
smps[0]->data[0].f = time_delta(&now, &c->start_time);
|
||||
|
||||
return nbytes;
|
||||
return 1;
|
||||
}
|
||||
|
||||
int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
|
||||
|
|
@ -222,12 +362,13 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel
|
|||
|
||||
nbytes = write(c->socket, &frame, sizeof(struct can_frame));
|
||||
if (nbytes != sizeof(struct can_frame)) {
|
||||
return strf("CAN write() error. write() returned %d bytes but expected %d",
|
||||
error("CAN write() error. write() returned %u bytes but expected %zu",
|
||||
nbytes, sizeof(struct can_frame));
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
return nbytes;
|
||||
return 1;
|
||||
}
|
||||
|
||||
int can_reverse(struct node *n)
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue