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

AP_CANManager: remove CANTester

This commit is contained in:
Peter Barker 2023-04-14 10:26:53 +10:00 committed by Andrew Tridgell
parent a4721b4ff8
commit b055b67a7f
6 changed files with 5 additions and 814 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_CANManager/AP_CANDriver.cpp
View File

@ -21,7 +21,6 @@
#include <AP_DroneCAN/AP_DroneCAN.h>
#include <AP_PiccoloCAN/AP_PiccoloCAN.h>
#include "AP_CANTester.h"
#include <AP_KDECAN/AP_KDECAN.h>
@ -31,7 +30,7 @@ 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: 0:Disabled,1:DroneCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2
// @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, AP_CANManager::Driver_Type_DroneCAN),
@ -43,12 +42,8 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
#endif
// index 3 was KDECAN
#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES && HAL_ENABLE_CANTESTER
// @Group: TST_
// @Path: ../AP_CANManager/AP_CANTester.cpp
AP_SUBGROUPPTR(_testcan, "TST_", 4, AP_CANManager::CANDriver_Params, CANTester),
#endif
// index 4 was CANTester
#if HAL_PICCOLO_CAN_ENABLE
// @Group: PC_

12
libraries/AP_CANManager/AP_CANManager.cpp
View File

@ -29,7 +29,6 @@
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_PiccoloCAN/AP_PiccoloCAN.h>
#include <AP_EFI/AP_EFI_NWPMU.h>
#include "AP_CANTester.h"
#include <GCS_MAVLink/GCS.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL_Linux/CANSocketIface.h>
@ -226,17 +225,6 @@ void AP_CANManager::init()
AP_Param::load_object_from_eeprom((AP_PiccoloCAN*)_drivers[drv_num], AP_PiccoloCAN::var_info);
} else
#endif
#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES && HAL_ENABLE_CANTESTER
if (drv_type[drv_num] == Driver_Type_CANTester) {
_drivers[drv_num] = _drv_param[drv_num]._testcan = new CANTester;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::allocation_error("CANTester %d", drv_num + 1);
continue;
}
AP_Param::load_object_from_eeprom((CANTester*)_drivers[drv_num], CANTester::var_info);
} else
#endif
{
continue;

2
libraries/AP_CANManager/AP_CANManager.h
View File

@ -62,7 +62,7 @@ public:
// 2 was KDECAN -- do not re-use
// 3 was ToshibaCAN -- do not re-use
Driver_Type_PiccoloCAN = 4,
Driver_Type_CANTester = 5,
// 5 was CANTester
Driver_Type_EFI_NWPMU = 6,
Driver_Type_USD1 = 7,
Driver_Type_KDECAN = 8,

666
libraries/AP_CANManager/AP_CANTester.cpp
View File

@ -1,666 +0,0 @@
/*
* 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"
#include "AP_CANTester.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED && HAL_ENABLE_CANTESTER
#include <AP_SerialManager/AP_SerialManager.h>
#include <stdio.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_DroneCAN/AP_DroneCAN.h>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <AP_HAL/utility/sparse-endian.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,6:TEST_UAVCAN_ESC,7:TEST_UAVCAN_FD_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 DroneCAN DNA Test********");
if (test_uavcan_dna()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN DNA Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN DNA Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for DroneCAN_DNA_TEST");
}
break;
case CANTester::TEST_UAVCAN_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running DroneCAN ESC Test********");
if (test_uavcan_esc(false)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN ESC Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for DroneCAN_ESC_TEST");
}
break;
case CANTester::TEST_UAVCAN_FD_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running DroneCAN FD ESC Test********");
if (test_uavcan_esc(true)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN FD ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN FD ESC Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_FD_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++) {
DEV_PRINTF("Loopback Test Results %d->%d:\n", i, (i+1)%2);
DEV_PRINTF("num_tx: %lu, failed_tx: %lu\n",
(long unsigned int)_loopback_stats[i].num_tx, (long unsigned int)_loopback_stats[i].failed_tx);
DEV_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 = 8;
frame.setCanFD(stats.tx_seq%2); //every other frame is canfd if supported
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 != 8) {
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 = (10 | AP_HAL::CANFrame::FlagEFF);
memset(bo_frame.data, 0xA, sizeof(bo_frame.data));
bo_frame.dlc =8;//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(4000);
gcs().send_text(MAV_SEVERITY_ERROR, "Running Loopback test.");
//Send Dummy Frames to clear the error
while (!write_frame(0, bo_frame,100)) {}
bo_frame.id -= 1;
while (!write_frame(1, bo_frame,100)) {}
//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);
}
/*****************************************
* DroneCAN 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;
}
auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
if (!node) {
return false;
}
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");
delete 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.
*/
auto *client = new uavcan::DynamicNodeIDClient(*node);
if (!client) {
delete node;
return false;
}
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);
delete client;
delete node;
return false;
}
delete client;
delete node;
return true;
}
/***********************************************
* DroneCAN 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 = 50000/(AP_HAL::millis() - last_millis);
num_received = 0;
}
}
bool CANTester::test_uavcan_esc(bool enable_canfd)
{
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);
if (enable_canfd) {
node->enableCanFd();
} else {
node->disableCanFd();
}
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 = C_TO_KELVIN(124 + i);
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!
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

106
libraries/AP_CANManager/AP_CANTester.h
View File

@ -1,106 +0,0 @@
/*
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_DroneCAN/AP_DroneCAN.h>
#ifndef HAL_ENABLE_CANTESTER
#define HAL_ENABLE_CANTESTER 0
#endif
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED && HAL_ENABLE_CANTESTER
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 */
CLASS_NO_COPY(CANTester);
void init(uint8_t driver_index, bool enable_filters) override;
bool add_interface(AP_HAL::CANIface* can_iface) override;
static CANTester *get_cantester(uint8_t driver_index);
private:
enum {
TEST_NONE,
TEST_LOOPBACK,
TEST_BUSOFF_RECOVERY,
TEST_UAVCAN_DNA,
TEST_UAVCAN_ESC = 6,
TEST_UAVCAN_FD_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_uavcan_esc(bool enable_canfd);
// 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<DRONECAN_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;
};
#endif //#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS

22
libraries/AP_CANManager/README.md
View File

@ -1,25 +1,5 @@
## Testing And Debugging
In case unavailability of all the CAN modules that we support, I have created a CAN Driver called CANTester. Currently there are following modes inside CANTester:
**1: TEST_LOOPBACK**
This test verifies if the low level ifaces are functioning properly. To do that messages are sent so as to ensure TX buffers are filled to the max, and also read the data of the bus. Sequence of incoming data is tested, and also that no transmitted packet was dropped during transmit.
**2: TEST_BUSOFF_RECOVERY**
This test is only applicable to boards with H7 chip, where the bus off needs to be handled manually. This test is implemented to check if Bus off recovery is handled and recovered properly. Busoff error can be generated by simply shorting the CANH and CANL.
**3: TEST_UAVCAN_DNA**
This test simply creates a uavcan node allocation client and tries to get node allocated, the allocated node needs to match the requested node id for success. This tests AP_UAVCAN and underlying DNA library.
**4: TEST_TOSHIBA_CAN**
This test simply emulates a ToshibaCan ESC on a bus and handles the data sent by Toshiba CAN driver, and and also responds with ESC telemetry.
A lot more tests will be needing to be added overtime to ensure robustness and maintainability of CAN Ecosystem.
**Testing under SITL**
https://github.com/linux-can/can-utils contains a nice set of utility to do CAN related testings on Linux system. I used Ubuntu for this development, for Ubuntu systems you can simply download this tool using `sudo apt-get install can-utils`
@ -56,4 +36,4 @@ sudo cangw -A -s slcan0 -d vcan0 -e
* Dump can messages:
```
sudo candump vcan0
```
```