Skip to content

Commit 690deb7

Browse files
manoukianvUltrawipf
authored andcommitted
refactor(axis): unify DSP and non-DSP logic in applySpeedLimiterTorque
1 parent df181e3 commit 690deb7

2 files changed

Lines changed: 31 additions & 37 deletions

File tree

‎Firmware/FFBoard/Inc/Axis.h‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -342,7 +342,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
342342
* @brief Calculates the FFB torque exponential torque, from the input torque and apply expo and scaler
343343
* @return The calculated exponential torque.
344344
*/
345-
int64_t calculateFFBTorque();
345+
int32_t calculateFFBTorque();
346346

347347
/**
348348
* @brief Starts a force fade-in.
@@ -357,7 +357,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
357357
* @brief Sets the FFB effect torque.
358358
* @param torque The new FFB effect torque from the EffectsCalculator.
359359
*/
360-
void setFfbEffectTorque(int64_t torque);
360+
void setFfbEffectTorque(int32_t torque);
361361

362362
/**
363363
* @brief Updates the total torque.
@@ -418,12 +418,12 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
418418
* @param torque A reference to the torque value to be modified.
419419
* @return torque update to apply to reduced de speed.
420420
*/
421-
int64_t applySpeedLimiterTorque(int64_t& torque);
421+
int32_t applySpeedLimiterTorque(int32_t& torque);
422422
/**
423423
* @brief Applies the torque slew rate limiter to the torque.
424424
* @param torque A reference to the torque value to be modified.
425425
*/
426-
void applyTorqueSlewRateLimiter(int64_t& torque);
426+
void applyTorqueSlewRateLimiter(int32_t& torque);
427427
/**
428428
* @brief Decodes the axis configuration from a 16-bit integer stored in flash.
429429
* @param val The 16-bit encoded configuration value.
@@ -478,7 +478,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
478478
float previousFrameSpeed = 0; //!< Instantaneous speed from the last cycle, used for acceleration calculation.
479479

480480
// Torque components
481-
int64_t ffbEffectTorque = 0; //!< Torque from HID FFB effects.
481+
int32_t ffbEffectTorque = 0; //!< Torque from HID FFB effects.
482482
int32_t mechanicalEffectTorque = 0; //!< Torque from mechanical effects (damper, friction, inertia).
483483

484484
// Power and scaling

‎Firmware/FFBoard/Src/Axis.cpp‎

Lines changed: 26 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -703,9 +703,9 @@ int32_t Axis::calculateExpoTorque(int32_t torque){
703703
}
704704
}
705705

706-
int64_t Axis::calculateFFBTorque() {
706+
int32_t Axis::calculateFFBTorque() {
707707

708-
int64_t torque = this->ffbEffectTorque;
708+
int32_t torque = this->ffbEffectTorque;
709709

710710
// 1. Game Clipping detection
711711
// If the game sends more than the theoretical maximum (+/- 32767), the signal is clipped at the source.
@@ -720,7 +720,7 @@ int64_t Axis::calculateFFBTorque() {
720720

721721
// 3. Game specific gain (effectRatioScaler)
722722
// Scale the FFB from game only (allows lowering game effects without lowering endstops)
723-
torque = (int64_t)((float)torque * effectRatioScaler);
723+
torque = (int32_t)((float)torque * effectRatioScaler);
724724

725725
return torque;
726726
}
@@ -747,7 +747,7 @@ int32_t Axis::calculateEndstopTorque(){
747747
return clip<int32_t,int32_t>(endstopTorque,-0x7fff,0x7fff);
748748
}
749749

750-
void Axis::setFfbEffectTorque(int64_t torque) {
750+
void Axis::setFfbEffectTorque(int32_t torque) {
751751
this->ffbEffectTorque = torque;
752752
}
753753

@@ -759,7 +759,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
759759

760760
// STEP 1: Process FFB torque from the game (via helper function)
761761
// (Reconstructed by CMSIS Spline, Expo applied, and scaled by FFB ratio)
762-
int64_t torque = calculateFFBTorque();
762+
int32_t torque = calculateFFBTorque();
763763

764764
// STEP 2: Mix in local mechanical effects
765765
// (Damper, Friction, Inertia generated locally at high frequency)
@@ -772,7 +772,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
772772

773773
// STEP 4: Master Volume Scaling
774774
// Map the virtual signal (+/- 32767) to the physical power limit ("power")
775-
torque = (int64_t)((float)torque * torqueScaler);
775+
torque = (int32_t)((float)torque * torqueScaler);
776776

777777
// STEP 5: Safety limits (Fade-in, Out of bounds)
778778
// Applied BEFORE dynamic limiters so that abrupt cuts are smoothed by the Slew Rate.
@@ -783,7 +783,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
783783
// Apply a fade-in effect for a smooth force ramp-up on startup or recovery.
784784
// Increases forceFadeMultiplier progressively based on forceFadeDuration and sample rate.
785785
if(forceFadeMultiplier < 1.0f){
786-
torque = (int64_t)((float)torque * forceFadeMultiplier);
786+
torque = (int32_t)((float)torque * forceFadeMultiplier);
787787
forceFadeMultiplier += forceFadeDuration / this->filter_f;
788788
}
789789

@@ -817,7 +817,7 @@ bool Axis::updateTorque(int32_t* totalTorque) {
817817
}
818818

819819

820-
void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
820+
void Axis::applyTorqueSlewRateLimiter(int32_t& torque)
821821
{
822822
// Limits the rate of change of the torque (slew rate), to smooths out sudden changes in torque.
823823
// Essential for a natural feel and to prevent "clanking" noises.
@@ -826,14 +826,14 @@ void Axis::applyTorqueSlewRateLimiter(int64_t& torque)
826826
}
827827

828828
// This prevents sudden torque jumps, resulting in a smoother feel.
829-
const int64_t previousTorque = metric.previous.torque;
830-
const int64_t maxTorqueChange = maxTorqueRateMS;
829+
const int32_t previousTorque = metric.previous.torque;
830+
const int32_t maxTorqueChange = maxTorqueRateMS;
831831

832832
// The torque is clipped to be within the range of [previous torque - limit, previous torque + limit].
833-
torque = clip<int64_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
833+
torque = clip<int32_t>(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange);
834834
}
835835

836-
int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
836+
int32_t Axis::applySpeedLimiterTorque(int32_t& torque){
837837
// Speed Limiter: A PI controller to reduce torque when speed exceeds maxSpeedDegS.
838838
// The limiter only acts when torque is applied in the direction of movement.
839839

@@ -842,18 +842,23 @@ int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
842842
return 0;
843843
}
844844

845-
int64_t resultTorque = 0;
846-
847-
#ifdef USE_DSP_FUNCTIONS
845+
int32_t resultTorque = 0;
848846
float effectiveSpeed = metric.current.speed * (torque > 0 ? 1.0f : -1.0f);
847+
849848
if (effectiveSpeed > maxSpeedDegS)
850849
{
851-
// --- PI Controller Logic ---
852850
// 1. Calculate the error term (how much we are over the speed limit).
853851
float speedError = effectiveSpeed - maxSpeedDegS;
852+
float reductionAmount = 0.0f;
854853

855-
// 2. Calculate the total reduction amount using the PID controller.
856-
float reductionAmount = arm_pid_f32(&speedLimiterPID, speedError);
854+
// 2. Calculate the total reduction amount using the PI controller.
855+
#ifdef USE_DSP_FUNCTIONS
856+
reductionAmount = arm_pid_f32(&speedLimiterPID, speedError);
857+
#else
858+
float speedreducer = speedError * ((float)0x7FFF / maxSpeedDegS);
859+
speedLimitReducerI = clip<float,int32_t>( speedLimitReducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
860+
reductionAmount = speedreducer * speedLimiterP + speedLimitReducerI;
861+
#endif
857862

858863
// 3. Apply the reduction to the main torque.
859864
// We must only reduce the magnitude of the torque, not invert it.
@@ -864,23 +869,12 @@ int64_t Axis::applySpeedLimiterTorque(int64_t& torque){
864869
resultTorque = -reductionAmount;
865870
}
866871
} else {
872+
#ifdef USE_DSP_FUNCTIONS
867873
arm_pid_reset_f32(&speedLimiterPID); // Reset PID if not active
868-
}
869874
#else
870-
float torqueSign = torque > 0 ? 1 : -1; // Used to prevent metrics against the force to go into the limiter
871-
// Speed. Mostly tuned...
872-
//spdlimiterAvg.addValue(metric.current.speed);
873-
float speedreducer = (float)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) * ((float)0x7FFF / maxSpeedDegS);
874-
speedLimitReducerI = clip<float,int32_t>( speedLimitReducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power);
875-
876-
// Only reduce torque. Don't invert it to prevent oscillation
877-
float torqueReduction = speedreducer * speedLimiterP + speedLimitReducerI;// accreducer * 0.025 + acclimitreducerI
878-
if(torque > 0){
879-
resultTorque = clip<float,int64_t>(torqueReduction,0,torque);
880-
}else{
881-
resultTorque = clip<float,int64_t>(-torqueReduction,torque,0);
882-
}
875+
speedLimitReducerI = 0; // Reset I term if not active
883876
#endif
877+
}
884878

885879
return resultTorque;
886880
}

0 commit comments

Comments
 (0)