mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
can: align code to VILLASnode coding-style
This commit is contained in:
parent
06fc6ed6ed
commit
33eef47f97
2 changed files with 188 additions and 151 deletions
|
|
@ -38,23 +38,23 @@ struct node;
|
|||
union signal_data;
|
||||
|
||||
struct can_signal {
|
||||
uint32_t id;
|
||||
int offset;
|
||||
int size;
|
||||
uint32_t id;
|
||||
int offset;
|
||||
int size;
|
||||
};
|
||||
|
||||
struct can {
|
||||
/* Settings */
|
||||
char *interface_name;
|
||||
double *sample_rate;
|
||||
struct can_signal *in;
|
||||
struct can_signal *out;
|
||||
/* Settings */
|
||||
char *interface_name;
|
||||
double *sample_rate;
|
||||
struct can_signal *in;
|
||||
struct can_signal *out;
|
||||
|
||||
/* States */
|
||||
int socket;
|
||||
union signal_data *sample_buf;
|
||||
size_t sample_buf_num;
|
||||
struct timespec start_time;
|
||||
/* States */
|
||||
int socket;
|
||||
union signal_data *sample_buf;
|
||||
size_t sample_buf_num;
|
||||
struct timespec start_time;
|
||||
};
|
||||
|
||||
/** @see node_type::init */
|
||||
|
|
|
|||
|
|
@ -67,12 +67,14 @@ int can_destroy(struct node *n)
|
|||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
free(c->interface_name);
|
||||
if (c->socket != 0) {
|
||||
|
||||
if (c->socket != 0)
|
||||
close(c->socket);
|
||||
}
|
||||
|
||||
free(c->sample_buf);
|
||||
free(c->in);
|
||||
free(c->out);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -87,11 +89,11 @@ int can_parse_signal(json_t *json, struct vlist *node_signals, struct can_signal
|
|||
json_error_t err;
|
||||
|
||||
ret = json_unpack_ex(json, &err, 0, "{ s?: s, s?: i, s?: i, s?: i }",
|
||||
"name", &name,
|
||||
"can_id", &can_id,
|
||||
"can_size", &can_size,
|
||||
"can_offset", &can_offset
|
||||
);
|
||||
"name", &name,
|
||||
"can_id", &can_id,
|
||||
"can_size", &can_size,
|
||||
"can_offset", &can_offset
|
||||
);
|
||||
|
||||
if (ret) {
|
||||
jerror(&err, "Failed to parse signal configuration for can");
|
||||
|
|
@ -115,11 +117,11 @@ int can_parse_signal(json_t *json, struct vlist *node_signals, struct can_signal
|
|||
can_signals[signal_index].offset = can_offset;
|
||||
ret = 0;
|
||||
goto out;
|
||||
} else {
|
||||
error("Signal configuration inconsistency detected: Signal with index %zu (\"%s\") does not match can_signal \"%s\"\n", signal_index, sig->name, name);
|
||||
}
|
||||
out:
|
||||
return ret;
|
||||
else
|
||||
error("Signal configuration inconsistency detected: Signal with index %zu (\"%s\") does not match can_signal \"%s\"\n", signal_index, sig->name, name);
|
||||
|
||||
out: return ret;
|
||||
}
|
||||
|
||||
int can_parse(struct node *n, json_t *cfg)
|
||||
|
|
@ -136,13 +138,13 @@ int can_parse(struct node *n, json_t *cfg)
|
|||
c->out = nullptr;
|
||||
|
||||
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
|
||||
);
|
||||
"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;
|
||||
|
|
@ -167,24 +169,27 @@ int can_parse(struct node *n, json_t *cfg)
|
|||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
json_array_foreach(json_out_signals, i, json_signal) {
|
||||
if (can_parse_signal(json_signal, &n->out.signals, c->out, i) != 0) {
|
||||
error("at signal %zu in node %s.",i , node_name(n));
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
out:
|
||||
if (ret != 0) {
|
||||
|
||||
out: if (ret != 0) {
|
||||
free(c->in);
|
||||
free(c->out);
|
||||
c->in = nullptr;
|
||||
c->out = nullptr;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
char *can_print(struct node *n)
|
||||
char * can_print(struct node *n)
|
||||
{
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
|
|
@ -206,9 +211,9 @@ int can_check(struct node *n)
|
|||
int can_prepare(struct node *n)
|
||||
{
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
c->sample_buf = (union signal_data*) calloc(
|
||||
vlist_length(&n->in.signals),
|
||||
sizeof(union signal_data));
|
||||
|
||||
c->sample_buf = (union signal_data*) calloc(vlist_length(&n->in.signals), sizeof(union signal_data));
|
||||
|
||||
return (c->sample_buf != 0 ? 0 : 1);
|
||||
}
|
||||
|
||||
|
|
@ -221,7 +226,7 @@ int can_start(struct node *n)
|
|||
struct can *c = (struct can *) n->_vd;
|
||||
c->start_time = time_now();
|
||||
|
||||
if((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
if ((c->socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
error("Error while opening CAN socket");
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -235,13 +240,14 @@ int can_start(struct node *n)
|
|||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
if(bind(c->socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
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);
|
||||
goto out;
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
out:
|
||||
return ret;
|
||||
|
||||
out: return ret;
|
||||
}
|
||||
|
||||
int can_stop(struct node *n)
|
||||
|
|
@ -262,57 +268,67 @@ int can_conv_to_raw(union signal_data* sig, struct signal *from, void* to, int s
|
|||
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;
|
||||
case SignalType::BOOLEAN:
|
||||
*(uint8_t*)to = sig->b;
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
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);
|
||||
|
||||
fail: error("unsupported conversion to %s from raw (%p, %d)", signal_type_to_str(from->type), to, size);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -322,55 +338,64 @@ int can_conv_from_raw(union signal_data* sig, void* from, int size, struct signa
|
|||
error("signal size cannot be larger than 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;
|
||||
case SignalType::BOOLEAN:
|
||||
sig->b = (bool)*(uint8_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;
|
||||
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:
|
||||
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:
|
||||
goto fail;
|
||||
}
|
||||
case SignalType::COMPLEX:
|
||||
if (size != 8)
|
||||
goto fail;
|
||||
|
||||
sig->z = std::complex<float>(*(float*)from, *((float*)from+1));
|
||||
return 0;
|
||||
|
||||
default:
|
||||
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:
|
||||
goto fail;
|
||||
}
|
||||
case SignalType::COMPLEX:
|
||||
if (size != 8) {
|
||||
goto fail;
|
||||
}
|
||||
sig->z = std::complex<float>(*(float*)from, *((float*)from+1));
|
||||
return 0;
|
||||
default:
|
||||
goto fail;
|
||||
}
|
||||
fail:
|
||||
error("unsupported conversion from %s to raw (%p, %d)",
|
||||
signal_type_to_str(to->type), from, size);
|
||||
fail:
|
||||
error("unsupported conversion from %s to raw (%p, %d)", signal_type_to_str(to->type), from, size);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -392,17 +417,17 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele
|
|||
error("CAN read() returned -1. Is the CAN interface up?");
|
||||
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));
|
||||
error("CAN read() error. read() returned %d bytes but expected %zu", nbytes, sizeof(struct can_frame));
|
||||
goto out;
|
||||
}
|
||||
|
||||
debug(0,"received can message: (id:%d, len:%u, data: 0x%x:0x%x)",
|
||||
frame.can_id,
|
||||
frame.can_dlc,
|
||||
((uint32_t*)&frame.data)[0],
|
||||
((uint32_t*)&frame.data)[1]);
|
||||
frame.can_id,
|
||||
frame.can_dlc,
|
||||
((uint32_t*)&frame.data)[0],
|
||||
((uint32_t*)&frame.data)[1]);
|
||||
|
||||
if (ioctl(c->socket, SIOCGSTAMP, &tv) == 0) {
|
||||
TIMEVAL_TO_TIMESPEC(&tv, &smps[nread]->ts.received);
|
||||
|
|
@ -417,15 +442,19 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele
|
|||
(struct signal*) vlist_at(&n->in.signals, i)) != 0) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
c->sample_buf_num++;
|
||||
found_id = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found_id) {
|
||||
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;
|
||||
|
|
@ -433,13 +462,15 @@ int can_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rele
|
|||
c->sample_buf_num = 0;
|
||||
smps[nread]->flags |= (int) SampleFlags::HAS_DATA;
|
||||
ret = 1;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
smps[nread]->length = 0;
|
||||
ret = 0;
|
||||
}
|
||||
out:
|
||||
/* Set signals, because other VILLASnode parts expect us to */
|
||||
|
||||
out: /* Set signals, because other VILLASnode parts expect us to */
|
||||
smps[nread]->signals = &n->in.signals;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -454,31 +485,34 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel
|
|||
|
||||
assert(cnt >= 1 && smps[0]->capacity >= 1);
|
||||
|
||||
frame = (struct can_frame*) calloc(sizeof(struct can_frame),
|
||||
vlist_length(&(n->out.signals)));
|
||||
frame = (struct can_frame*) calloc(sizeof(struct can_frame), vlist_length(&(n->out.signals)));
|
||||
|
||||
for (nwrite=0; nwrite < cnt; nwrite++) {
|
||||
for (size_t i=0; i < vlist_length(&(n->out.signals)); i++) {
|
||||
if (c->out[i].offset != 0) { /* frame is shared */
|
||||
if (c->out[i].offset != 0) /* frame is shared */
|
||||
continue;
|
||||
}
|
||||
|
||||
frame[fsize].can_dlc = c->out[i].size;
|
||||
frame[fsize].can_id = c->out[i].id;
|
||||
|
||||
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++) {
|
||||
if (c->out[i].offset == 0) { /* frame already stored */
|
||||
continue;
|
||||
}
|
||||
|
||||
for (size_t j=0; j < fsize; j++) {
|
||||
if (c->out[i].id != frame[j].can_id) {
|
||||
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],
|
||||
|
|
@ -488,25 +522,27 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t j=0; j < fsize; j++) {
|
||||
debug(0,"writing can message: (id:%d, dlc:%u, data:0x%x:0x%x)", frame[j].can_id,
|
||||
frame[j].can_dlc,
|
||||
((uint32_t*)&frame[j].data)[0],
|
||||
((uint32_t*)&frame[j].data)[1]);
|
||||
debug(0,"writing can message: (id:%d, dlc:%u, data:0x%x:0x%x)",
|
||||
frame[j].can_id,
|
||||
frame[j].can_dlc,
|
||||
((uint32_t*)&frame[j].data)[0],
|
||||
((uint32_t*)&frame[j].data)[1]
|
||||
);
|
||||
|
||||
if ((nbytes = write(c->socket, &frame[j], sizeof(struct can_frame))) == -1) {
|
||||
error("CAN write() returned -1. Is the CAN interface up?");
|
||||
return nwrite;
|
||||
}
|
||||
|
||||
if ((unsigned)nbytes != sizeof(struct can_frame)) {
|
||||
error("CAN write() error. write() returned %d bytes but expected %zu",
|
||||
nbytes, sizeof(struct can_frame));
|
||||
nbytes, sizeof(struct can_frame));
|
||||
return nwrite;
|
||||
}
|
||||
}
|
||||
}
|
||||
return nwrite;
|
||||
}
|
||||
|
||||
return nwrite;
|
||||
}
|
||||
|
|
@ -514,6 +550,7 @@ int can_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *rel
|
|||
int can_poll_fds(struct node *n, int fds[])
|
||||
{
|
||||
struct can *c = (struct can *) n->_vd;
|
||||
|
||||
fds[0] = c->socket;
|
||||
|
||||
return 1; /* The number of file descriptors which have been set in fds */
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue