diff --git a/libraries/AP_CANManager/AP_CANTester.cpp b/libraries/AP_CANManager/AP_CANTester.cpp index 15abf98de8..e68847d33e 100644 --- a/libraries/AP_CANManager/AP_CANTester.cpp +++ b/libraries/AP_CANManager/AP_CANTester.cpp @@ -30,7 +30,6 @@ #include #include #include -#include "AP_CANTester_KDECAN.h" extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo CANTester::var_info[] = { @@ -38,7 +37,7 @@ 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,5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC, 7:TEST_UAVCAN_FD_ESC + // @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), @@ -192,18 +191,6 @@ void CANTester::main_thread() gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for DroneCAN_DNA_TEST"); } 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 DroneCAN ESC Test********"); @@ -500,47 +487,6 @@ bool CANTester::test_uavcan_dna() 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; -} - /*********************************************** * DroneCAN ESC * * *********************************************/ diff --git a/libraries/AP_CANManager/AP_CANTester.h b/libraries/AP_CANManager/AP_CANTester.h index 6fb07c7618..9b51a07182 100644 --- a/libraries/AP_CANManager/AP_CANTester.h +++ b/libraries/AP_CANManager/AP_CANTester.h @@ -40,7 +40,6 @@ public: 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); @@ -51,8 +50,8 @@ private: TEST_LOOPBACK, TEST_BUSOFF_RECOVERY, TEST_UAVCAN_DNA, - TEST_KDE_CAN, - TEST_UAVCAN_ESC, + + TEST_UAVCAN_ESC = 6, TEST_UAVCAN_FD_ESC, TEST_END, }; @@ -78,8 +77,6 @@ private: bool test_uavcan_dna(); - bool test_kdecan(); - bool test_uavcan_esc(bool enable_canfd); // write frame on CAN bus, returns true on success @@ -104,7 +101,6 @@ private: 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 diff --git a/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp b/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp deleted file mode 100644 index a3e7ea979a..0000000000 --- a/libraries/AP_CANManager/AP_CANTester_KDECAN.cpp +++ /dev/null @@ -1,236 +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 . - */ -/* - * Author: Francisco Ferreira - * Modified for CANManager by Siddharth B Purohit - */ -#include -#include - -#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED -#include "AP_CANTester_KDECAN.h" -#include "AP_CANManager.h" -#include -#include -#include -#include - -#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; iis_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) { - uint64_t mcu_id; - memcpy (&mcu_id, recv_frame.data, sizeof(mcu_id)); - mcu_id = be64toh(mcu_id); - if (_esc_info[i].mcu_id == mcu_id) { - 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_ms != 0 - && _esc_info[i].enum_timeout_ms >= AP_HAL::millis()) { - _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_ms = 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_ms = AP_HAL::millis() + be16toh_ptr(&recv_frame.data[0]); - gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Starting enumeration for ESC %d, timeout %u", i, (unsigned)_esc_info[i].enum_timeout_ms); - i++; - continue; - } - case TELEMETRY_OBJ_ADDR: { - uint8_t data[8] {}; - put_le16_ptr(&data[0], get_random16()); - put_le16_ptr(&data[2], get_random16()); - put_le16_ptr(&data[4], 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) -{ - DEV_PRINTF("KDECANTester: TimeStamp: %u\n", (unsigned)AP_HAL::micros()); - for (uint16_t i=0; i<100; i++) { - if (counters[i].frame_id == 0) { - break; - } - DEV_PRINTF("0x%08x: %u\n", (unsigned)counters[i].frame_id, (unsigned)counters[i].count); - counters[i].count = 0; - } -} - -bool AP_CANTester_KDECAN::send_enumeration(uint8_t num) -{ - if (_esc_info[num].enum_timeout_ms == 0 || - AP_HAL::millis() > _esc_info[num].enum_timeout_ms) { - _esc_info[num].enum_timeout_ms = 0; - gcs().send_text(MAV_SEVERITY_ALERT, "KDECANTester: Not running enumeration for ESC %d\n", num); - return false; - } - - while (true) { - uint64_t mcu = 0; - 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, - (uint8_t*)&mcu, - 8 }; - int16_t res = _can_iface->send(res_frame, AP_HAL::micros64() + 1000, 0); - if (res == 1) { - return true; - } - } -} -#endif diff --git a/libraries/AP_CANManager/AP_CANTester_KDECAN.h b/libraries/AP_CANManager/AP_CANTester_KDECAN.h deleted file mode 100644 index 0bc2d0740c..0000000000 --- a/libraries/AP_CANManager/AP_CANTester_KDECAN.h +++ /dev/null @@ -1,82 +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 . - */ -/* - * Author: Francisco Ferreira - * Modified for CANManager by Siddharth B Purohit - */ - -#include "AP_CANDriver.h" -#include -#include -#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; - uint32_t enum_timeout_ms; - - esc_info() : node_id(1), mcu_id(0), enum_timeout_ms(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 diff --git a/libraries/AP_CANManager/README.md b/libraries/AP_CANManager/README.md index ae170560e8..f341b441db 100644 --- a/libraries/AP_CANManager/README.md +++ b/libraries/AP_CANManager/README.md @@ -18,10 +18,6 @@ This test simply creates a uavcan node allocation client and tries to get node a 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. -**5: TEST_KDE_CAN** - -This test simply emulates a KDECAN ESC on a bus and handles the data sent by KDE CAN driver, and and also responds with ESC telemetry. Need to supply this command `long MAV_CMD_PREFLIGHT_UAVCAN 1` after enabling KDECAN because need to do Enumeration before use. - A lot more tests will be needing to be added overtime to ensure robustness and maintainability of CAN Ecosystem. **Testing under SITL**