From 82b1fd1326d82d2978bb96530168bcb8eb438370 Mon Sep 17 00:00:00 2001 From: Nick Williams <5554798+metanoic@users.noreply.github.com> Date: Sun, 22 Jul 2018 10:12:25 -0500 Subject: [PATCH 01/11] taskName is deprecated. Use 'label' instead. https://github.com/Microsoft/vscode/issues/29852 --- Firmware/.vscode/tasks.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/.vscode/tasks.json b/Firmware/.vscode/tasks.json index 2376b650..d5b7d969 100644 --- a/Firmware/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -4,7 +4,7 @@ "version": "2.0.0", "tasks": [ { - "taskName": "build", + "label": "build", "type": "shell", "command": "make", "group": { @@ -19,13 +19,13 @@ ] }, { - "taskName": "flash", + "label": "flash", "type": "shell", "command": "make flash", "problemMatcher": [] }, { - "taskName": "openocd", + "label": "openocd", "type": "shell", "command": "openocd -f \"interface/stlink-v2.cfg\" -f \"target/stm32f4x_stlink.cfg\" -c \"gdb_port 3333; log_output openocd.log\"", "problemMatcher": [] From f05cfa01a98134cd411c57caae740bfee65864b4 Mon Sep 17 00:00:00 2001 From: Nick Williams <5554798+metanoic@users.noreply.github.com> Date: Sun, 22 Jul 2018 10:31:11 -0500 Subject: [PATCH 02/11] add flashbmp to makefile and accompanying task --- Firmware/.vscode/launch.json | 18 ++++++++++++++++-- Firmware/.vscode/tasks.json | 8 +++++++- Firmware/Makefile | 9 +++++++++ 3 files changed, 32 insertions(+), 3 deletions(-) diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 71e7db4c..1b99a2a6 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -9,7 +9,7 @@ "type": "cortex-debug", "servertype": "openocd", "request": "launch", - "name": "Debug ODrive", + "name": "Debug ODrive - ST-Link", "executable": "${workspaceRoot}/build/ODriveFirmware.elf", "configFiles": [ "interface/stlink-v2.cfg", @@ -22,7 +22,7 @@ "type": "cortex-debug", "servertype": "openocd", "request": "launch", - "name": "Debug ODrive - FreeRTOS", + "name": "Debug ODrive - ST-Link - FreeRTOS", "executable": "${workspaceRoot}/build/ODriveFirmware.elf", "rtos": "FreeRTOS", "configFiles": [ @@ -31,5 +31,19 @@ ], "cwd": "${workspaceRoot}" }, + { + // For the Cortex-Debug extensions + "type": "cortex-debug", + "servertype": "bmp", + "request": "launch", + "name": "Debug ODrive - Black Magic Probe", + "executable": "${workspaceRoot}/build/ODriveFirmware.elf", + "device": "STM32F4xx", + "BMPGDBSerialPort": "${env:BMP_PORT}", + "interface": "swd", + "targetId": 1, + "armToolchainPath": "${env:ARM_GCC_ROOT}/bin/", + "cwd": "${workspaceRoot}" + } ] } \ No newline at end of file diff --git a/Firmware/.vscode/tasks.json b/Firmware/.vscode/tasks.json index d5b7d969..c82adc83 100644 --- a/Firmware/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -19,11 +19,17 @@ ] }, { - "label": "flash", + "label": "flash - ST-Link", "type": "shell", "command": "make flash", "problemMatcher": [] }, + { + "label": "flash - Black Magic Probe", + "type": "shell", + "command": "make flashbmp", + "problemMatcher": [] + }, { "label": "openocd", "type": "shell", diff --git a/Firmware/Makefile b/Firmware/Makefile index 82ae758c..b498343c 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -20,6 +20,15 @@ flash: all -c 'reset run' \ -c exit +flashbmp: all + arm-none-eabi-gdb --ex 'target extended-remote $(BMP_PORT)' \ + --ex 'monitor swdp_scan' \ + --ex 'attach 1' \ + --ex 'load' \ + --ex 'detach' \ + --ex 'quit' \ + $(FIRMWARE) + gdb: all arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit From b2812f167c8ddb8f2005850a4e22945dfdc2c252 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 5 Feb 2020 19:38:07 -0800 Subject: [PATCH 03/11] execute property write hooks --- CHANGELOG.md | 1 + Firmware/fibre/cpp/include/fibre/protocol.hpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index bd9beae1..e9842158 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * AC Induction Motor support. * Tracking of rotor flux through rotor time constant * Automatic d axis current for Maximum Torque Per Amp (MTPA) +* ASCII "w" commands now execute write hooks. ### Changed * Changed ratiometric `motor.config.current_lim_tolerance` to absolute `motor.config.current_lim_margin` diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp index c1f3cc68..77b031f2 100644 --- a/Firmware/fibre/cpp/include/fibre/protocol.hpp +++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp @@ -812,7 +812,11 @@ public: // special-purpose function - to be moved bool set_string(char * buffer, size_t length) final { - return from_string(buffer, length, property_, 0); + bool wrote = from_string(buffer, length, property_, 0); + if (wrote && written_hook_ != nullptr) { + written_hook_(ctx_); + } + return wrote; } bool set_from_float(float value) final { From b0e7dcc6fd9b85b1524fbf2ee0d47447922bac63 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 18 May 2020 18:49:13 +0200 Subject: [PATCH 04/11] 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 05/11] 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: From 3d710a538509c7bbcffb700f4c5c27ca6ecdd35e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 11:57:04 +0200 Subject: [PATCH 06/11] don't treat watchdog_timeout == 0 as special case this special treatment is now redundant with enable_watchdog --- Firmware/MotorControl/axis.cpp | 2 -- Firmware/MotorControl/axis.hpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index b68ef01b..1d416e68 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -202,9 +202,7 @@ void Axis::watchdog_feed() { // @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired. bool Axis::watchdog_check() { - // reset value = 0 means watchdog disabled. if (!config_.enable_watchdog) return true; - if (get_watchdog_reset() == 0) return true; // explicit check here to ensure that we don't underflow back to UINT32_MAX if (watchdog_current_value_ > 0) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 5f397df9..020cd12a 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -76,7 +76,7 @@ public: float counts_per_step = 2.0f; - float watchdog_timeout = 0.0f; // [s] (0 disables watchdog) + float watchdog_timeout = 0.0f; // [s] bool enable_watchdog = false; // Defaults loaded from hw_config in load_configuration in main.cpp From 3d073250222fafb28a7f12a3096846f362b38c03 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 12:27:28 +0200 Subject: [PATCH 07/11] remove aligned formatting of assignments --- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/axis.hpp | 6 ++-- Firmware/MotorControl/controller.cpp | 4 +-- Firmware/MotorControl/controller.hpp | 44 +++++++++++++------------- Firmware/MotorControl/encoder.cpp | 38 +++++++++++----------- Firmware/MotorControl/encoder.hpp | 8 ++--- Firmware/MotorControl/endstop.hpp | 4 +-- Firmware/MotorControl/motor.cpp | 4 +-- Firmware/communication/can_helpers.hpp | 4 +-- Firmware/communication/can_simple.cpp | 6 ++-- 10 files changed, 60 insertions(+), 60 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 1d416e68..cb704404 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -87,7 +87,7 @@ static void run_state_machine_loop_wrapper(void* ctx) { // @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t)); - thread_id_ = osThreadCreate(osThread(thread_def), this); + thread_id_ = osThreadCreate(osThread(thread_def), this); thread_id_valid_ = true; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 020cd12a..6be3c60c 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -139,10 +139,10 @@ public: bool watchdog_check(); void clear_errors() { - motor_.error_ = Motor::ERROR_NONE; - controller_.error_ = Controller::ERROR_NONE; + motor_.error_ = Motor::ERROR_NONE; + controller_.error_ = Controller::ERROR_NONE; sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE; - encoder_.error_ = Encoder::ERROR_NONE; + encoder_.error_ = Encoder::ERROR_NONE; error_ = Axis::ERROR_NONE; } diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index beb93528..ed9163f7 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -165,8 +165,8 @@ bool Controller::update(float* current_setpoint_output) { } break; case INPUT_MODE_CURRENT_RAMP: { float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate); - float full_step = input_current_ - current_setpoint_; - float step = std::clamp(full_step, -max_step_size, max_step_size); + float full_step = input_current_ - current_setpoint_; + float step = std::clamp(full_step, -max_step_size, max_step_size); current_setpoint_ += step; } break; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 56a60020..4d207a83 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -44,34 +44,34 @@ public: bool calib_anticogging = false; float calib_pos_threshold = 1.0f; float calib_vel_threshold = 1.0f; - float cogging_ratio = 1.0f; - bool enable = true; + float cogging_ratio = 1.0f; + bool enable = true; } Anticogging_t; struct Config_t { ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t - float pos_gain = 20.0f; // [(counts/s) / counts] - float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] - // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] - float vel_limit = 20000.0f; // [counts/s] Infinity to disable. - float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. - float vel_ramp_rate = 10000.0f; // [(counts/s) / s] - float current_ramp_rate = 1.0f; // A / sec - bool setpoints_in_cpr = false; - float inertia = 0.0f; // [A/(count/s^2)] - float input_filter_bandwidth = 2.0f; // [1/s] - float homing_speed = 2000.0f; // [counts/s] + float pos_gain = 20.0f; // [(counts/s) / counts] + float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] + float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_limit = 20000.0f; // [counts/s] Infinity to disable. + float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. + float vel_ramp_rate = 10000.0f; // [(counts/s) / s] + float current_ramp_rate = 1.0f; // A / sec + bool setpoints_in_cpr = false; + float inertia = 0.0f; // [A/(count/s^2)] + float input_filter_bandwidth = 2.0f; // [1/s] + float homing_speed = 2000.0f; // [counts/s] Anticogging_t anticogging; - float gain_scheduling_width = 10.0f; - bool enable_gain_scheduling = false; - bool enable_vel_limit = true; - bool enable_overspeed_error = true; - bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator) - uint8_t axis_to_mirror = -1; - float mirror_ratio = 1.0f; - uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration() + float gain_scheduling_width = 10.0f; + bool enable_gain_scheduling = false; + bool enable_vel_limit = true; + bool enable_overspeed_error = true; + bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator) + uint8_t axis_to_mirror = -1; + float mirror_ratio = 1.0f; + uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration() }; explicit Controller(Config_t& config); diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 77b809aa..e0e29a0b 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -109,8 +109,8 @@ void Encoder::set_linear_count(int32_t count) { uint32_t prim = cpu_enter_critical(); // Update states - shadow_count_ = count; - pos_estimate_ = static_cast(count); + shadow_count_ = count; + pos_estimate_ = static_cast(count); tim_cnt_sample_ = count; //Write hardware last @@ -132,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { // Update states count_in_cpr_ = mod(count, config_.cpr); - pos_cpr_ = static_cast(count_in_cpr_); + pos_cpr_ = static_cast(count_in_cpr_); cpu_exit_critical(prim); } @@ -182,7 +182,7 @@ bool Encoder::run_direction_find() { // TODO: Do the scan with current, not voltage! bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; - static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast(current_meas_hz)); + static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast(current_meas_hz)); // Require index found if enabled if (config_.use_index && !index_found_) { @@ -219,7 +219,7 @@ bool Encoder::run_offset_calibration() { // scan forward i = 0; axis_->run_control_loop([&]() { - float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) - config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) - config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -248,9 +248,9 @@ bool Encoder::run_offset_calibration() { //TODO avoid recomputing elec_rad_per_enc every time // Check CPR - float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc; - calib_scan_response_ = std::abs(shadow_count_ - init_enc_val); + calib_scan_response_ = std::abs(shadow_count_ - init_enc_val); if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) { set_error(ERROR_CPR_POLEPAIRS_MISMATCH); return false; @@ -259,7 +259,7 @@ bool Encoder::run_offset_calibration() { // scan backwards i = 0; axis_->run_control_loop([&]() { - float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) + config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) + config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -273,8 +273,8 @@ bool Encoder::run_offset_calibration() { if (axis_->error_ != Axis::ERROR_NONE) return false; - config_.offset = encvaluesum / (num_steps * 2); - int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); + config_.offset = encvaluesum / (num_steps * 2); + int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); config_.offset_float = static_cast(residual) / static_cast(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase is_ready_ = true; @@ -334,12 +334,12 @@ bool Encoder::abs_spi_init(){ spi->Init.CLKPhase = SPI_PHASE_2EDGE; spi->Init.NSS = SPI_NSS_SOFT; spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; - spi->Init.FirstBit = SPI_FIRSTBIT_MSB; - spi->Init.TIMode = SPI_TIMODE_DISABLE; - spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - spi->Init.CRCPolynomial = 10; + spi->Init.FirstBit = SPI_FIRSTBIT_MSB; + spi->Init.TIMode = SPI_TIMODE_DISABLE; + spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + spi->Init.CRCPolynomial = 10; if (mode_ == MODE_SPI_ABS_AEAT) { - spi->Init.CLKPolarity = SPI_POLARITY_HIGH; + spi->Init.CLKPolarity = SPI_POLARITY_HIGH; } HAL_SPI_DeInit(spi); HAL_SPI_Init(spi); @@ -508,9 +508,9 @@ bool Encoder::update() { pos_estimate_ += current_meas_period * vel_estimate_; pos_cpr_ += current_meas_period * vel_estimate_; // discrete phase detector - float delta_pos = static_cast(shadow_count_) - static_cast(std::floor(pos_estimate_)); + float delta_pos = static_cast(shadow_count_) - static_cast(std::floor(pos_estimate_)); float delta_pos_cpr = static_cast(count_in_cpr_) - static_cast(std::floor(pos_cpr_)); - delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(config_.cpr)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(config_.cpr)); // pll feedback pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; @@ -518,7 +518,7 @@ bool Encoder::update() { vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr; bool snap_to_zero_vel = false; if (std::abs(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) { - vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter + vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter snap_to_zero_vel = true; } @@ -544,7 +544,7 @@ bool Encoder::update() { //// compute electrical phase //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); - float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float); + float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float); // ph = fmodf(ph, 2*M_PI); phase_ = wrap_pm_pi(ph); diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 28830c38..fcf94101 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -8,10 +8,10 @@ class Encoder { public: enum Error_t { - ERROR_NONE = 0, - ERROR_UNSTABLE_GAIN = 0x01, - ERROR_CPR_POLEPAIRS_MISMATCH = 0x02, - ERROR_NO_RESPONSE = 0x04, + ERROR_NONE = 0, + ERROR_UNSTABLE_GAIN = 0x01, + ERROR_CPR_POLEPAIRS_MISMATCH = 0x02, + ERROR_NO_RESPONSE = 0x04, ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, ERROR_ILLEGAL_HALL_STATE = 0x10, ERROR_INDEX_NOT_FOUND_YET = 0x20, diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 6983fa6c..e9cb23ab 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -42,8 +42,8 @@ class Endstop { } private: - bool pin_state_ = false; - float pos_when_pressed_ = 0.0f; + bool pin_state_ = false; + float pos_when_pressed_ = 0.0f; Timer debounceTimer_; }; #endif \ No newline at end of file diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 223aba1c..faafb7f5 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -329,7 +329,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) { float c = our_arm_cos_f32(pwm_phase); float s = our_arm_sin_f32(pwm_phase); float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; + float v_beta = c*v_q + s*v_d; return enqueue_voltage_timings(v_alpha, v_beta); } @@ -400,7 +400,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha float c_p = our_arm_cos_f32(pwm_phase); float s_p = our_arm_sin_f32(pwm_phase); float mod_alpha = c_p * mod_d - s_p * mod_q; - float mod_beta = c_p * mod_q + s_p * mod_d; + float mod_beta = c_p * mod_q + s_p * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl.final_v_alpha = mod_to_V * mod_alpha; diff --git a/Firmware/communication/can_helpers.hpp b/Firmware/communication/can_helpers.hpp index b5e40385..779b6a7a 100644 --- a/Firmware/communication/can_helpers.hpp +++ b/Firmware/communication/can_helpers.hpp @@ -26,7 +26,7 @@ struct can_Signal_t { template T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) { uint64_t tempVal = 0; - uint64_t mask = (1ULL << length) - 1; + uint64_t mask = (1ULL << length) - 1; if (isIntel) { std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); @@ -50,7 +50,7 @@ float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t len template void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) { - T scaledVal = (val - offset) / factor; + T scaledVal = (val - offset) / factor; uint64_t valAsBits = 0; std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal)); diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index 7182d27a..b00b13e0 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -279,14 +279,14 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) { } void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true); - axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); + axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true); + axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); axis->controller_.input_current_ = can_getSignal(msg, 48, 16, true, 0.01f, 0); axis->controller_.input_pos_updated(); } void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.input_vel_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); + axis->controller_.input_vel_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); axis->controller_.input_current_ = can_getSignal(msg, 32, 16, true, 0.01f, 0.0f); } From b3912d58c2e62880f0b542ef38c09cf8244b5dcc Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 13:11:05 +0200 Subject: [PATCH 08/11] revert numerical static_cast to C style casts As per https://github.com/madcowswe/ODrive/pull/410#discussion_r427725994 --- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/encoder.cpp | 24 ++++++++++++------------ Firmware/MotorControl/low_level.cpp | 2 +- Firmware/MotorControl/motor.cpp | 2 +- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index cb704404..d7659d35 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -377,7 +377,7 @@ bool Axis::run_homing() { controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating // Set our current position in encoder counts to make control more logical - encoder_.set_linear_count(static_cast(controller_.pos_setpoint_)); + encoder_.set_linear_count((int32_t)controller_.pos_setpoint_); controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index ed9163f7..60af0ee0 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -293,7 +293,7 @@ bool Controller::update(float* current_setpoint_output) { // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_valid_ && config_.anticogging.enable) { - Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast(anticogging_pos), 3600), 0, 3600)]; + Iq += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)]; } float v_err = 0.0f; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index e0e29a0b..d1545fc1 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -110,7 +110,7 @@ void Encoder::set_linear_count(int32_t count) { // Update states shadow_count_ = count; - pos_estimate_ = static_cast(count); + pos_estimate_ = (float)count; tim_cnt_sample_ = count; //Write hardware last @@ -132,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { // Update states count_in_cpr_ = mod(count, config_.cpr); - pos_cpr_ = static_cast(count_in_cpr_); + pos_cpr_ = (float)count_in_cpr_; cpu_exit_critical(prim); } @@ -182,7 +182,7 @@ bool Encoder::run_direction_find() { // TODO: Do the scan with current, not voltage! bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; - static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast(current_meas_hz)); + static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz); // Require index found if enabled if (config_.use_index && !index_found_) { @@ -219,7 +219,7 @@ bool Encoder::run_offset_calibration() { // scan forward i = 0; axis_->run_control_loop([&]() { - float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) - config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -248,7 +248,7 @@ bool Encoder::run_offset_calibration() { //TODO avoid recomputing elec_rad_per_enc every time // Check CPR - float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc; calib_scan_response_ = std::abs(shadow_count_ - init_enc_val); if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) { @@ -259,7 +259,7 @@ bool Encoder::run_offset_calibration() { // scan backwards i = 0; axis_->run_control_loop([&]() { - float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) + config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -275,7 +275,7 @@ bool Encoder::run_offset_calibration() { config_.offset = encvaluesum / (num_steps * 2); int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); - config_.offset_float = static_cast(residual) / static_cast(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase + config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase is_ready_ = true; return true; @@ -508,13 +508,13 @@ bool Encoder::update() { pos_estimate_ += current_meas_period * vel_estimate_; pos_cpr_ += current_meas_period * vel_estimate_; // discrete phase detector - float delta_pos = static_cast(shadow_count_) - static_cast(std::floor(pos_estimate_)); - float delta_pos_cpr = static_cast(count_in_cpr_) - static_cast(std::floor(pos_cpr_)); - delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(config_.cpr)); + float delta_pos = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_)); + float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; - pos_cpr_ = fmodf_pos(pos_cpr_, static_cast(config_.cpr)); + pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr; bool snap_to_zero_vel = false; if (std::abs(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) { @@ -543,7 +543,7 @@ bool Encoder::update() { //// compute electrical phase //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float); // ph = fmodf(ph, 2*M_PI); phase_ = wrap_pm_pi(ph); diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index fda04cf4..0dcc8754 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -647,7 +647,7 @@ void update_brake_current() { return; } - int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int high_on = (int)(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; if (low_off < 0) low_off = 0; safety_critical_apply_brake_resistor_timings(low_off, high_on); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index faafb7f5..ac03e863 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -216,7 +216,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) { // TODO check Ibeta balance to verify good motor connection bool Motor::measure_phase_resistance(float test_current, float max_voltage) { static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = static_cast(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s + static const int num_test_cycles = (int)(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s float test_voltage = 0.0f; size_t i = 0; From 5ca7ae1f3e804a4bb70b27393e7b870bde0df26d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 13:26:43 +0200 Subject: [PATCH 09/11] change inverter temp initial value to NaN --- Firmware/MotorControl/motor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 8c627aa7..58f27d83 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -193,7 +193,7 @@ public: DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault; DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) float thermal_current_lim_ = 10.0f; //[A] - float inverter_temp_ = 20.0f; + float inverter_temp_ = NAN; // [°C] NaN while the ODrive is initializing. // Communication protocol definitions auto make_protocol_definitions() { From 8d7ddf203c9dd7aae3a710d7eda64f0b66bd7c97 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 13:30:30 +0200 Subject: [PATCH 10/11] remove obsolete comment --- Firmware/MotorControl/utils.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp index 224f3716..0c191dfe 100644 --- a/Firmware/MotorControl/utils.hpp +++ b/Firmware/MotorControl/utils.hpp @@ -86,7 +86,6 @@ static inline float wrap_pm(float x, float pm_range) { return fmodf_pos(x + pm_range, 2.0f * pm_range) - pm_range; } -//beware of inserting large angles! static inline float wrap_pm_pi(float theta) { return wrap_pm(theta, M_PI); } From 9446a28142882ade336df206ce1dfa0cd26b067a Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 15:19:46 +0200 Subject: [PATCH 11/11] increase test tolerance --- tools/odrive/tests/pwm_input_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/odrive/tests/pwm_input_test.py b/tools/odrive/tests/pwm_input_test.py index a37cc155..f871b4dd 100644 --- a/tools/odrive/tests/pwm_input_test.py +++ b/tools/odrive/tests/pwm_input_test.py @@ -81,7 +81,7 @@ class TestPwmInput(): full_scale = max_val - min_val slope, offset, fitted_curve = fit_sawtooth(data, min_val, max_val) test_assert_eq(slope, full_scale / 1.0, accuracy=0.001) - test_curve_fit(data, fitted_curve, max_mean_err = full_scale * 0.02, inlier_range = full_scale * 0.03, max_outliers = len(data[:,0]) * 0.01) + test_curve_fit(data, fitted_curve, max_mean_err = full_scale * 0.02, inlier_range = full_scale * 0.05, max_outliers = len(data[:,0]) * 0.01)