From 42384f3ef9b8b7eeec8f3e91818a30673e16414b Mon Sep 17 00:00:00 2001 From: Paul Belanger Date: Wed, 6 Mar 2019 14:00:47 -0500 Subject: [PATCH 1/2] Initial implementation of comms watchdog - Added watchdog timeout property to Axis::Config_t: axis.config.watchdog_timeout - Added axis protocol function to feed watchdog timer: axis.watchdog_feed() - Axis::run_control_loop now checks for watchdog expiration. - Ascii protocol: add support for watchdog - The following ASCII commands now automatically update the watchdog: p, v, t, c, q - Added a 'u' command to update the watchdog of a motor without modifying setpoints. - Updated ascii protocol documentation to reflect new commands and effects. - Updated getting started guide to mention watchdog settings and functions in protocol. Please note: due to unavailability of hardware at this time, I have been unable to test this code on an Odrive. --- Firmware/MotorControl/axis.cpp | 33 +++++++++++++++++++++++ Firmware/MotorControl/axis.hpp | 22 +++++++++++++-- Firmware/communication/ascii_protocol.cpp | 29 +++++++++++++++++--- docs/ascii-protocol.md | 18 +++++++++++++ docs/getting-started.md | 12 +++++++++ 5 files changed, 108 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 8be43d15..759d237b 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -28,6 +28,7 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, trap_.axis_ = this; decode_step_dir_pins(); + update_watchdog_settings(); } static void step_cb_wrapper(void* ctx) { @@ -88,6 +89,18 @@ void Axis::decode_step_dir_pins() { dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin); } +// @brief: Setup the watchdog reset value from the configuration watchdog timeout interval. +void Axis::update_watchdog_settings() { + + if(config_.watchdog_timeout <= 0.0f) { // watchdog disabled + watchdog_reset_value_ = 0; + } else if(config_.watchdog_timeout >= UINT32_MAX / (current_meas_hz+1)) { //overflow! + watchdog_reset_value_ = UINT32_MAX; + } else { + watchdog_reset_value_ = static_cast(config_.watchdog_timeout * current_meas_hz); + } +} + // @brief (de)activates step/dir input void Axis::set_step_dir_active(bool active) { if (active) { @@ -141,6 +154,26 @@ bool Axis::do_updates() { return check_for_errors(); } +// @brief Feed the watchdog to prevent watchdog timeouts. +void Axis::watchdog_feed() { + watchdog_current_value_ = watchdog_reset_value_; +} + +// @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(watchdog_reset_value_ == 0) return true; + + // explicit check here to ensure that we don't underflow back to UINT32_MAX + if(watchdog_current_value_ > 0) { + watchdog_current_value_--; + return true; + } else { + error_ |= ERROR_WATCHDOG_TIMER_EXPIRED; + return false; + } +} + bool Axis::run_sensorless_spin_up() { // Early Spin-up: spiral up current float x = 0.0f; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a6b6bdae..759f4d8d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -20,6 +20,7 @@ public: ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value ERROR_CONTROLLER_FAILED = 0x200, ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, + ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, }; // Warning: Do not reorder these enum values. @@ -47,6 +48,8 @@ public: // For M0 this has no effect if enable_uart is true float counts_per_step = 2.0f; + float watchdog_timeout = 0.0f; // [s] (0 disables watchdog) + // Defaults loaded from hw_config in load_configuration in main.cpp uint16_t step_gpio_pin = 0; uint16_t dir_gpio_pin = 0; @@ -79,6 +82,8 @@ public: void step_cb(); void set_step_dir_active(bool enable); void decode_step_dir_pins(); + void update_watchdog_settings(); + static void load_default_step_dir_pin_config( const AxisHardwareConfig_t& hw_config, Config_t* config); @@ -87,6 +92,9 @@ public: bool do_checks(); bool do_updates(); + void watchdog_feed(); + bool watchdog_check(); + // True if there are no errors bool inline check_for_errors() { @@ -121,8 +129,11 @@ public: // Update all estimators // Note: updates run even if checks fail bool updates_ok = do_updates(); + + // make sure the watchdog is being fed. + bool watchdog_ok = watchdog_check(); - if (!checks_ok || !updates_ok) { + if (!checks_ok || !updates_ok || !watchdog_ok) { // It's not useful to quit idle since that is the safe action // Also leaving idle would rearm the motors if (current_state_ != AXIS_STATE_IDLE) @@ -185,6 +196,10 @@ public: State_t& current_state_ = task_chain_[0]; uint32_t loop_counter_ = 0; + // watchdog + uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings() + uint32_t watchdog_current_value_= 0; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -201,6 +216,8 @@ public: make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control), make_protocol_property("enable_step_dir", &config_.enable_step_dir), make_protocol_property("counts_per_step", &config_.counts_per_step), + make_protocol_property("watchdog_timeout", &config_.watchdog_timeout, + [](void* ctx) { static_cast(ctx)->update_watchdog_settings(); }, this), make_protocol_property("step_gpio_pin", &config_.step_gpio_pin, [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, @@ -215,7 +232,8 @@ public: make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()), - make_protocol_object("trap_traj", trap_.make_protocol_definitions()) + make_protocol_object("trap_traj", trap_.make_protocol_definitions()), + make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed) ); } }; diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 8a0d3287..1e1c9ba4 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -99,7 +99,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& vel_feed_forward = 0.0f; if (numscan < 4) current_feed_forward = 0.0f; - axes[motor_number]->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + Axis* axis = axes[motor_number]; + axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + axis->watchdog_feed(); } } else if (cmd[0] == 'q') { // position control with limits @@ -117,6 +119,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& axis->controller_.config_.vel_limit = vel_limit; if (numscan >= 4) axis->motor_.config_.current_lim = current_lim; + + axis->watchdog_feed(); } } else if (cmd[0] == 'v') { // velocity control @@ -130,7 +134,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else { if (numscan < 3) current_feed_forward = 0.0f; - axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + Axis* axis = axes[motor_number]; + axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + axis->watchdog_feed(); } } else if (cmd[0] == 'c') { // current control @@ -142,7 +148,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - axes[motor_number]->controller_.set_current_setpoint(current_setpoint); + Axis* axis = axes[motor_number]; + axis->controller_.set_current_setpoint(current_setpoint); + axis->watchdog_feed(); } } else if (cmd[0] == 't') { // trapezoidal trajectory @@ -154,7 +162,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - axes[motor_number]->controller_.move_to_pos(goal_point); + Axis* axis = axes[motor_number]; + axis->controller_.move_to_pos(goal_point); + axis->watchdog_feed(); } } else if (cmd[0] == 'f') { // feedback @@ -240,6 +250,17 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } } + }else if (cmd[0] == 'u') { // Update axis watchdog. + unsigned motor_number; + int numscan = sscanf(cmd, "u %u", &motor_number); + if(numscan < 1){ + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + }else { + axes[motor_number]->watchdog_feed(); + } + } else if (cmd[0] != 0) { respond(response_channel, use_checksum, "unknown command"); } diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 0c510fe6..d782aa79 100644 --- a/docs/ascii-protocol.md +++ b/docs/ascii-protocol.md @@ -36,6 +36,8 @@ Example: `t 0 -20000` For general moving around of the axis, this is the recommended command. +This command updates the watchdog timer for the motor. + #### Motor Position command For basic use where you send one setpoint at at a time, use the `q` command. If you have a realtime controller that is streaming setpoints and tracking a trajectory, use the `p` command. @@ -64,6 +66,7 @@ Example: `p 0 -20000 0 0` Note that if you don't know what feed-forward is or what it's used for, simply omit it. +This command updates the watchdog timer for the motor. #### Motor Velocity command ``` @@ -78,6 +81,8 @@ Example: `v 0 1000 0` Note that if you don't know what feed-forward is or what it's used for, simply omit it. +This command updates the watchdog timer for the motor. + #### Motor Current command ``` c motor current @@ -86,6 +91,19 @@ c motor current * `motor` is the motor number, `0` or `1`. * `current` is the desired current in A. +This command updates the watchdog timer for the motor. + + +#### Update motor watchdog +``` +u motor +``` +* `u` for /u/pdate. +* `motor` is the motor number, `0` or `1`. + +This command updates the watchdog timer for the motor, without changing any +setpoints. + #### Parameter reading/writing Not all parameters can be accessed via the ASCII protocol but at least all parameters with float and integer type are supported. diff --git a/docs/getting-started.md b/docs/getting-started.md index 982ec861..8060bbb3 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -331,6 +331,18 @@ You can now control the current with `axis.controller.current_setpoint = 3` [A]. *Note: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder.* + +## Watchdog Timer +Each axis has a configurable watchdog timer that can stop the motors if the +control connection to the ODrive is interrupted. + +Each axis has a configurable watchdog timeout: `axis.config.watchdog_timeout`, +measured in seconds. A value of `0` disables the watchdog functionality. Any value +`> 0` will stop the motors if the watchdog has not been fed in the configured +time interval. + +The watchdog is fed using the `axis.watchdog_feed()` method of each axis. + ## What's next? You can now: * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance. From e94230275eae8865b0aea40bdf16347297f4552c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 18 Mar 2019 21:09:46 -0700 Subject: [PATCH 2/2] add python side enum, fix instant timout on 1st iteration --- Firmware/MotorControl/axis.cpp | 3 +++ tools/odrive/enums.py | 1 + 2 files changed, 4 insertions(+) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index d20f292a..80987c00 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -99,6 +99,9 @@ void Axis::update_watchdog_settings() { } else { watchdog_reset_value_ = static_cast(config_.watchdog_timeout * current_meas_hz); } + + // Do a feed to avoid instant timeout + watchdog_feed(); } // @brief (de)activates step/dir input diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index fc5439bd..d4425a21 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -27,6 +27,7 @@ class errors: ERROR_ENCODER_FAILED = 0x100 # Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value ERROR_CONTROLLER_FAILED = 0x200 ERROR_POS_CTRL_DURING_SENSORLESS = 0x400 + ERROR_WATCHDOG_TIMER_EXPIRED = 0x800 class motor: ERROR_NONE = 0