Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_CANManager: add support for CANFD tests

Browse Source
This commit is contained in:
Siddharth Purohit 2021-05-03 18:22:35 +05:30 committed by Andrew Tridgell
parent 068208e4d7
commit 287151a858
2 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_CANManager/AP_CANTester.cpp
View File

@ -39,9 +39,10 @@ const AP_Param::GroupInfo CANTester::var_info[] = {
// @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
// @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, 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
@ -219,7 +220,7 @@ void CANTester::main_thread()
case CANTester::TEST_UAVCAN_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN ESC Test********");
if (test_uavcan_esc()) {
if (test_uavcan_esc(false)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Fail********");
@ -228,6 +229,18 @@ void CANTester::main_thread()
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_ESC_TEST");
}
break;
case CANTester::TEST_UAVCAN_FD_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN FD ESC Test********");
if (test_uavcan_esc(true)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN 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;
}
@ -340,7 +353,8 @@ void CANTester::reset_frame(AP_HAL::CANFrame& frame)
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;
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));
@ -349,7 +363,7 @@ void CANTester::create_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL
void CANTester::check_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
if (frame.dlc != AP_HAL::CANFrame::MaxDataLen) {
if (frame.dlc != 8) {
stats.bad_rx_data++;
}
@ -380,7 +394,7 @@ bool CANTester::test_busoff_recovery()
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 = AP_HAL::CANFrame::MaxDataLen;
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.");
@ -428,7 +442,7 @@ bool CANTester::test_uavcan_dna()
return false;
}
auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false);
if (!node) {
return false;
}
@ -746,12 +760,12 @@ void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::e
num_received++;
// update rate every 50 packets
if (num_received == 50) {
uavcan_esc_command_rate = 100000/(AP_HAL::millis() - last_millis);
uavcan_esc_command_rate = 50000/(AP_HAL::millis() - last_millis);
num_received = 0;
}
}
bool CANTester::test_uavcan_esc()
bool CANTester::test_uavcan_esc(bool enable_canfd)
{
bool ret = true;
uavcan::CanIfaceMgr _uavcan_iface_mgr {};
@ -763,7 +777,7 @@ bool CANTester::test_uavcan_esc()
uavcan::Node<0> *node = nullptr;
{
node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false);
if (node == nullptr) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node");
ret = false;
@ -840,7 +854,7 @@ bool CANTester::test_uavcan_esc()
goto exit;
}
esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node);
esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node, enable_canfd);
if (esc_status_publisher == nullptr) {
ret = false;
goto exit;

3
libraries/AP_CANManager/AP_CANTester.h
View File

@ -51,6 +51,7 @@ private:
TEST_TOSHIBA_CAN,
TEST_KDE_CAN,
TEST_UAVCAN_ESC,
TEST_UAVCAN_FD_ESC,
TEST_END,
};
@ -79,7 +80,7 @@ private:
bool test_kdecan();
bool test_uavcan_esc();
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);