Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_CANManager: add CANManager library

This commit is contained in:
Siddharth Purohit 2020-05-31 17:13:28 +05:30 committed by Andrew Tridgell
parent 904981a0c3
commit 979b0b82d0
11 changed files with 2736 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_CANManager/AP_CANDriver.cpp Normal file
View File

@ -0,0 +1,59 @@
/*
* This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_CANManager.h"
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_ToshibaCAN/AP_ToshibaCAN.h>
#include <AP_PiccoloCAN/AP_PiccoloCAN.h>
#include "AP_CANTester.h"
#include <AP_KDECAN/AP_KDECAN.h>
// table of user settable CAN bus parameters
const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
// @Param: PROTOCOL
// @DisplayName: Enable use of specific protocol over virtual driver
// @Description: Enabling this option starts selected protocol that will use this virtual driver
// @Values{Copter,Plane,Sub}: 0:Disabled,1:UAVCAN,2:KDECAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester
// @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, AP_CANManager::Driver_Type_UAVCAN),
// @Group: UC_
// @Path: ../AP_UAVCAN/AP_UAVCAN.cpp
AP_SUBGROUPPTR(_uavcan, "UC_", 2, AP_CANManager::CANDriver_Params, AP_UAVCAN),
#if (APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub))
// @Group: KDE_
// @Path: ../AP_KDECAN/AP_KDECAN.cpp
AP_SUBGROUPPTR(_kdecan, "KDE_", 3, AP_CANManager::CANDriver_Params, AP_KDECAN),
#endif
#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES
// @Group: TST_
// @Path: ../AP_CANManager/AP_CANTester.cpp
AP_SUBGROUPPTR(_testcan, "TST_", 4, AP_CANManager::CANDriver_Params, CANTester),
#endif
AP_GROUPEND
};
#endif

38
libraries/AP_CANManager/AP_CANDriver.h Normal file
View File

@ -0,0 +1,38 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Siddharth Bharat Purohit
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_CANManager;
class AP_CANDriver
{
public:
friend class AP_CANManager;
// init method for protocol drivers, specify driver index and if filters
// are to be enabled
virtual void init(uint8_t driver_index, bool enable_filters) = 0;
// link protocol drivers with interfaces by adding reference to CANIface
virtual bool add_interface(AP_HAL::CANIface* can_iface) = 0;
};

42
libraries/AP_CANManager/AP_CANIfaceParams.cpp Normal file
View File

@ -0,0 +1,42 @@
/*
* This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_CANManager.h"
// table of user settable CAN bus parameters
const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = {
// @Param: DRIVER
// @DisplayName: Index of virtual driver to be used with physical CAN interface
// @Description: Enabling this option enables use of CAN buses.
// @Values: 0:Disabled,1:First driver,2:Second driver
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("DRIVER", 1, AP_CANManager::CANIface_Params, _driver_number, HAL_CAN_DRIVER_DEFAULT, AP_PARAM_FLAG_ENABLE),
// @Param: BITRATE
// @DisplayName: Bitrate of CAN interface
// @Description: Bit rate can be set up to from 10000 to 1000000
// @Range: 10000 1000000
// @User: Advanced
AP_GROUPINFO("BITRATE", 2, AP_CANManager::CANIface_Params, _bitrate, 1000000),
// Index 3 occupied by @Param: DEBUG
AP_GROUPEND
};
#endif

317
libraries/AP_CANManager/AP_CANManager.cpp Normal file
View File

@ -0,0 +1,317 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_CANManager - board specific configuration for CAN interface
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_CANManager.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_KDECAN/AP_KDECAN.h>
#include <AP_ToshibaCAN/AP_ToshibaCAN.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_PiccoloCAN/AP_PiccoloCAN.h>
#include "AP_CANTester.h"
#include <GCS_MAVLink/GCS_MAVLink.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL_Linux/CANSocketIface.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_HAL_SITL/CANSocketIface.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/CANIface.h>
#endif
#define LOG_TAG "CANMGR"
#define LOG_BUFFER_SIZE 1024
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AP_CANManager::var_info[] = {
#if HAL_NUM_CAN_IFACES > 0
// @Group: P1_
// @Path: ../AP_CANManager/AP_CANIfaceParams.cpp
AP_SUBGROUPINFO(_interfaces[0], "P1_", 1, AP_CANManager, AP_CANManager::CANIface_Params),
#endif
#if HAL_NUM_CAN_IFACES > 1
// @Group: P2_
// @Path: ../AP_CANManager/AP_CANIfaceParams.cpp
AP_SUBGROUPINFO(_interfaces[1], "P2_", 2, AP_CANManager, AP_CANManager::CANIface_Params),
#endif
#if HAL_NUM_CAN_IFACES > 2
// @Group: P3_
// @Path: ../AP_CANManager/AP_CANIfaceParams.cpp
AP_SUBGROUPINFO(_interfaces[2], "P3_", 3, AP_CANManager, AP_CANManager::CANIface_Params),
#endif
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 0
// @Group: D1_
// @Path: ../AP_CANManager/AP_CANDriver.cpp
AP_SUBGROUPINFO(_drv_param[0], "D1_", 4, AP_CANManager, AP_CANManager::CANDriver_Params),
#endif
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1
// @Group: D2_
// @Path: ../AP_CANManager/AP_CANDriver.cpp
AP_SUBGROUPINFO(_drv_param[1], "D2_", 5, AP_CANManager, AP_CANManager::CANDriver_Params),
#endif
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 2
// @Group: D3_
// @Path: ../AP_CANManager/AP_CANDriver.cpp
AP_SUBGROUPINFO(_drv_param[2], "D3_", 6, AP_CANManager, AP_CANManager::CANDriver_Params),
#endif
// @Group: SLCAN_
// @Path: ../AP_CANManager/AP_SLCANIface.cpp
AP_SUBGROUPINFO(_slcan_interface, "SLCAN_", 7, AP_CANManager, SLCAN::CANIface),
// @Param: LOGLEVEL
// @DisplayName: Loglevel
// @Description: Loglevel for recording initialisation and debug information from CAN Interface
// @Range: 0 4
// @Values: 0: Log None, 1: Log Error, 2: Log Warning and below, 3: Log Info and below, 4: Log Everything
// @User: Advanced
AP_GROUPINFO("LOGLEVEL", 8, AP_CANManager, _loglevel, AP_CANManager::LOG_NONE),
AP_GROUPEND
};
AP_CANManager *AP_CANManager::_singleton;
AP_CANManager::AP_CANManager()
{
AP_Param::setup_object_defaults(this, var_info);
if (_singleton != nullptr) {
AP_HAL::panic("AP_CANManager must be singleton");
}
_singleton = this;
}
void AP_CANManager::init()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (AP::sitl() != nullptr) {
if (AP::sitl()->speedup > 1) {
log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "CAN is not supported under speedup.");
return;
}
} else {
AP_HAL::panic("CANManager: SITL not initialised!");
}
#endif
// We only allocate log buffer only when under debug
if (_loglevel != AP_CANManager::LOG_NONE) {
_log_buf = new char[LOG_BUFFER_SIZE];
_log_pos = 0;
}
//Reset all SLCAN related params that needs resetting at boot
_slcan_interface.reset_params();
// loop through interfaces and allocate and initialise Iface,
// Also allocate Driver objects, and add interfaces to them
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
// Get associated Driver to the interface
uint8_t drv_num = _interfaces[i]._driver_number;
if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
continue;
}
drv_num--;
if (hal.can[i] == nullptr) {
// So if this interface is not allocated allocate it here,
// also pass the index of the CANBus
const_cast <AP_HAL::HAL&> (hal).can[i] = new HAL_CANIface(i);
}
// Initialise the interface we just allocated
if (hal.can[i] != nullptr) {
if (!hal.can[i]->init(_interfaces[i]._bitrate, AP_HAL::CANIface::NormalMode)) {
log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "Failed to initialise CAN Interface %d", i+1);
continue;
}
} else {
continue;
}
AP_HAL::CANIface* iface = hal.can[i];
// Check if this interface need hooking up to slcan passthrough
// instead of a driver
if (_slcan_interface.init_passthrough(i)) {
// we have slcan bridge setup pass that on as can iface
iface = &_slcan_interface;
}
// Find the driver type that we need to allocate and register this interface with
Driver_Type drv_type = _driver_type_cache[drv_num] = (Driver_Type) _drv_param[drv_num]._driver_type.get();
log_text(AP_CANManager::LOG_INFO, LOG_TAG, "CAN Interface %d initialized well", i + 1);
if (_drivers[drv_num] != nullptr) {
//We already initialised the driver just add interface and move on
log_text(AP_CANManager::LOG_INFO, LOG_TAG, "Adding Interface %d to Driver %d", i + 1, drv_num + 1);
_drivers[drv_num]->add_interface(iface);
continue;
}
_num_drivers++;
if (_num_drivers > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
// We are exceeding number of drivers,
// this can't be happening time to panic
AP_BoardConfig::config_error("Max number of CAN Drivers exceeded\n\r");
}
// Allocate the set type of Driver
if (drv_type == Driver_Type_UAVCAN) {
_drivers[drv_num] = _drv_param[drv_num]._uavcan = new AP_UAVCAN;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::config_error("Failed to allocate uavcan %d\n\r", i + 1);
continue;
}
AP_Param::load_object_from_eeprom((AP_UAVCAN*)_drivers[drv_num], AP_UAVCAN::var_info);
} else if (drv_type == Driver_Type_KDECAN) {
#if (APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub))
// To be replaced with macro saying if KDECAN library is included
_drivers[drv_num] = _drv_param[drv_num]._kdecan = new AP_KDECAN;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::config_error("Failed to allocate KDECAN %d\n\r", drv_num + 1);
continue;
}
AP_Param::load_object_from_eeprom((AP_KDECAN*)_drivers[drv_num], AP_KDECAN::var_info);
#endif
} else if (drv_type == Driver_Type_ToshibaCAN) {
_drivers[drv_num] = new AP_ToshibaCAN;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::config_error("Failed to allocate ToshibaCAN %d\n\r", drv_num + 1);
continue;
}
} else if (drv_type == Driver_Type_PiccoloCAN) {
#if HAL_PICCOLO_CAN_ENABLE
_drivers[drv_num] = new AP_PiccoloCAN;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::config_error("Failed to allocate PiccoloCAN %d\n\r", drv_num + 1);
continue;
}
#endif
} else if (drv_type == Driver_Type_CANTester) {
#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES
_drivers[drv_num] = _drv_param[drv_num]._testcan = new CANTester;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::config_error("Failed to allocate CANTester %d\n\r", drv_num + 1);
continue;
}
AP_Param::load_object_from_eeprom((CANTester*)_drivers[drv_num], CANTester::var_info);
#endif
} else {
continue;
}
// Hook this interface to the selected Driver Type
_drivers[drv_num]->add_interface(iface);
log_text(AP_CANManager::LOG_INFO, LOG_TAG, "Adding Interface %d to Driver %d", i + 1, drv_num + 1);
}
for (uint8_t drv_num = 0; drv_num < HAL_MAX_CAN_PROTOCOL_DRIVERS; drv_num++) {
//initialise all the Drivers
if (_drivers[drv_num] == nullptr) {
continue;
}
if (_slcan_interface.get_drv_num() != drv_num) {
_drivers[drv_num]->init(drv_num, true);
} else {
_drivers[drv_num]->init(drv_num, false);
}
}
}
// Method used by CAN related library methods to report status and debug info
// The result of this method can be accessed via ftp get @SYS/can_log.txt
void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...)
{
if (_log_buf == nullptr) {
return;
}
if (loglevel > _loglevel) {
return;
}
if ((LOG_BUFFER_SIZE - _log_pos) < (10 + strlen(tag) + strlen(fmt))) {
// reset log pos
_log_pos = 0;
}
//Tag Log Message
switch (loglevel) {
case AP_CANManager::LOG_DEBUG :
_log_pos += hal.util->snprintf(&_log_buf[_log_pos], LOG_BUFFER_SIZE - _log_pos, "\n%s DEBUG :", tag);
break;
case AP_CANManager::LOG_INFO :
_log_pos += hal.util->snprintf(&_log_buf[_log_pos], LOG_BUFFER_SIZE - _log_pos, "\n%s INFO :", tag);
break;
case AP_CANManager::LOG_WARNING :
_log_pos += hal.util->snprintf(&_log_buf[_log_pos], LOG_BUFFER_SIZE - _log_pos, "\n%s WARN :", tag);
break;
case AP_CANManager::LOG_ERROR :
_log_pos += hal.util->snprintf(&_log_buf[_log_pos], LOG_BUFFER_SIZE - _log_pos, "\n%s ERROR :", tag);
break;
default :
return;
}
va_list arg_list;
va_start(arg_list, fmt);
_log_pos += hal.util->vsnprintf(&_log_buf[_log_pos], LOG_BUFFER_SIZE - _log_pos, fmt, arg_list);
va_end(arg_list);
}
// log retrieve method used by file sys method to report can log
uint32_t AP_CANManager::log_retrieve(char* data, uint32_t max_size) const
{
if (_log_buf == nullptr) {
gcs().send_text(MAV_SEVERITY_ERROR, "Log buffer not available");
return 0;
}
uint32_t read_len = MIN(max_size, _log_pos);
memcpy(data, _log_buf, read_len);
return read_len;
}
AP_CANManager& AP::can()
{
return *AP_CANManager::get_singleton();
}
#endif

150
libraries/AP_CANManager/AP_CANManager.h Normal file
View File

@ -0,0 +1,150 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Siddharth Bharat Purohit
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_Param/AP_Param.h>
#include "AP_SLCANIface.h"
#include "AP_CANDriver.h"
class AP_CANManager
{
public:
AP_CANManager();
/* Do not allow copies */
AP_CANManager(const AP_CANManager &other) = delete;
AP_CANManager &operator=(const AP_CANManager&) = delete;
static AP_CANManager* get_singleton()
{
return _singleton;
}
enum LogLevel {
LOG_NONE,
LOG_ERROR,
LOG_WARNING,
LOG_INFO,
LOG_DEBUG,
};
enum Driver_Type : uint8_t {
Driver_Type_None = 0,
Driver_Type_UAVCAN = 1,
Driver_Type_KDECAN = 2,
Driver_Type_ToshibaCAN = 3,
Driver_Type_PiccoloCAN = 4,
Driver_Type_CANTester = 5,
};
void init(void);
// returns number of active CAN Drivers
uint8_t get_num_drivers(void) const
{
return _num_drivers;
}
// return driver for index i
AP_CANDriver* get_driver(uint8_t i) const
{
if (i < HAL_NUM_CAN_IFACES) {
return _drivers[i];
}
return nullptr;
}
// Method to log status and debug information for review while debugging
void log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...);
uint32_t log_retrieve(char* data, uint32_t max_size) const;
// return driver type index i
Driver_Type get_driver_type(uint8_t i) const
{
if (i < HAL_NUM_CAN_IFACES) {
return _driver_type_cache[i];
}
return Driver_Type_None;
}
static const struct AP_Param::GroupInfo var_info[];
private:
// Parameter interface for CANIfaces
class CANIface_Params
{
friend class AP_CANManager;
public:
CANIface_Params()
{
AP_Param::setup_object_defaults(this, var_info);
}
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Int8 _driver_number;
AP_Int32 _bitrate;
};
//Parameter Interface for CANDrivers
class CANDriver_Params
{
friend class AP_CANManager;
public:
CANDriver_Params()
{
AP_Param::setup_object_defaults(this, var_info);
}
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Int8 _driver_type;
AP_CANDriver* _testcan;
AP_CANDriver* _uavcan;
AP_CANDriver* _kdecan;
};
CANIface_Params _interfaces[HAL_NUM_CAN_IFACES];
AP_CANDriver* _drivers[HAL_MAX_CAN_PROTOCOL_DRIVERS] {};
CANDriver_Params _drv_param[HAL_MAX_CAN_PROTOCOL_DRIVERS];
Driver_Type _driver_type_cache[HAL_MAX_CAN_PROTOCOL_DRIVERS];
AP_Int8 _loglevel;
uint8_t _num_drivers;
SLCAN::CANIface _slcan_interface;
static AP_CANManager *_singleton;
char* _log_buf;
uint32_t _log_pos;
};
namespace AP
{
AP_CANManager& can();
}
#endif

