Update teensy4_mcu.cpp _writeDutyCycle6PWM to handle phase_state to allow disabling the motor

.github/PULL_REQUEST_TEMPLATE.md

@@ -0,0 +1,30 @@
+# Description
+
+Please include a summary of the changes and the related issue. Please also include relevant motivation and context. List any architectures/sensors/motors that the PR is targeting if appicable.
+
+The best approach would be to create an issue and reference it in this pull request.
+Example: 
+
+Fixes # (issue)
+
+## Type of change
+
+Please delete options that are not relevant.
+
+- [ ] Bug fix (non-breaking change which fixes an issue)
+- [ ] New feature (non-breaking change which adds functionality)
+- [ ] Breaking change (fix or feature that would cause existing functionality to not work as expected)
+- [ ] This change requires a documentation update
+
+# How Has This Been Tested?
+
+Please describe the tests that you ran to verify your changes. Provide instructions so we can reproduce. Please also list any relevant details for your test configuration
+
+**Test Configuration/Setup**:
+* Hardware:
+* IDE: Arduino/Platformio
+* MCU package version (stm32duino/arduino-esp32/..)
+
+# **IMPORTANT** when opening the pull request
+
+* Your pull request MUST target the `dev` branch on this repository. By default you'll probably target `master`, make sure to change it.

src/drivers/hardware_specific/teensy/teensy4_mcu.cpp

@@ -526,16 +526,59 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons
 }
 
 
+// Helper function to enable or disable pwm pins based on phase state (allows for disabling the motor)
+static inline void _teensy4_apply_phase_state(
+    IMXRT_FLEXPWM_t *flexpwm, int submodule, PhaseState state
+)
+{
+  uint16_t submodule_mask = 1u << submodule;
+  uint16_t enable_a       = FLEXPWM_OUTEN_PWMA_EN( submodule_mask );
+  uint16_t enable_b       = FLEXPWM_OUTEN_PWMB_EN( submodule_mask );
+
+  switch ( state )
+  {
+    case PhaseState::PHASE_OFF:
+      flexpwm->OUTEN &= ~( enable_a | enable_b );
+      break;
+    case PhaseState::PHASE_HI:
+      flexpwm->OUTEN |= enable_a;
+      flexpwm->OUTEN &= ~enable_b;
+      break;
+    case PhaseState::PHASE_LO:
+      flexpwm->OUTEN &= ~enable_a;
+      flexpwm->OUTEN |= enable_b;
+      break;
+    case PhaseState::PHASE_ON:
+    default:
+      flexpwm->OUTEN |= ( enable_a | enable_b );
+      break;
+  }
+}
 
 // function setting the pwm duty cycle to the hardware
 // - Stepper motor - 6PWM setting
 // - hardware specific
 void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
-  Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params;
-  _UNUSED(phase_state);
-  write_pwm_pair (p->flextimers[0], p->submodules[0], dc_a);
-  write_pwm_pair (p->flextimers[1], p->submodules[1], dc_b);
-  write_pwm_pair (p->flextimers[2], p->submodules[2], dc_c);
+  Teensy4DriverParams *p = (Teensy4DriverParams *)( (TeensyDriverParams *)params )->additional_params;
+
+  PhaseState phase_a = phase_state ? phase_state[ 0 ] : PhaseState::PHASE_ON;
+  PhaseState phase_b = phase_state ? phase_state[ 1 ] : PhaseState::PHASE_ON;
+  PhaseState phase_c = phase_state ? phase_state[ 2 ] : PhaseState::PHASE_ON;
+
+  if ( PhaseState::PHASE_OFF == phase_a )
+    dc_a = 0.0f;
+  if ( PhaseState::PHASE_OFF == phase_b )
+    dc_b = 0.0f;
+  if ( PhaseState::PHASE_OFF == phase_c )
+    dc_c = 0.0f;
+
+  _teensy4_apply_phase_state( p->flextimers[ 0 ], p->submodules[ 0 ], phase_a );
+  _teensy4_apply_phase_state( p->flextimers[ 1 ], p->submodules[ 1 ], phase_b );
+  _teensy4_apply_phase_state( p->flextimers[ 2 ], p->submodules[ 2 ], phase_c );
+
+  write_pwm_pair( p->flextimers[ 0 ], p->submodules[ 0 ], dc_a );
+  write_pwm_pair( p->flextimers[ 1 ], p->submodules[ 1 ], dc_b );
+  write_pwm_pair( p->flextimers[ 2 ], p->submodules[ 2 ], dc_c );
 }
 
 void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float duty)