diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 0ba614c3..80987c00 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,21 @@ 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); + } + + // Do a feed to avoid instant timeout + watchdog_feed(); +} + // @brief (de)activates step/dir input void Axis::set_step_dir_active(bool active) { if (active) { @@ -141,6 +157,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_lockin_spin() { // Spiral up current for softer rotor lock-in lockin_state_ = LOCKIN_STATE_RAMP; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index dd8e88ef..9e78fdab 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, }; enum State_t { @@ -59,6 +60,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; @@ -93,6 +96,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); @@ -101,6 +106,10 @@ public: bool do_checks(); bool do_updates(); + void watchdog_feed(); + bool watchdog_check(); + + // True if there are no errors bool inline check_for_errors() { return error_ == ERROR_NONE; @@ -134,8 +143,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) @@ -199,6 +211,10 @@ public: uint32_t loop_counter_ = 0; LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE; + // 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( @@ -216,6 +232,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, @@ -236,7 +254,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. 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