889
libraries/AP_CANManager/AP_CANTester.cpp Normal file
View File

@ -0,0 +1,889 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Siddharth Bharat Purohit
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_CANManager.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_CANTester.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <stdio.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <AP_ToshibaCAN/AP_ToshibaCAN.h>
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_CANTester_KDECAN.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo CANTester::var_info[] = {
// @Param: ID
// @DisplayName: CAN Test Index
// @Description: Selects the Index of Test that needs to be run recursively, this value gets reset to 0 at boot.
// @Range: 0 4
// @Values: 0:TEST_NONE, 1:TEST_LOOPBACK,2:TEST_BUSOFF_RECOVERY,3:TEST_UAVCAN_DNA,4:TEST_TOSHIBA_CAN, 5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC
// @User: Advanced
AP_GROUPINFO("ID", 1, CANTester, _test_id, 0),
// @Param: LPR8
// @DisplayName: CANTester LoopRate
// @Description: Selects the Looprate of Test methods
// @Units: us
// @User: Advanced
AP_GROUPINFO("LPR8", 2, CANTester, _loop_rate, 10000),
AP_GROUPEND
};
#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "CANTester", fmt, #args); } while (0)
bool CANTester::add_interface(AP_HAL::CANIface* can_iface)
{
if (_num_ifaces >= HAL_NUM_CAN_IFACES) {
debug_can(AP_CANManager::LOG_ERROR, "Max Number of CanIfaces exceeded");
return false;
}
_can_ifaces[_num_ifaces] = can_iface;
if (_can_ifaces[_num_ifaces] == nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "CAN driver not found");
return false;
}
if (!_can_ifaces[_num_ifaces]->is_initialized()) {
debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized");
return false;
}
_num_ifaces++;
return true;
}
void CANTester::init(uint8_t driver_index, bool enable_filters)
{
_driver_index = driver_index;
// Reset Test mask
_test_id.set_and_save(0);
debug_can(AP_CANManager::LOG_DEBUG, "starting init");
if (_initialized) {
debug_can(AP_CANManager::LOG_ERROR, "already initialized");
return;
}
if (_can_ifaces[0] == nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "Interface not found");
return;
}
// kick start tester thread
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&CANTester::main_thread, void), "can_tester", 4096, AP_HAL::Scheduler::PRIORITY_CAN, 1)) {
debug_can(AP_CANManager::LOG_ERROR, "couldn't create thread");
return;
}
_initialized = true;
debug_can(AP_CANManager::LOG_DEBUG, "init done");
return;
}
// write frame on CAN bus
bool CANTester::write_frame(uint8_t iface, AP_HAL::CANFrame &out_frame, uint64_t timeout)
{
if (!_can_ifaces[iface]->set_event_handle(&_event_handle)) {
debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
return false;
}
// wait for space in buffer to send command
bool read_select = false;
bool write_select = true;
out_frame.id += iface; // distinguish between multiple ifaces
bool ret = _can_ifaces[iface]->select(read_select, write_select, &out_frame, AP_HAL::native_micros64() + timeout);
if (!ret || !write_select) {
return false;
}
uint64_t deadline = AP_HAL::native_micros64() + 2000000;
// hal.console->printf("%x TDEAD: %lu\n", out_frame.id, deadline);
// send frame and return success
return (_can_ifaces[iface]->send(out_frame, deadline, AP_HAL::CANIface::AbortOnError) == 1);
}
// read frame on CAN bus, returns true on success
bool CANTester::read_frame(uint8_t iface, AP_HAL::CANFrame &recv_frame, uint64_t timeout, AP_HAL::CANIface::CanIOFlags &flags)
{
if (!_can_ifaces[iface]->set_event_handle(&_event_handle)) {
debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
return false;
}
// wait for space in buffer to read
bool read_select = true;
bool write_select = false;
bool ret = _can_ifaces[iface]->select(read_select, write_select, nullptr, AP_HAL::native_micros64() + timeout);
if (!ret || !read_select) {
// return false if no data is available to read
return false;
}
uint64_t time;
// read frame and return success
return (_can_ifaces[iface]->receive(recv_frame, time, flags) == 1);
}
void CANTester::main_thread()
{
while (true) {
switch (_test_id) {
case CANTester::TEST_LOOPBACK:
if (_can_ifaces[1] != nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running Loopback Test*******");
if (test_loopback(_loop_rate)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Loopback Test Pass*******");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********Loopback Test Fail*******");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Can't do Loopback Test with single iface");
}
break;
case CANTester::TEST_BUSOFF_RECOVERY:
if (_can_ifaces[1] != nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running Busoff Recovery Test********");
if (test_busoff_recovery()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Busoff Recovery Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********Busoff Recovery Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Can't do Busoff Recovery Test with single iface");
}
break;
case CANTester::TEST_UAVCAN_DNA:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN DNA Test********");
if (test_uavcan_dna()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_DNA_TEST");
}
break;
case CANTester::TEST_TOSHIBA_CAN:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running Toshiba CAN Test********");
if (test_toshiba_can()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Toshiba CAN Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********Toshiba CAN Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for TEST_TOSHIBA_CAN");
}
break;
case CANTester::TEST_KDE_CAN:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running KDE CAN Test********");
if (test_kdecan()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********KDE CAN Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********KDE CAN Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for TEST_KDE_CAN");
}
break;
case CANTester::TEST_UAVCAN_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN ESC Test********");
if (test_uavcan_esc()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_ESC_TEST");
}
break;
default:
break;
}
for (uint8_t i = 0; i < 2; i++) {
if (_can_ifaces[i] != nullptr) {
_can_ifaces[i]->flush_tx();
}
}
hal.scheduler->delay(5000);
for (uint8_t i = 0; i < 2; i++) {
if (_can_ifaces[i] != nullptr) {
_can_ifaces[i]->clear_rx();
}
}
}
}
/*****************************************
* Loopback Test *
* ***************************************/
#define NUM_LOOPBACK_RUNS 1000UL
#define LOOPBACK_MAGIC 0x34567819UL
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define NUM_MAX_TX_FRAMES 1
#else
#define NUM_MAX_TX_FRAMES 64 // arbitrary value to max out the buffers
#endif
bool CANTester::test_loopback(uint32_t loop_rate)
{
AP_HAL::CANFrame frame;
AP_HAL::CANIface::CanIOFlags flags;
uint32_t num_loops = NUM_LOOPBACK_RUNS;
memset(&_loopback_stats[0], 0, sizeof(_loopback_stats[0]));
memset(&_loopback_stats[1], 0, sizeof(_loopback_stats[1]));
while (num_loops--) {
for (uint8_t i = 0; i < 2; i++) {
// Write as many frames as we can on an iface
for (uint32_t tx_frames = 0; tx_frames < NUM_MAX_TX_FRAMES; tx_frames++) {
create_loopback_frame(_loopback_stats[i], frame);
if (write_frame(i, frame, 0)) {
_loopback_stats[i].tx_seq++;
_loopback_stats[i].num_tx++;
} else {
break;
}
}
// Also read as much as we can from the second iface
while (true) {
reset_frame(frame);
if (read_frame((i+1)%2, frame, 0, flags)) {
if (frame.id != ((13 | AP_HAL::CANFrame::FlagEFF) + i)) {
continue;
}
check_loopback_frame(_loopback_stats[i], frame);
_loopback_stats[i].num_rx++;
} else {
break;
}
}
}
hal.scheduler->delay_microseconds(loop_rate);
}
_can_ifaces[0]->flush_tx();
hal.scheduler->delay_microseconds(1000);
_can_ifaces[1]->flush_tx();
hal.scheduler->delay_microseconds(1000);
// flush the rx data still buffered in the interface
for (uint8_t i = 0; i < 2; i++) {
while (true) {
reset_frame(frame);
if (read_frame((i+1)%2, frame, 0, flags)) {
if (frame.id != ((13 | AP_HAL::CANFrame::FlagEFF) + i)) {
continue;
}
check_loopback_frame(_loopback_stats[i], frame);
_loopback_stats[i].num_flushed_rx++;
} else {
break;
}
}
}
for (uint8_t i = 0; i < _num_ifaces; i++) {
hal.console->printf("Loopback Test Results %d->%d:\n", i, (i+1)%2);
hal.console->printf("num_tx: %lu, failed_tx: %lu\n",
(long unsigned int)_loopback_stats[i].num_tx, (long unsigned int)_loopback_stats[i].failed_tx);
hal.console->printf("num_rx: %lu, flushed_rx: %lu, bad_rx_data: %lu, bad_rx_seq: %lu\n",
(long unsigned int)_loopback_stats[i].num_rx, (long unsigned int)_loopback_stats[i].num_flushed_rx,
(long unsigned int)_loopback_stats[i].bad_rx_data, (long unsigned int)_loopback_stats[i].bad_rx_seq);
if (_loopback_stats[i].num_rx < 0.9f * _loopback_stats[i].num_tx ||
_loopback_stats[i].failed_tx || _loopback_stats[i].bad_rx_seq ||
_loopback_stats[i].bad_rx_data) {
return false;
}
}
return true;
}
void CANTester::reset_frame(AP_HAL::CANFrame& frame)
{
frame.id = 0;
memset(frame.data, 0, sizeof(frame.data));
frame.dlc = 0;
}
void CANTester::create_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
frame.id = 13 | AP_HAL::CANFrame::FlagEFF;
frame.dlc = AP_HAL::CANFrame::MaxDataLen;
memcpy(frame.data, &stats.tx_seq, sizeof(stats.tx_seq));
uint32_t loopback_magic = LOOPBACK_MAGIC;
memcpy(&frame.data[4], &loopback_magic, sizeof(loopback_magic));
}
void CANTester::check_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
if (frame.dlc != AP_HAL::CANFrame::MaxDataLen) {
stats.bad_rx_data++;
}
uint32_t loopback_magic = LOOPBACK_MAGIC;
if (memcmp(&frame.data[4], &loopback_magic, sizeof(loopback_magic)) != 0) {
stats.bad_rx_data++;
return;
}
uint16_t curr_seq;
memcpy(&curr_seq, frame.data, sizeof(curr_seq));
if (stats.next_valid_seq != curr_seq) {
stats.bad_rx_seq++;
}
stats.next_valid_seq = curr_seq + 1;
}
/*****************************************
* Busoff Recovery Test *
* ***************************************/
bool CANTester::test_busoff_recovery()
{
uint32_t num_busoff_runs = 100000;
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
AP_HAL::CANFrame bo_frame;
bo_frame.id = (12 | AP_HAL::CANFrame::FlagEFF);
memset(bo_frame.data, 0xA, sizeof(bo_frame.data));
bo_frame.dlc = AP_HAL::CANFrame::MaxDataLen;
bool bus_off_detected = false;
// Bus Fault can be introduced by shorting CANH and CANL
gcs().send_text(MAV_SEVERITY_ERROR, "Introduce Bus Off Fault on the bus.");
while (num_busoff_runs--) {
if (bus_off_detected) {
break;
}
//Spam the bus with same frame
_can_ifaces[0]->send(bo_frame, AP_HAL::native_micros64()+1000, 0);
_can_ifaces[1]->receive(bo_frame, timestamp, flags);
_can_ifaces[1]->send(bo_frame, AP_HAL::native_micros64()+1000, 0);
_can_ifaces[0]->receive(bo_frame, timestamp, flags);
bus_off_detected = _can_ifaces[0]->is_busoff() || _can_ifaces[1]->is_busoff();
hal.scheduler->delay_microseconds(50);
}
if (!bus_off_detected) {
gcs().send_text(MAV_SEVERITY_ERROR, "BusOff not detected on the bus");
return false;
}
gcs().send_text(MAV_SEVERITY_ERROR, "BusOff detected remove Fault.");
hal.scheduler->delay(1000);
gcs().send_text(MAV_SEVERITY_ERROR, "Running Loopback test.");
//Send Dummy Frames to clear the error
_can_ifaces[0]->send(bo_frame, AP_HAL::native_micros64(), 0);
bo_frame.id += 1;
_can_ifaces[1]->send(bo_frame, AP_HAL::native_micros64(), 0);
//Clear the CAN bus Rx Buffer
hal.scheduler->delay(1000);
_can_ifaces[0]->clear_rx();
_can_ifaces[1]->clear_rx();
return test_loopback(_loop_rate);
}
/*****************************************
* UAVCAN DNA Test *
* ***************************************/
bool CANTester::test_uavcan_dna()
{
uavcan::CanIfaceMgr _uavcan_iface_mgr {};
if (!_uavcan_iface_mgr.add_interface(_can_ifaces[0])) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to add iface");
return false;
}
uavcan::Node<0> node(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
node.setName("org.ardupilot.dnatest");
uavcan::protocol::HardwareVersion hw_version;
const uint8_t uid_buf_len = hw_version.unique_id.capacity();
uint8_t uid_len = uid_buf_len;
uint8_t unique_id[uid_buf_len];
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
unique_id[uid_len - 1] -= 5;
uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
}
node.setHardwareVersion(hw_version); // Copying the value to the node's internals
/*
* Starting the node normally, in passive mode (i.e. without node ID assigned).
*/
const int node_start_res = node.start();
if (node_start_res < 0) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to start the node");
return false;
}
/*
* Initializing the dynamic node ID allocation client.
* By default, the client will use TransferPriority::OneHigherThanLowest for communications with the allocator;
* this can be overriden through the third argument to the start() method.
*/
uavcan::DynamicNodeIDClient client(node);
int expected_node_id = 100;
int client_start_res = client.start(node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE
expected_node_id);
if (client_start_res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT,"Failed to start the dynamic node");
}
/*
* Waiting for the client to obtain for us a node ID.
* This may take a few seconds.
*/
gcs().send_text(MAV_SEVERITY_ALERT, "Allocation is in progress");
uint32_t num_runs = 100;
while (!client.isAllocationComplete() && num_runs--) {
const int res = node.spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter
if (res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT, "Transient failure");
}
}
gcs().send_text(MAV_SEVERITY_ALERT, "Dynamic NodeID %d allocated node ID %d",
int(client.getAllocatedNodeID().get()),
int(client.getAllocatorNodeID().get()));
if (client.getAllocatedNodeID().get() != expected_node_id) {
gcs().send_text(MAV_SEVERITY_ALERT, "Unexpected Node Id, expected %d", expected_node_id);
return false;
}
return true;
}
/*****************************************
* TOSHIBA CAN Test *
*****************************************/
#define NUM_TOSHIBA_TEST_RUN 1000
bool CANTester::test_toshiba_can()
{
AP_HAL::CANFrame frame;
uint16_t num_runs = NUM_TOSHIBA_TEST_RUN;
uint32_t num_errors = 0;
uint32_t num_lock_cmds = 0;
uint32_t num_request_data_cmds = 0;
uint32_t num_motor_cmds = 0;
uint32_t start_time = AP_HAL::native_millis();
AP_HAL::CANIface::CanIOFlags flags;
while (num_runs--) {
if (!read_frame(0, frame, _loop_rate, flags)) {
continue;
}
if (flags & AP_HAL::CANIface::Loopback) {
continue;
}
switch (frame.id) {
case AP_ToshibaCAN::COMMAND_LOCK: {
AP_ToshibaCAN::motor_lock_cmd_t lock_frame;
if (sizeof(lock_frame) != frame.dlc) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad lock command length");
num_errors++;
}
memcpy(&lock_frame, frame.data, sizeof(lock_frame));
if (lock_frame.motor1 != 1 && lock_frame.motor1 != 2 &&
lock_frame.motor2 != 1 && lock_frame.motor2 != 2 &&
lock_frame.motor3 != 1 && lock_frame.motor3 != 2 &&
lock_frame.motor4 != 1 && lock_frame.motor4 != 2 &&
lock_frame.motor5 != 1 && lock_frame.motor5 != 2 &&
lock_frame.motor6 != 1 && lock_frame.motor6 != 2 &&
lock_frame.motor7 != 1 && lock_frame.motor7 != 2 &&
lock_frame.motor8 != 1 && lock_frame.motor8 != 2 &&
lock_frame.motor9 != 1 && lock_frame.motor9 != 2 &&
lock_frame.motor10 != 1 && lock_frame.motor10 != 2 &&
lock_frame.motor11 != 1 && lock_frame.motor11 != 2 &&
lock_frame.motor12 != 1 && lock_frame.motor12 != 2) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad lock frame received!");
num_errors++;
}
num_lock_cmds++;
break;
}
case AP_ToshibaCAN::COMMAND_REQUEST_DATA: {
AP_ToshibaCAN::motor_request_data_cmd_t request_data_cmd;
if (sizeof(request_data_cmd) != frame.dlc) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad request data command length");
num_errors++;
}
memcpy(&request_data_cmd, frame.data, sizeof(request_data_cmd));
if (!((request_data_cmd.motor1 == request_data_cmd.motor2 &&
request_data_cmd.motor2 == request_data_cmd.motor3 &&
request_data_cmd.motor3 == request_data_cmd.motor4 &&
request_data_cmd.motor4 == request_data_cmd.motor5 &&
request_data_cmd.motor5 == request_data_cmd.motor6 &&
request_data_cmd.motor6 == request_data_cmd.motor7 &&
request_data_cmd.motor7 == request_data_cmd.motor8 &&
request_data_cmd.motor8 == request_data_cmd.motor9 &&
request_data_cmd.motor9 == request_data_cmd.motor10 &&
request_data_cmd.motor10 == request_data_cmd.motor11 &&
request_data_cmd.motor11 == request_data_cmd.motor12) &&
(request_data_cmd.motor1 == 0 ||
request_data_cmd.motor1 == 1 ||
request_data_cmd.motor1 == 2 ||
request_data_cmd.motor1 == 3 ||
request_data_cmd.motor1 == 4 ||
request_data_cmd.motor1 == 5))) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad request frame received!");
num_errors++;
}
num_request_data_cmds++;
send_toshiba_can_reply(request_data_cmd.motor1);
break;
}
case AP_ToshibaCAN::COMMAND_MOTOR1:
case AP_ToshibaCAN::COMMAND_MOTOR2:
case AP_ToshibaCAN::COMMAND_MOTOR3: {
AP_ToshibaCAN::motor_rotation_cmd_t rotation_cmd;
if (frame.dlc != sizeof(rotation_cmd)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad motor command length");
num_errors++;
}
memcpy(&rotation_cmd, frame.data, sizeof(rotation_cmd));
if ((rotation_cmd.motor1 > AP_ToshibaCAN::TOSHIBACAN_OUTPUT_MAX) ||
(rotation_cmd.motor2 > AP_ToshibaCAN::TOSHIBACAN_OUTPUT_MAX) ||
(rotation_cmd.motor3 > AP_ToshibaCAN::TOSHIBACAN_OUTPUT_MAX) ||
(rotation_cmd.motor4 > AP_ToshibaCAN::TOSHIBACAN_OUTPUT_MAX)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad motor command data");
num_errors++;
}
num_motor_cmds++;
break;
}
default: {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Unsupported Command");
num_errors++;
break;
}
}
}
uint32_t num_secs = (AP_HAL::native_millis() - start_time)/1000;
gcs().send_text(MAV_SEVERITY_ALERT, "Num Errors: %lu Cmds Lock: %lu Request: %lu Motor: %lu",
(long unsigned int)num_errors,
(long unsigned int)num_lock_cmds,
(long unsigned int)num_request_data_cmds,
(long unsigned int)num_motor_cmds);
gcs().send_text(MAV_SEVERITY_ALERT, "Rates Lock: %lu Request: %lu Motor: %lu",
(long unsigned int)num_lock_cmds/num_secs,
(long unsigned int)num_request_data_cmds/num_secs,
(long unsigned int)num_motor_cmds/num_secs);
if (num_errors) {
return false;
} else {
return true;
}
}
bool CANTester::send_toshiba_can_reply(uint32_t cmd)
{
AP_HAL::CANFrame send_frame;
for (uint8_t sub_id = 0; sub_id < TOSHIBACAN_MAX_NUM_ESCS; sub_id++) {
// decode rpm and voltage data
switch (cmd) {
case 1: {
// copy contents to our structure
AP_ToshibaCAN::motor_reply_data1_t reply_data;
reply_data.rpm = htobe16(100);
reply_data.current_ma = htobe16(1000);
reply_data.voltage_mv = htobe16(12000);
memcpy(send_frame.data, &reply_data, sizeof(reply_data.data));
send_frame.id = AP_ToshibaCAN::MOTOR_DATA1 + sub_id;
memcpy(send_frame.data, &reply_data, sizeof(reply_data));
send_frame.dlc = 8;
write_frame(0, send_frame, 1000);
continue;
}
// decode temperature data
case 2: {
// motor data2 data format is 8 bytes (64 bits)
// 10 bits: U temperature
// 10 bits: V temperature
// 10 bits: W temperature
// 10 bits: motor temperature
// remaining 24 bits: reserved
const uint16_t temp = (300 * 5) + 20;
send_frame.data[0] = (temp >> 2) & 0xFF;
send_frame.data[1] = ((temp << 6) | ((temp >> 4) & 0x3F)) & 0xFF;
send_frame.data[2] = (((temp << 4) & 0xF0) | ((temp >> 6) & 0x0F)) & 0xFF;
send_frame.data[3] = (((temp << 2) & 0xFC) | ((temp >> 8) & 0x03)) & 0xFF;
send_frame.data[4] = temp & 0xFF;
send_frame.id = AP_ToshibaCAN::MOTOR_DATA2 + sub_id;
send_frame.dlc = 8;
write_frame(0, send_frame, 1000);
continue;
}
// encode cumulative usage data
case 3: {
// motor data3 data format is 8 bytes (64 bits)
// 3 bytes: usage in seconds
// 2 bytes: number of times rotors started and stopped
// 3 bytes: reserved
uint32_t secs = AP_HAL::native_millis()/1000;
send_frame.data[0] = (secs >> 16) & 0xFF;
send_frame.data[0] = (secs >> 8) & 0xFF;
send_frame.data[0] = (secs & 0xFF);
send_frame.id = AP_ToshibaCAN::MOTOR_DATA3 + sub_id;
send_frame.dlc = 8;
write_frame(0, send_frame, 1000);
continue;
}
default:
return false;
}
}
return true;
}
/*****************************************
* KDE CAN Test *
*****************************************/
bool CANTester::test_kdecan()
{
AP_CANTester_KDECAN* kdecan_test = new AP_CANTester_KDECAN;
if (kdecan_test == nullptr) {
gcs().send_text(MAV_SEVERITY_ERROR, "Failed to allocate KDECAN Tester");
return false;
}
kdecan_test->init(_can_ifaces[0]);
while (true) {
kdecan_test->loop();
static uint32_t last_print_ms;
static uint32_t last_enumsend_ms;
static uint8_t enum_count = 0;
uint32_t now = AP_HAL::millis();
if (now - last_print_ms >= 1000) {
last_print_ms = now;
kdecan_test->print_stats();
}
if (!_kdecan_enumeration) {
enum_count = 0;
}
if (now - last_enumsend_ms >= 2000 && enum_count < 4 && _kdecan_enumeration) {
last_enumsend_ms = now;
if (kdecan_test->send_enumeration(enum_count)) {
enum_count++;
}
}
}
return true;
}
bool CANTester::run_kdecan_enumeration(bool start_stop)
{
_kdecan_enumeration = start_stop;
return true;
}
/***********************************************
* UAVCAN ESC *
* *********************************************/
#define NUM_ESCS 4
static uavcan::Publisher<uavcan::equipment::esc::Status>* esc_status_publisher;
static uavcan::Subscriber<uavcan::equipment::esc::RawCommand> *esc_command_listener;
static uint16_t uavcan_esc_command_rate = 0;
void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg);
void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg)
{
static uint16_t num_received = 0;
static uint32_t last_millis;
if (num_received == 0) {
last_millis = AP_HAL::millis();
}
num_received++;
// update rate every 50 packets
if (num_received == 50) {
uavcan_esc_command_rate = 100000/(AP_HAL::millis() - last_millis);
num_received = 0;
}
}
bool CANTester::test_uavcan_esc()
{
bool ret = true;
uavcan::CanIfaceMgr _uavcan_iface_mgr {};
if (!_uavcan_iface_mgr.add_interface(_can_ifaces[0])) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to add iface");
return false;
}
uavcan::Node<0> *node = nullptr;
{
node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
if (node == nullptr) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node");
ret = false;
goto exit;
} else {
node->setName("org.ardupilot.esctest");
}
}
{
uavcan::protocol::HardwareVersion hw_version;
const uint8_t uid_buf_len = hw_version.unique_id.capacity();
uint8_t uid_len = uid_buf_len;
uint8_t unique_id[uid_buf_len];
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
// Generate random uid
unique_id[uid_len - 1] += 5;
uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
}
node->setHardwareVersion(hw_version); // Copying the value to the node's internals
}
/*
* Starting the node normally, in passive mode (i.e. without node ID assigned).
*/
{
const int node_start_res = node->start();
if (node_start_res < 0) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to start the node");
ret = false;
goto exit;
}
}
{
/*
* Initializing the dynamic node ID allocation client.
* By default, the client will use TransferPriority::OneHigherThanLowest for communications with the allocator;
* this can be overriden through the third argument to the start() method.
*/
uavcan::DynamicNodeIDClient client(*node);
int client_start_res = client.start(node->getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE
uavcan::NodeID(0));
if (client_start_res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT,"Failed to start the dynamic node");
ret = false;
goto exit;
}
/*
* Waiting for the client to obtain for us a node ID.
* This may take a few seconds.
*/
gcs().send_text(MAV_SEVERITY_ALERT, "Allocation is in progress");
while (!client.isAllocationComplete()) {
const int res = node->spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter
if (res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT, "Transient failure");
}
}
gcs().send_text(MAV_SEVERITY_ALERT, "Dynamic NodeID %d allocated node ID %d",
int(client.getAllocatedNodeID().get()),
int(client.getAllocatorNodeID().get()));
if (client.getAllocatedNodeID().get() == 255) {
gcs().send_text(MAV_SEVERITY_ALERT, "Node Allocation Failed");
ret = false;
goto exit;
}
esc_command_listener = new uavcan::Subscriber<uavcan::equipment::esc::RawCommand>(*node);
if (esc_command_listener) {
esc_command_listener->start(handle_raw_command);
} else {
ret = false;
goto exit;
}
esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node);
if (esc_status_publisher == nullptr) {
ret = false;
goto exit;
}
esc_status_publisher->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
esc_status_publisher->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
node->setNodeID(client.getAllocatedNodeID());
node->setModeOperational();
}
// Allocations done lets begin
if (ret) {
while (true) {
node->spin(uavcan::MonotonicDuration::fromMSec(1000));
gcs().send_text(MAV_SEVERITY_ALERT, "UC ESC Command Rate: %d", uavcan_esc_command_rate);
uavcan_esc_command_rate = 0;
// send fake ESC stats as well
for (uint8_t i = 0; i < NUM_ESCS; i++) {
uavcan::equipment::esc::Status status_msg;
status_msg.esc_index = i;
status_msg.error_count = 0;
status_msg.voltage = 30 + 2*((float)get_random16()/INT16_MAX);
status_msg.current = 10 + 10*((float)get_random16()/INT16_MAX);
status_msg.temperature = 124 + i + C_TO_KELVIN;
status_msg.rpm = 1200 + 300*((float)get_random16()/INT16_MAX);
status_msg.power_rating_pct = 70 + 20*((float)get_random16()/INT16_MAX);
esc_status_publisher->broadcast(status_msg);
}
}
}
exit:
// Clean up!
if (node != nullptr) {
delete node;
}
if (esc_command_listener != nullptr) {
delete esc_command_listener;
esc_command_listener = nullptr;
}
if (esc_status_publisher != nullptr) {
delete esc_status_publisher;
esc_status_publisher = nullptr;
}
return ret;
}
CANTester *CANTester::get_cantester(uint8_t driver_index)
{
if (driver_index >= AP::can().get_num_drivers() ||
AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_CANTester) {
return nullptr;
}
return static_cast<CANTester*>(AP::can().get_driver(driver_index));
}
#endif //#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS

