1 |
|
|
/** |
2 |
|
|
* @file can_bus.cpp |
3 |
|
|
* @author Felix Widmaier (felix.widmaier@tuebingen.mpg.de) |
4 |
|
|
* @author Manuel Wuthrich (manuel.wuthrich@gmail.com) |
5 |
|
|
* @author Maximilien Naveau (maximilien.naveau@gmail.com) |
6 |
|
|
* @brief This file defines classes that allow communication with a Can network. |
7 |
|
|
* @version 0.1 |
8 |
|
|
* @date 2018-11-23 |
9 |
|
|
* |
10 |
|
|
* @copyright Copyright (c) 2018 |
11 |
|
|
* |
12 |
|
|
*/ |
13 |
|
|
|
14 |
|
|
#include <sstream> |
15 |
|
|
|
16 |
|
|
#include <blmc_drivers/devices/can_bus.hpp> |
17 |
|
|
|
18 |
|
|
namespace blmc_drivers |
19 |
|
|
{ |
20 |
|
|
|
21 |
|
|
CanBus::CanBus(const std::string& can_interface_name, |
22 |
|
|
const size_t& history_length) |
23 |
|
|
{ |
24 |
|
|
input_ = std::make_shared<CanframeTimeseries>(history_length); |
25 |
|
|
sent_input_ = std::make_shared<CanframeTimeseries>(history_length); |
26 |
|
|
output_ = std::make_shared<CanframeTimeseries>(history_length); |
27 |
|
|
name_ = can_interface_name; |
28 |
|
|
|
29 |
|
|
can_connection_.set(setup_can(can_interface_name, 0)); |
30 |
|
|
|
31 |
|
|
is_loop_active_ = true; |
32 |
|
|
rt_thread_.create_realtime_thread(&CanBus::loop, this); |
33 |
|
|
} |
34 |
|
|
|
35 |
|
|
CanBus::~CanBus() |
36 |
|
|
{ |
37 |
|
|
is_loop_active_ = false; |
38 |
|
|
rt_thread_.join(); |
39 |
|
|
osi::close_can_device(can_connection_.get().socket); |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
void CanBus::send_if_input_changed() |
43 |
|
|
{ |
44 |
|
|
if(input_->has_changed_since_tag()) |
45 |
|
|
{ |
46 |
|
|
CanframeTimeseries::Index |
47 |
|
|
timeindex_to_send = input_->newest_timeindex(); |
48 |
|
|
CanBusFrame frame_to_send = (*input_)[timeindex_to_send]; |
49 |
|
|
input_->tag(timeindex_to_send); |
50 |
|
|
sent_input_->append(frame_to_send); |
51 |
|
|
|
52 |
|
|
send_frame(frame_to_send); |
53 |
|
|
} |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
void CanBus::loop() |
57 |
|
|
{ |
58 |
|
|
while (is_loop_active_) |
59 |
|
|
{ |
60 |
|
|
output_->append(receive_frame()); |
61 |
|
|
} |
62 |
|
|
} |
63 |
|
|
|
64 |
|
|
void CanBus::send_frame(const CanBusFrame& unstamped_can_frame) |
65 |
|
|
{ |
66 |
|
|
// get address --------------------------------------------------------- |
67 |
|
|
int socket = can_connection_.get().socket; |
68 |
|
|
struct sockaddr_can address = can_connection_.get().send_addr; |
69 |
|
|
|
70 |
|
|
// put data into can frame --------------------------------------------- |
71 |
|
|
can_frame_t can_frame; |
72 |
|
|
can_frame.can_id = unstamped_can_frame.id; |
73 |
|
|
can_frame.can_dlc = unstamped_can_frame.dlc; |
74 |
|
|
|
75 |
|
|
memcpy(can_frame.data, unstamped_can_frame.data.begin(), |
76 |
|
|
unstamped_can_frame.dlc); |
77 |
|
|
|
78 |
|
|
// send ---------------------------------------------------------------- |
79 |
|
|
osi::send_to_can_device(socket, |
80 |
|
|
(void *)&can_frame, |
81 |
|
|
sizeof(can_frame_t), |
82 |
|
|
0, |
83 |
|
|
(struct sockaddr *)&address, |
84 |
|
|
sizeof(address)); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
CanBusFrame CanBus::receive_frame() |
88 |
|
|
{ |
89 |
|
|
int socket = can_connection_.get().socket; |
90 |
|
|
|
91 |
|
|
// data we want to obtain ---------------------------------------------- |
92 |
|
|
can_frame_t can_frame; |
93 |
|
|
nanosecs_abs_t timestamp; |
94 |
|
|
struct sockaddr_can message_address; |
95 |
|
|
|
96 |
|
|
// setup message such that data can be received to variables above ----- |
97 |
|
|
struct iovec input_output_vector; |
98 |
|
|
input_output_vector.iov_base = (void *)&can_frame; |
99 |
|
|
input_output_vector.iov_len = sizeof(can_frame_t); |
100 |
|
|
|
101 |
|
|
struct msghdr message_header; |
102 |
|
|
message_header.msg_iov = &input_output_vector; |
103 |
|
|
message_header.msg_iovlen = 1; |
104 |
|
|
message_header.msg_name = (void *)&message_address; |
105 |
|
|
message_header.msg_namelen = sizeof(struct sockaddr_can); |
106 |
|
|
message_header.msg_control = (void *)×tamp; |
107 |
|
|
message_header.msg_controllen = sizeof(nanosecs_abs_t); |
108 |
|
|
|
109 |
|
|
// receive message from can bus ---------------------------------------- |
110 |
|
|
osi::receive_message_from_can_device(socket, &message_header, 0); |
111 |
|
|
|
112 |
|
|
// process received data and put into felix widmaier's format ---------- |
113 |
|
|
if (message_header.msg_controllen == 0) |
114 |
|
|
{ |
115 |
|
|
// No timestamp for this frame available. Make sure we dont get |
116 |
|
|
// garbage. |
117 |
|
|
timestamp = 0; |
118 |
|
|
} |
119 |
|
|
|
120 |
|
|
CanBusFrame out_frame; |
121 |
|
|
out_frame.id = can_frame.can_id; |
122 |
|
|
out_frame.dlc = can_frame.can_dlc; |
123 |
|
|
for(size_t i = 0; i < can_frame.can_dlc; i++) |
124 |
|
|
{ |
125 |
|
|
out_frame.data[i] = can_frame.data[i]; |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
return out_frame; |
129 |
|
|
} |
130 |
|
|
|
131 |
|
|
CanBusConnection CanBus::setup_can(std::string name, uint32_t err_mask) |
132 |
|
|
{ |
133 |
|
|
int socket_number; |
134 |
|
|
sockaddr_can recv_addr; |
135 |
|
|
sockaddr_can send_addr; |
136 |
|
|
struct ifreq ifr; |
137 |
|
|
|
138 |
|
|
int ret; |
139 |
|
|
|
140 |
|
|
ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW); |
141 |
|
|
if (ret < 0) { |
142 |
|
|
rt_fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret)); |
143 |
|
|
rt_printf("Couldn't setup CAN connection. Exit."); |
144 |
|
|
exit(-1); |
145 |
|
|
} |
146 |
|
|
socket_number = ret; |
147 |
|
|
|
148 |
|
|
strncpy(ifr.ifr_name, name.c_str(), IFNAMSIZ); |
149 |
|
|
ret = rt_dev_ioctl(socket_number, SIOCGIFINDEX, &ifr); |
150 |
|
|
if (ret < 0) |
151 |
|
|
{ |
152 |
|
|
rt_fprintf(stderr, "rt_dev_ioctl GET_IFINDEX: %s\n", |
153 |
|
|
strerror(-ret)); |
154 |
|
|
osi::close_can_device(socket_number); |
155 |
|
|
rt_printf("Couldn't setup CAN connection. Exit."); |
156 |
|
|
exit(-1); |
157 |
|
|
} |
158 |
|
|
|
159 |
|
|
// Set error mask |
160 |
|
|
if (err_mask) { |
161 |
|
|
ret = rt_dev_setsockopt(socket_number, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, |
162 |
|
|
&err_mask, sizeof(err_mask)); |
163 |
|
|
if (ret < 0) |
164 |
|
|
{ |
165 |
|
|
rt_fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret)); |
166 |
|
|
osi::close_can_device(socket_number); |
167 |
|
|
rt_printf("Couldn't setup CAN connection. Exit."); |
168 |
|
|
exit(-1); |
169 |
|
|
} |
170 |
|
|
} |
171 |
|
|
|
172 |
|
|
// Bind to socket |
173 |
|
|
recv_addr.can_family = AF_CAN; |
174 |
|
|
recv_addr.can_ifindex = ifr.ifr_ifindex; |
175 |
|
|
ret = rt_dev_bind(socket_number, (struct sockaddr *)&recv_addr, |
176 |
|
|
sizeof(struct sockaddr_can)); |
177 |
|
|
if (ret < 0) |
178 |
|
|
{ |
179 |
|
|
rt_fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret)); |
180 |
|
|
osi::close_can_device(socket_number); |
181 |
|
|
rt_printf("Couldn't setup CAN connection. Exit."); |
182 |
|
|
exit(-1); |
183 |
|
|
} |
184 |
|
|
|
185 |
|
|
#ifdef __XENO__ |
186 |
|
|
// Enable timestamps for frames |
187 |
|
|
ret = rt_dev_ioctl(socket, |
188 |
|
|
RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS); |
189 |
|
|
if (ret) { |
190 |
|
|
rt_fprintf(stderr, "rt_dev_ioctl TAKE_TIMESTAMP: %s\n", |
191 |
|
|
strerror(-ret)); |
192 |
|
|
osi::close_can_device(socket); |
193 |
|
|
rt_printf("Couldn't setup CAN connection. Exit."); |
194 |
|
|
exit(-1); |
195 |
|
|
} |
196 |
|
|
#elif defined RT_PREEMPT |
197 |
|
|
// TODO: Need to support timestamps. |
198 |
|
|
#endif |
199 |
|
|
|
200 |
|
|
// TODO why the memset? |
201 |
|
|
memset(&send_addr, 0, sizeof(send_addr)); |
202 |
|
|
send_addr.can_family = AF_CAN; |
203 |
|
|
send_addr.can_ifindex = ifr.ifr_ifindex; |
204 |
|
|
|
205 |
|
|
CanBusConnection can_connection; |
206 |
|
|
can_connection.send_addr = send_addr; |
207 |
|
|
can_connection.socket = socket_number; |
208 |
|
|
|
209 |
|
|
return can_connection; |
210 |
|
|
} |
211 |
|
|
|
212 |
✓✗✓✗
|
3 |
} // namespace blmc_drivers |
213 |
|
|
|