From c1e5b9780724e1e612b38098b6283d1b5f618284 Mon Sep 17 00:00:00 2001 From: Anton Evmenenko Date: Wed, 21 May 2025 00:58:14 +0200 Subject: [PATCH 1/2] Current sign restored using the commanded voltage --- src/common/base_classes/FOCDriver.h | 4 ++++ src/current_sense/LowsideCurrentSense.cpp | 6 ++++++ src/drivers/BLDCDriver3PWM.cpp | 6 ++++-- src/drivers/BLDCDriver6PWM.cpp | 5 ++++- src/drivers/StepperDriver2PWM.cpp | 4 +++- src/drivers/StepperDriver4PWM.cpp | 20 +++++++++++--------- 6 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 263460b3..8dec8f5a 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -41,6 +41,10 @@ class FOCDriver{ bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH) + float Ua; //!< currently set phase A voltage + float Ub; //!< currently set phase B voltage + float Uc; //!< currently set phase C voltage + /** get the driver type*/ virtual DriverType type() = 0; }; diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index a0026ae3..d7c650c3 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -91,5 +91,11 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps + + if (driver_type == DriverType::Stepper){ + current.a *= _sign(driver->Ua); + current.b *= _sign(driver->Ub); + } + return current; } diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 637c8db5..4ca4e660 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -74,8 +74,10 @@ void BLDCDriver3PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) } // Set voltage to the pwm pin -void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { - +void BLDCDriver3PWM::setPwm(float Ua_, float Ub_, float Uc_) { + Ua = Ua_; + Ub = Ub_; + Uc = Uc_; // limit the voltage in driver Ua = _constrain(Ua, 0.0f, voltage_limit); Ub = _constrain(Ub, 0.0f, voltage_limit); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 4981858f..22eebe3a 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -77,7 +77,10 @@ int BLDCDriver6PWM::init() { // Set voltage to the pwm pin -void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { +void BLDCDriver6PWM::setPwm(float Ua_, float Ub_, float Uc_) { + Ua = Ua_; + Ub = Ub_; + Uc = Uc_; // limit the voltage in driver Ua = _constrain(Ua, 0, voltage_limit); Ub = _constrain(Ub, 0, voltage_limit); diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index e8ccc6c6..ebbfed4f 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -94,7 +94,9 @@ void StepperDriver2PWM::setPhaseState(PhaseState sa, PhaseState sb) { } // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ua, float Ub) { +void StepperDriver2PWM::setPwm(float Ua_, float Ub_) { + Ua = Ua_; + Ub = Ub_; float duty_cycle1(0.0f),duty_cycle2(0.0f); // limit the voltage in driver Ua = _constrain(Ua, -voltage_limit, voltage_limit); diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index 52f1c1d1..34bae8fb 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -70,21 +70,23 @@ void StepperDriver4PWM::setPhaseState(PhaseState sa, PhaseState sb) { // Set voltage to the pwm pin -void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { +void StepperDriver4PWM::setPwm(float Ua_, float Ub_) { + Ua = Ua_; + Ub = Ub_; float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); // limit the voltage in driver - Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); - Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + Ua = _constrain(Ua, -voltage_limit, voltage_limit); + Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing - if( Ualpha > 0 ) - duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + if( Ua > 0 ) + duty_cycle1B = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f); else - duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f); + duty_cycle1A = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f); - if( Ubeta > 0 ) - duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + if( Ub > 0 ) + duty_cycle2B = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f); else - duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f); + duty_cycle2A = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f); // write to hardware _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params); } \ No newline at end of file From 850637fd8807d39a656dc70138728627a8af075a Mon Sep 17 00:00:00 2001 From: Anton Evmenenko Date: Wed, 21 May 2025 03:42:57 +0200 Subject: [PATCH 2/2] Limit changes to stepper motors only --- src/common/base_classes/FOCDriver.h | 4 ---- src/common/base_classes/StepperDriver.h | 2 ++ src/current_sense/LowsideCurrentSense.cpp | 5 +++-- src/drivers/BLDCDriver3PWM.cpp | 6 ++---- src/drivers/BLDCDriver6PWM.cpp | 5 +---- 5 files changed, 8 insertions(+), 14 deletions(-) diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 8dec8f5a..263460b3 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -41,10 +41,6 @@ class FOCDriver{ bool enable_active_high = true; //!< enable pin should be set to high to enable the driver (default is HIGH) - float Ua; //!< currently set phase A voltage - float Ub; //!< currently set phase B voltage - float Uc; //!< currently set phase C voltage - /** get the driver type*/ virtual DriverType type() = 0; }; diff --git a/src/common/base_classes/StepperDriver.h b/src/common/base_classes/StepperDriver.h index 9864b235..49910179 100644 --- a/src/common/base_classes/StepperDriver.h +++ b/src/common/base_classes/StepperDriver.h @@ -6,6 +6,8 @@ class StepperDriver: public FOCDriver{ public: + float Ua; //!< currently set phase A voltage + float Ub; //!< currently set phase B voltage /** * Set phase voltages to the hardware diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index d7c650c3..dc5896ec 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -93,8 +93,9 @@ PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps if (driver_type == DriverType::Stepper){ - current.a *= _sign(driver->Ua); - current.b *= _sign(driver->Ub); + static StepperDriver* stepper_driver = static_cast(driver); + current.a *= _sign(stepper_driver->Ua); + current.b *= _sign(stepper_driver->Ub); } return current; diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 4ca4e660..637c8db5 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -74,10 +74,8 @@ void BLDCDriver3PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) } // Set voltage to the pwm pin -void BLDCDriver3PWM::setPwm(float Ua_, float Ub_, float Uc_) { - Ua = Ua_; - Ub = Ub_; - Uc = Uc_; +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver Ua = _constrain(Ua, 0.0f, voltage_limit); Ub = _constrain(Ub, 0.0f, voltage_limit); diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index 22eebe3a..4981858f 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -77,10 +77,7 @@ int BLDCDriver6PWM::init() { // Set voltage to the pwm pin -void BLDCDriver6PWM::setPwm(float Ua_, float Ub_, float Uc_) { - Ua = Ua_; - Ub = Ub_; - Uc = Uc_; +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver Ua = _constrain(Ua, 0, voltage_limit); Ub = _constrain(Ub, 0, voltage_limit);