Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : * AP_CANManager - board specific configuration for CAN interface
17 : */
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include <AP_Common/AP_Common.h>
21 : #include "AP_CANManager.h"
22 :
23 : #if HAL_CANMANAGER_ENABLED
24 :
25 : #include <AP_BoardConfig/AP_BoardConfig.h>
26 : #include <AP_Vehicle/AP_Vehicle_Type.h>
27 : #include <AP_DroneCAN/AP_DroneCAN.h>
28 : #include <AP_KDECAN/AP_KDECAN.h>
29 : #include <AP_SerialManager/AP_SerialManager.h>
30 : #include <AP_PiccoloCAN/AP_PiccoloCAN.h>
31 : #include <AP_EFI/AP_EFI_NWPMU.h>
32 : #include <GCS_MAVLink/GCS.h>
33 : #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
34 : #include <AP_HAL_Linux/CANSocketIface.h>
35 : #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
36 : #include <AP_HAL_SITL/CANSocketIface.h>
37 : #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
38 : #include <hal.h>
39 : #include <AP_HAL_ChibiOS/CANIface.h>
40 : #endif
41 :
42 : #include <AP_Common/sorting.h>
43 : #include <AP_Logger/AP_Logger.h>
44 :
45 : extern const AP_HAL::HAL& hal;
46 :
47 : // table of user settable parameters
48 : const AP_Param::GroupInfo AP_CANManager::var_info[] = {
49 :
50 : #if HAL_NUM_CAN_IFACES > 0
51 : // @Group: P1_
52 : // @Path: ../AP_CANManager/AP_CANIfaceParams.cpp
53 : AP_SUBGROUPINFO(_interfaces[0], "P1_", 1, AP_CANManager, AP_CANManager::CANIface_Params),
54 : #endif
55 :
56 : #if HAL_NUM_CAN_IFACES > 1
57 : // @Group: P2_
58 : // @Path: ../AP_CANManager/AP_CANIfaceParams.cpp
59 : AP_SUBGROUPINFO(_interfaces[1], "P2_", 2, AP_CANManager, AP_CANManager::CANIface_Params),
60 : #endif
61 :
62 : #if HAL_NUM_CAN_IFACES > 2
63 : // @Group: P3_
64 : // @Path: ../AP_CANManager/AP_CANIfaceParams.cpp
65 : AP_SUBGROUPINFO(_interfaces[2], "P3_", 3, AP_CANManager, AP_CANManager::CANIface_Params),
66 : #endif
67 :
68 : #if HAL_MAX_CAN_PROTOCOL_DRIVERS > 0
69 : // @Group: D1_
70 : // @Path: ../AP_CANManager/AP_CANManager_CANDriver_Params.cpp
71 : AP_SUBGROUPINFO(_drv_param[0], "D1_", 4, AP_CANManager, AP_CANManager::CANDriver_Params),
72 : #endif
73 :
74 : #if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1
75 : // @Group: D2_
76 : // @Path: ../AP_CANManager/AP_CANManager_CANDriver_Params.cpp
77 : AP_SUBGROUPINFO(_drv_param[1], "D2_", 5, AP_CANManager, AP_CANManager::CANDriver_Params),
78 : #endif
79 :
80 : #if HAL_MAX_CAN_PROTOCOL_DRIVERS > 2
81 : // @Group: D3_
82 : // @Path: ../AP_CANManager/AP_CANManager_CANDriver_Params.cpp
83 : AP_SUBGROUPINFO(_drv_param[2], "D3_", 6, AP_CANManager, AP_CANManager::CANDriver_Params),
84 : #endif
85 :
86 : #if AP_CAN_SLCAN_ENABLED
87 : // @Group: SLCAN_
88 : // @Path: ../AP_CANManager/AP_SLCANIface.cpp
89 : AP_SUBGROUPINFO(_slcan_interface, "SLCAN_", 7, AP_CANManager, SLCAN::CANIface),
90 : #endif
91 :
92 : // @Param: LOGLEVEL
93 : // @DisplayName: Loglevel
94 : // @Description: Loglevel for recording initialisation and debug information from CAN Interface
95 : // @Range: 0 4
96 : // @Values: 0: Log None, 1: Log Error, 2: Log Warning and below, 3: Log Info and below, 4: Log Everything
97 : // @User: Advanced
98 : AP_GROUPINFO("LOGLEVEL", 8, AP_CANManager, _loglevel, AP_CANManager::LOG_NONE),
99 :
100 : AP_GROUPEND
101 : };
102 :
103 : AP_CANManager *AP_CANManager::_singleton;
104 :
105 10490 : AP_CANManager::AP_CANManager()
106 : {
107 2098 : AP_Param::setup_object_defaults(this, var_info);
108 2098 : if (_singleton != nullptr) {
109 0 : AP_HAL::panic("AP_CANManager must be singleton");
110 : }
111 2098 : _singleton = this;
112 2098 : }
113 :
114 : #if !AP_TEST_DRONECAN_DRIVERS
115 2094 : void AP_CANManager::init()
116 : {
117 2094 : WITH_SEMAPHORE(_sem);
118 :
119 : // we need to mutate the HAL to install new CAN interfaces
120 2094 : AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable();
121 :
122 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
123 2094 : if (AP::sitl() == nullptr) {
124 0 : AP_HAL::panic("CANManager: SITL not initialised!");
125 : }
126 : #endif
127 :
128 : #if AP_CAN_SLCAN_ENABLED
129 : //Reset all SLCAN related params that needs resetting at boot
130 : _slcan_interface.reset_params();
131 : #endif
132 :
133 2094 : AP_CAN::Protocol drv_type[HAL_MAX_CAN_PROTOCOL_DRIVERS] = {};
134 : // loop through interfaces and allocate and initialise Iface,
135 : // Also allocate Driver objects, and add interfaces to them
136 6282 : for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
137 : // Get associated Driver to the interface
138 4188 : uint8_t drv_num = _interfaces[i]._driver_number;
139 4188 : if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
140 4183 : continue;
141 : }
142 5 : drv_num--;
143 :
144 5 : if (hal_mutable.can[i] == nullptr) {
145 : // So if this interface is not allocated allocate it here,
146 : // also pass the index of the CANBus
147 5 : hal_mutable.can[i] = NEW_NOTHROW HAL_CANIface(i);
148 : }
149 :
150 : // Initialise the interface we just allocated
151 5 : if (hal_mutable.can[i] == nullptr) {
152 0 : continue;
153 : }
154 5 : AP_HAL::CANIface* iface = hal_mutable.can[i];
155 :
156 : // Find the driver type that we need to allocate and register this interface with
157 5 : drv_type[drv_num] = (AP_CAN::Protocol) _drv_param[drv_num]._driver_type.get();
158 5 : bool can_initialised = false;
159 : // Check if this interface need hooking up to slcan passthrough
160 : // instead of a driver
161 : #if AP_CAN_SLCAN_ENABLED
162 : if (_slcan_interface.init_passthrough(i)) {
163 : // we have slcan bridge setup pass that on as can iface
164 : can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000);
165 : iface = &_slcan_interface;
166 : } else {
167 : #else
168 : if (true) {
169 : #endif
170 5 : can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000);
171 : }
172 :
173 5 : if (!can_initialised) {
174 0 : continue;
175 : }
176 :
177 :
178 5 : if (_drivers[drv_num] != nullptr) {
179 : //We already initialised the driver just add interface and move on
180 0 : _drivers[drv_num]->add_interface(iface);
181 0 : continue;
182 : }
183 :
184 5 : if (_num_drivers >= HAL_MAX_CAN_PROTOCOL_DRIVERS) {
185 : // We are exceeding number of drivers,
186 : // this can't be happening time to panic
187 0 : AP_BoardConfig::config_error("Max number of CAN Drivers exceeded\n\r");
188 : }
189 :
190 : // Allocate the set type of Driver
191 5 : switch (drv_type[drv_num]) {
192 : #if HAL_ENABLE_DRONECAN_DRIVERS
193 5 : case AP_CAN::Protocol::DroneCAN:
194 5 : _drivers[drv_num] = _drv_param[drv_num]._uavcan = NEW_NOTHROW AP_DroneCAN(drv_num);
195 :
196 5 : if (_drivers[drv_num] == nullptr) {
197 0 : AP_BoardConfig::allocation_error("uavcan %d", i + 1);
198 : continue;
199 : }
200 :
201 5 : AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[drv_num], AP_DroneCAN::var_info);
202 5 : break;
203 : #endif
204 : #if AP_PICCOLOCAN_ENABLED
205 0 : case AP_CAN::Protocol::PiccoloCAN:
206 0 : _drivers[drv_num] = _drv_param[drv_num]._piccolocan = NEW_NOTHROW AP_PiccoloCAN;
207 :
208 0 : if (_drivers[drv_num] == nullptr) {
209 0 : AP_BoardConfig::allocation_error("PiccoloCAN %d", drv_num + 1);
210 : continue;
211 : }
212 :
213 0 : AP_Param::load_object_from_eeprom((AP_PiccoloCAN*)_drivers[drv_num], AP_PiccoloCAN::var_info);
214 0 : break;
215 : #endif
216 0 : default:
217 0 : continue;
218 : }
219 :
220 5 : _num_drivers++;
221 :
222 : // Hook this interface to the selected Driver Type
223 5 : _drivers[drv_num]->add_interface(iface);
224 : }
225 :
226 6282 : for (uint8_t drv_num = 0; drv_num < HAL_MAX_CAN_PROTOCOL_DRIVERS; drv_num++) {
227 : //initialise all the Drivers
228 :
229 : // Cache the driver type, initialized or not, so we can detect that it is in the params at boot via get_driver_type().
230 : // This allows drivers that are initialized by CANSensor instead of CANManager to know if they should init or not
231 4188 : _driver_type_cache[drv_num] = drv_type[drv_num];
232 :
233 4188 : if (_drivers[drv_num] == nullptr) {
234 4183 : continue;
235 : }
236 :
237 5 : _drivers[drv_num]->init(drv_num);
238 : }
239 :
240 : #if AP_CAN_LOGGING_ENABLED
241 2094 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::check_logging_enable, void));
242 : #endif
243 2094 : }
244 : #else
245 : void AP_CANManager::init()
246 : {
247 : WITH_SEMAPHORE(_sem);
248 : for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
249 : if ((AP_CAN::Protocol) _drv_param[i]._driver_type.get() == AP_CAN::Protocol::DroneCAN) {
250 : _drivers[i] = _drv_param[i]._uavcan = NEW_NOTHROW AP_DroneCAN(i);
251 :
252 : if (_drivers[i] == nullptr) {
253 : AP_BoardConfig::allocation_error("uavcan %d", i + 1);
254 : continue;
255 : }
256 :
257 : AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[i], AP_DroneCAN::var_info);
258 : _drivers[i]->init(i);
259 : _driver_type_cache[i] = (AP_CAN::Protocol) _drv_param[i]._driver_type.get();
260 : }
261 : }
262 : }
263 : #endif
264 :
265 : /*
266 : register a new CAN driver
267 : */
268 0 : bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver)
269 : {
270 0 : WITH_SEMAPHORE(_sem);
271 :
272 : // we need to mutate the HAL to install new CAN interfaces
273 0 : AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable();
274 :
275 0 : for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
276 0 : uint8_t drv_num = _interfaces[i]._driver_number;
277 0 : if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
278 0 : continue;
279 : }
280 : // from 1 based to 0 based
281 0 : drv_num--;
282 :
283 0 : if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type.get()) {
284 0 : continue;
285 : }
286 0 : if (_drivers[drv_num] != nullptr) {
287 0 : continue;
288 : }
289 0 : if (_num_drivers >= HAL_MAX_CAN_PROTOCOL_DRIVERS) {
290 0 : continue;
291 : }
292 :
293 0 : if (hal_mutable.can[i] == nullptr) {
294 : // if this interface is not allocated allocate it here,
295 : // also pass the index of the CANBus
296 0 : hal_mutable.can[i] = NEW_NOTHROW HAL_CANIface(i);
297 : }
298 :
299 : // Initialise the interface we just allocated
300 0 : if (hal_mutable.can[i] == nullptr) {
301 0 : continue;
302 : }
303 0 : AP_HAL::CANIface* iface = hal_mutable.can[i];
304 :
305 0 : _drivers[drv_num] = driver;
306 0 : _drivers[drv_num]->add_interface(iface);
307 :
308 0 : _drivers[drv_num]->init(drv_num);
309 0 : _driver_type_cache[drv_num] = dtype;
310 :
311 0 : _num_drivers++;
312 :
313 0 : return true;
314 : }
315 0 : return false;
316 0 : }
317 :
318 : // register a new auxillary sensor driver for 11 bit address frames
319 0 : bool AP_CANManager::register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index)
320 : {
321 0 : WITH_SEMAPHORE(_sem);
322 :
323 0 : for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
324 0 : uint8_t drv_num = _interfaces[i]._driver_number;
325 0 : if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
326 0 : continue;
327 : }
328 : // from 1 based to 0 based
329 0 : drv_num--;
330 :
331 0 : if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type_11bit.get()) {
332 0 : continue;
333 : }
334 0 : if (_drivers[drv_num] != nullptr &&
335 0 : _drivers[drv_num]->add_11bit_driver(sensor)) {
336 0 : driver_index = drv_num;
337 0 : return true;
338 : }
339 : }
340 0 : return false;
341 :
342 0 : }
343 :
344 : #if AP_CAN_LOGGING_ENABLED
345 : /*
346 : handler for CAN frames for frame logging
347 : */
348 0 : void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame, AP_HAL::CANIface::CanIOFlags flags)
349 : {
350 : #if HAL_CANFD_SUPPORTED
351 0 : if (frame.canfd) {
352 0 : struct log_CAFD pkt {
353 : LOG_PACKET_HEADER_INIT(LOG_CAFD_MSG),
354 0 : time_us : AP_HAL::micros64(),
355 : bus : bus,
356 0 : id : frame.id,
357 0 : dlc : frame.dlc
358 0 : };
359 0 : memcpy(pkt.data, frame.data, frame.dlcToDataLength(frame.dlc));
360 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
361 0 : return;
362 : }
363 : #endif
364 0 : struct log_CANF pkt {
365 : LOG_PACKET_HEADER_INIT(LOG_CANF_MSG),
366 0 : time_us : AP_HAL::micros64(),
367 : bus : bus,
368 0 : id : frame.id,
369 0 : dlc : frame.dlc
370 0 : };
371 0 : memcpy(pkt.data, frame.data, frame.dlc);
372 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
373 : }
374 :
375 : /*
376 : see if we need to enable/disable the CAN logging callback
377 : */
378 243142750 : void AP_CANManager::check_logging_enable(void)
379 : {
380 729428250 : for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
381 486285500 : const bool enabled = _interfaces[i].option_is_set(CANIface_Params::Options::LOG_ALL_FRAMES);
382 486285500 : uint8_t &logging_id = _interfaces[i].logging_id;
383 486285500 : auto *can = hal.can[i];
384 486285500 : if (can == nullptr) {
385 485504127 : continue;
386 : }
387 781373 : if (enabled && logging_id == 0) {
388 0 : can->register_frame_callback(
389 : FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &, AP_HAL::CANIface::CanIOFlags),
390 : logging_id);
391 781373 : } else if (!enabled && logging_id != 0) {
392 0 : can->unregister_frame_callback(logging_id);
393 : }
394 : }
395 243142750 : }
396 :
397 : #endif // AP_CAN_LOGGING_ENABLED
398 :
399 125953352 : AP_CANManager& AP::can()
400 : {
401 125953352 : return *AP_CANManager::get_singleton();
402 : }
403 :
404 : #endif
405 :
|
