zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_CANManager: remove ToshibaCAN support

apm_2208
Randy Mackay 3 years ago
parent
d503246be2
commit
533a16287e
5 changed files with 3 additions and 214 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      libraries/AP_CANManager/AP_CANDriver.cpp
  2. 8
      libraries/AP_CANManager/AP_CANManager.cpp
  3. 2
      libraries/AP_CANManager/AP_CANManager.h
  4. 201
      libraries/AP_CANManager/AP_CANTester.cpp
  5. 3
      libraries/AP_CANManager/AP_CANTester.h

3
libraries/AP_CANManager/AP_CANDriver.cpp
Unescape View File

@ -20,7 +20,6 @@ @@ -20,7 +20,6 @@
#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>
@ -32,7 +31,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { @@ -32,7 +31,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,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake
// @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, AP_CANManager::Driver_Type_UAVCAN),

8
libraries/AP_CANManager/AP_CANManager.cpp
Unescape View File

@ -25,7 +25,6 @@ @@ -25,7 +25,6 @@
#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_EFI/AP_EFI_NWPMU.h>
@ -215,13 +214,6 @@ void AP_CANManager::init() @@ -215,13 +214,6 @@ void AP_CANManager::init()
AP_Param::load_object_from_eeprom((AP_KDECAN*)_drivers[drv_num], AP_KDECAN::var_info);
#endif
} else if (drv_type[drv_num] == Driver_Type_ToshibaCAN) {
_drivers[drv_num] = new AP_ToshibaCAN;
if (_drivers[drv_num] == nullptr) {
AP_BoardConfig::allocation_error("ToshibaCAN %d", drv_num + 1);
continue;
}
} else if (drv_type[drv_num] == Driver_Type_PiccoloCAN) {
#if HAL_PICCOLO_CAN_ENABLE
_drivers[drv_num] = _drv_param[drv_num]._piccolocan = new AP_PiccoloCAN;

2
libraries/AP_CANManager/AP_CANManager.h
Unescape View File

@ -55,7 +55,7 @@ public: @@ -55,7 +55,7 @@ public:
Driver_Type_None = 0,
Driver_Type_UAVCAN = 1,
// 2 was KDECAN -- do not re-use
Driver_Type_ToshibaCAN = 3,
// 3 was ToshibaCAN -- do not re-use
Driver_Type_PiccoloCAN = 4,
Driver_Type_CANTester = 5,
Driver_Type_EFI_NWPMU = 6,

201
libraries/AP_CANManager/AP_CANTester.cpp
Unescape View File

@ -29,7 +29,6 @@ @@ -29,7 +29,6 @@
#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;
@ -39,7 +38,7 @@ const AP_Param::GroupInfo CANTester::var_info[] = { @@ -39,7 +38,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,4:TEST_TOSHIBA_CAN, 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,5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC, 7:TEST_UAVCAN_FD_ESC
// @User: Advanced
AP_GROUPINFO("ID", 1, CANTester, _test_id, 0),
@ -193,18 +192,6 @@ void CANTester::main_thread() @@ -193,18 +192,6 @@ void CANTester::main_thread()
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********");
@ -513,192 +500,6 @@ bool CANTester::test_uavcan_dna() @@ -513,192 +500,6 @@ bool CANTester::test_uavcan_dna()
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 *
*****************************************/

3
libraries/AP_CANManager/AP_CANTester.h
Unescape View File

@ -52,7 +52,6 @@ private: @@ -52,7 +52,6 @@ private:
TEST_LOOPBACK,
TEST_BUSOFF_RECOVERY,
TEST_UAVCAN_DNA,
TEST_TOSHIBA_CAN,
TEST_KDE_CAN,
TEST_UAVCAN_ESC,
TEST_UAVCAN_FD_ESC,
@ -79,8 +78,6 @@ private: @@ -79,8 +78,6 @@ private:
bool test_busoff_recovery();
bool test_uavcan_dna();
bool test_toshiba_can();
bool send_toshiba_can_reply(uint32_t cmd);
bool test_kdecan();

Write Preview
Loading…
Cancel
Save