109
libraries/AP_CANManager/AP_CANTester.h Normal file
View File

@ -0,0 +1,109 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_CANDriver.h"
#include <AP_HAL/Semaphores.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES
class CANTester : public AP_CANDriver
{
public:
CANTester()
{
// update protected var for parameter interface
AP_Param::setup_object_defaults(this, var_info);
}
static const struct AP_Param::GroupInfo var_info[];
/* Do not allow copies */
CANTester(const CANTester &other) = delete;
CANTester &operator=(const CANTester&) = delete;
void init(uint8_t driver_index, bool enable_filters) override;
bool add_interface(AP_HAL::CANIface* can_iface) override;
bool run_kdecan_enumeration(bool start_stop);
static CANTester *get_cantester(uint8_t driver_index);
private:
enum {
TEST_NONE,
TEST_LOOPBACK,
TEST_BUSOFF_RECOVERY,
TEST_UAVCAN_DNA,
TEST_TOSHIBA_CAN,
TEST_KDE_CAN,
TEST_UAVCAN_ESC,
TEST_END,
};
struct loopback_stats_s {
uint32_t num_tx;
uint32_t failed_tx;
uint32_t num_rx;
uint32_t num_flushed_rx;
uint32_t bad_rx_data;
uint32_t bad_rx_seq;
uint16_t tx_seq;
uint16_t next_valid_seq;
} _loopback_stats[HAL_NUM_CAN_IFACES];
void main_thread();
bool test_loopback(uint32_t loop_rate);
void create_loopback_frame(loopback_stats_s &stats, AP_HAL::CANFrame& frame);
void check_loopback_frame(loopback_stats_s &stats, AP_HAL::CANFrame& frame);
bool test_busoff_recovery();
bool test_uavcan_dna();
bool test_toshiba_can();
bool send_toshiba_can_reply(uint32_t cmd);
bool test_kdecan();
bool test_uavcan_esc();
// write frame on CAN bus, returns true on success
bool write_frame(uint8_t iface, AP_HAL::CANFrame &out_frame, uint64_t timeout);
// read frame on CAN bus, returns true on success
bool read_frame(uint8_t iface, AP_HAL::CANFrame &recv_frame, uint64_t timeout, AP_HAL::CANIface::CanIOFlags &flags);
void reset_frame(AP_HAL::CANFrame& frame);
bool _initialized;
uint8_t _driver_index;
AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES];
// Classes required for UAVCAN Test
class RaiiSynchronizer {};
uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, CANTester::RaiiSynchronizer> _node_allocator;
HAL_EventHandle _event_handle;
AP_Int8 _test_driver_index;
AP_Int8 _test_port;
AP_Int32 _test_id;
AP_Int32 _loop_rate;
uint8_t _num_ifaces;
bool _kdecan_enumeration;
};
#endif //#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS

