Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-22 08:04:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #411 from madcowswe/feature/extended_can_id

Browse Source 
Add support for extended CAN IDs
This commit is contained in:
Paul Guenette
2020-05-19 18:53:45 -04:00
committed by GitHub
parent bd26cc9ef3 16059a9e9b
commit 62506fbbe1
6 changed files with 49 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.hpp
+3 -1
View File
@@ -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()),
Firmware/communication/can_simple.cpp
+13 -13
View File
@@ -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<uint32_t>(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) {
Firmware/communication/can_simple.hpp
+1 -1
View File
@@ -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
docs/can-protocol.md
+3 -3
View File
@@ -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<br>Encoder Vel Estimate | 0<br>4 | IEEE 754 Float<br>IEEE 754 Float | 32<br>32 | 1<br>1 | 0<br>0 | Intel<br>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 `<odrv>.can.set_baud_rate(<value>)`. The baud rate can be done without rebooting the device. If you'd like to keep the baud rate, simply call `<odrv>.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
tools/odrive/tests/can_test.py
+27 -18
View File
@@ -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
tools/odrive/tests/test_runner.py
+2 -2
View File
@@ -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: