Correct motore idle power handling on steppers

synthetos · Jul 26, 2020 · de76996 · de76996
1 parent c9b6a57
commit de76996
Showing 1 changed file with 10 additions and 14 deletions.
diff --git a/g2core/device/step_dir_driver/step_dir_driver.h b/g2core/device/step_dir_driver/step_dir_driver.h
@@ -86,7 +86,7 @@ struct StepDirStepper final : Stepper  {
     float _power_level; // the power level now
 
     void _updatePowerLevel() {
-        if (MOTOR_IDLE == _power_state) {
+        if ((MOTOR_IDLE == _power_state) || (MOTOR_OFF == _power_state)) {
             _power_level = _idle_power_level;
         } else {
             _power_level = _active_power_level;
@@ -166,18 +166,12 @@ struct StepDirStepper final : Stepper  {
             timeout_ms = _motor_activity_timeout_ms;
         }
 
+        this->enable(); // sets _power_state, that will be adjusted next
+
         _power_state = MOTOR_POWER_TIMEOUT_COUNTDOWN;
         if (_power_mode == MOTOR_POWERED_IN_CYCLE || _power_mode == MOTOR_POWER_REDUCED_WHEN_IDLE) {
             _motor_activity_timeout.set(timeout_ms);
         }
-
-        if (!_enable.isNull()) {
-            if (_enable_polarity == IO_ACTIVE_HIGH) {
-                _enable.set();
-            } else {
-                _enable.clear();
-            }
-        }
     };
 
     void _enableImpl() override {
@@ -201,15 +195,19 @@ struct StepDirStepper final : Stepper  {
         if (this->getPowerMode() == MOTOR_ALWAYS_POWERED) {
             return;
         }
+
         if (!_enable.isNull()) {
             if (_enable_polarity == IO_ACTIVE_HIGH) {
                 _enable.clear();
             } else {
                 _enable.set();
             }
         }
+
         _motor_activity_timeout.clear();
+
         _power_state = MOTOR_OFF;
+        _updatePowerLevel();
     };
 
     void stepStart() override {
@@ -291,9 +289,7 @@ struct StepDirStepper final : Stepper  {
     // HOT - called from the DDA interrupt
     void motionStopped() //HOT_FUNC
     {
-        if (_power_mode == MOTOR_POWERED_IN_CYCLE) {
-            this->enable();
-        } else if (_power_mode == MOTOR_POWER_REDUCED_WHEN_IDLE) {
+        if ((_power_mode == MOTOR_POWERED_IN_CYCLE) || (_power_mode == MOTOR_POWER_REDUCED_WHEN_IDLE)) {
             _power_state = MOTOR_POWER_TIMEOUT_START;
         } else if (_power_mode == MOTOR_POWERED_ONLY_WHEN_MOVING) {
             if (_power_state == MOTOR_RUNNING) {
@@ -311,15 +307,15 @@ struct StepDirStepper final : Stepper  {
 
     void periodicCheck(bool have_actually_stopped) override
     {
-        if (_power_state == MOTOR_POWER_TIMEOUT_START && _power_mode != MOTOR_ALWAYS_POWERED) {
+        if ((_power_state == MOTOR_POWER_TIMEOUT_START) && (_power_mode != MOTOR_ALWAYS_POWERED)) {
             if (_power_mode == MOTOR_POWERED_ONLY_WHEN_MOVING) {
                 this->disable();
                 return;
             }
 
             // start timeouts initiated during a load so the loader does not need to burn these cycles
             _power_state = MOTOR_POWER_TIMEOUT_COUNTDOWN;
-            if (_power_mode == MOTOR_POWERED_IN_CYCLE || _power_mode == MOTOR_POWER_REDUCED_WHEN_IDLE) {
+            if ((_power_mode == MOTOR_POWERED_IN_CYCLE) || (_power_mode == MOTOR_POWER_REDUCED_WHEN_IDLE)) {
                 _motor_activity_timeout.set(_motor_activity_timeout_ms);
             }
         }