230
libraries/AP_CANManager/AP_CANTester_KDECAN.cpp Normal file
View File

@ -0,0 +1,230 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Francisco Ferreira
* Modified for CANManager by Siddharth B Purohit
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_CANTester_KDECAN.h"
#include "AP_CANManager.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Vehicle/AP_Vehicle.h>
#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "TestKDECAN", fmt, #args); } while (0)
extern const AP_HAL::HAL& hal;
void AP_CANTester_KDECAN::count_msg(uint32_t frame_id)
{
for (uint16_t i=0; i<ARRAY_SIZE(counters); i++) {
if (counters[i].frame_id == frame_id) {
counters[i].count++;
break;
}
if (counters[i].frame_id == 0) {
counters[i].frame_id = frame_id;
counters[i].count++;
break;
}
}
}
bool AP_CANTester_KDECAN::init(AP_HAL::CANIface* can_iface)
{
_can_iface = can_iface;
if (_can_iface == nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "CANIface is null, abort!");
return false;
}
if (!_can_iface->is_initialized()) {
debug_can(AP_CANManager::LOG_ERROR, "Can not initialised");
return false;
}
if (!_can_iface->set_event_handle(&_event_handle)) {
debug_can(AP_CANManager::LOG_ERROR, "Failed to set Event Handle");
return false;
}
debug_can(AP_CANManager::LOG_ERROR, "init done");
return true;
}
void AP_CANTester_KDECAN::loop(void)
{
if (_can_iface == nullptr) {
return;
}
AP_HAL::CANFrame empty_frame { (0 | AP_HAL::CANFrame::FlagEFF), nullptr, 0 };
bool read_select = true;
bool write_select = false;
bool select_ret = _can_iface->select(read_select, write_select, nullptr, AP_HAL::micros64() + 1000);
if (select_ret && read_select) {
AP_HAL::CANFrame recv_frame;
uint64_t rx_time;
AP_HAL::CANIface::CanIOFlags flags {};
int16_t res = _can_iface->receive(recv_frame, rx_time, flags);
if (res == 1) {
uint32_t id = recv_frame.id & AP_HAL::CANFrame::MaskExtID;
uint8_t object_address = id & 0xFF;
uint8_t esc_num = uint8_t((id >> 8) & 0xFF);
count_msg(id);
uint8_t i = 0;
uint8_t n = NUM_ESCS;
if (esc_num != BROADCAST_NODE_ID) {
for (; i < NUM_ESCS; i++) {
if (object_address == UPDATE_NODE_ID_OBJ_ADDR) {
if (_esc_info[i].mcu_id == be64toh(*((be64_t*) &(recv_frame.data[0])))) {
n = i + 1;
break;
}
} else if (_esc_info[i].node_id == esc_num) {
n = i + 1;
break;
}
}
}
while (i < n) {
AP_HAL::CANFrame res_frame;
switch (object_address) {
case ESC_INFO_OBJ_ADDR: {
uint8_t info[5] { 1, 2, 3, 4, 0 };
res_frame.dlc = 5;
memcpy(res_frame.data, info, 5);
break;
}
case SET_PWM_OBJ_ADDR: {
if ((1 << (esc_num - 2) & _mask_received_pwm) && _mask_received_pwm != ((1 << _max_node_id) - 1)) {
count_msg(0xFFFFFFF0);
_mask_received_pwm = 0;
}
_mask_received_pwm |= 1 << (esc_num - 2);
if (_mask_received_pwm == ((1 << _max_node_id) - 1)) {
count_msg(0xFFFFFFFF);
_mask_received_pwm = 0;
}
res_frame.dlc = 0;
break;
}
case UPDATE_NODE_ID_OBJ_ADDR: {
if (_esc_info[i].enum_timeout != 0 && _esc_info[i].enum_timeout >= AP_HAL::micros64()) {
_esc_info[i].node_id = esc_num;
_max_node_id = MAX(_max_node_id, esc_num - 2 + 1);
gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Set node ID %d for ESC %d\n", esc_num, i);
}
_esc_info[i].enum_timeout = 0;
res_frame.dlc = 1;
memcpy(res_frame.data, &(_esc_info[i].node_id), 1);
break;
}
case START_ENUM_OBJ_ADDR: {
_esc_info[i].enum_timeout = AP_HAL::micros64() + be16toh(*((be16_t*) &(recv_frame.data[0]))) * 1000;
gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Starting enumeration for ESC %d, timeout %" PRIu64 "\n", i, _esc_info[i].enum_timeout);
i++;
continue;
}
case TELEMETRY_OBJ_ADDR: {
uint8_t data[8] {};
*((be16_t*) &data[0]) = htobe16(get_random16());
*((be16_t*) &data[2]) = htobe16(get_random16());
*((be16_t*) &data[4]) = htobe16(get_random16());
data[6] = uint8_t(float(rand()) / RAND_MAX * 40.0f + 15);
res_frame.dlc = 8;
memcpy(res_frame.data, data, 8);
break;
}
case VOLTAGE_OBJ_ADDR:
case CURRENT_OBJ_ADDR:
case RPM_OBJ_ADDR:
case TEMPERATURE_OBJ_ADDR:
case GET_PWM_INPUT_OBJ_ADDR:
case GET_PWM_OUTPUT_OBJ_ADDR:
case MCU_ID_OBJ_ADDR:
default:
// discard frame
return;
}
res_frame.id = (_esc_info[i].node_id << 16) | object_address | AP_HAL::CANFrame::FlagEFF;
read_select = false;
write_select = true;
select_ret = _can_iface->select(read_select, write_select, &res_frame, AP_HAL::micros64() + 1000);
if (!select_ret) {
break;
}
int16_t res2 = _can_iface->send(res_frame, AP_HAL::micros64() + 500000, 0);
if (res2 == 1) {
i++;
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Failed to transmit frame Err: %d 0x%lx", res2, (long unsigned)res_frame.id);
}
}
}
}
}
void AP_CANTester_KDECAN::print_stats(void)
{
hal.console->printf("KDECANTester: TimeStamp: %u\n", (unsigned)AP_HAL::micros());
for (uint16_t i=0; i<100; i++) {
if (counters[i].frame_id == 0) {
break;
}
hal.console->printf("0x%08" PRIX32 ": %" PRIu32 "\n", counters[i].frame_id, counters[i].count);
counters[i].count = 0;
}
}
bool AP_CANTester_KDECAN::send_enumeration(uint8_t num)
{
if (_esc_info[num].enum_timeout == 0 || AP_HAL::micros64() > _esc_info[num].enum_timeout) {
_esc_info[num].enum_timeout = 0;
gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Not running enumeration for ESC %d\n", num);
return false;
}
while (true) {
uint8_t mcu[8] {};
*((be64_t*) mcu) = htobe64(_esc_info[num].mcu_id);
AP_HAL::CANFrame res_frame { (_esc_info[num].node_id << 16) | START_ENUM_OBJ_ADDR | AP_HAL::CANFrame::FlagEFF,
mcu,
8 };
int16_t res = _can_iface->send(res_frame, AP_HAL::micros64() + 1000, 0);
if (res == 1) {
return true;
}
}
}
#endif

82
libraries/AP_CANManager/AP_CANTester_KDECAN.h Normal file
View File

@ -0,0 +1,82 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Francisco Ferreira
* Modified for CANManager by Siddharth B Purohit
*/
#include "AP_CANDriver.h"
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS
#define NUM_ESCS 4
class AP_CANTester_KDECAN
{
public:
AP_CANTester_KDECAN()
{
for (uint8_t i = 0; i < NUM_ESCS; i++) {
_esc_info[i].mcu_id = 0xA5961824E7BD3C00 | i;
}
}
bool init(AP_HAL::CANIface* can_iface);
void loop(void);
void print_stats(void);
bool send_enumeration(uint8_t num);
private:
uint8_t _driver_index = 0;
uint8_t _interface = 0;
AP_HAL::CANIface* _can_iface;
uint8_t _mask_received_pwm = 0;
struct esc_info {
uint8_t node_id;
uint64_t mcu_id;
uint64_t enum_timeout;
esc_info() : node_id(1), mcu_id(0), enum_timeout(0) {}
} _esc_info[NUM_ESCS];
uint8_t _max_node_id = 0;
static const uint8_t BROADCAST_NODE_ID = 1;
static const uint8_t ESC_INFO_OBJ_ADDR = 0;
static const uint8_t SET_PWM_OBJ_ADDR = 1;
static const uint8_t VOLTAGE_OBJ_ADDR = 2;
static const uint8_t CURRENT_OBJ_ADDR = 3;
static const uint8_t RPM_OBJ_ADDR = 4;
static const uint8_t TEMPERATURE_OBJ_ADDR = 5;
static const uint8_t GET_PWM_INPUT_OBJ_ADDR = 6;
static const uint8_t GET_PWM_OUTPUT_OBJ_ADDR = 7;
static const uint8_t MCU_ID_OBJ_ADDR = 8;
static const uint8_t UPDATE_NODE_ID_OBJ_ADDR = 9;
static const uint8_t START_ENUM_OBJ_ADDR = 10;
static const uint8_t TELEMETRY_OBJ_ADDR = 11;
struct {
uint32_t frame_id;
uint32_t count;
} counters[100];
void count_msg(uint32_t frame_id);
HAL_EventHandle _event_handle;
};
#endif

687
libraries/AP_CANManager/AP_SLCANIface.cpp Normal file
View File

@ -0,0 +1,687 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Siddharth Bharat Purohit
* Referenced from implementation by Pavel Kirienko <pavel.kirienko@zubax.com>
* for Zubax Babel
*/
#include "AP_SLCANIface.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_CANManager.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <stdio.h>
#include <AP_Vehicle/AP_Vehicle.h>
#define LOG_TAG "SLCAN"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo SLCAN::CANIface::var_info[] = {
// @Param: CPORT
// @DisplayName: SLCAN Route
// @Description: CAN Interface ID to be routed to SLCAN, 0 means no routing
// @Values: 0:Disabled,1:First interface,2:Second interface
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("CPORT", 1, SLCAN::CANIface, _slcan_can_port, 0),
// @Param: SERNUM
// @DisplayName: SLCAN Serial Port
// @Description: Serial Port ID to be used for temporary SLCAN iface, -1 means no temporary serial. This parameter is automatically reset on reboot or on timeout. See CAN_SLCAN_TIMOUT for timeout details
// @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
// @User: Standard
AP_GROUPINFO("SERNUM", 2, SLCAN::CANIface, _slcan_ser_port, -1),
// @Param: TIMOUT
// @DisplayName: SLCAN Timeout
// @Description: Duration of inactivity after which SLCAN is switched back to original driver in seconds.
// @Range: 0 127
// @User: Standard
AP_GROUPINFO("TIMOUT", 3, SLCAN::CANIface, _slcan_timeout, 0),
// @Param: SDELAY
// @DisplayName: SLCAN Start Delay
// @Description: Duration after which slcan starts after setting SERNUM in seconds.
// @Range: 0 127
// @User: Standard
AP_GROUPINFO("SDELAY", 4, SLCAN::CANIface, _slcan_start_delay, 1),
AP_GROUPEND
};
////////Helper Methods//////////
static bool hex2nibble_error;
static uint8_t nibble2hex(uint8_t x)
{
// Allocating in RAM because it's faster
static uint8_t ConversionTable[] = {
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
};
return ConversionTable[x & 0x0F];
}
static uint8_t hex2nibble(char c)
{
// Must go into RAM, not flash, because flash is slow
static uint8_t NumConversionTable[] = {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9
};
static uint8_t AlphaConversionTable[] = {
10, 11, 12, 13, 14, 15
};
uint8_t out = 255;
if (c >= '0' && c <= '9') {
out = NumConversionTable[int(c) - int('0')];
} else if (c >= 'a' && c <= 'f') {
out = AlphaConversionTable[int(c) - int('a')];
} else if (c >= 'A' && c <= 'F') {
out = AlphaConversionTable[int(c) - int('A')];
}
if (out == 255) {
hex2nibble_error = true;
}
return out;
}
int SLCAN::CANIface::set_port(AP_HAL::UARTDriver* port)
{
if (port == nullptr) {
return -1;
}
_port = port;
return 0;
}
bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame)
{
AP_HAL::CANIface::CanRxItem frm;
frm.frame = frame;
frm.flags = 0;
frm.timestamp_us = AP_HAL::native_micros64();
return rx_queue_.push(frm);
}
/**
* General frame format:
* <type> <id> <dlc> <data>
* The emitting functions below are highly optimized for speed.
*/
bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
{
AP_HAL::CANFrame f;
hex2nibble_error = false;
f.id = f.FlagEFF |
(hex2nibble(cmd[1]) << 28) |
(hex2nibble(cmd[2]) << 24) |
(hex2nibble(cmd[3]) << 20) |
(hex2nibble(cmd[4]) << 16) |
(hex2nibble(cmd[5]) << 12) |
(hex2nibble(cmd[6]) << 8) |
(hex2nibble(cmd[7]) << 4) |
(hex2nibble(cmd[8]) << 0);
if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::MaxDataLen)) {
return false;
}
f.dlc = cmd[9] - '0';
if (f.dlc > AP_HAL::CANFrame::MaxDataLen) {
return false;
}
{
const char* p = &cmd[10];
for (unsigned i = 0; i < f.dlc; i++) {
f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1));
p += 2;
}
}
if (hex2nibble_error) {
return false;
}
return push_Frame(f);
}
bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd)
{
AP_HAL::CANFrame f;
hex2nibble_error = false;
f.id = (hex2nibble(cmd[1]) << 8) |
(hex2nibble(cmd[2]) << 4) |
(hex2nibble(cmd[3]) << 0);
if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::MaxDataLen)) {
return false;
}
f.dlc = cmd[4] - '0';
if (f.dlc > AP_HAL::CANFrame::MaxDataLen) {
return false;
}
{
const char* p = &cmd[5];
for (unsigned i = 0; i < f.dlc; i++) {
f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1));
p += 2;
}
}
if (hex2nibble_error) {
return false;
}
return push_Frame(f);
}
bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd)
{
AP_HAL::CANFrame f;
hex2nibble_error = false;
f.id = f.FlagEFF | f.FlagRTR |
(hex2nibble(cmd[1]) << 28) |
(hex2nibble(cmd[2]) << 24) |
(hex2nibble(cmd[3]) << 20) |
(hex2nibble(cmd[4]) << 16) |
(hex2nibble(cmd[5]) << 12) |
(hex2nibble(cmd[6]) << 8) |
(hex2nibble(cmd[7]) << 4) |
(hex2nibble(cmd[8]) << 0);
if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::MaxDataLen)) {
return false;
}
f.dlc = cmd[9] - '0';
if (f.dlc > AP_HAL::CANFrame::MaxDataLen) {
return false;
}
if (hex2nibble_error) {
return false;
}
return push_Frame(f);
}
bool SLCAN::CANIface::handle_FrameRTRStd(const char* cmd)
{
AP_HAL::CANFrame f;
hex2nibble_error = false;
f.id = f.FlagRTR |
(hex2nibble(cmd[1]) << 8) |
(hex2nibble(cmd[2]) << 4) |
(hex2nibble(cmd[3]) << 0);
if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::MaxDataLen)) {
return false;
}
f.dlc = cmd[4] - '0';
if (f.dlc <= AP_HAL::CANFrame::MaxDataLen) {
return false;
}
if (hex2nibble_error) {
return false;
}
return push_Frame(f);
}
static inline const char* getASCIIStatusCode(bool status)
{
return status ? "\r" : "\a";
}
bool SLCAN::CANIface::init_passthrough(uint8_t i)
{
// we setup undelying can iface here which we use for passthrough
if (initialized_ ||
_slcan_can_port <= 0 ||
_slcan_can_port != i+1) {
return false;
}
_can_iface = hal.can[i];
_drv_num = _slcan_can_port - 1;
_prev_ser_port = -1;
AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "Setting SLCAN Passthrough for CAN%d\n", _slcan_can_port - 1);
return true;
}
/**
* General frame format:
* <type> <id> <dlc> <data> [timestamp msec] [flags]
* Types:
* R - RTR extended
* r - RTR standard
* T - Data extended
* t - Data standard
* Flags:
* L - this frame is a loopback frame; timestamp field contains TX timestamp
*/
int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t timestamp_usec)
{
if (_port == nullptr) {
return -1;
}
constexpr unsigned SLCANMaxFrameSize = 40;
uint8_t buffer[SLCANMaxFrameSize] = {'\0'};
uint8_t* p = &buffer[0];
/*
* Frame type
*/
if (frame.isRemoteTransmissionRequest()) {
*p++ = frame.isExtended() ? 'R' : 'r';
} else if (frame.isErrorFrame()) {
return -1; // Not supported
} else {
*p++ = frame.isExtended() ? 'T' : 't';
}
/*
* ID
*/
{
const uint32_t id = frame.id & frame.MaskExtID;
if (frame.isExtended()) {
*p++ = nibble2hex(id >> 28);
*p++ = nibble2hex(id >> 24);
*p++ = nibble2hex(id >> 20);
*p++ = nibble2hex(id >> 16);
*p++ = nibble2hex(id >> 12);
}
*p++ = nibble2hex(id >> 8);
*p++ = nibble2hex(id >> 4);
*p++ = nibble2hex(id >> 0);
}
/*
* DLC
*/
*p++ = char('0' + frame.dlc);
/*
* Data
*/
for (unsigned i = 0; i < frame.dlc; i++) {
const uint8_t byte = frame.data[i];
*p++ = nibble2hex(byte >> 4);
*p++ = nibble2hex(byte);
}
/*
* Timestamp
*/
{
// SLCAN format - [0, 60000) milliseconds
const auto slcan_timestamp = uint16_t(timestamp_usec / 1000U);
*p++ = nibble2hex(slcan_timestamp >> 12);
*p++ = nibble2hex(slcan_timestamp >> 8);
*p++ = nibble2hex(slcan_timestamp >> 4);
*p++ = nibble2hex(slcan_timestamp >> 0);
}
/*
* Finalization
*/
*p++ = '\r';
const auto frame_size = unsigned(p - &buffer[0]);
if (_port->txspace() < _pending_frame_size) {
_pending_frame_size = frame_size;
return 0;
}
//Write to Serial
if (!_port->write_locked(&buffer[0], frame_size, _serial_lock_key)) {
return 0;
}
return 1;
}
//Accepts command string, returns response string or nullptr if no response is needed.
const char* SLCAN::CANIface::processCommand(char* cmd)
{
if (_port == nullptr) {
return nullptr;
}
/*
* High-traffic SLCAN commands go first
*/
if (cmd[0] == 'T') {
return handle_FrameDataExt(cmd) ? "Z\r" : "\a";
} else if (cmd[0] == 't') {
return handle_FrameDataStd(cmd) ? "z\r" : "\a";
} else if (cmd[0] == 'R') {
return handle_FrameRTRExt(cmd) ? "Z\r" : "\a";
} else if (cmd[0] == 'r' && cmd[1] <= '9') { // The second condition is needed to avoid greedy matching
// See long commands below
return handle_FrameRTRStd(cmd) ? "z\r" : "\a";
}
uint8_t resp_bytes[40];
uint16_t resp_len;
/*
* Regular SLCAN commands
*/
switch (cmd[0]) {
case 'S': // Set CAN bitrate
case 'O': // Open CAN in normal mode
case 'L': // Open CAN in listen-only mode
case 'l': // Open CAN with loopback enabled
case 'C': // Close CAN
case 'M': // Set CAN acceptance filter ID
case 'm': // Set CAN acceptance filter mask
case 'U': // Set UART baud rate, see http://www.can232.com/docs/can232_v3.pdf
case 'Z': { // Enable/disable RX and loopback timestamping
return getASCIIStatusCode(true); // Returning success for compatibility reasons
}
case 'F': { // Get status flags
resp_len = snprintf((char*)resp_bytes, sizeof(resp_bytes), "F%02X\r", unsigned(0)); // Returning success for compatibility reasons
if (resp_len > 0) {
_port->write_locked(resp_bytes, resp_len, _serial_lock_key);
}
return nullptr;
}
case 'V': { // HW/SW version
resp_len = snprintf((char*)resp_bytes, sizeof(resp_bytes),"V%x%x%x%x\r", 1, 0, 1, 0);
if (resp_len > 0) {
_port->write_locked(resp_bytes, resp_len, _serial_lock_key);
}
return nullptr;
}
case 'N': { // Serial number
const uint8_t uid_buf_len = 12;
uint8_t uid_len = uid_buf_len;
uint8_t unique_id[uid_buf_len];
char buf[uid_buf_len * 2 + 1] = {'\0'};
char* pos = &buf[0];
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
for (uint8_t i = 0; i < uid_buf_len; i++) {
*pos++ = nibble2hex(unique_id[i] >> 4);
*pos++ = nibble2hex(unique_id[i]);
}
}
*pos++ = '\0';
resp_len = snprintf((char*)resp_bytes, sizeof(resp_bytes),"N%s\r", &buf[0]);
if (resp_len > 0) {
_port->write_locked(resp_bytes, resp_len, _serial_lock_key);
}
return nullptr;
}
default: {
break;
}
}
return getASCIIStatusCode(false);
}
// add bytes to parse the received SLCAN Data stream
inline void SLCAN::CANIface::addByte(const uint8_t byte)
{
if (_port == nullptr) {
return;
}
if ((byte >= 32 && byte <= 126)) { // Normal printable ASCII character
if (pos_ < SLCAN_BUFFER_SIZE) {
buf_[pos_] = char(byte);
pos_ += 1;
} else {
pos_ = 0; // Buffer overrun; silently drop the data
}
} else if (byte == '\r') { // End of command (SLCAN)
// Processing the command
buf_[pos_] = '\0';
const char* const response = processCommand(reinterpret_cast<char*>(&buf_[0]));
pos_ = 0;
// Sending the response if provided
if (response != nullptr) {
_port->write_locked(reinterpret_cast<const uint8_t*>(response),
strlen(response), _serial_lock_key);
}
} else if (byte == 8 || byte == 127) { // DEL or BS (backspace)
if (pos_ > 0) {
pos_ -= 1;
}
} else { // This also includes Ctrl+C, Ctrl+D
pos_ = 0; // Invalid byte - drop the current command
}
}
void SLCAN::CANIface::update_slcan_port()
{
if (_prev_ser_port != _slcan_ser_port) {
if (!_slcan_start_req) {
_slcan_start_req_time = AP_HAL::native_millis();
_slcan_start_req = true;
}
if (((AP_HAL::native_millis() - _slcan_start_req_time) < ((uint32_t)_slcan_start_delay*1000))) {
return;
}
_port = AP::serialmanager().get_serial_by_id(_slcan_ser_port);
if (_port == nullptr) {
_slcan_ser_port.set_and_save(-1);
return;
}
_port->lock_port(_serial_lock_key, _serial_lock_key);
_prev_ser_port = _slcan_ser_port;
gcs().send_text(MAV_SEVERITY_INFO, "CANManager: Starting SLCAN Passthrough on Serial %d with CAN%d", _slcan_ser_port.get(), _drv_num);
_last_had_activity = AP_HAL::native_millis();
}
if (_port == nullptr) {
return;
}
if (((AP_HAL::native_millis() - _last_had_activity) > ((uint32_t)_slcan_timeout*1000)) &&
(uint32_t)_slcan_timeout != 0) {
_port->lock_port(0, 0);
_port = nullptr;
_slcan_ser_port.set_and_save(-1);
_prev_ser_port = -1;
_slcan_start_req = false;
}
}
bool SLCAN::CANIface::set_event_handle(AP_HAL::EventHandle* evt_handle)
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->set_event_handle(evt_handle);
}
return false;
}
uint16_t SLCAN::CANIface::getNumFilters() const
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->getNumFilters();
}
return 0;
}
uint32_t SLCAN::CANIface::getErrorCount() const
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->getErrorCount();
}
return 0;
}
uint32_t SLCAN::CANIface::get_stats(char* data, uint32_t max_size)
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->get_stats(data, max_size);
}
return 0;
}
bool SLCAN::CANIface::is_busoff() const
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->is_busoff();
}
return false;
}
bool SLCAN::CANIface::configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs)
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->configureFilters(filter_configs, num_configs);
}
return true;
}
void SLCAN::CANIface::flush_tx()
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
_can_iface->flush_tx();
}
if (_port) {
_port->flush();
}
}
void SLCAN::CANIface::clear_rx()
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
_can_iface->clear_rx();
}
rx_queue_.clear();
}
bool SLCAN::CANIface::is_initialized() const
{
// When in passthrough mode methods is handled through can iface
if (_can_iface) {
return _can_iface->is_initialized();
}
return false;
}
bool SLCAN::CANIface::select(bool &read, bool &write, const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline)
{
update_slcan_port();
bool ret = false;
// When in passthrough mode select is handled through can iface
if (_can_iface) {
ret = _can_iface->select(read, write, pending_tx, blocking_deadline);
}
if (_port == nullptr) {
return ret;
}
// if under passthrough, we only do send when can_iface also allows it
if (_port->available_locked(_serial_lock_key) || rx_queue_.available()) {
// allow for receiving messages over slcan
read = true;
ret = true;
}
return ret;
}
// send method to transmit the frame through SLCAN interface
int16_t SLCAN::CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, AP_HAL::CANIface::CanIOFlags flags)
{
update_slcan_port();
int16_t ret = 0;
// When in passthrough mode select is handled through can iface
if (_can_iface) {
ret = _can_iface->send(frame, tx_deadline, flags);
}
if (_port == nullptr) {
return ret;
}
if (frame.isErrorFrame() || frame.dlc > 8) {
return ret;
}
reportFrame(frame, AP_HAL::native_micros64());
return ret;
}
// receive method to read the frame recorded in the buffer
int16_t SLCAN::CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& rx_time,
AP_HAL::CANIface::CanIOFlags& out_flags)
{
update_slcan_port();
// When in passthrough mode select is handled through can iface
if (_can_iface) {
int16_t ret = _can_iface->receive(out_frame, rx_time, out_flags);
if (ret > 0) {
// we also pass this frame through to slcan iface,
// and immediately return
reportFrame(out_frame, AP_HAL::native_micros64());
return ret;
} else if (ret < 0) {
return ret;
}
}
// We found nothing in HAL's CANIface recieve, so look in SLCANIface
if (_port == nullptr) {
return 0;
}
if (_port->available_locked(_serial_lock_key)) {
uint32_t num_bytes = _port->available_locked(_serial_lock_key);
// flush bytes from port
while (num_bytes--) {
int16_t ret = _port->read_locked(_serial_lock_key);
if (ret <= 0) {
break;
}
addByte(ret);
}
}
if (rx_queue_.available()) {
// if we already have something in buffer transmit it
CanRxItem frm;
if (!rx_queue_.pop(frm)) {
return 0;
}
out_frame = frm.frame;
rx_time = frm.timestamp_us;
out_flags = frm.flags;
_last_had_activity = AP_HAL::millis();
// Also send this frame over can_iface when in passthrough mode,
// We just push this frame without caring for priority etc
if (_can_iface) {
_can_iface->send(out_frame, AP_HAL::native_micros64() + 1000, out_flags);
}
return 1;
}
return 0;
}
void SLCAN::CANIface::reset_params()
{
_slcan_ser_port.set_and_save(-1);
}
#endif

133
libraries/AP_CANManager/AP_SLCANIface.h Normal file
View File

@ -0,0 +1,133 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Siddharth Bharat Purohit
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include "AP_HAL/utility/RingBuffer.h"
#include <AP_Param/AP_Param.h>
#define SLCAN_BUFFER_SIZE 200
#define SLCAN_RX_QUEUE_SIZE 64
#ifndef HAL_CAN_RX_STORAGE_SIZE
#define HAL_CAN_RX_QUEUE_SIZE 128
#endif
static_assert(HAL_CAN_RX_QUEUE_SIZE <= 254, "Invalid CAN Rx queue size");
namespace SLCAN
{
class CANIface: public AP_HAL::CANIface
{
int16_t reportFrame(const AP_HAL::CANFrame& frame, uint64_t timestamp_usec);
const char* processCommand(char* cmd);
// pushes received frame into queue, false if failed
bool push_Frame(AP_HAL::CANFrame &frame);
// Methods to handle different types of frames,
// return true if successfully received frame type
bool handle_FrameRTRStd(const char* cmd);
bool handle_FrameRTRExt(const char* cmd);
bool handle_FrameDataStd(const char* cmd);
bool handle_FrameDataExt(const char* cmd);
// Parsing bytes received on the serial port
inline void addByte(const uint8_t byte);
// track changes to slcan serial port
void update_slcan_port();
bool initialized_;
char buf_[SLCAN_BUFFER_SIZE + 1]; // buffer to record raw frame nibbles before parsing
uint32_t _pending_frame_size = 0; // holds the size of frame to be tx
int16_t pos_ = 0; // position in the buffer recording nibble frames before parsing
AP_HAL::UARTDriver* _port; // UART interface port reference to be used for SLCAN iface
ObjectBuffer<AP_HAL::CANIface::CanRxItem> rx_queue_; // Parsed Rx Frame queue
const uint32_t _serial_lock_key = 0x53494442; // Key used to lock UART port for use by slcan
AP_Int8 _slcan_can_port;
AP_Int8 _slcan_ser_port;
AP_Int8 _slcan_timeout;
AP_Int8 _slcan_start_delay;
bool _slcan_start_req;
uint32_t _slcan_start_req_time;
int8_t _prev_ser_port;
int8_t _drv_num = -1;
uint32_t _last_had_activity;
AP_HAL::CANIface* _can_iface; // Can interface to be used for interaction by SLCAN interface
HAL_Semaphore port_sem;
public:
CANIface():
rx_queue_(HAL_CAN_RX_QUEUE_SIZE)
{
AP_Param::setup_object_defaults(this, var_info);
}
static const struct AP_Param::GroupInfo var_info[];
bool init(const uint32_t bitrate, const OperatingMode mode) override
{
return false;
}
// Initialisation of SLCAN Passthrough method of operation
bool init_passthrough(uint8_t i);
// Set UART port to be used with slcan interface
int set_port(AP_HAL::UARTDriver* port);
void reset_params();
int8_t get_drv_num() const
{
return _drv_num;
}
// Overriden methods
bool set_event_handle(AP_HAL::EventHandle* evt_handle) override;
uint16_t getNumFilters() const override;
uint32_t getErrorCount() const override;
uint32_t get_stats(char* data, uint32_t max_size) override;
bool is_busoff() const override;
bool configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs) override;
void flush_tx() override;
void clear_rx() override;
bool is_initialized() const override;
bool select(bool &read, bool &write,
const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override;
int16_t send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
AP_HAL::CANIface::CanIOFlags flags) override;
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& rx_time,
AP_HAL::CANIface::CanIOFlags& out_flags) override;
};
}
#endif