From b0e7dcc6fd9b85b1524fbf2ee0d47447922bac63 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 18 May 2020 18:49:13 +0200 Subject: [PATCH 1/2] Add support for extended CAN IDs Previously, coexistence with devices that use extended CAN IDs was not possible since the ODrive would simply clip the 18 upper bits of an extended message ID. This adds proper support for extended CAN IDs by changing `can_node_id` from 8 to 32 bit and introducing the new option `can_node_id_extended`. --- Firmware/MotorControl/axis.hpp | 4 +++- Firmware/communication/can_simple.cpp | 26 +++++++++++++------------- Firmware/communication/can_simple.hpp | 2 +- docs/can-protocol.md | 6 +++--- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 5b489a93..5f397df9 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -86,7 +86,8 @@ public: LockinConfig_t calibration_lockin = default_calibration(); LockinConfig_t sensorless_ramp = default_sensorless(); LockinConfig_t lockin; - uint8_t can_node_id = 0; // Both axes will have the same id to start + uint32_t can_node_id = 0; // Both axes will have the same id to start + bool can_node_id_extended = false; uint32_t can_heartbeat_rate_ms = 100; }; @@ -314,6 +315,7 @@ public: make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance), make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)), make_protocol_property("can_node_id", &config_.can_node_id), + make_protocol_property("can_node_id_extended", &config_.can_node_id_extended), make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms)), make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index da22bb70..7182d27a 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -26,7 +26,7 @@ void CANSimple::handle_can_message(can_Message_t& msg) { bool validAxis = false; for (uint8_t i = 0; i < AXIS_COUNT; i++) { - if (axes[i]->config_.can_node_id == nodeID) { + if ((axes[i]->config_.can_node_id == nodeID) && (axes[i]->config_.can_node_id_extended == msg.isExt)) { axis = axes[i]; if (!validAxis) { validAxis = true; @@ -137,7 +137,7 @@ void CANSimple::get_motor_error_callback(Axis* axis, can_Message_t& msg) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; txmsg.buf[0] = axis->motor_.error_; @@ -154,7 +154,7 @@ void CANSimple::get_encoder_error_callback(Axis* axis, can_Message_t& msg) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; txmsg.buf[0] = axis->encoder_.error_; @@ -171,7 +171,7 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; txmsg.buf[0] = axis->sensorless_estimator_.error_; @@ -184,7 +184,7 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) { } void CANSimple::set_axis_nodeid_callback(Axis* axis, can_Message_t& msg) { - axis->config_.can_node_id = msg.buf[0] & 0x3F; // Node ID bitmask + axis->config_.can_node_id = can_getSignal(msg, 0, 32, true); } void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) { @@ -199,7 +199,7 @@ void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; // Undefined behaviour! @@ -230,7 +230,7 @@ void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_SENSORLESS_ESTIMATES; // heartbeat ID - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; // Undefined behaviour! @@ -261,7 +261,7 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_ENCODER_COUNT; - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; txmsg.buf[0] = axis->encoder_.shadow_count_; @@ -325,7 +325,7 @@ void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_IQ; - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; uint32_t floatBytes; @@ -354,7 +354,7 @@ void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) { txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_GET_VBUS_VOLTAGE; - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; uint32_t floatBytes; @@ -386,7 +386,7 @@ void CANSimple::send_heartbeat(Axis* axis) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; txmsg.id += MSG_ODRIVE_HEARTBEAT; // heartbeat ID - txmsg.isExt = false; + txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; // Axis errors in 1st 32-bit value @@ -403,8 +403,8 @@ void CANSimple::send_heartbeat(Axis* axis) { odCAN->write(txmsg); } -uint8_t CANSimple::get_node_id(uint32_t msgID) { - return ((msgID >> NUM_CMD_ID_BITS) & 0x03F); // Upper 6 bits +uint32_t CANSimple::get_node_id(uint32_t msgID) { + return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits } uint8_t CANSimple::get_cmd_id(uint32_t msgID) { diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index 4f98f0a8..c4b6d6ed 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -64,7 +64,7 @@ class CANSimple { static void clear_errors_callback(Axis* axis, can_Message_t& msg); // Utility functions - static uint8_t get_node_id(uint32_t msgID); + static uint32_t get_node_id(uint32_t msgID); static uint8_t get_cmd_id(uint32_t msgID); // Fetch a specific signal from the message diff --git a/docs/can-protocol.md b/docs/can-protocol.md index f316e73e..4fc06275 100644 --- a/docs/can-protocol.md +++ b/docs/can-protocol.md @@ -16,7 +16,7 @@ We've implemented a very basic CAN protocol that we call "CAN Simple" to get use ### CAN Frame At its most basic, the CAN Simple frame looks like this: -* Upper 6 bits - Node ID - max 0x3F +* Upper 6 bits - Node ID - max 0x3F (or 0xFFFFFF when using extended CAN IDs) * Lower 5 bits - Command ID - max 0x1F To understand how the Node ID and Command ID interact, let's look at an example @@ -40,7 +40,7 @@ CMD ID | Name | Sender | Signals | Start byte | Signal Type | Bits | Factor | Of 0x003 | Get Motor Error\* | Axis | Motor Error | 0 | Unsigned Int | 32 | 1 | 0 | Intel 0x004 | Get Encoder Error\* | Axis | Encoder Error | 0 | Unsigned Int | 32 | 1 | 0 | Intel 0x005 | Get Sensorless Error\* | Axis | Sensorless Error | 0 | Unsigned Int | 32 | 1 | 0 | Intel -0x006 | Set Axis Node ID | Master | Axis CAN Node ID | 0 | Unsigned Int | 16 | 1 | 0 | Intel +0x006 | Set Axis Node ID | Master | Axis CAN Node ID | 0 | Unsigned Int | 32 | 1 | 0 | Intel 0x007 | Set Axis Requested State | Master | Axis Requested State | 0 | Unsigned Int | 32 | 1 | 0 | Intel 0x008 | Set Axis Startup Config | Master | - Not yet implemented - | - | - | - | - | - | - 0x009 | Get Encoder Estimates\* | Master | Encoder Pos Estimate
Encoder Vel Estimate | 0
4 | IEEE 754 Float
IEEE 754 Float | 32
32 | 1
1 | 0
0 | Intel
Intel @@ -72,7 +72,7 @@ Configuration of the CAN parameters should be done via USB before putting the de To set the desired baud rate, use `.can.set_baud_rate()`. The baud rate can be done without rebooting the device. If you'd like to keep the baud rate, simply call `.save_configuration()` before rebooting. -Each axis looks like a separate node on the bus. Thus, they've inherited a new configuration property: `can_node_id`. This ID can be from 0 to 63 (0x3F) inclusive. +Each axis looks like a separate node on the bus. Thus, they both have the two properties `can_node_id` and `can_node_id_extended`. The node ID can be from 0 to 63 (0x3F) inclusive, or, if extended CAN IDs are used, from 0 to 16777215 (0xFFFFFF). ### Example Configuration From 16059a9e9b37de612dfa44d38499a8fce4d7209c Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 18 May 2020 18:52:24 +0200 Subject: [PATCH 2/2] add test for extended CAN IDs --- tools/odrive/tests/can_test.py | 45 ++++++++++++++++++------------- tools/odrive/tests/test_runner.py | 4 +-- 2 files changed, 29 insertions(+), 20 deletions(-) diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index e69a8bd9..e3535f54 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -17,8 +17,8 @@ command_set = { 'estop': (0x002, []), # tested 'get_motor_error': (0x003, [('motor_error', 'I', 1)]), # untested 'get_encoder_error': (0x004, [('encoder_error', 'I', 1)]), # untested - 'get_sensorless_error': (0x004, [('sensorless_error', 'I', 1)]), # untested - 'set_node_id': (0x006, [('node_id', 'H', 1)]), # tested + 'get_sensorless_error': (0x005, [('sensorless_error', 'I', 1)]), # untested + 'set_node_id': (0x006, [('node_id', 'I', 1)]), # tested 'set_requested_state': (0x007, [('requested_state', 'I', 1)]), # tested # 0x008 not yet implemented 'get_encoder_estimates': (0x009, [('encoder_pos_estimate', 'f', 1), ('encoder_vel_estimate', 'f', 1)]), # partially tested @@ -39,7 +39,7 @@ command_set = { 'clear_errors': (0x018, []), # partially tested } -def command(bus, node_id_, cmd_name, **kwargs): +def command(bus, node_id_, extended_id, cmd_name, **kwargs): cmd_spec = command_set[cmd_name] cmd_id = cmd_spec[0] fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian @@ -49,10 +49,10 @@ def command(bus, node_id_, cmd_name, **kwargs): fields = [((kwargs[n] / s) if f == 'f' else int(kwargs[n] / s)) for (n, f, s) in cmd_spec[1]] data = struct.pack(fmt, *fields) - msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), data=data) + msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), extended_id=extended_id, data=data) bus.send(msg) -async def record_messages(bus, node_id, cmd_name, timeout = 5.0): +async def record_messages(bus, node_id, extended_id, cmd_name, timeout = 5.0): """ Returns an async generator that yields a dictionary for each CAN message that is received, provided that the CAN ID matches the expected value. @@ -71,7 +71,7 @@ async def record_messages(bus, node_id, cmd_name, timeout = 5.0): start = time.monotonic() while True: msg = await reader.get_message() - if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and not msg.is_remote_frame): + if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and (msg.is_extended_id == extended_id) and not msg.is_remote_frame): fields = struct.unpack(fmt, msg.data[:(struct.calcsize(fmt))]) res = {n: (fields[i] * s) for (i, (n, f, s)) in enumerate(cmd_spec[1])} res['t'] = time.monotonic() @@ -81,13 +81,13 @@ async def record_messages(bus, node_id, cmd_name, timeout = 5.0): finally: notifier.stop() -async def request(bus, node_id, cmd_name, timeout = 1.0): +async def request(bus, node_id, extended_id, cmd_name, timeout = 1.0): cmd_spec = command_set[cmd_name] cmd_id = cmd_spec[0] - msg_generator = record_messages(bus, node_id, cmd_name, timeout) + msg_generator = record_messages(bus, node_id, extended_id, cmd_name, timeout) - msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), data=[], is_remote_frame=True) + msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), extended_id=extended_id, data=[], is_remote_frame=True) bus.send(msg) async for msg in msg_generator: @@ -102,34 +102,43 @@ async def get_all(async_iterator): class TestSimpleCAN(): def get_test_cases(self, testrig: TestRig): for odrive in testrig.get_components(ODriveComponent): - can_interfaces = testrig.get_connected_components(odrive.can, CanInterfaceComponent) - yield (odrive, list(can_interfaces)) + can_interfaces = list(testrig.get_connected_components(odrive.can, CanInterfaceComponent)) + yield (odrive, can_interfaces, 0, False) # standard ID + yield (odrive, can_interfaces, 0xfedcba, True) # extended ID - def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, logger: Logger): + def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, node_id: int, extended_id: bool, logger: Logger): # make sure no gpio input is overwriting our values odrive.unuse_gpios() - node_id = 0 axis = odrive.handle.axis0 + axis.clear_errors() axis.config.can_node_id = node_id + axis.config.can_node_id_extended = extended_id time.sleep(0.1) - def my_cmd(cmd_name, **kwargs): command(canbus.handle, node_id, cmd_name, **kwargs) - def my_req(cmd_name, **kwargs): return asyncio.run(request(canbus.handle, node_id, cmd_name, **kwargs)) + def my_cmd(cmd_name, **kwargs): command(canbus.handle, node_id, extended_id, cmd_name, **kwargs) + def my_req(cmd_name, **kwargs): return asyncio.run(request(canbus.handle, node_id, extended_id, cmd_name, **kwargs)) def fence(): my_req('get_vbus_voltage') # fence to ensure the CAN command was sent test_assert_eq(my_req('get_vbus_voltage')['vbus_voltage'], odrive.handle.vbus_voltage, accuracy=0.01) my_cmd('set_node_id', node_id=node_id+20) - asyncio.run(request(canbus.handle, node_id+20, 'get_vbus_voltage')) + asyncio.run(request(canbus.handle, node_id+20, extended_id, 'get_vbus_voltage')) test_assert_eq(axis.config.can_node_id, node_id+20) # Reset node ID to default value - command(canbus.handle, node_id+20, 'set_node_id', node_id=node_id) + command(canbus.handle, node_id+20, extended_id, 'set_node_id', node_id=node_id) fence() test_assert_eq(axis.config.can_node_id, node_id) + # Check that extended node IDs are not carelessly projected to 6-bit IDs + extended_id = not extended_id + my_cmd('estop') # should not be accepted + extended_id = not extended_id + fence() + test_assert_eq(axis.error, errors.axis.ERROR_NONE) + axis.encoder.set_linear_count(123) test_assert_eq(my_req('get_encoder_estimates')['encoder_pos_estimate'], 123.0, accuracy=0.01) test_assert_eq(my_req('get_encoder_count')['encoder_shadow_count'], 123.0, accuracy=0.01) @@ -205,7 +214,7 @@ class TestSimpleCAN(): logger.debug('testing heartbeat...') # note that this will include the heartbeats that were received during the # watchdog test (which takes 4.8s). - heartbeats = asyncio.run(get_all(record_messages(canbus.handle, node_id, 'heartbeat', timeout = 1.0))) + heartbeats = asyncio.run(get_all(record_messages(canbus.handle, node_id, extended_id, 'heartbeat', timeout = 1.0))) test_assert_eq(len(heartbeats), 5.8 / 0.1, accuracy=0.05) test_assert_eq([msg['error'] for msg in heartbeats[0:35]], [0] * 35) # before watchdog expiry test_assert_eq([msg['error'] for msg in heartbeats[-10:]], [errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED] * 10) # after watchdog expiry diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index 80bb363d..a6dbffc0 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -679,7 +679,7 @@ def select_params(param_options): # Select parameters from the resource list # (this could be arbitrarily complex to improve parallelization of the tests) for combination in get_combinations(param_options): - if all_unique(combination): + if all_unique([x for x in combination if isinstance(x, Component)]): return list(combination) return None @@ -708,7 +708,7 @@ def run(tests): test_cases = list(test.get_test_cases(testrig)) if len(test_cases) == 0: - logger.warn('no resources are available to conduct the test {}'.format(type(test).__name__)) + logger.warn('no test cases are available to conduct the test {}'.format(type(test).__name__)) continue for test_case in test_cases: