Apollo 10.0
自动驾驶开放平台
esd_can_client.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
24
27
28namespace apollo {
29namespace drivers {
30namespace canbus {
31namespace can {
32
34
35bool EsdCanClient::Init(const CANCardParameter &parameter) {
36 if (!parameter.has_channel_id()) {
37 AERROR << "Init CAN failed: parameter does not have channel id. The "
38 "parameter is "
39 << parameter.DebugString();
40 return false;
41 }
42 port_ = parameter.channel_id();
43
44 int num_ports = parameter.num_ports();
45
46 if (port_ > num_ports || port_ < 0) {
47 AERROR << "Can port number [" << port_ << "] is out of range [0, "
48 << num_ports << ") !";
49 return false;
50 }
51
52 return true;
53}
54
56 if (dev_handler_) {
57 Stop();
58 }
59}
60
62 if (is_started_) {
63 return ErrorCode::OK;
64 }
65
66 // open device
67 // guss net is the device minor number, if one card is 0,1
68 // if more than one card, when install driver u can specify the minior id
69 // int32_t ret = canOpen(net, pCtx->mode, txbufsize, rxbufsize, 0, 0,
70 // &dev_handler_);
71 uint32_t mode = 0;
72
73 if (FLAGS_esd_can_extended_frame) {
74 mode = NTCAN_MODE_NO_RTR;
75 }
76
77 // mode |= NTCAN_MODE_NO_RTR;
78 int32_t ret = canOpen(port_, mode, NTCAN_MAX_TX_QUEUESIZE,
79 NTCAN_MAX_RX_QUEUESIZE, 5, 5, &dev_handler_);
80 if (ret != NTCAN_SUCCESS) {
81 AERROR << "open device error code [" << ret << "]: " << GetErrorString(ret);
82 return ErrorCode::CAN_CLIENT_ERROR_BASE;
83 }
84
85 // init config and state
86 // After a CAN handle is created with canOpen() the CAN-ID filter is
87 // cleared
88 // (no CAN messages
89 // will pass the filter). To receive a CAN message with a certain CAN-ID
90 // or an
91 // NTCAN-Event with
92 // a certain Event-ID it is required to enable this ID in the handle
93 // filter as
94 // otherwise a
95 // received message or event is discarded by the driver for this handle.
96 // 1. set receive message_id filter, ie white list
97
98 int32_t id_count = 0x800;
99 ret = canIdRegionAdd(dev_handler_, 0, &id_count);
100
101 if (FLAGS_esd_can_extended_frame) {
102 id_count = 0x1FFFFFFE;
103 ret = canIdRegionAdd(dev_handler_, 0x20000000, &id_count);
104 }
105
106 if (ret != NTCAN_SUCCESS) {
107 AERROR << "add receive msg id filter error code: " << ret << ", "
108 << GetErrorString(ret);
109 return ErrorCode::CAN_CLIENT_ERROR_BASE;
110 }
111
112 // 2. set baudrate to 500k
113 ret = canSetBaudrate(dev_handler_, NTCAN_BAUD_500);
114 if (ret != NTCAN_SUCCESS) {
115 AERROR << "set baudrate error code: " << ret << ", " << GetErrorString(ret);
116 return ErrorCode::CAN_CLIENT_ERROR_BASE;
117 }
118
119 is_started_ = true;
120 return ErrorCode::OK;
121}
122
124 if (is_started_) {
125 is_started_ = false;
126 int32_t ret = canClose(dev_handler_);
127 if (ret != NTCAN_SUCCESS) {
128 AERROR << "close error code:" << ret << ", " << GetErrorString(ret);
129 } else {
130 AINFO << "close esd can ok. port:" << port_;
131 }
132 }
133}
134
135// Synchronous transmission of CAN messages
136ErrorCode EsdCanClient::Send(const std::vector<CanFrame> &frames,
137 int32_t *const frame_num) {
138 CHECK_NOTNULL(frame_num);
139 CHECK_EQ(frames.size(), static_cast<size_t>(*frame_num));
140
141 if (!is_started_) {
142 AERROR << "Esd can client has not been initiated! Please init first!";
143 return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
144 }
145 for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) {
146 send_frames_[i].id = frames[i].id;
147 send_frames_[i].len = frames[i].len;
148 std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len);
149 }
150
151 // Synchronous transmission of CAN messages
152 int32_t ret = canWrite(dev_handler_, send_frames_, frame_num, nullptr);
153 if (ret != NTCAN_SUCCESS) {
154 AERROR << "send message failed, error code: " << ret << ", "
155 << GetErrorString(ret);
156 return ErrorCode::CAN_CLIENT_ERROR_BASE;
157 }
158 return ErrorCode::OK;
159}
160
161// buf size must be 8 bytes, every time, we receive only one frame
162ErrorCode EsdCanClient::Receive(std::vector<CanFrame> *const frames,
163 int32_t *const frame_num) {
164 if (!is_started_) {
165 AERROR << "Esd can client is not init! Please init first!";
166 return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
167 }
168
169 if (*frame_num > MAX_CAN_RECV_FRAME_LEN || *frame_num < 0) {
170 AERROR << "recv can frame num not in range[0, " << MAX_CAN_RECV_FRAME_LEN
171 << "], frame_num:" << *frame_num;
172 // TODO(Authors): check the difference of returning frame_num/error_code
173 return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
174 }
175
176 const int32_t ret = canRead(dev_handler_, recv_frames_, frame_num, nullptr);
177 // rx timeout not log
178 if (ret == NTCAN_RX_TIMEOUT) {
179 return ErrorCode::OK;
180 }
181 if (ret != NTCAN_SUCCESS) {
182 AERROR << "receive message failed, error code: " << ret << ", "
183 << GetErrorString(ret);
184 return ErrorCode::CAN_CLIENT_ERROR_BASE;
185 }
186
187 for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
188 CanFrame cf;
189 cf.id = recv_frames_[i].id;
190 cf.len = recv_frames_[i].len;
191 std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].len);
192 frames->push_back(cf);
193 }
194
195 return ErrorCode::OK;
196}
197
198/************************************************************************/
199/************************************************************************/
200/* Function: GetErrorString() */
201/* Return ASCII representation of NTCAN return code */
202/************************************************************************/
203/************************************************************************/
204const int32_t ERROR_BUF_SIZE = 200;
205std::string EsdCanClient::GetErrorString(const NTCAN_RESULT ntstatus) {
206 struct ERR2STR {
207 NTCAN_RESULT ntstatus;
208 const char *str;
209 };
210
211 int8_t str_buf[ERROR_BUF_SIZE];
212
213 static const struct ERR2STR err2str[] = {
214 {NTCAN_SUCCESS, "NTCAN_SUCCESS"},
215 {NTCAN_RX_TIMEOUT, "NTCAN_RX_TIMEOUT"},
216 {NTCAN_TX_TIMEOUT, "NTCAN_TX_TIMEOUT"},
217 {NTCAN_TX_ERROR, "NTCAN_TX_ERROR"},
218 {NTCAN_CONTR_OFF_BUS, "NTCAN_CONTR_OFF_BUS"},
219 {NTCAN_CONTR_BUSY, "NTCAN_CONTR_BUSY"},
220 {NTCAN_CONTR_WARN, "NTCAN_CONTR_WARN"},
221 {NTCAN_NO_ID_ENABLED, "NTCAN_NO_ID_ENABLED"},
222 {NTCAN_ID_ALREADY_ENABLED, "NTCAN_ID_ALREADY_ENABLED"},
223 {NTCAN_ID_NOT_ENABLED, "NTCAN_ID_NOT_ENABLED"},
224 {NTCAN_INVALID_FIRMWARE, "NTCAN_INVALID_FIRMWARE"},
225 {NTCAN_MESSAGE_LOST, "NTCAN_MESSAGE_LOST"},
226 {NTCAN_INVALID_PARAMETER, "NTCAN_INVALID_PARAMETER"},
227 {NTCAN_INVALID_HANDLE, "NTCAN_INVALID_HANDLE"},
228 {NTCAN_NET_NOT_FOUND, "NTCAN_NET_NOT_FOUND"},
229#ifdef NTCAN_IO_INCOMPLETE
230 {NTCAN_IO_INCOMPLETE, "NTCAN_IO_INCOMPLETE"},
231#endif
232#ifdef NTCAN_IO_PENDING
233 {NTCAN_IO_PENDING, "NTCAN_IO_PENDING"},
234#endif
235#ifdef NTCAN_INVALID_HARDWARE
236 {NTCAN_INVALID_HARDWARE, "NTCAN_INVALID_HARDWARE"},
237#endif
238#ifdef NTCAN_PENDING_WRITE
239 {NTCAN_PENDING_WRITE, "NTCAN_PENDING_WRITE"},
240#endif
241#ifdef NTCAN_PENDING_READ
242 {NTCAN_PENDING_READ, "NTCAN_PENDING_READ"},
243#endif
244#ifdef NTCAN_INVALID_DRIVER
245 {NTCAN_INVALID_DRIVER, "NTCAN_INVALID_DRIVER"},
246#endif
247#ifdef NTCAN_OPERATION_ABORTED
248 {NTCAN_OPERATION_ABORTED, "NTCAN_OPERATION_ABORTED"},
249#endif
250#ifdef NTCAN_WRONG_DEVICE_STATE
251 {NTCAN_WRONG_DEVICE_STATE, "NTCAN_WRONG_DEVICE_STATE"},
252#endif
253 {NTCAN_INSUFFICIENT_RESOURCES, "NTCAN_INSUFFICIENT_RESOURCES"},
254#ifdef NTCAN_HANDLE_FORCED_CLOSE
255 {NTCAN_HANDLE_FORCED_CLOSE, "NTCAN_HANDLE_FORCED_CLOSE"},
256#endif
257#ifdef NTCAN_NOT_IMPLEMENTED
258 {NTCAN_NOT_IMPLEMENTED, "NTCAN_NOT_IMPLEMENTED"},
259#endif
260#ifdef NTCAN_NOT_SUPPORTED
261 {NTCAN_NOT_SUPPORTED, "NTCAN_NOT_SUPPORTED"},
262#endif
263#ifdef NTCAN_SOCK_CONN_TIMEOUT
264 {NTCAN_SOCK_CONN_TIMEOUT, "NTCAN_SOCK_CONN_TIMEOUT"},
265#endif
266#ifdef NTCAN_SOCK_CMD_TIMEOUT
267 {NTCAN_SOCK_CMD_TIMEOUT, "NTCAN_SOCK_CMD_TIMEOUT"},
268#endif
269#ifdef NTCAN_SOCK_HOST_NOT_FOUND
270 {NTCAN_SOCK_HOST_NOT_FOUND, "NTCAN_SOCK_HOST_NOT_FOUND"},
271#endif
272#ifdef NTCAN_CONTR_ERR_PASSIVE
273 {NTCAN_CONTR_ERR_PASSIVE, "NTCAN_CONTR_ERR_PASSIVE"},
274#endif
275#ifdef NTCAN_ERROR_NO_BAUDRATE
276 {NTCAN_ERROR_NO_BAUDRATE, "NTCAN_ERROR_NO_BAUDRATE"},
277#endif
278#ifdef NTCAN_ERROR_LOM
279 {NTCAN_ERROR_LOM, "NTCAN_ERROR_LOM"},
280#endif
281 {(NTCAN_RESULT)0xffffffff, "NTCAN_UNKNOWN"} /* stop-mark */
282 };
283
284 const struct ERR2STR *es = err2str;
285
286 do {
287 if (es->ntstatus == ntstatus) {
288 break;
289 }
290 es++;
291 } while ((uint32_t)es->ntstatus != 0xffffffff);
292
293#ifdef NTCAN_ERROR_FORMAT_LONG
294 {
295 NTCAN_RESULT res;
296 char sz_error_text[60];
297
298 res = canFormatError(ntstatus, NTCAN_ERROR_FORMAT_LONG, sz_error_text,
299 static_cast<uint32_t>(sizeof(sz_error_text) - 1));
300 if (NTCAN_SUCCESS == res) {
301 snprintf(reinterpret_cast<char *>(str_buf), ERROR_BUF_SIZE, "%s - %s",
302 es->str, sz_error_text);
303 } else {
304 snprintf(reinterpret_cast<char *>(str_buf), ERROR_BUF_SIZE, "%s(0x%08x)",
305 es->str, ntstatus);
306 }
307 }
308#else
309 snprintf(reinterpret_cast<char *>(str_buf), ERROR_BUF_SIZE, "%s(0x%08x)",
310 es->str, ntstatus);
311#endif /* of NTCAN_ERROR_FORMAT_LONG */
312
313 return std::string((const char *)(str_buf));
314}
315
316} // namespace can
317} // namespace canbus
318} // namespace drivers
319} // namespace apollo
Defines the Byte class.
bool is_started_
The CAN client is started.
Definition can_client.h:165
void Stop() override
Stop the ESD CAN client.
apollo::common::ErrorCode Receive(std::vector< CanFrame > *const frames, int32_t *const frame_num) override
Receive messages
bool Init(const CANCardParameter &parameter) override
Initialize the ESD CAN client by specified CAN card parameters.
apollo::common::ErrorCode Send(const std::vector< CanFrame > &frames, int32_t *const frame_num) override
Send messages
apollo::common::ErrorCode Start() override
Start the ESD CAN client.
std::string GetErrorString(const int32_t status) override
Get the error string.
Defines the EsdCanClient class which inherits CanClient.
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
const int32_t MAX_CAN_RECV_FRAME_LEN
const int32_t MAX_CAN_SEND_FRAME_LEN
class register implement
Definition arena_queue.h:37
The class which defines the information to send and receive.
Definition can_client.h:48
uint8_t len
Message length
Definition can_client.h:52
uint8_t data[8]
Message content
Definition can_client.h:54