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 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: