diff --git a/.github/workflows/build-firmware.yml b/.github/workflows/build-firmware.yml index d3e3377b1..e31bb5ccc 100644 --- a/.github/workflows/build-firmware.yml +++ b/.github/workflows/build-firmware.yml @@ -32,6 +32,8 @@ jobs: steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v4 + with: + submodules: 'recursive' - {uses: ./.github/actions/build-firmware, with: {target: '${{ matrix.target }}', path: 'Output'}} diff --git a/.gitmodules b/.gitmodules index ed7c594be..313b67a9d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,3 +6,6 @@ path = Hardware url = https://github.com/Ultrawipf/OpenFFBoard-hardware.git branch = master +[submodule "Firmware/Libraries/CMSIS-DSP"] + path = Firmware/Libraries/CMSIS-DSP + url = https://github.com/ARM-software/CMSIS-DSP.git diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h index 5b044c187..f37f16850 100644 --- a/Firmware/FFBoard/Inc/Axis.h +++ b/Firmware/FFBoard/Inc/Axis.h @@ -2,7 +2,9 @@ * Axis.h * * Created on: 21.01.2021 - * Author: Yannick / Lidders + * Author: Yannick / Lidders / Vincent + * + * Release 27.10.25: Vincent, add reconstruction filter, equalizer, slew rate. */ #ifndef SRC_AXIS_H_ @@ -10,7 +12,6 @@ #include #include #include "usb_hid_ffb_desc.h" -#include "TMC4671.h" #include "PersistentStorage.h" #include "ButtonSource.h" #include "EncoderLocal.h" @@ -24,6 +25,10 @@ #include "ExtiHandler.h" #include "EffectsCalculator.h" +#ifdef USE_DSP_FUNCTIONS +#include "arm_math.h" +#endif + #define INTERNAL_AXIS_DAMPER_SCALER 0.7 #define INTERNAL_AXIS_FRICTION_SCALER 0.7 #define INTERNAL_AXIS_INERTIA_SCALER 0.7 @@ -34,22 +39,28 @@ #define AXIS_SPEEDLIMITER_I 0.03 #endif - +/** + * @brief Global control flags for all axes. + */ struct Control_t { - bool emergency = false; - bool usb_disabled = true; - bool update_disabled = true; - bool request_update_disabled = false; + bool emergency = false; //!< Emergency stop is active. + bool usb_disabled = true; //!< FFB is disabled by USB. + bool update_disabled = true; //!< FFB updates are disabled. + bool request_update_disabled = false; //!< A request to disable FFB updates is pending. // bool usb_update_flag = false; // bool update_flag = false; - bool resetEncoder = false; + bool resetEncoder = false; //!< A request to reset the encoder is pending. }; -struct AxisFlashAddrs +/** + * @brief Defines the flash memory addresses for axis-specific settings. + */ +struct AxisFlashAddresses { uint16_t config = ADR_AXIS1_CONFIG; uint16_t maxSpeed = ADR_AXIS1_MAX_SPEED; uint16_t maxAccel = ADR_AXIS1_MAX_ACCEL; + uint16_t maxSlewRateDrv = ADR_AXIS1_MAX_SLEWRATE_DRV; uint16_t endstop = ADR_AXIS1_ENDSTOP; uint16_t power = ADR_AXIS1_POWER; @@ -60,234 +71,498 @@ struct AxisFlashAddrs uint16_t speedAccelFilter = ADR_AXIS1_SPEEDACCEL_FILTER; uint16_t postprocess1 = ADR_AXIS1_POSTPROCESS1; + // NOTE: The following addresses must be defined in constants.h + uint16_t equalizer1 = ADR_AXIS1_EQ1; + uint16_t equalizer2 = ADR_AXIS1_EQ2; + uint16_t equalizer3 = ADR_AXIS1_EQ3; + uint16_t handsOffConfig = ADR_AXIS1_HANDSOFF_CONF; + uint16_t handsOffAccel = ADR_AXIS1_HANDSOFF_ACCEL; }; +/** + * @brief Configuration for an axis, including driver and encoder types. + */ struct AxisConfig { - uint8_t drvtype = 0; - uint8_t enctype = 0; - //bool invert = false; + uint8_t drvtype = 0; //!< Motor driver type ID. + uint8_t enctype = 0; //!< Encoder type ID. }; +/** + * @brief Holds the physical metrics of an axis at a point in time. + */ struct metric_t { - float accel = 0; // in deg/s² - float speed = 0; // in deg/s - int32_t pos_scaled_16b = 0; // scaled position as 16b int -0x7fff to 0x7fff matching FFB ranges - float pos_f = 0; // scaled position as float. -1 to 1 range - float posDegrees = 0; // Position in degrees. Not scaled to selected range - int32_t torque = 0; // total of effect + endstop torque + float accel = 0; //!< Acceleration in deg/s². + float speed = 0; //!< Speed in deg/s. + int32_t pos_scaled_16b = 0; //!< Scaled position as a 16-bit integer (-0x7fff to 0x7fff). + float pos_f = 0; //!< Scaled position as a float (-1.0 to 1.0). + float posDegrees = 0; //!< Position in degrees, not scaled to the selected range. + int32_t torque = 0; //!< Total torque applied to the axis. }; +/** + * @brief Holds the current and previous metrics for an axis, used for calculating derivatives. + */ struct axis_metric_t { - metric_t current; - metric_t previous; + metric_t current; //!< Current metrics. + metric_t previous; //!< Metrics from the previous update cycle. }; +/** + * @brief Represents a gear ratio for scaling encoder values. + */ struct GearRatio_t{ - uint8_t denominator = 0; - uint8_t numerator = 0; - float gearRatio = 1.0; + uint8_t denominator = 0; //!< Denominator of the gear ratio. + uint8_t numerator = 0; //!< Numerator of the gear ratio. + float gearRatio = 1.0; //!< The calculated gear ratio (numerator/denominator). }; - enum class Axis_commands : uint32_t{ power=0x00,degrees=0x01,esgain,zeroenc,invert,idlespring,axisdamper,enctype,drvtype, - pos,maxspeed,maxtorquerate,fxratio,curtorque,curpos,curspd,curaccel,reductionScaler, + pos,curtorque,curpos,curspd,curaccel, + fxratio,reductionScaler, filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia, - expo,exposcale + maxspeed,slewrate, + calibrate_maxSlewRateDrv, + maxSlewRateDrv, + expo,exposcale, + equalizer,eqb1,eqb2,eqb3,eqb4,eqb5,eqb6, + handsoff, handsoff_speed, handsoff_accel }; +/** + * @brief This class represents a single FFB axis. + * It handles the motor driver, encoder, and all related calculations for force feedback effects. + */ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandler { public: + /** + * @brief Construct a new Axis object. + * @param axis The character identifier for this axis (e.g., 'x', 'y'). + * @param control A pointer to the global control structure. + */ Axis(char axis, volatile Control_t* control); virtual ~Axis(); - static ClassIdentifier info; + static ClassIdentifier info; //!< Static class identifier. const ClassIdentifier getInfo(); const ClassType getClassType() override {return ClassType::Axis;}; virtual std::string getHelpstring() { return "FFB axis" ;} -#ifdef TMC4671DRIVER - void setupTMC4671(); -#endif // Dynamic classes + /** + * @brief Sets the motor driver type. + * @param drvtype The type of the motor driver. + */ void setDrvType(uint8_t drvtype); + + /** + * @brief Sets the encoder type. + * @param enctype The type of the encoder. + */ void setEncType(uint8_t enctype); + + /** + * @brief Gets the motor driver type. + * @return The type of the motor driver. + */ uint8_t getDrvType(); + + /** + * @brief Gets the encoder type. + * @return The type of the encoder. + */ uint8_t getEncType(); + /** + * @brief Gets the encoder instance. + * @return A pointer to the encoder instance. + */ Encoder* getEncoder(); + + /** + * @brief Gets the motor driver instance. + * @return A pointer to the motor driver instance. + */ MotorDriver* getDriver(); - void usbSuspend(); // Called on usb disconnect and suspend - void usbResume(); // Called on usb resume + /** + * @brief Called on USB disconnect and suspend. + */ + void usbSuspend(); + /** + * @brief Called on USB resume. Enables the motor driver. + */ + void usbResume(); + + /** + * @brief Saves axis settings to flash memory. + * @override from PersistentStorage + */ void saveFlash() override; + + /** + * @brief Restores axis settings from flash memory. + * @override from PersistentStorage + */ void restoreFlash() override; - void prepareForUpdate(); // called before the effects are calculated - void updateDriveTorque(); //int32_t effectTorque); + /** + * @brief Prepares the axis for an update cycle. Called from the main loop before effects are calculated. + * Reads the encoder, scales the value, and updates metrics (speed, acceleration). + */ + void prepareForUpdate(); + + /** + * @brief Sends the final calculated torque to the motor driver. + */ + void updateDriveTorque(); + + /** + * @brief Triggers an emergency stop. Disables the motor driver. + * @param reset If true, resets the axis after stopping. + */ void emergencyStop(bool reset); + /** + * @brief Sets the position of the axis. + * @param val The new position value. + */ void setPos(uint16_t val); + + /** + * @brief Zeros the current encoder position. + */ void zeroPos(); + /** + * @brief Checks if FFB is globally active. + * @return true if FFB is active, false otherwise. + */ bool getFfbActive(); + /** + * @brief Scales an encoder value. + * @param angle The angle to scale. + * @param degrees The total degrees of rotation. + * @return A pair containing the scaled integer value and the float value. + */ std::pair scaleEncValue(float angle, uint16_t degrees); - float getEncAngle(Encoder *enc); + /** + * @brief Gets the angle from the encoder. + * @param enc A pointer to the encoder. + * @return The angle in degrees. + */ + float getEncAngle(Encoder *enc); + /** + * @brief Sets the maximum power (torque) of the motor. + * @param power The new power value. + */ void setPower(uint16_t power); - + /** + * @brief Callback for handling errors. + * @param error The error that occurred. + * @param cleared Whether the error has been cleared. + * @override from ErrorHandler + */ void errorCallback(const Error &error, bool cleared) override; - //ParseStatus command(ParsedCommand_old* cmd,std::string* reply) override; + /** + * @brief Registers the commands for this axis with the command handler. + */ void registerCommands(); - CommandStatus command(const ParsedCommand& cmd,std::vector& replies); - - ClassChooser drv_chooser; - ClassChooser enc_chooser; - + /** + * @brief Handles command line interface commands for this axis. + * @param cmd The parsed command. + * @param replies A vector of replies to be sent back. + * @return The status of the command execution. + * @override from CommandHandler + */ + CommandStatus command(const ParsedCommand& cmd,std::vector& replies) override; + + ClassChooser driverChooser; //!< Class chooser for motor drivers. + ClassChooser encoderChooser; //!< Class chooser for encoders. + + /** + * @brief Gets the last scaled encoder value. + * @return The last scaled encoder value. + */ int32_t getLastScaledEnc(); + + /** + * @brief Resets the metrics of the axis. + * @param new_pos The new position to reset to. + */ void resetMetrics(float new_pos); + + /** + * @brief Updates the metrics of the axis. + * @param new_pos The new position. + */ void updateMetrics(float new_pos); + + /** + * @brief Updates the idle spring force. + * @return The calculated idle spring force. + */ int32_t updateIdleSpringForce(); + + /** + * @brief Sets the idle spring strength. + * @param spring The new spring strength. + */ void setIdleSpringStrength(uint8_t spring); - void setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter); - void calculateAxisEffects(bool ffb_on); - int32_t getTorque(); // current torque scaled as a 32 bit signed value - int16_t updateEndstop(); - int32_t calculateExpoTorque(int32_t torque); + /** + * @brief Sets the strength and filter for an effect. + * @param val The new strength value. + * @param valToSet A reference to the value to be set. + * @param filter A reference to the biquad filter. + */ + void setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter); + /** + * @brief Calculates the mechanical effects (damper, friction, inertia) that are always active. + * These are separate from the HID FFB effects sent by the game. + * @param ffb_on A flag indicating if FFB is on. + */ + void calculateMechanicalEffects(bool ffb_on); + + /** + * @brief Gets the current torque. + * @return The current torque scaled as a 32-bit signed value. + */ + int32_t getTorque(); + + /** + * @brief Calculates the endstop torque. + * @return The calculated endstop torque. + */ + int32_t calculateEndstopTorque(); + + /** + * @brief Calculates the FFB torque exponential torque, from the input torque and apply expo and scaler + * @return The calculated exponential torque. + */ + int64_t calculateFFBTorque(); + + /** + * @brief Starts a force fade-in. + * @param start The starting force multiplier. + * @param fadeTime The duration of the fade-in. + */ void startForceFadeIn(float start = 0,float fadeTime = 0.5); metric_t* getMetrics(); - void setEffectTorque(int32_t torque); + /** + * @brief Sets the FFB effect torque. + * @param torque The new FFB effect torque from the EffectsCalculator. + */ + void setFfbEffectTorque(int64_t torque); + + /** + * @brief Updates the total torque. + * @param totalTorque A pointer to the total torque value. + * @return true if the torque was updated, false otherwise. + */ bool updateTorque(int32_t* totalTorque); void updateSamplerate(float newSamplerate); void updateFilters(uint8_t profileId); + /** + * @brief Sets the gear ratio. + * @param numerator The numerator of the gear ratio. + * @param denominator The denominator of the gear ratio. + */ void setGearRatio(uint8_t numerator,uint8_t denominator); - static const std::vector> axis1_drivers; - static const std::vector> axis2_drivers; + static const std::vector> axis1_drivers; //!< List of available motor drivers for the first axis. + static const std::vector> axis2_drivers; //!< List of available motor drivers for the second axis. private: - // Axis damper is lower than default scale of HID Damper + // Internal constants const float AXIS_DAMPER_RATIO = INTERNAL_SCALER_DAMPER * INTERNAL_AXIS_DAMPER_SCALER / 255.0; const float AXIS_INERTIA_RATIO = INTERNAL_SCALER_INERTIA * INTERNAL_AXIS_INERTIA_SCALER / 255.0; - AxisFlashAddrs flashAddrs; - volatile Control_t* control; + // Private methods + /** + * @brief Sets the gain for a specific equalizer band. + * @param band The equalizer band index (0-4). + * @param gain The gain in 0.1 dB steps (-120 to 120). + */ + void setEqGain(uint8_t band, int8_t gain); + /** + * @brief Sets the degrees of rotation for the axis. + * @param degrees The new range of rotation. + */ + void setDegrees(uint16_t degrees); + /** + * @brief Returns the current power setting of the axis. + * @return The power value. + */ + uint16_t getPower(); + /** + * @brief Returns the calculated torque scaler. + * @return The torque scaler value. + */ + bool isInverted(); + /** + * @brief Sets the ratio between game effects and endstop force and updates the internal torque scaler based on power and fx_ratio + * @param val The new ratio value (0-255). + */ + void setEffectRatio(uint8_t val); + /** + * @brief Sets the exponential torque curve. + * @param val The new expo value. + */ + void setExpo(int val); + + float calculateExpoTorque(float torque); + /** + * @brief Applies the speed limiter PI controller to the torque. + * @param torque A reference to the torque value to be modified. + * @return torque update to apply to reduced de speed. + */ + int64_t applySpeedLimiterTorque(int64_t& torque); + /** + * @brief Applies the torque slew rate limiter to the torque. + * @param torque A reference to the torque value to be modified. + */ + void applyTorqueSlewRateLimiter(int64_t& torque); + /** + * @brief Decodes the axis configuration from a 16-bit integer stored in flash. + * @param val The 16-bit encoded configuration value. + */ + static AxisConfig decodeConfFromInt(uint16_t val); + /** + * @brief Encodes the axis configuration into a 16-bit integer for flash storage. + * @param conf The AxisConfig struct to encode. + * @return The encoded 16-bit value. + */ + static uint16_t encodeConfToInt(AxisConfig conf); - //TIM_HandleTypeDef *timer_update; - AxisConfig conf; + /** + * @brief Checks if the user has let go of the wheel and updates the torque accordingly. + */ + void updateHandsOffState(); - std::unique_ptr drv = std::make_unique(); // dummy - std::shared_ptr enc = nullptr; - bool outOfBounds = false; + // Member variables + AxisFlashAddresses flashAddresses; //!< Flash memory addresses for this axis. + volatile Control_t* control; //!< Pointer to the global control structure. + AxisConfig conf; //!< Configuration for this axis (driver and encoder types). + char axis; //!< Axis identifier ('X', 'Y', 'Z'). - static AxisConfig decodeConfFromInt(uint16_t val); - static uint16_t encodeConfToInt(AxisConfig conf); + std::unique_ptr drv = std::make_unique(); //!< Unique pointer to the active motor driver. + std::shared_ptr enc = nullptr; //!< Shared pointer to the active encoder. - const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds"); + bool outOfBounds = false; //!< Flag indicating if the axis is out of its valid range. + const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds"); //!< Error object for out-of-bounds condition. - float forceFadeTime = 1.0; - float forceFadeCurMult = 1.0; + // Force fade-in effect + float forceFadeDuration = 1.0; //!< Duration of the force fade-in in seconds. + float forceFadeMultiplier = 1.0; //!< Current multiplier for the force fade-in. -#ifdef TMC4671DRIVER - TMC4671Limits tmclimits = TMC4671Limits({.pid_torque_flux_ddt = 32767, - .pid_uq_ud = 30000, - .pid_torque_flux = 30000, - .pid_acc_lim = 2147483647, - .pid_vel_lim = 2147483647, - .pid_pos_low = -2147483647, - .pid_pos_high = 2147483647}); -#endif - float encoderOffset = 0; // Offset for absolute encoders - uint16_t degreesOfRotation = 900; // How many degrees of range for the full gamepad range - uint16_t lastdegreesOfRotation = degreesOfRotation; // Used to store the previous value - uint16_t nextDegreesOfRotation = degreesOfRotation; // Buffer when changing range + float encoderOffset = 0; //!< Offset for absolute encoders. + uint16_t degreesOfRotation = 900; //!< Current degrees of rotation. + uint16_t previousDegreesOfRotation = degreesOfRotation; //!< Previous degrees of rotation (for smooth transitions). + uint16_t nextDegreesOfRotation = degreesOfRotation; //!< Target degrees of rotation. // Limiters - uint16_t maxSpeedDegS = 0; // Set to non zero to enable. example 1000. 8b * 10? - //float maxAccelDegSS = 0; - uint32_t maxTorqueRateMS = 0; // 8b * 128? - float speedLimiterP = AXIS_SPEEDLIMITER_P; - float speedLimiterI = AXIS_SPEEDLIMITER_I; - - float spdlimitreducerI = 0; - //float acclimitreducerI = 0; - //const uint8_t accelFactor = 10.0; // Conversion factor between internal and external acc limit + uint16_t maxSlewRate_Driver = MAX_SLEW_RATE; //!< Maximum slew rate as measured by the driver (in units/ms). + uint16_t maxSpeedDegS = 0; //!< Maximum speed in degrees per second. 0 to disable. + uint32_t maxTorqueRateMS = 0; //!< Maximum torque rate of change per millisecond. 0 to disable. + + float speedLimiterP = AXIS_SPEEDLIMITER_P; //!< Proportional term for the speed limiter. + float speedLimiterI = AXIS_SPEEDLIMITER_I; //!< Integral term for the speed limiter. +#ifdef USE_DSP_FUNCTIONS + arm_pid_instance_f32 speedLimiterPID; //!< PID instance for the speed limiter. +#else + // Speed limiter PID + float speedLimitReducerI = 0; +#endif - void setDegrees(uint16_t degrees); + // Axis metrics + axis_metric_t metric; //!< Current and previous physical metrics of the axis. + float previousFrameSpeed = 0; //!< Instantaneous speed from the last cycle, used for acceleration calculation. - uint16_t getPower(); - float getTorqueScaler(); - bool isInverted(); - char axis; + // Torque components + int64_t ffbEffectTorque = 0; //!< Torque from HID FFB effects. + int32_t mechanicalEffectTorque = 0; //!< Torque from mechanical effects (damper, friction, inertia). + // Power and scaling + uint16_t power = 5000; //!< Maximum motor power/torque. + uint8_t effectRatio = 204; //!< Ratio of HID effects vs. endstop effects (0-255). + float effectRatioScaler = 0; //!< Scaler for HID effects based on effectRatio. + float torqueScaler = 0; //!< Final torque scaler based on power. - // Merge normalized - axis_metric_t metric; - float _lastSpeed = 0; - int32_t effectTorque = 0; - int32_t axisEffectTorque = 0; - uint8_t fx_ratio_i = 204; // Reduce effects to a certain ratio of the total power to have a margin for the endstop. 80% = 204 - uint16_t power = 5000; - float torqueScaler = 0; // power * fx_ratio as a ratio between 0 & 1 - float effect_margin_scaler = 0; - bool invertAxis = true; // By default most motors and encoders count up CCW while gamepads are counting up CW. - uint8_t endstopStrength = 127; // Sets how much extra torque per count above endstop is added. High = stiff endstop. Low = softer - const float endstopGain = 25; // Overall max endstop intensity + // Axis configuration + bool invertAxis = true; //!< Invert axis direction. + uint8_t endstopStrength = 127; //!< Stiffness of the endstop effect. + const float endstopGain = 25; //!< Overall maximum endstop intensity. + // Idle spring effect + uint8_t idleSpringStrength = 127; //!< Strength of the idle spring. + int16_t idleSpringClip = 0; //!< Maximum force for the idle spring. + float idleSpringScale = 0; //!< Scaler for the idle spring force. + bool motorWasNotReady = true; //!< Flag to detect motor readiness transition. - uint8_t idlespringstrength = 127; - int16_t idlespringclip = 0; - float idlespringscale = 0; - bool motorWasNotReady = true; + // Slew rate calibration tracking: true when Axis requested a calibration and + // is waiting for the driver to finish measuring the max slew rate. + bool awaitingSlewCalibration = false; + // Filters // TODO tune these and check if it is really stable and beneficial to the FFB. index 4 placeholder - const std::array filterSpeedCst = { {{ 40, 55 }, { 70, 55 }, { 120, 55 }, {180, 55}} }; - const std::array filterAccelCst = { {{ 40, 30 }, { 55, 30 }, { 70, 30 }, {120, 55}} }; - const biquad_constant_t filterDamperCst = {60, 55}; - const biquad_constant_t filterFrictionCst = {50, 20}; - const biquad_constant_t filterInertiaCst = {20, 20}; - uint8_t filterProfileId = 1; // Default medium (1) as this is the most common encoder resolution and users can go lower or higher if required. - float filter_f = 1000; // 1khz default. should be set at runtime once the actual rate is known - const int32_t intFxClip = 20000; - uint8_t damperIntensity = 30; - - uint8_t frictionIntensity = 0; - uint8_t inertiaIntensity = 0; - + const std::array filterSpeedCst = { {{ 40, 55 }, { 70, 55 }, { 120, 55 }, {180, 55}} }; //!< Speed filter profiles. + const std::array filterAccelCst = { {{ 40, 30 }, { 55, 30 }, { 70, 30 }, {120, 55}} }; //!< Acceleration filter profiles. + const biquad_constant_t filterDamperCst = {60, 55}; //!< Damper filter constants. + const biquad_constant_t filterFrictionCst = {50, 20}; //!< Friction filter constants. + const biquad_constant_t filterInertiaCst = {20, 20}; //!< Inertia filter constants. + uint8_t filterProfileId = 1; //!< Currently selected filter profile ID. + float filter_f = 1000.0; // 1khz + const int32_t internalFxClip = 20000; //!< Clipping value for internal effects. + + // Internal effects intensity + uint8_t damperIntensity = 30; //!< Intensity of the internal damper effect. + uint8_t frictionIntensity = 0; //!< Intensity of the internal friction effect. + uint8_t inertiaIntensity = 0; //!< Intensity of the internal inertia effect. + + // Biquad filter instances Biquad speedFilter = Biquad(BiquadType::lowpass, filterSpeedCst[filterProfileId].freq/filter_f, filterSpeedCst[filterProfileId].q/100.0, 0.0); Biquad accelFilter = Biquad(BiquadType::lowpass, filterAccelCst[filterProfileId].freq/filter_f, filterAccelCst[filterProfileId].q/100.0, 0.0); - Biquad damperFilter = Biquad(BiquadType::lowpass, filterDamperCst.freq/filter_f, filterDamperCst.q / 100.0, 0.0); // enable on class constructor - Biquad frictionFilter = Biquad(BiquadType::lowpass, filterFrictionCst.freq/filter_f, filterFrictionCst.q / 100.0, 0.0); // enable on class constructor - Biquad inertiaFilter = Biquad(BiquadType::lowpass, filterInertiaCst.freq/filter_f, filterInertiaCst.q / 100.0, 0.0); // enable on class constructor - - - void setFxRatio(uint8_t val); - void updateTorqueScaler(); - - void setExpo(int val); - - - GearRatio_t gearRatio; - - int expoValInt = 0; // expo v = val*2 => v<0 ? 1/-v : v - float expo = 1; - float expoScaler = 50; // 0.28 to 3.54 - + Biquad damperFilter = Biquad(BiquadType::lowpass, filterDamperCst.freq/filter_f, filterDamperCst.q / 100.0, 0.0); + Biquad frictionFilter = Biquad(BiquadType::lowpass, filterFrictionCst.freq/filter_f, filterFrictionCst.q / 100.0, 0.0); + Biquad inertiaFilter = Biquad(BiquadType::lowpass, filterInertiaCst.freq/filter_f, filterInertiaCst.q / 100.0, 0.0); + + // Equalizer + static const uint8_t num_eq_bands = 6; + const std::array eq_frequencies = {10, 15, 25, 40, 60, 100}; + std::array eqFilters; + std::array eqGains = {0,0,0,0,0}; // Gains from -120 to 120, represents gain in dB * 10 + bool equalizerEnabled = false; //!< Enable/disable the equalizer. + + // Post-processing + GearRatio_t gearRatio; //!< Gear ratio between encoder and axis. + int expoValue = 0; //!< Raw integer value for the expo curve. Formula: v = val*2 => v<0 ? 1/-v : v + float expo = 1; //!< Calculated exponent for the torque curve. + float expoScaler = 50; //!< Scaler for the expo calculation : 0.28 to 3.54 + + // Hands-off detection + bool handsOffCheckEnabled = false; + float handsOffAccelThreshold = 0.1; + uint16_t handsOffSpeedThreshold = 720; // deg/s + bool handsOff = false; + float accel_buffer[16] = {0}; + uint8_t accel_buffer_idx = 0; + uint32_t handsOffTimer = 0; }; #endif /* SRC_AXIS_H_ */ diff --git a/Firmware/FFBoard/Inc/EffectsCalculator.h b/Firmware/FFBoard/Inc/EffectsCalculator.h index feff5f0a0..e22d71cd6 100644 --- a/Firmware/FFBoard/Inc/EffectsCalculator.h +++ b/Firmware/FFBoard/Inc/EffectsCalculator.h @@ -27,7 +27,16 @@ class Axis; struct metric_t; -// default effect gains +enum class ReconFilterMode : uint8_t { + NO_RECONSTRUCTION = 0, + LINEAR_INTERPOLATION = 1, // More responsive, attempts to match the game's signal precisely. Can feel "grainy" if slew rate is high. + SPLINE_CUBIC_NATURAL = 2, // Highest fidelity, smooth curve through last 4 points. More CPU intensive. + SPLINE_CUBIC_HERMITE = 3 // Mixed solution, good fidelity and optimized timing +}; + +/** + * @brief Default gains for conditional effects. + */ struct effect_gain_t { uint8_t friction = 254; uint8_t spring = 64; @@ -35,6 +44,9 @@ struct effect_gain_t { uint8_t inertia = 127; }; +/** + * @brief Scaler values for conditional effects to adjust their intensity. + */ struct effect_scaler_t { float friction = 1.0; //0.4 * 40; float spring = 16.0; @@ -42,6 +54,9 @@ struct effect_scaler_t { float inertia = 2.0;//0.5 * 40; }; +/** + * @brief Biquad filter coefficients for various effects. + */ struct effect_biquad_t { biquad_constant_t constant = { 500, 70 }; biquad_constant_t friction = { 50, 20 }; @@ -49,10 +64,13 @@ struct effect_biquad_t { biquad_constant_t inertia = { 15, 20 }; }; +/** + * @brief Statistics for each effect type. + */ struct effect_stat_t { - std::array current={0}; - std::array max={0}; - uint16_t nb=0; + std::array current={0}; //!< Current force value. + std::array max={0}; //!< Maximum force value since reset. + uint16_t nb=0; //!< Number of times this effect has been used. }; enum class EffectsCalculator_commands : uint32_t { @@ -60,8 +78,13 @@ enum class EffectsCalculator_commands : uint32_t { damper_f, damper_q, friction_f, friction_q, inertia_f, inertia_q, filterProfileId, frictionPctSpeedToRampup, monitorEffect, effectsDetails, effectsForces, + reconFilterMode }; +/** + * @brief This class is responsible for calculating the forces of all active FFB effects. + * It runs in its own thread to perform the calculations. + */ class EffectsCalculator: public PersistentStorage, public CommandHandler, cpp_freertos::Thread { @@ -69,36 +92,132 @@ class EffectsCalculator: public PersistentStorage, EffectsCalculator(); virtual ~EffectsCalculator(); - static ClassIdentifier info; + static ClassIdentifier info; //!< Static class identifier. + /** + * @brief Returns the class identifier. + * @return The class identifier. + */ const ClassIdentifier getInfo(); const ClassType getClassType() override {return ClassType::Internal;}; - void saveFlash(); - void restoreFlash(); + /** + * @brief Saves effect settings to flash memory. + * @override from PersistentStorage + */ + void saveFlash() override; + /** + * @brief Restores effect settings from flash memory. + * @override from PersistentStorage + */ + void restoreFlash() override; + + /** + * @brief Checks if the effects calculator is active. + * @return true if active, false otherwise. + */ bool isActive(); + + /** + * @brief Sets the active state of the effects calculator. + * @param active The new active state. + */ void setActive(bool active); + + /** + * @brief This is the main calculation method. It iterates through all active effects and sums up the forces for each axis. + * @param axes A reference to the vector of axes. + */ void calculateEffects(std::vector> &axes); + + /** + * @brief Sets the filters for a specific effect. + * @param effect A pointer to the effect. + */ virtual void setFilters(FFB_Effect* effect); + + /** + * @brief Sets the global gain for all effects. + * @param gain The new gain value. + */ void setGain(uint8_t gain); + + /** + * @brief Gets the global gain. + * @return The current global gain value. + */ uint8_t getGain(); + + /** + * @brief Logs the usage of an effect type for statistics. + * @param type The effect type. + * @param remove True to decrement the usage count, false to increment. + */ void logEffectType(uint8_t type,bool remove = false); - //void setDirectionEnableMask(uint8_t mask); + + /** + * @brief Calculates and stores statistics for a given effect type. + * @param type The effect type. + * @param force The calculated force. + * @param axis The axis index. + */ void calcStatsEffectType(uint8_t type, int32_t force,uint8_t axis); + + /** + * @brief Logs the state of an effect (start/stop). + * @param type The effect type. + * @param state The new state. + */ void logEffectState(uint8_t type,uint8_t state); + + /** + * @brief Resets the statistics of active effects. + * @param reinit If true, re-initializes the stats based on currently active effects. + */ void resetLoggedActiveEffects(bool reinit); + /** + * @brief Finds a free slot for a new effect. + * @param type The type of the effect. + * @return The index of the free slot, or -1 if no slot is available. + */ int32_t find_free_effect(uint8_t type); + + /** + * @brief Frees an effect slot. + * @param idx The index of the effect to free. + */ void free_effect(uint16_t idx); - CommandStatus command(const ParsedCommand& cmd,std::vector& replies); + /** + * @brief Met à jour les tampons d'interpolation pour un effet non-conditionnel. + * Appelé par le handler HID (ex: 60Hz). + * @param effect Pointeur vers l'effet à modifier. + * @param new_magnitude Nouvelle magnitude/amplitude cible. + * @param new_offset Nouvel offset cible (utilisé uniquement si is_periodic est true). + * @param is_periodic Si true, met à jour les deux tampons (mag + offset). + */ + void updateEffectReconstruction(FFB_Effect* effect, float new_magnitude, float new_offset, bool is_periodic); + + + /** + * @brief Handles command line interface commands for the effects calculator. + * @param cmd The parsed command. + * @param replies A vector of replies to be sent back. + * @return The status of the command execution. + * @override from CommandHandler + */ + CommandStatus command(const ParsedCommand& cmd,std::vector& replies) override; virtual std::string getHelpstring() { return "Controls internal FFB effects"; } static const uint32_t max_effects = MAX_EFFECTS; - std::array effects; // Main effects storage + std::array effects; //!< Main effects storage array. - // Thread impl - void Run(); + /** + * @brief The main loop for the effects calculator thread. + * @override from cpp_freertos::Thread + */ + void Run() override; void updateSamplerate(float newSamplerate); // Must be called if update rate is changed to update filters and effects @@ -106,51 +225,147 @@ class EffectsCalculator: public PersistentStorage, protected: private: - //uint8_t directionEnableMask = 0; // Filters - effect_biquad_t filter[2]; // 0 is the default profile and the custom for CFFilter, CUSTOM_PROFILE_ID is the custom slot - uint8_t filterProfileId = 0; - uint32_t calcfrequency = 1000; // HID frequency 1khz - const float qfloatScaler = 0.01; + effect_biquad_t filter[2]; //!< Filter coefficients for CFFilter, default (0) and custom (1) profiles. + uint8_t filterProfileId = 0; //!< Currently active filter profile (0 or 1). + uint32_t calcfrequency = 1000; //!< Calculation frequency in Hz (matches HID rate 1khz). + const float qfloatScaler = 0.01; //!< Scaler for Q factor. - // Rescale factor for conditional effect to boost or decrease the intensity - uint8_t global_gain = 0xff; - effect_gain_t gain; - effect_scaler_t scaler; - uint8_t frictionPctSpeedToRampup = 25; // define the max value of the range (0..5% of maxspeed) where torque is rampup on friction + // Gains and scalers + uint8_t global_gain = 0xff; //!< Global gain for all effects. + effect_gain_t gain; //!< Per-effect type gains. + effect_scaler_t scaler; //!< Per-effect type intensity scalers. + uint8_t frictionPctSpeedToRampup = 25; //!< Speed percentage for friction ramp-up. : define the max value of the range (0..5% of maxspeed) where torque is rampup on friction // FFB status - bool effects_active = false; // If FFB is on - uint32_t effects_used = 0; - std::array effects_stats; // [0..12 effect types] - std::array effects_statslast; // [0..12 effect types] - bool isMonitorEffect = false; + bool effects_active = false; //!< True if FFB is globally active. + uint32_t effects_used = 0; //!< Bitmask of effect types used since reset. + std::array effects_stats; //!< Statistics for each effect type, [0..12 effect types]. + std::array effects_statslast; //!< Statistics from the previous cycle, [0..12 effect types]. + bool isMonitorEffect = false; //!< Flag to enable effect monitoring. + + /** + * @brief Calculates the force for a single component of an effect on a specific axis. + * Dispatches between conditional and non-conditional effects. + */ + int32_t calculateEffectForceOnAxis(FFB_Effect *effect, int32_t forceVector, std::vector> &axes, uint8_t axis); - int32_t calcComponentForce(FFB_Effect *effect, int32_t forceVector, std::vector> &axes, uint8_t axis); + /** + * @brief Calculates the base force for non-conditional effects (Constant, Ramp, Periodic). + */ int32_t calcNonConditionEffectForce(FFB_Effect* effect); + + /** + * @brief Calculates the speed threshold for friction ramp-up. + */ float speedRampupPct(); + + /** + * @brief Calculates the force for conditional effects (Spring, Damper, Inertia, Friction). + */ int32_t calcConditionEffectForce(FFB_Effect *effect, float metric, uint8_t gain, uint8_t idx, float scale, float angle_ratio); - int32_t getEnvelopeMagnitude(FFB_Effect *effect); + + /** + * @brief Calculates the magnitude of an effect with an envelope. + */ + int32_t getEnvelopeMagnitude(FFB_Effect *effect, int32_t baseMagnitude); + + /** + * @brief Generates a string listing the effects used. + */ std::string listEffectsUsed(bool details = false,uint8_t axis = 0); - //std::string listForceEffects(); + + /** + * @brief Validates and sanitizes filter coefficients. + */ void checkFilterCoeff(biquad_constant_t *filter, uint32_t freq,uint8_t q); + + /** + * @brief Updates the filters for all active effects of a specific type. + */ void updateFilterSettingsForEffects(uint8_t type_effect); + + // --- Reconstruction Filter --- + ReconFilterMode reconFilterMode = ReconFilterMode::NO_RECONSTRUCTION; //!< Current reconstruction filter mode. + + /** + * @brief Internal method to push a new value into the reconstruction filter. + * @param state Pointer to the reconstruction filter state. + * @param newValue The new value to push into the filter. + */ + void pushReconstructionSample(ReconFilterState* state, float newValue); + + /** + * @brief Retrieves the interpolated value from the reconstruction filter. + * @param state Pointer to the reconstruction filter state. + * @param fallbackValue Value to return if the filter is not ready. + * @return The interpolated value from the filter. + */ + float evaluateReconstructionFilter(ReconFilterState* state, float fallbackValue); }; /** * Helper interface class for common effects calculator related control functions */ +/** + * @brief Helper interface class for common effects calculator related control functions. + * This class provides a common interface for controlling FFB effects. + */ class EffectsControlItf{ public: - virtual void set_FFB(bool state) = 0; // Enables or disables FFB + /** + * @brief Enables or disables FFB. + * @param state The new state of the FFB. + */ + virtual void set_FFB(bool state) = 0; + + /** + * @brief Stops the FFB. + */ virtual void stop_FFB(){set_FFB(false);}; + + /** + * @brief Starts the FFB. + */ virtual void start_FFB(){set_FFB(true);}; + + /** + * @brief Resets all FFB effects. + */ virtual void reset_ffb() = 0; - virtual uint32_t getConstantForceRate(); // Returns an estimate of the constant force effect update rate in hz - virtual uint32_t getRate(); // Returns an estimate of the overall effect update speed in hz + + /** + * @brief Returns an estimate of the constant force effect update rate in Hz. + * @return The estimated update rate. + */ + virtual uint32_t getConstantForceRate(); + + /** + * @brief Returns an estimate of the overall effect update speed in Hz. + * @return The estimated update rate. + */ + virtual uint32_t getRate(); + + /** + * @brief Checks if FFB is active. + * @return true if FFB is active, false otherwise. + */ virtual bool getFfbActive() = 0; + + /** + * @brief Sets the global gain for all effects. + * @param gain The new gain value. + */ virtual void set_gain(uint8_t gain) = 0; + + /** + * @brief To be called when a constant force update event occurs. Used for rate calculation. + */ virtual void cfUpdateEvent(); + + /** + * @brief To be called when any FFB update event occurs. Used for rate calculation. + */ virtual void fxUpdateEvent(); virtual void updateSamplerate(float newSamplerate) = 0; // Should be called when update loop rate is changed @@ -158,8 +373,8 @@ class EffectsControlItf{ FastMovingAverage fxPeriodAvg{5}; FastMovingAverage cfUpdatePeriodAvg{5}; - uint32_t lastFxUpdate = 0; - uint32_t lastCfUpdate = 0; + uint32_t lastFxUpdate = 0; //!< Timestamp of the last FFB update. + uint32_t lastCfUpdate = 0; //!< Timestamp of the last constant force update. }; #endif /* EFFECTSCALCULATOR_H_ */ diff --git a/Firmware/FFBoard/Inc/Filters.h b/Firmware/FFBoard/Inc/Filters.h index 83a31bf17..92c2a7959 100644 --- a/Firmware/FFBoard/Inc/Filters.h +++ b/Firmware/FFBoard/Inc/Filters.h @@ -2,13 +2,17 @@ * Filters.h * * Created on: Feb 13, 2020 - * Author: Yannick + * Author: Yannick, Vincent */ #ifndef FILTERS_H_ #define FILTERS_H_ #include "cppmain.h" +#ifdef USE_DSP_FUNCTIONS +#include "arm_math.h" +#endif + #ifdef __cplusplus // Frequency in hz, q in float q*100. Example: Q 0.5 -> 50 @@ -40,14 +44,27 @@ class Biquad{ float getFc() const; void setQ(float Q); float getQ() const; + void setPeakGain(float peakGainDB); void calcBiquad(void); -protected: +#ifdef USE_DSP_FUNCTIONS + const float* getCoeffs() const { return pCoeffs; } +#endif +protected: BiquadType type; - float a0, a1, a2, b1, b2; float Fc, Q, peakGain; + +#ifdef USE_DSP_FUNCTIONS + // CMSIS-DSP instance + arm_biquad_casd_df1_inst_f32 S; + float32_t pCoeffs[5]; + float32_t pState[4]; // For a single biquad stage (DF1 float requires 4 states) +#else float z1, z2; + float a0, a1, a2, b1, b2; +#endif + }; diff --git a/Firmware/FFBoard/Inc/HidFFB.h b/Firmware/FFBoard/Inc/HidFFB.h index 7dae12394..bb0d0cfcd 100644 --- a/Firmware/FFBoard/Inc/HidFFB.h +++ b/Firmware/FFBoard/Inc/HidFFB.h @@ -20,54 +20,184 @@ #define Z_AXIS_ENABLE 4 #define DIRECTION_ENABLE(AXES) (1 << AXES) +/** + * @brief This class handles the Force Feedback (FFB) HID communication. + * It inherits from UsbHidHandler to process HID reports and from EffectsControlItf to control FFB effects. + * It is responsible for parsing HID FFB reports from the host and managing the lifecycle of FFB effects. + */ class HidFFB: public UsbHidHandler, public EffectsControlItf { public: + /** + * @brief Construct a new HidFFB object. + * @param ec A shared pointer to the EffectsCalculator instance. + * @param axisCount The number of axes to be controlled. + */ HidFFB(std::shared_ptr ec,uint8_t axisCount); virtual ~HidFFB(); + /** + * @brief Handles outgoing HID reports from the host (PC -> Device). This is the main entry point for FFB commands. + * @param report_id The report ID. + * @param report_type The type of the report. + * @param buffer A pointer to the report data. + * @param bufsize The size of the report data. + * @override from UsbHidHandler + */ void hidOut(uint8_t report_id, hid_report_type_t report_type,const uint8_t* buffer, uint16_t bufsize) override; + + /** + * @brief Handles incoming HID GET_FEATURE requests from the host (Device -> PC). + * Used for status reports like block load and PID pool. + * @param report_id The report ID. + * @param report_type The type of the report. + * @param buffer A pointer to the buffer where the report data should be stored. + * @param reqlen The requested length of the report. + * @return The actual length of the report. + * @override from UsbHidHandler + */ uint16_t hidGet(uint8_t report_id, hid_report_type_t report_type,uint8_t* buffer, uint16_t reqlen) override; - bool getFfbActive(); + /** + * @brief Checks if the FFB is active. + * @return true if FFB is active, false otherwise. + * @override from EffectsControlItf + */ + bool getFfbActive() override; + + /** + * @brief Sends a HID report to the host. + * @param report A pointer to the report data. + * @param len The length of the report data. + * @return true if the report was sent successfully, false otherwise. + */ static bool HID_SendReport(uint8_t *report,uint16_t len); - void reset_ffb(); - void start_FFB(); - void stop_FFB(); - void set_FFB(bool state); - void set_gain(uint8_t gain); + /** + * @brief Resets all FFB effects. + * @override from EffectsControlItf + */ + void reset_ffb() override; + + /** + * @brief Starts the FFB. + * @override from EffectsControlItf + */ + void start_FFB() override; + + /** + * @brief Stops the FFB. + * @override from EffectsControlItf + */ + void stop_FFB() override; + + /** + * @brief Sets the FFB state (active/inactive). + * @param state The new state of the FFB. + * @override from EffectsControlItf + */ + void set_FFB(bool state) override; + + /** + * @brief Sets the global gain for all FFB effects. + * @param gain The new gain value (0-255). + * @override from EffectsControlItf + */ + void set_gain(uint8_t gain) override; + /** + * @brief Sends a status report for a specific effect to the host. + * @param effect The ID of the effect. + */ void sendStatusReport(uint8_t effect); + + /** + * @brief Sets the direction enable mask for the axes. + * This mask determines how direction parameters in HID reports are interpreted. + * @param mask The new direction enable mask. + */ void setDirectionEnableMask(uint8_t mask); void updateSamplerate(float newSamplerate); private: - // HID - std::shared_ptr effects_calc; - std::array& effects; // Must be passed in constructor + // HID processing methods + std::shared_ptr effects_calc; //!< A shared pointer to the EffectsCalculator instance. + std::array& effects; //!< A reference to the array of FFB effects, managed by EffectsCalculator, Must be passed in constructor. + + /** + * @brief Handles the "Create New Effect" HID report. Allocates a new effect. + * @param effect Pointer to the HID report data. + */ void new_effect(FFB_CreateNewEffect_Feature_Data_t* effect); + + /** + * @brief Frees an effect slot. This is called when a HID_ID_BLKFRREP is received. + * @param id The block index of the effect to free. + */ void free_effect(uint16_t id); + + /** + * @brief Handles FFB control commands (Enable, Disable, Stop, Reset, etc.). + * @param cmd The control command byte. + */ void ffb_control(uint8_t cmd); + + /** + * @brief Handles the "Set Effect" HID report. Configures a previously created effect. + * @param effect Pointer to the HID report data. + */ void set_effect(FFB_SetEffect_t* effect); + + /** + * @brief Handles the "Set Condition" HID report (Spring, Damper, Friction, Inertia). + * @param cond Pointer to the HID report data. + */ void set_condition(FFB_SetCondition_Data_t* cond); + + /** + * @brief Handles the "Set Envelope" HID report. + * @param report Pointer to the HID report data. + */ void set_envelope(FFB_SetEnvelope_Data_t* report); + + /** + * @brief Handles the "Set Ramp" HID report. + * @param report Pointer to the HID report data. + */ void set_ramp(FFB_SetRamp_Data_t* report); + + /** + * @brief Handles the "Set Constant Force" HID report. + * @param effect Pointer to the HID report data. + */ void set_constant_effect(FFB_SetConstantForce_Data_t* effect); + + /** + * @brief Handles the "Set Periodic" HID report (Sine, Square, etc.). + * @param report Pointer to the HID report data. + */ void set_periodic(FFB_SetPeriodic_Data_t* report); - void set_effect_operation(FFB_EffOp_Data_t* report); + /** + * @brief Handles the "Effect Operation" HID report (Start, Stop, Start Solo). + * @param report Pointer to the HID report data. + */ + void set_effect_operation(FFB_EffOp_Data_t* report); + /** + * @brief Sets the default filters for a new effect based on its type. + * @param effect Pointer to the effect to configure. + */ void set_filters(FFB_Effect* effect); - uint8_t directionEnableMask; // Has to be adjusted if bit is not last bit after axis enable bits - uint16_t used_effects = 0; - bool ffb_active = false; - FFB_BlockLoad_Feature_Data_t blockLoad_report; - FFB_PIDPool_Feature_Data_t pool_report; + uint8_t directionEnableMask; //!< Mask to enable/disable directions for axes. Adjusted based on axis count. + uint16_t used_effects = 0; //!< The number of currently active effects. + bool ffb_active = false; //!< Flag indicating if FFB is globally active. + FFB_BlockLoad_Feature_Data_t blockLoad_report; //!< HID report structure for block load status. + FFB_PIDPool_Feature_Data_t pool_report; //!< HID report structure for PID pool status. - reportFFB_status_t reportFFBStatus; + reportFFB_status_t reportFFBStatus; //!< HID report structure for general FFB status. - uint8_t axisCount; + uint8_t axisCount; //!< The number of axes supported by this FFB instance. }; #endif /* HIDFFB_H_ */ diff --git a/Firmware/FFBoard/Inc/MotorDriver.h b/Firmware/FFBoard/Inc/MotorDriver.h index 0446ac3ba..b643a66ac 100644 --- a/Firmware/FFBoard/Inc/MotorDriver.h +++ b/Firmware/FFBoard/Inc/MotorDriver.h @@ -2,12 +2,14 @@ * MotorDriver.h * * Created on: Feb 1, 2020 - * Author: Yannick + * Author: Yannick, Vincent */ #ifndef MOTORDRIVER_H_ #define MOTORDRIVER_H_ +#define MAX_SLEW_RATE 65535 + #include "cppmain.h" #include "ChoosableClass.h" #include "PersistentStorage.h" @@ -25,11 +27,32 @@ class MotorDriver : public ChoosableClass{ const ClassType getClassType() override {return ClassType::Motordriver;}; static const std::vector> all_drivers; + virtual void setupDriver(); virtual void turn(int16_t power); virtual void stopMotor(); virtual void startMotor(); virtual void emergencyStop(bool reset = false); + virtual void setPowerLimit(uint16_t power){}; // specific motor driver manager power, this is used to send powerLimit to the driver, like TMC4671. + + // + /** + * Slew rate calibration interface (no-op by default) + * If driver can't calibration the max slew rate will by MAX_SLEW_RATE (65535) + * @return false is the driver not implemented this process or true if calibration start + */ + virtual bool startSlewRateCalibration() { return false; }; + /** + * Check if calibration is in process + * @return the state of calibration process + */ + virtual bool isSlewRateCalibrationInProgress() { return false; }; + /** + * Get the value of the Slew Rate after a calibration + * @return + */ + virtual uint16_t getDrvSlewRate() { return MAX_SLEW_RATE; }; + virtual bool motorReady(); // Returns true if the driver is active and ready to receive commands virtual Encoder* getEncoder(); // Encoder is managed by the motor driver. Must always return an encoder diff --git a/Firmware/FFBoard/Inc/ffb_defs.h b/Firmware/FFBoard/Inc/ffb_defs.h index f6f0a8eef..ca3d70840 100644 --- a/Firmware/FFBoard/Inc/ffb_defs.h +++ b/Firmware/FFBoard/Inc/ffb_defs.h @@ -11,6 +11,7 @@ #include "cppmain.h" #include "Filters.h" #include "constants.h" // For #define MAX_AXIS + #define FFB_ID_OFFSET 0x00 #define MAX_EFFECTS 40 @@ -195,7 +196,25 @@ typedef struct } __attribute__((packed)) reportFFB_status_t; - +/** + * @brief Contient l'état et les tampons pour + * l'interpolation d'un seul paramètre (ex: magnitude ou offset). + */ +typedef struct +{ +#ifdef USE_DSP_FUNCTIONS + float32_t spline_x[4] = {0}; // Buffer time(en us) + float32_t spline_y[4] = {0}; // Buffer value + float32_t spline_y2[4] = {0}; // Buffer for Spline Natural + float32_t spline_scratch[8] = {0}; // Buffer for Spline Natural + arm_spline_instance_f32 spline_instance; // Instance pour CMSIS-DSP + bool spline_arm_initialized = false; // CMSIS-DSP initialized ? +#else + float spline_x[4] = {0}; // Buffer time(en us) + float spline_y[4] = {0}; // Buffer value +#endif + bool isSplineReady = false; // Le tampon est-il plein ? +} ReconFilterState; typedef struct { @@ -333,6 +352,9 @@ typedef struct uint16_t samplePeriod = 0; bool useEnvelope = false; bool useSingleCondition = true; + + ReconFilterState recon_magnitude; // State pour Magnitude (ou Amplitude) + ReconFilterState recon_offset; // State pour Offset (périodiques) } FFB_Effect; diff --git a/Firmware/FFBoard/Inc/flash_helpers.h b/Firmware/FFBoard/Inc/flash_helpers.h index 68961e711..09fab245d 100644 --- a/Firmware/FFBoard/Inc/flash_helpers.h +++ b/Firmware/FFBoard/Inc/flash_helpers.h @@ -2,7 +2,7 @@ * flash_helpers.h * * Created on: 31.01.2020 - * Author: Yannick + * Author: Yannick, Vincent */ #ifndef FLASH_HELPERS_H_ @@ -32,6 +32,11 @@ bool Flash_ReadWriteDefault(uint16_t adr,uint16_t *buf,uint16_t def); // returns void Flash_Dump(std::vector> *result,bool includeAll = false); bool Flash_Format(); +#ifdef COGGING_TABLE_FLASH_START_ADDRESS +bool Flash_WriteCoggingTable(uint8_t table_idx, int16_t* data); +bool Flash_ReadCoggingTable(uint8_t table_idx, int16_t* data); +#endif + bool OTP_Write(uint16_t adroffset,uint64_t dat); bool OTP_Read(uint16_t adroffset,uint64_t* dat); diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp index 41f487b28..1804c089d 100644 --- a/Firmware/FFBoard/Src/Axis.cpp +++ b/Firmware/FFBoard/Src/Axis.cpp @@ -2,17 +2,32 @@ * Axis.cpp * * Created on: 31.01.2020 - * Author: Yannick + * Author: Yannick, Vincent + * */ #include "Axis.h" #include "voltagesense.h" + +// Load the driver is they are declared in targer_constants.h +#ifdef TMC4671DRIVER #include "TMC4671.h" +#endif +#ifdef PWMDRIVER #include "MotorPWM.h" -#include "VescCAN.h" +#endif +#ifdef ODRIVE #include "ODriveCAN.h" +#endif +#ifdef VESC +#include "VescCAN.h" +#endif +#ifdef SIMPLEMOTION #include "MotorSimplemotion.h" +#endif +#ifdef RMDCAN #include "RmdMotorCAN.h" +#endif #include "critical.hpp" ////////////////////////////////////////////// @@ -86,39 +101,59 @@ const std::vector> Axis::axis2_drivers = /** * Axis class manages motor drivers and passes effect torque to the motor drivers */ -Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_AXIS), drv_chooser(MotorDriver::all_drivers),enc_chooser{Encoder::all_encoders} +Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_AXIS), driverChooser(MotorDriver::all_drivers),encoderChooser{Encoder::all_encoders} { + +#ifdef USE_DSP_FUNCTIONS + speedLimiterPID.Kp = speedLimiterP; + speedLimiterPID.Ki = speedLimiterI; + speedLimiterPID.Kd = 0.0f; + arm_pid_init_f32(&speedLimiterPID, 1); +#endif + this->axis = axis; this->control = control; if (axis == 'X') { - drv_chooser = ClassChooser(axis1_drivers); + driverChooser = ClassChooser(axis1_drivers); setInstance(0); - this->flashAddrs = AxisFlashAddrs({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL, + this->flashAddresses = AxisFlashAddresses({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL, ADR_AXIS1_MAX_SLEWRATE_DRV, ADR_AXIS1_ENDSTOP, ADR_AXIS1_POWER, ADR_AXIS1_DEGREES,ADR_AXIS1_EFFECTS1,ADR_AXIS1_EFFECTS2,ADR_AXIS1_ENC_RATIO, - ADR_AXIS1_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1}); + ADR_AXIS1_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1, + ADR_AXIS1_EQ1,ADR_AXIS1_EQ2,ADR_AXIS1_EQ3, + ADR_AXIS1_HANDSOFF_CONF, ADR_AXIS1_HANDSOFF_ACCEL + }); } else if (axis == 'Y') { - drv_chooser = ClassChooser(axis2_drivers); + driverChooser = ClassChooser(axis2_drivers); setInstance(1); - this->flashAddrs = AxisFlashAddrs({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL, + this->flashAddresses = AxisFlashAddresses({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL, ADR_AXIS2_MAX_SLEWRATE_DRV, ADR_AXIS2_ENDSTOP, ADR_AXIS2_POWER, ADR_AXIS2_DEGREES,ADR_AXIS2_EFFECTS1,ADR_AXIS2_EFFECTS2, ADR_AXIS2_ENC_RATIO, - ADR_AXIS2_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1}); + ADR_AXIS2_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1, + ADR_AXIS2_EQ1,ADR_AXIS2_EQ2,ADR_AXIS2_EQ3, + ADR_AXIS2_HANDSOFF_CONF, ADR_AXIS2_HANDSOFF_ACCEL + }); } else if (axis == 'Z') { setInstance(2); - this->flashAddrs = AxisFlashAddrs({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL, + this->flashAddresses = AxisFlashAddresses({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL, ADR_AXIS3_MAX_SLEWRATE_DRV, ADR_AXIS3_ENDSTOP, ADR_AXIS3_POWER, ADR_AXIS3_DEGREES,ADR_AXIS3_EFFECTS1,ADR_AXIS3_EFFECTS2,ADR_AXIS3_ENC_RATIO, - ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1}); + ADR_AXIS3_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1, + ADR_AXIS3_EQ1,ADR_AXIS3_EQ2,ADR_AXIS3_EQ3, + ADR_AXIS3_HANDSOFF_CONF, ADR_AXIS3_HANDSOFF_ACCEL + }); } + // Initialize equalizer filters + for (uint8_t idx = 0; idx < num_eq_bands; idx++) { + eqFilters[idx].setBiquad(BiquadType::peak, eq_frequencies[idx] / filter_f, 1.0, 0.0); + } - restoreFlash(); // Load parameters CommandHandler::registerCommands(); // Internal commands registerCommands(); - updateTorqueScaler(); // In case no flash setting has been loaded yet + restoreFlash(); // Load parameters } Axis::~Axis() @@ -145,7 +180,7 @@ void Axis::registerCommands(){ registerCommand("drvtype", Axis_commands::drvtype, "Motor driver type get/set/list",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); registerCommand("pos", Axis_commands::pos, "Encoder position",CMDFLAG_GET); registerCommand("maxspeed", Axis_commands::maxspeed, "Speed limit in deg/s",CMDFLAG_GET | CMDFLAG_SET); - registerCommand("maxtorquerate", Axis_commands::maxtorquerate, "Torque rate limit in counts/ms",CMDFLAG_GET | CMDFLAG_SET); + registerCommand("slewrate", Axis_commands::slewrate, "Torque rate limit in counts/ms",CMDFLAG_GET | CMDFLAG_SET); registerCommand("fxratio", Axis_commands::fxratio, "Effect ratio. Reduces game effects excluding endstop. 255=100%",CMDFLAG_GET | CMDFLAG_SET); registerCommand("curtorque", Axis_commands::curtorque, "Axis torque",CMDFLAG_GET); registerCommand("curpos", Axis_commands::curpos, "Axis position",CMDFLAG_GET); @@ -160,16 +195,31 @@ void Axis::registerCommands(){ registerCommand("cpr", Axis_commands::cpr, "Reported encoder CPR",CMDFLAG_GET); registerCommand("expo", Axis_commands::expo, "Exponential curve correction (x^(val/exposcale)+1)", CMDFLAG_GET | CMDFLAG_SET); registerCommand("exposcale", Axis_commands::exposcale, "Scaler constant for expo", CMDFLAG_GET); + + registerCommand("equalizer", Axis_commands::equalizer, "Equalizer enable", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb1", Axis_commands::eqb1, "Equalizer band 1 gain (-120/120)", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb2", Axis_commands::eqb2, "Eq bd 2", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb3", Axis_commands::eqb3, "Eq bd 3", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb4", Axis_commands::eqb4, "Eq bd 4", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb5", Axis_commands::eqb5, "Eq bd 5", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("eqb6", Axis_commands::eqb6, "Eq bd 6", CMDFLAG_GET | CMDFLAG_SET); + + registerCommand("handsoff", Axis_commands::handsoff, "Hands-off enable", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("handsoff_speed", Axis_commands::handsoff_speed, "Hoff speed thrld (deg/s)", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("handsoff_accel", Axis_commands::handsoff_accel, "Hoff accel std dev thrld (float, val/1000)", CMDFLAG_GET | CMDFLAG_SET); + + registerCommand("maxSlewRateDrv", Axis_commands::maxSlewRateDrv, "Max driver torque in counts/ms",CMDFLAG_GET); + registerCommand("calibrate_maxSlewRateDrv", Axis_commands::calibrate_maxSlewRateDrv, "Start driver slewRate calib", CMDFLAG_GET); } /* * Read parameters from flash and restore settings */ void Axis::restoreFlash(){ - //NormalizedAxis::restoreFlash(); + // TODO: This seems to be a remnant of a previous architecture (NormalizedAxis). Confirm and remove. // read all constants uint16_t value; - if (Flash_Read(flashAddrs.config, &value)){ + if (Flash_Read(flashAddresses.config, &value)){ this->conf = Axis::decodeConfFromInt(value); }else{ pulseErrLed(); @@ -178,57 +228,65 @@ void Axis::restoreFlash(){ setDrvType(this->conf.drvtype); setEncType(this->conf.enctype); - if (Flash_Read(flashAddrs.maxSpeed, &value)){ + if (Flash_Read(flashAddresses.maxSpeed, &value)){ this->maxSpeedDegS = value; }else{ pulseErrLed(); } -// -// if (Flash_Read(flashAddrs.maxAccel, &value)){ -// this->maxTorqueRateMS = value; -// }else{ -// pulseErrLed(); -// } - - - uint16_t esval, power; - if(Flash_Read(flashAddrs.endstop, &esval)) { - fx_ratio_i = esval & 0xff; - endstopStrength = (esval >> 8) & 0xff; + + // save the max torque for the slew rate + if (Flash_Read(flashAddresses.maxAccel, &value)){ + this->maxTorqueRateMS = value; + }else{ + pulseErrLed(); + } + + // save the max torque for the slew rate + if (Flash_Read(flashAddresses.maxSlewRateDrv, &value)){ + this->maxSlewRate_Driver = value; + }else{ + pulseErrLed(); + } + + + uint16_t endstopRawValue, power; + if(Flash_Read(flashAddresses.endstop, &endstopRawValue)) { + setEffectRatio(endstopRawValue & 0xff); + endstopStrength = (endstopRawValue >> 8) & 0xff; } - if(Flash_Read(flashAddrs.power, &power)){ + if(Flash_Read(flashAddresses.power, &power)){ setPower(power); } - uint16_t deg_t; - if(Flash_Read(flashAddrs.degrees, °_t)){ - this->degreesOfRotation = deg_t & 0x7fff; - this->invertAxis = (deg_t >> 15) & 0x1; + uint16_t degreesRawValue; + if(Flash_Read(flashAddresses.degrees, °reesRawValue)){ + this->degreesOfRotation = degreesRawValue & 0x7fff; + this->invertAxis = (degreesRawValue >> 15) & 0x1; setDegrees(degreesOfRotation); } uint16_t effects; - if(Flash_Read(flashAddrs.effects1, &effects)){ + if(Flash_Read(flashAddresses.effects1, &effects)){ setIdleSpringStrength(effects & 0xff); setFxStrengthAndFilter((effects >> 8) & 0xff,damperIntensity,damperFilter); }else{ - setIdleSpringStrength(idlespringstrength); // Use default + setIdleSpringStrength(idleSpringStrength); // Use default } - if(Flash_Read(flashAddrs.effects2, &effects)){ + if(Flash_Read(flashAddresses.effects2, &effects)){ setFxStrengthAndFilter(effects & 0xff,frictionIntensity,frictionFilter); setFxStrengthAndFilter((effects >> 8) & 0xff,inertiaIntensity,inertiaFilter); } uint16_t ratio; - if(Flash_Read(flashAddrs.encoderRatio, &ratio)){ + if(Flash_Read(flashAddresses.encoderRatio, &ratio)){ setGearRatio(ratio & 0xff, (ratio >> 8) & 0xff); } uint16_t filterStorage; - if (Flash_Read(flashAddrs.speedAccelFilter, &filterStorage)) + if (Flash_Read(flashAddresses.speedAccelFilter, &filterStorage)) { uint8_t profile = filterStorage & 0xFF; this->filterProfileId = profile; @@ -239,32 +297,70 @@ void Axis::restoreFlash(){ } uint16_t pp1; - if(Flash_Read(flashAddrs.postprocess1, &pp1)){ + if(Flash_Read(flashAddresses.postprocess1, &pp1)){ setExpo((int8_t)(pp1 & 0xff)); + equalizerEnabled = (pp1 >> 8) & 0x01; + } + + uint16_t eq_bank_1,eq_bank_2,eq_bank_3; + if(Flash_Read(flashAddresses.equalizer1, &eq_bank_1)){ + setEqGain(0, (int8_t)(eq_bank_1 & 0xff)); + setEqGain(1, (int8_t)((eq_bank_1 >> 8) & 0xff)); + } + if(Flash_Read(flashAddresses.equalizer2, &eq_bank_2)){ + setEqGain(2, (int8_t)(eq_bank_2 & 0xff)); + setEqGain(3, (int8_t)((eq_bank_2 >> 8) & 0xff)); + } + if(Flash_Read(flashAddresses.equalizer3, &eq_bank_3)){ + setEqGain(4, (int8_t)(eq_bank_3 & 0xff)); + setEqGain(5, (int8_t)((eq_bank_3 >> 8) & 0xff)); } + uint16_t handsOffConf; + if(Flash_Read(flashAddresses.handsOffConfig, &handsOffConf)){ + handsOffCheckEnabled = (handsOffConf >> 15) & 0x01; + handsOffSpeedThreshold = handsOffConf & 0x7FFF; + } + uint16_t handsOffAccelVal; + if(Flash_Read(flashAddresses.handsOffAccel, &handsOffAccelVal)){ + handsOffAccelThreshold = (float)handsOffAccelVal / 1000.0; + } } // Saves parameters to flash. void Axis::saveFlash(){ - //NormalizedAxis::saveFlash(); - Flash_Write(flashAddrs.config, Axis::encodeConfToInt(this->conf)); - Flash_Write(flashAddrs.maxSpeed, this->maxSpeedDegS); -// Flash_Write(flashAddrs.maxAccel, (uint16_t)(this->maxTorqueRateMS)); - - Flash_Write(flashAddrs.endstop, fx_ratio_i | (endstopStrength << 8)); - Flash_Write(flashAddrs.power, power); - Flash_Write(flashAddrs.degrees, (degreesOfRotation & 0x7fff) | (invertAxis << 15)); - Flash_Write(flashAddrs.effects1, idlespringstrength | (damperIntensity << 8)); - Flash_Write(flashAddrs.effects2, frictionIntensity | (inertiaIntensity << 8)); - Flash_Write(flashAddrs.encoderRatio, gearRatio.numerator | (gearRatio.denominator << 8)); + // TODO: This seems to be a remnant of a previous architecture (NormalizedAxis). Confirm and remove. + Flash_Write(flashAddresses.config, Axis::encodeConfToInt(this->conf)); + Flash_Write(flashAddresses.maxSpeed, this->maxSpeedDegS); + Flash_Write(flashAddresses.maxAccel, (uint16_t)(this->maxTorqueRateMS)); + Flash_Write(flashAddresses.maxSlewRateDrv, (uint16_t)(this->maxSlewRate_Driver)); + + Flash_Write(flashAddresses.endstop, effectRatio | (endstopStrength << 8)); + Flash_Write(flashAddresses.power, power); + Flash_Write(flashAddresses.degrees, (degreesOfRotation & 0x7fff) | (invertAxis << 15)); + Flash_Write(flashAddresses.effects1, idleSpringStrength | (damperIntensity << 8)); + Flash_Write(flashAddresses.effects2, frictionIntensity | (inertiaIntensity << 8)); + Flash_Write(flashAddresses.encoderRatio, gearRatio.numerator | (gearRatio.denominator << 8)); // save CF biquad uint16_t filterStorage = (uint16_t)this->filterProfileId & 0xFF; - Flash_Write(flashAddrs.speedAccelFilter, filterStorage); + Flash_Write(flashAddresses.speedAccelFilter, filterStorage); // Postprocessing - Flash_Write(flashAddrs.postprocess1, expoValInt & 0xff); - + uint16_t pp1 = (expoValue & 0xff) | (equalizerEnabled << 8); + Flash_Write(flashAddresses.postprocess1, pp1); + + // Equalizer gains + uint16_t eq1 = (eqGains[0] & 0xff) | ((eqGains[1] & 0xff) << 8); + uint16_t eq2 = (eqGains[2] & 0xff) | ((eqGains[3] & 0xff) << 8); + uint16_t eq3 = (eqGains[4] & 0xff) | ((eqGains[5] & 0xff) << 8); + Flash_Write(flashAddresses.equalizer1, eq1); + Flash_Write(flashAddresses.equalizer2, eq2); + Flash_Write(flashAddresses.equalizer3, eq3); + + // Hands-off detection + uint16_t handsOffConf = (handsOffSpeedThreshold & 0x7FFF) | (handsOffCheckEnabled << 15); + Flash_Write(flashAddresses.handsOffConfig, handsOffConf); + Flash_Write(flashAddresses.handsOffAccel, (uint16_t)(handsOffAccelThreshold * 1000.0)); } @@ -307,6 +403,7 @@ void Axis::prepareForUpdate(){ return; } + // TODO: The motorReady() check was commented out. Review if this is still the desired behavior or if it should be restored. //if (!drv->motorReady()) return; float angle = getEncAngle(getEncoder()); @@ -339,6 +436,7 @@ void Axis::prepareForUpdate(){ }else if(abs(scaledEnc) <= 0x7fff) { outOfBounds = false; + // TODO: This error clearing seems to have been moved to the errorCallback. Confirm this is correct and remove this line. //ErrorHandler::clearError(outOfBoundsError); } @@ -348,8 +446,31 @@ void Axis::prepareForUpdate(){ startForceFadeIn(0, 1.0); } + // Check for pending slew rate calibration result + if(this->awaitingSlewCalibration){ + // If driver reports calibration finished, retrieve measured value and persist + if(!drv->isSlewRateCalibrationInProgress()){ + // Get value from drv + this->maxSlewRate_Driver = drv->getDrvSlewRate(); + + // If the driver's max slew rate is lowest thant current max flew rate, cap the value and send the new value to the UI + if (this->maxSlewRate_Driver < this->maxTorqueRateMS) { + this->maxTorqueRateMS = this->maxSlewRate_Driver; + CommandHandler::broadcastCommandReply(CommandReply(this->maxTorqueRateMS), (uint32_t)Axis_commands::slewrate, CMDtype::get); + } + + // Broadcast a friendly completion message and the numeric value + CommandHandler::broadcastCommandReply(CommandReply("Slew rate calibration complete",0), (uint32_t)Axis_commands::calibrate_maxSlewRateDrv, CMDtype::get); + CommandHandler::broadcastCommandReply(CommandReply(this->maxSlewRate_Driver), (uint32_t)Axis_commands::maxSlewRateDrv, CMDtype::get); + + + this->awaitingSlewCalibration = false; + } + } + this->updateMetrics(angle); + this->updateHandsOffState(); } @@ -374,17 +495,11 @@ void Axis::updateDriveTorque(){ void Axis::setPower(uint16_t power) { this->power = power; - updateTorqueScaler(); -#ifdef TMC4671DRIVER - // Update hardware limits for TMC for safety - TMC4671 *drv = dynamic_cast(this->drv.get()); + torqueScaler = ((float)power / (float)0x7fff); if (drv != nullptr) { - //tmclimits.pid_uq_ud = power; - //tmclimits.pid_torque_flux = power; - drv->setTorqueLimit(power); + drv->setPowerLimit(power); } -#endif } @@ -393,29 +508,27 @@ void Axis::setPower(uint16_t power) */ void Axis::setDrvType(uint8_t drvtype) { - if (!drv_chooser.isValidClassId(drvtype)) + if (!driverChooser.isValidClassId(drvtype)) { return; } cpp_freertos::CriticalSection::Enter(); - this->drv.reset(drv_chooser.Create((uint16_t)drvtype)); + MotorDriver* drv = driverChooser.Create((uint16_t)drvtype); + this->drv.reset(drv); if (drv == nullptr) { cpp_freertos::CriticalSection::Exit(); return; } this->conf.drvtype = drvtype; + this->maxTorqueRateMS = drv->getDrvSlewRate(); // Pass encoder to driver again if(!this->drv->hasIntegratedEncoder()){ this->drv->setEncoder(this->enc); } -#ifdef TMC4671DRIVER - if (dynamic_cast(drv.get())) - { - setupTMC4671(); - } -#endif + + drv->setupDriver(); if (!tud_connected()) { control->usb_disabled = false; @@ -428,25 +541,6 @@ void Axis::setDrvType(uint8_t drvtype) cpp_freertos::CriticalSection::Exit(); } -#ifdef TMC4671DRIVER -// Special tmc setup methods -void Axis::setupTMC4671() -{ - TMC4671 *drv = static_cast(this->drv.get()); -// drv->setAxis(axis); - drv->setExternalEncoderAllowed(true); - drv->restoreFlash(); - tmclimits.pid_torque_flux = getPower(); - drv->setLimits(tmclimits); - //drv->setBiquadTorque(TMC4671Biquad(tmcbq_500hz_07q_25k)); - - - // Enable driver - - drv->setMotionMode(MotionMode::torque); - drv->Start(); // Start thread -} -#endif /** @@ -454,11 +548,11 @@ void Axis::setupTMC4671() */ void Axis::setEncType(uint8_t enctype) { - if (enc_chooser.isValidClassId(enctype) && !drv->hasIntegratedEncoder()) + if (encoderChooser.isValidClassId(enctype) && !drv->hasIntegratedEncoder()) { this->conf.enctype = (enctype); - this->enc = std::shared_ptr(enc_chooser.Create(enctype)); // Make new encoder + this->enc = std::shared_ptr(encoderChooser.Create(enctype)); // Make new encoder if(drv && !drv->hasIntegratedEncoder()) this->drv->setEncoder(this->enc); }else{ @@ -563,16 +657,16 @@ int32_t Axis::getLastScaledEnc() { * Changes intensity of idle spring when FFB is off */ int32_t Axis::updateIdleSpringForce() { - return clip((int32_t)(-metric.current.pos_scaled_16b*idlespringscale),-idlespringclip,idlespringclip); + return clip((int32_t)(-metric.current.pos_scaled_16b*idleSpringScale),-idleSpringClip,idleSpringClip); } /* * Set the strength of the spring effect if FFB is disabled */ void Axis::setIdleSpringStrength(uint8_t spring){ - idlespringstrength = spring; - idlespringclip = clip((int32_t)spring*35,0,10000); - idlespringscale = 0.5f + ((float)spring * 0.01f); + idleSpringStrength = spring; + idleSpringClip = clip((int32_t)spring*35,0,10000); + idleSpringScale = 0.5f + ((float)spring * 0.01f); } /** @@ -586,26 +680,27 @@ void Axis::setFxStrengthAndFilter(uint8_t val,uint8_t& valToSet, Biquad& filter) } /** - * Called before HID effects are calculated - * Should calculate always on and idle effects specific to the axis like idlespring and friction + * Calculates the internal mechanical effects (damper, friction, inertia) that are always active. + * Called before HID effects are calculated. + * Also calculates idle spring when FFB is inactive. */ -void Axis::calculateAxisEffects(bool ffb_on){ - axisEffectTorque = 0; +void Axis::calculateMechanicalEffects(bool ffb_on){ + mechanicalEffectTorque = 0; if(!ffb_on){ - axisEffectTorque += updateIdleSpringForce(); + mechanicalEffectTorque += updateIdleSpringForce(); } // Always active damper if(damperIntensity != 0){ float speedFiltered = (metric.current.speed) * (float)damperIntensity * AXIS_DAMPER_RATIO; - axisEffectTorque -= damperFilter.process(clip(speedFiltered, -intFxClip, intFxClip)); + mechanicalEffectTorque -= damperFilter.process(clip(speedFiltered, -internalFxClip, internalFxClip)); } // Always active inertia if(inertiaIntensity != 0){ float accelFiltered = metric.current.accel * (float)inertiaIntensity * AXIS_INERTIA_RATIO; - axisEffectTorque -= inertiaFilter.process(clip(accelFiltered, -intFxClip, intFxClip)); + mechanicalEffectTorque -= inertiaFilter.process(clip(accelFiltered, -internalFxClip, internalFxClip)); } // Always active friction. Based on effectsCalculator implementation @@ -614,12 +709,17 @@ void Axis::calculateAxisEffects(bool ffb_on){ float speedRampupCeil = 4096; float rampupFactor = 1.0; if (fabs (speed) < speedRampupCeil) { // if speed in the range to rampup we apply a sine curve - float phaseRad = M_PI * ((fabs (speed) / speedRampupCeil) - 0.5);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode - rampupFactor = ( 1 + sin(phaseRad ) ) / 2; // sin value is -1..1 range, we translate it to 0..2 and we scale it by 2 +#ifdef USE_DSP_FUNCTIONS + float phaseRad = PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode + rampupFactor = ( 1.0f + arm_sin_f32(phaseRad ) ) / 2.0f; // sin value is -1..1 range, we translate it to 0..2 and we scale it by 2 +#else + float phaseRad = M_PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode + rampupFactor = ( 1.0f + sinf(phaseRad ) ) / 2.0f; // sin value is -1..1 range, we translate it to 0..2 and we scale it by 2 +#endif } int8_t sign = speed >= 0 ? 1 : -1; float force = (float)frictionIntensity * rampupFactor * sign * INTERNAL_AXIS_FRICTION_SCALER * 32; - axisEffectTorque -= frictionFilter.process(clip(force, -intFxClip, intFxClip)); + mechanicalEffectTorque -= frictionFilter.process(clip(force, -internalFxClip, internalFxClip)); } } @@ -627,9 +727,9 @@ void Axis::calculateAxisEffects(bool ffb_on){ /** * Changes the ratio of effects to endstop strength. 255 = same strength, 0 = no effects */ -void Axis::setFxRatio(uint8_t val) { - fx_ratio_i = val; - updateTorqueScaler(); +void Axis::setEffectRatio(uint8_t val) { + effectRatio = val; + effectRatioScaler = ((float)effectRatio/255.0); } /** @@ -643,6 +743,10 @@ void Axis::resetMetrics(float new_pos= 0) { // pos is degrees // Reset filters speedFilter.calcBiquad(); accelFilter.calcBiquad(); + +#ifdef USE_DSP_FUNCTIONS + arm_pid_reset_f32(&speedLimiterPID); // reset the PID limit +#endif } /** @@ -659,9 +763,12 @@ void Axis::updateMetrics(float new_pos) { // pos is degrees // compute speed and accel from raw instant speed normalized float currentSpeed = (new_pos - metric.previous.posDegrees) * this->filter_f; // deg/s metric.current.speed = speedFilter.process(currentSpeed); - metric.current.accel = accelFilter.process((currentSpeed - _lastSpeed))* this->filter_f; // deg/s/s - _lastSpeed = currentSpeed; + metric.current.accel = accelFilter.process((currentSpeed - previousFrameSpeed))* this->filter_f; // deg/s/s + previousFrameSpeed = currentSpeed; + // Update hands-off detection buffer + accel_buffer[accel_buffer_idx] = metric.current.accel; + accel_buffer_idx = (accel_buffer_idx + 1) % (sizeof(accel_buffer) / sizeof(accel_buffer[0])); } @@ -671,28 +778,41 @@ uint16_t Axis::getPower(){ } /** - * Calculates an exponential torque correction curve + * Calculates an exponential torque correction curve and scale for FFBEffect */ -int32_t Axis::calculateExpoTorque(int32_t torque){ - float torquef = (float)torque / (float)0x7fff; // This down and upscaling may introduce float artifacts. Do this before scaling down. - if(torquef < 0){ - return -powf(-torquef,expo) * 0x7fff; - }else{ - return powf(torquef,expo) * 0x7fff; +int64_t Axis::calculateFFBTorque(){ + + // Apply equalizer + float filtered_torque = this->ffbEffectTorque; + + if(equalizerEnabled){ + for (uint8_t i = 0; i < num_eq_bands; ++i) { + filtered_torque = eqFilters[i].process(filtered_torque); + } } -} -void Axis::updateTorqueScaler() { - effect_margin_scaler = ((float)fx_ratio_i/255.0); - torqueScaler = ((float)power / (float)0x7fff); -} + // Game effects. Down and up-scaling may introduce float artifacts. Do this before scaling down. + float torque = (float)filtered_torque / (float)0x7fff; -float Axis::getTorqueScaler(){ - return torqueScaler; -} + // 1. Game Clipping detection + // If the game sends more than the theoretical maximum (+/- 32767), the signal is clipped at the source. + if(abs(torque) >= 0x7fff){ + pulseClipLed(); // Visual alert: game signal is clipping + } + + // 2. Apply Expo (Linearization or sensation curve) + if(expo != 1){ + torque = calculateExpoTorque(torque); + } + + // 3. Game specific gain (effectRatioScaler) + // Scale the FFB from game only (allows lowering game effects without lowering endstops) + torque = (int64_t)((float)torque * effectRatioScaler); + return torque; +} -int32_t Axis::getTorque() { return metric.previous.torque; } +int32_t Axis::getTorque() { return metric.current.torque; } // Fix: move from previous to current bool Axis::isInverted() { return invertAxis; @@ -701,20 +821,21 @@ bool Axis::isInverted() { /** * Calculate soft endstop effect */ -int16_t Axis::updateEndstop(){ - int8_t clipdir = cliptest(metric.current.pos_scaled_16b, -0x7fff, 0x7fff); - if(clipdir == 0){ +int32_t Axis::calculateEndstopTorque(){ + // TODO Check the type int8_t and the range clipping is -0x7fff..0x7fff + int8_t clipDirection = cliptest(metric.current.pos_scaled_16b, -0x7fff, 0x7fff); + if(clipDirection == 0){ return 0; } - float addtorque = clipdir*metric.current.posDegrees - (float)this->degreesOfRotation/2.0; // degress of rotation counts total range so multiply by 2 - addtorque *= (float)endstopStrength * endstopGain; // Apply endstop gain for stiffness. - addtorque *= -clipdir; + float endstopTorque = clipDirection*metric.current.posDegrees - (float)this->degreesOfRotation/2.0; // degress of rotation counts total range so multiply by 2 + endstopTorque *= (float)endstopStrength * endstopGain; // Apply endstop gain for stiffness. + endstopTorque *= -clipDirection; - return clip(addtorque,-0x7fff,0x7fff); + return clip(endstopTorque,-0x7fff,0x7fff); } -void Axis::setEffectTorque(int32_t torque) { - effectTorque = torque; +void Axis::setFfbEffectTorque(int64_t torque) { + this->ffbEffectTorque = torque; } /** pass in ptr to receive the sum of the effects + endstop torque @@ -723,73 +844,128 @@ void Axis::setEffectTorque(int32_t torque) { bool Axis::updateTorque(int32_t* totalTorque) { - if(abs(effectTorque) >= 0x7fff){ - pulseClipLed(); - } + // STEP 1: Process FFB torque from the game (via helper function) + // (Reconstructed by CMSIS Spline, Expo applied, and scaled by FFB ratio) + int64_t torque = calculateFFBTorque(); - // Scale effect torque - int32_t torque = effectTorque; // Game effects - if(expo != 1){ - torque = calculateExpoTorque(torque); + // STEP 2: Mix in local mechanical effects + // (Damper, Friction, Inertia generated locally at high frequency) + torque += mechanicalEffectTorque; + + // STEP 3: Add endstops + // Note: Historically endstops are added before the Master Scaler. + // If hard endstops (like Simucube) are desired, this should be moved to STEP 5. + torque += calculateEndstopTorque(); + + // STEP 4: Master Volume Scaling + // Map the virtual signal (+/- 32767) to the physical power limit ("power") + torque = (int64_t)((float)torque * torqueScaler); + + // STEP 5: Safety limits (Fade-in, Out of bounds) + // Applied BEFORE dynamic limiters so that abrupt cuts are smoothed by the Slew Rate. + if(outOfBounds){ + torque = 0; } - torque *= effect_margin_scaler; - torque += axisEffectTorque; // Independent effects - torque += updateEndstop(); - torque *= torqueScaler; // Scale to power + // Apply a fade-in effect for a smooth force ramp-up on startup or recovery. + // Increases forceFadeMultiplier progressively based on forceFadeDuration and sample rate. + if(forceFadeMultiplier < 1.0f){ + torque = (int64_t)((float)torque * forceFadeMultiplier); + forceFadeMultiplier += forceFadeDuration / this->filter_f; + } - // TODO speed and accel limiters - if(maxSpeedDegS > 0){ + // STEP 6: Dynamic limiters (Speed & Slew Rate) + // CRITICAL: Slew Rate compares the target value with "metric.previous.torque" + // (which is the clipped physical torque from the previous cycle). + // It MUST be applied on the final scaled torque! + torque -= applySpeedLimiterTorque(torque); + applyTorqueSlewRateLimiter(torque); - float torqueSign = torque > 0 ? 1 : -1; // Used to prevent metrics against the force to go into the limiter - // Speed. Mostly tuned... - //spdlimiterAvg.addValue(metric.current.speed); - float speedreducer = (float)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) * ((float)0x7FFF / maxSpeedDegS); - spdlimitreducerI = clip( spdlimitreducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power); + // STEP 7: Axis inversion and final hardware clipping + torque = (invertAxis) ? -torque : torque; - // Accel limit. Not really useful. Maybe replace with torque slew rate limit? -// float accreducer = (float)((metric.current.accel*torqueSign) - (float)maxAccelDegSS) * getAccelScalerNormalized(); -// acclimitreducerI = clip( acclimitreducerI + ((accreducer * 0.02) * torqueScaler),0,power); + int32_t torqueAfterClipping = clip((int32_t)torque, -power, power); + + if (torqueAfterClipping != torque){ + pulseClipLed(); // Visual alert: MOTOR cannot provide requested power (Hardware clipping) + } + // Store the actually applied torque for the next iteration (used by the slew rate limiter). + metric.current.torque = torqueAfterClipping; + + // return result + *totalTorque = torqueAfterClipping; + + return (metric.current.torque != metric.previous.torque); +} - // Only reduce torque. Don't invert it to prevent oscillation - float torqueReduction = speedreducer * speedLimiterP + spdlimitreducerI;// accreducer * 0.025 + acclimitreducerI - if(torque > 0){ - torqueReduction = clip(torqueReduction,0,torque); - }else{ - torqueReduction = clip(-torqueReduction,torque,0); - } - torque -= torqueReduction; - } - // Torque slew rate limiter - if(maxTorqueRateMS > 0){ - torque = clip(torque, metric.previous.torque - maxTorqueRateMS,metric.previous.torque + maxTorqueRateMS); - } -// if(torque - metric.previous.torque) - if(outOfBounds){ - torque = 0; +void Axis::applyTorqueSlewRateLimiter(int64_t& torque) +{ + // Limits the rate of change of the torque (slew rate), to smooths out sudden changes in torque. + // Essential for a natural feel and to prevent "clanking" noises. + if(maxTorqueRateMS == 0) { + return; // Limiter is disabled } - // Fade in - if(forceFadeCurMult < 1){ - torque = torque * forceFadeCurMult; - forceFadeCurMult += forceFadeTime / this->filter_f; // Fade time - } + // This prevents sudden torque jumps, resulting in a smoother feel. + const int64_t previousTorque = metric.previous.torque; + const int64_t maxTorqueChange = maxTorqueRateMS; - // Torque calculated. Now sending to driver - torque = (invertAxis) ? -torque : torque; - metric.current.torque = torque; - torque = clip(torque, -power, power); + // The torque is clipped to be within the range of [previous torque - limit, previous torque + limit]. + torque = clip(torque, previousTorque - maxTorqueChange, previousTorque + maxTorqueChange); +} - bool torqueChanged = metric.current.torque != metric.previous.torque; +int64_t Axis::applySpeedLimiterTorque(int64_t& torque){ + // Speed Limiter: A PI controller to reduce torque when speed exceeds maxSpeedDegS. + // The limiter only acts when torque is applied in the direction of movement. - if (abs(torque) == power){ - pulseClipLed(); + // if limiter is disabled, return + if(maxSpeedDegS <= 0) { + return 0; } - *totalTorque = torque; - return (torqueChanged); + int64_t resultTorque = 0; + +#ifdef USE_DSP_FUNCTIONS + float effectiveSpeed = metric.current.speed * (torque > 0 ? 1.0f : -1.0f); + if (effectiveSpeed > maxSpeedDegS) + { + // --- PI Controller Logic --- + // 1. Calculate the error term (how much we are over the speed limit). + float speedError = effectiveSpeed - maxSpeedDegS; + + // 2. Calculate the total reduction amount using the PID controller. + float reductionAmount = arm_pid_f32(&speedLimiterPID, speedError); + + // 3. Apply the reduction to the main torque. + // We must only reduce the magnitude of the torque, not invert it. + reductionAmount = clip(reductionAmount, 0.0f, fabsf((float)torque)); + if(torque > 0) { + resultTorque = reductionAmount; + } else { + resultTorque = -reductionAmount; + } + } else { + arm_pid_reset_f32(&speedLimiterPID); // Reset PID if not active + } +#else + float torqueSign = torque > 0 ? 1 : -1; // Used to prevent metrics against the force to go into the limiter + // Speed. Mostly tuned... + //spdlimiterAvg.addValue(metric.current.speed); + float speedreducer = (float)((metric.current.speed*torqueSign) - (float)maxSpeedDegS) * ((float)0x7FFF / maxSpeedDegS); + speedLimitReducerI = clip( speedLimitReducerI + ((speedreducer * speedLimiterI) * torqueScaler),0,power); + + // Only reduce torque. Don't invert it to prevent oscillation + float torqueReduction = speedreducer * speedLimiterP + speedLimitReducerI;// accreducer * 0.025 + acclimitreducerI + if(torque > 0){ + resultTorque = clip(torqueReduction,0,torque); + }else{ + resultTorque = clip(-torqueReduction,torque,0); + } +#endif + + return resultTorque; } void Axis::updateSamplerate(float newSamplerate){ @@ -812,8 +988,8 @@ void Axis::updateFilters(uint8_t profileId){ * Starts fading in force from start to 1 over fadeTime */ void Axis::startForceFadeIn(float start,float fadeTime){ - this->forceFadeTime = fadeTime; - this->forceFadeCurMult = clip(start, 0, 1); + this->forceFadeDuration = fadeTime; + this->forceFadeMultiplier = clip(start, 0, 1); } @@ -824,9 +1000,9 @@ void Axis::setDegrees(uint16_t degrees){ degrees &= 0x7fff; if(degrees == 0){ - nextDegreesOfRotation = lastdegreesOfRotation; + nextDegreesOfRotation = previousDegreesOfRotation; }else{ - lastdegreesOfRotation = degreesOfRotation; + previousDegreesOfRotation = degreesOfRotation; nextDegreesOfRotation = degrees; } } @@ -834,7 +1010,7 @@ void Axis::setDegrees(uint16_t degrees){ void Axis::setExpo(int val){ val = clip(val, -127, 127); - expoValInt = val; + expoValue = val; if(val == 0){ expo = 1; // Explicitly force expo off return; @@ -847,6 +1023,12 @@ void Axis::setExpo(int val){ } } +float Axis::calculateExpoTorque(float torque){ + float torqueSign = torque > 0 ? 1.0f : -1.0f; + torque = powf(fabsf(torque), expo) * torqueSign; + return torque; +} + CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& replies){ switch(static_cast(cmd.cmdId)){ @@ -863,15 +1045,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& break; case Axis_commands::degrees: - handleGetSetFunc(cmd, replies, degreesOfRotation, &Axis::setDegrees,this); -// if (cmd.type == CMDtype::get) -// { -// replies.emplace_back(degreesOfRotation); -// } -// else if (cmd.type == CMDtype::set) -// { -// setDegrees(cmd.val); -// } + handleGetSetFunc(cmd, replies, degreesOfRotation, &Axis::setDegrees,this); break; case Axis_commands::esgain: @@ -897,7 +1071,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& case Axis_commands::idlespring: if (cmd.type == CMDtype::get) { - replies.emplace_back(idlespringstrength); + replies.emplace_back(idleSpringStrength); } else if (cmd.type == CMDtype::set) { @@ -940,7 +1114,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& case Axis_commands::enctype: if(cmd.type == CMDtype::info){ - enc_chooser.replyAvailableClasses(replies,this->getEncType()); + encoderChooser.replyAvailableClasses(replies,this->getEncType()); }else if(cmd.type == CMDtype::get){ replies.emplace_back(this->getEncType()); }else if(cmd.type == CMDtype::set){ @@ -950,7 +1124,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& case Axis_commands::drvtype: if(cmd.type == CMDtype::info){ - drv_chooser.replyAvailableClasses(replies,this->getDrvType()); + driverChooser.replyAvailableClasses(replies,this->getDrvType()); }else if(cmd.type == CMDtype::get){ replies.emplace_back(this->getDrvType()); }else if(cmd.type == CMDtype::set){ @@ -978,15 +1152,46 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& handleGetSet(cmd, replies, this->maxSpeedDegS); break; - case Axis_commands::maxtorquerate: - handleGetSet(cmd, replies, this->maxTorqueRateMS); + case Axis_commands::slewrate: + { + if(cmd.type == CMDtype::get){ + // If driver has a more restrictive calibrated value, update the axis limit + if(maxSlewRate_Driver < this->maxTorqueRateMS) { + this->maxTorqueRateMS = maxSlewRate_Driver; + } + replies.emplace_back(this->maxTorqueRateMS); + }else if(cmd.type == CMDtype::set){ + this->maxTorqueRateMS = clip(cmd.val, 0, maxSlewRate_Driver); + } + } + break; + + case Axis_commands::calibrate_maxSlewRateDrv: + { + if(cmd.type == CMDtype::get){ + // Start calibration on driver and set awaiting flag if start is OK + if (drv->startSlewRateCalibration()) { + this->awaitingSlewCalibration = true; + } else { + // Inform user that calibration can't started + CommandHandler::broadcastCommandReply(CommandReply("Slew rate calibration unsupported",1), (uint32_t)Axis_commands::calibrate_maxSlewRateDrv, CMDtype::get); + } + replies.emplace_back(1); // ack + } + break; + } + + case Axis_commands::maxSlewRateDrv: + if (cmd.type == CMDtype::get) { + replies.emplace_back(maxSlewRate_Driver); + } break; case Axis_commands::fxratio: if(cmd.type == CMDtype::get){ - replies.emplace_back(this->fx_ratio_i); + replies.emplace_back(this->effectRatio); }else if(cmd.type == CMDtype::set){ - setFxRatio(cmd.val); + setEffectRatio(cmd.val); } break; @@ -1043,6 +1248,7 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& if(this->getEncoder() != nullptr){ cpr = this->getEncoder()->getCpr(); } + // TODO: For TMC4671 drivers, CPR reporting might be inconsistent. Investigate if a prescale is needed or if the UI should handle the readout correction. //#ifdef TMC4671DRIVER // CPR should be consistent with position. Maybe change TMC to prescale to encoder count or correct readout in UI // TMC4671 *tmcdrv = dynamic_cast(this->drv.get()); // Special case for TMC. Get the actual encoder resolution // if (tmcdrv && tmcdrv->hasIntegratedEncoder()) @@ -1057,21 +1263,126 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector& break; case Axis_commands::expo: - handleGetSetFunc(cmd, replies, expoValInt, &Axis::setExpo, this); // need to also provide the expoScaler constant + handleGetSetFunc(cmd, replies, expoValue, &Axis::setExpo, this); // need to also provide the expoScaler constant break; case Axis_commands::exposcale: handleGetSet(cmd, replies, expoScaler); // need to also provide the expoScaler constant break; + case Axis_commands::equalizer: + handleGetSet(cmd, replies, this->equalizerEnabled); + break; + + case Axis_commands::eqb1: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[0]); } + else if(cmd.type == CMDtype::set){ setEqGain(0, cmd.val); } + break; + case Axis_commands::eqb2: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[1]); } + else if(cmd.type == CMDtype::set){ setEqGain(1, cmd.val); } + break; + case Axis_commands::eqb3: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[2]); } + else if(cmd.type == CMDtype::set){ setEqGain(2, cmd.val); } + break; + case Axis_commands::eqb4: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[3]); } + else if(cmd.type == CMDtype::set){ setEqGain(3, cmd.val); } + break; + case Axis_commands::eqb5: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[4]); } + else if(cmd.type == CMDtype::set){ setEqGain(4, cmd.val); } + break; + case Axis_commands::eqb6: + if(cmd.type == CMDtype::get){ replies.emplace_back(eqGains[5]); } + else if(cmd.type == CMDtype::set){ setEqGain(5, cmd.val); } + break; + + case Axis_commands::handsoff: + handleGetSet(cmd, replies, this->handsOffCheckEnabled); + break; + + case Axis_commands::handsoff_speed: + handleGetSet(cmd, replies, this->handsOffSpeedThreshold); + break; + + case Axis_commands::handsoff_accel: + if (cmd.type == CMDtype::get) { + replies.emplace_back(this->handsOffAccelThreshold * 1000); + } else if (cmd.type == CMDtype::set) { + this->handsOffAccelThreshold = (float)cmd.val / 1000.0; + } + break; + default: return CommandStatus::NOT_FOUND; } return CommandStatus::OK; } +void Axis::updateHandsOffState() { + if (!handsOffCheckEnabled) { + handsOff = false; + return; + } + + // Downsample the check to 100Hz + static uint8_t check_counter = 0; + if(check_counter++ < 10){ + return; + } + check_counter = 0; + + + // Safety override: if speed is very high, cut torque + if (abs(metric.current.speed) > handsOffSpeedThreshold) { + handsOff = true; + return; + } + + // Main check: only run if FFB torque from game is active + const int32_t MIN_TORQUE_THRESHOLD = 1000; + if (abs(ffbEffectTorque) > MIN_TORQUE_THRESHOLD) { + // Calculate standard deviation of acceleration buffer + float std_dev = 0.0; + uint16_t buf_size = sizeof(accel_buffer) / sizeof(accel_buffer[0]); +#ifdef USE_DSP_FUNCTIONS + arm_std_f32(accel_buffer, buf_size, &std_dev); +#else + // Simple std dev calculation + float mean = 0.0; + for (uint16_t i = 0; i < buf_size; i++) { + mean += accel_buffer[i]; + } + mean /= (float)buf_size; + + float variance = 0.0; + for (uint16_t i = 0; i < buf_size; i++) { + variance += (accel_buffer[i] - mean) * (accel_buffer[i] - mean); + } + variance /= (float)buf_size; + std_dev = sqrtf(variance); +#endif + if (std_dev < handsOffAccelThreshold) { + if (handsOffTimer < 50) { // 50 * 10ms = 500ms timeout + handsOffTimer++; + } else { + handsOff = true; + } + } else { + handsOffTimer = 0; + handsOff = false; + } + } else { + // Torque is low, assume hands are on + handsOffTimer = 0; + handsOff = false; + } +} + /* * Helper functions for encoding and decoding flash variables */ @@ -1090,3 +1401,10 @@ uint16_t Axis::encodeConfToInt(AxisConfig conf) val |= ((uint8_t)conf.drvtype & 0x3f) << 6; return val; } + +void Axis::setEqGain(uint8_t band, int8_t gain) { + if (band >= num_eq_bands) return; + gain = clip(gain, -120, 120); + eqGains[band] = gain; + eqFilters[band].setPeakGain(gain / 10.0); +} diff --git a/Firmware/FFBoard/Src/EffectsCalculator.cpp b/Firmware/FFBoard/Src/EffectsCalculator.cpp index 5684fd975..755cdcf3a 100644 --- a/Firmware/FFBoard/Src/EffectsCalculator.cpp +++ b/Firmware/FFBoard/Src/EffectsCalculator.cpp @@ -11,6 +11,9 @@ #include "Axis.h" #include "ledEffects.h" +#ifdef USE_DSP_FUNCTIONS +#include "arm_math.h" +#endif #define EFFECT_STATE_INACTIVE 0 @@ -49,6 +52,7 @@ EffectsCalculator::EffectsCalculator() : CommandHandler("fx", CLSID_EFFECTSCALC) registerCommand("filterProfile_id", EffectsCalculator_commands::filterProfileId, "Conditional effects filter profile: 0 default; 1 custom", CMDFLAG_GET | CMDFLAG_SET); registerCommand("frictionPctSpeedToRampup", EffectsCalculator_commands::frictionPctSpeedToRampup, "% of max speed for gradual increase", CMDFLAG_GET | CMDFLAG_SET); + registerCommand("reconFilterMode", EffectsCalculator_commands::reconFilterMode, "Recon. filter: 0=None, 1=Linear, 2=CubicNatural, 3=CubicHermite", CMDFLAG_GET | CMDFLAG_SET); //this->Start(); // Enable if we want to periodically monitor } @@ -103,67 +107,59 @@ An inertia condition uses axis acceleration as the metric. */ void EffectsCalculator::calculateEffects(std::vector> &axes) { - for (auto &axis : axes) { - axis->setEffectTorque(0); - axis->calculateAxisEffects(isActive()); - } - - if(!isActive()){ - return; - } - - int32_t force = 0; int axisCount = axes.size(); - int32_t forces[MAX_AXIS] = {0}; + int64_t forces[MAX_AXIS] = {0}; - for (uint8_t i = 0; i < effects_stats.size(); i++) - { - effects_stats[i].current = {0}; // Reset active effect forces - } + if(isActive()){ + int32_t force = 0; + for (uint8_t i = 0; i < effects_stats.size(); i++) + { + effects_stats[i].current = {0}; // Reset active effect forces + } - for (uint8_t fxi = 0; fxi < MAX_EFFECTS; fxi++) - { - FFB_Effect *effect = &effects[fxi]; + for (uint8_t fxi = 0; fxi < MAX_EFFECTS; fxi++) + { + FFB_Effect *effect = &effects[fxi]; - // Effect activated and not infinite (0 or 0xffff) - if (effect->state != EFFECT_STATE_INACTIVE && effect->duration != FFB_EFFECT_DURATION_INFINITE && effect->duration != 0){ - // Start delay not yet reached - if(HAL_GetTick() < effect->startTime){ - continue; + // Effect activated and not infinite (0 or 0xffff) + if (effect->state != EFFECT_STATE_INACTIVE && effect->duration != FFB_EFFECT_DURATION_INFINITE && effect->duration != 0){ + // Start delay not yet reached + if(HAL_GetTick() < effect->startTime){ + continue; + } + // If effect has expired make inactive + if (HAL_GetTick() - effect->startTime > effect->duration) + { + effect->state = EFFECT_STATE_INACTIVE; + for(uint8_t axis=0 ; axis < axisCount ; axis++) + calcStatsEffectType(effect->type, 0,axis); // record a 0 on the ended force + } } - // If effect has expired make inactive - if (HAL_GetTick() - effect->startTime > effect->duration) + + // Filter out inactive effects + if (effect->state == EFFECT_STATE_INACTIVE) { - effect->state = EFFECT_STATE_INACTIVE; - for(uint8_t axis=0 ; axis < axisCount ; axis++) - calcStatsEffectType(effect->type, 0,axis); // record a 0 on the ended force + continue; } - } - // Filter out inactive effects - if (effect->state == EFFECT_STATE_INACTIVE) - { - continue; - } + force = calcNonConditionEffectForce(effect); // Compute the effect force - force = calcNonConditionEffectForce(effect); - - for(uint8_t axis=0 ; axis < axisCount ; axis++) // Calculate effects for all axes - { - int32_t axisforce = calcComponentForce(effect, force, axes, axis); - calcStatsEffectType(effect->type, axisforce,axis); - forces[axis] += axisforce; // Do not clip yet to allow effects to subtract force correctly. Will not overflow as maxeffects * 0x7fff is less than int32 range + for(uint8_t axis=0 ; axis < axisCount ; axis++) // Calculate effects for all axes + { + int32_t axisforce = calculateEffectForceOnAxis(effect, force, axes, axis); + calcStatsEffectType(effect->type, axisforce,axis); + forces[axis] += axisforce; // Do not clip yet to allow effects to subtract force correctly. Will not overflow as maxeffects * 0x7fff is less than int32 range + } } + effects_statslast = effects_stats; } // Apply summed force to axes for(uint8_t i=0 ; i < axisCount ; i++) { - int32_t force = clip(forces[i], -0x7fff, 0x7fff); // Clip - axes[i]->setEffectTorque(force); + axes[i]->calculateMechanicalEffects(isActive()); + axes[i]->setFfbEffectTorque(forces[i]); } - - effects_statslast = effects_stats; } /** @@ -171,13 +167,40 @@ void EffectsCalculator::calculateEffects(std::vector> &axe * Periodic and constant effects */ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { - int32_t force_vector = 0; - int32_t magnitude = effect->magnitude; - // If using an envelope modulate the magnitude based on time - if(effect->useEnvelope){ - magnitude = getEnvelopeMagnitude(effect); + // Sanity check effect type must be non-conditional + // Avoid calculating reconstructed forces for conditional effects here + if (effect->type == FFB_EFFECT_SPRING || + effect->type == FFB_EFFECT_DAMPER || + effect->type == FFB_EFFECT_FRICTION || + effect->type == FFB_EFFECT_INERTIA) { + return 0; } + + int32_t force_vector = 0; + + // Get interpolated magnitude (or amplitude) + float interpolated_magnitude = evaluateReconstructionFilter( + &effect->recon_magnitude, // Use the new structure + (float)effect->magnitude // Fallback value (for NONE mode) + ); + + // Get interpolated offset + float interpolated_offset_float = evaluateReconstructionFilter( + &effect->recon_offset, // Use the new structure + (float)effect->offset // Fallback value (for NONE mode) + ); + int32_t offset_lrf = (int32_t)interpolated_offset_float; + + // Magnitude with envelope if used + int32_t magnitude; + if(effect->useEnvelope){ + magnitude = getEnvelopeMagnitude(effect, (int32_t)interpolated_magnitude); + } else { + magnitude = (int32_t)interpolated_magnitude; + } + + switch (effect->type){ case FFB_EFFECT_CONSTANT: @@ -198,14 +221,14 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { { uint32_t elapsed_time = HAL_GetTick() - effect->startTime; // Square is ms aligned int32_t force = ((elapsed_time + effect->phase) % ((uint32_t)effect->period + 2)) < (uint32_t)(effect->period + 2) / 2 ? -magnitude : magnitude; - force_vector = force + effect->offset; + force_vector = force + offset_lrf; break; } case FFB_EFFECT_TRIANGLE: { int32_t force = 0; - int32_t offset = effect->offset; + int32_t offset = offset_lrf; float elapsed_time = micros() - ((float)effect->startTime*1000.0); uint32_t phase = effect->phase; uint32_t period = effect->period; @@ -228,7 +251,7 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { case FFB_EFFECT_SAWTOOTHUP: { - float offset = effect->offset; + float offset = offset_lrf; float elapsed_time = micros() - ((float)effect->startTime*1000.0); uint32_t phase = effect->phase; uint32_t period = effect->period; @@ -246,7 +269,7 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { case FFB_EFFECT_SAWTOOTHDOWN: { - float offset = effect->offset; + float offset = offset_lrf; float elapsed_time = micros() - ((float)effect->startTime*1000.0); float phase = effect->phase; uint32_t period = effect->period; @@ -267,8 +290,12 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { float t = (micros()/1000.0) - (float)effect->startTime; float freq = 1.0f / (float)(std::max(effect->period, 2)); float phase = (float)effect->phase / (float)35999; //degrees - float sine = sinf(2.0 * M_PI * (t * freq + phase)) * magnitude; - force_vector = (int32_t)(effect->offset + sine); +#ifdef USE_DSP_FUNCTIONS + float sine = arm_sin_f32(2.0f * PI * (t * freq + phase)) * magnitude; +#else + float sine = sinf(2.0f * M_PI * (t * freq + phase)) * magnitude; +#endif + force_vector = (int32_t)(offset_lrf + sine); break; } default: @@ -282,10 +309,15 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { /** - * Calculates the force of an effect + * @brief Calculates the final force of a single effect on a specific axis. + * It applies directional scaling and computes conditional effects (spring, damper, etc.) based on the axis's metrics. + * @param effect The effect to calculate. + * @param forceVector The base force from a non-conditional calculation (e.g., sine wave value). + * @param axes The list of all axes. + * @param axis The index of the axis to calculate the force for. + * @return The calculated torque for the axis. */ - -int32_t EffectsCalculator::calcComponentForce(FFB_Effect *effect, int32_t forceVector, std::vector> &axes, uint8_t axis) +int32_t EffectsCalculator::calculateEffectForceOnAxis(FFB_Effect *effect, int32_t forceVector, std::vector> &axes, uint8_t axis) { int32_t result_torque = 0; // uint16_t direction; @@ -360,9 +392,13 @@ int32_t EffectsCalculator::calcComponentForce(FFB_Effect *effect, int32_t forceV float rampupFactor = 1.0; if (fabs (speed) < speedRampupCeil) { // if speed in the range to rampup we apply a sinus curbe to ramup - float phaseRad = M_PI * ((fabs (speed) / speedRampupCeil) - 0.5);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode - rampupFactor = ( 1 + sin(phaseRad ) ) / 2; // sin value is -1..1 range, we translate it to 0..2 and we scale it by 2 - +#ifdef USE_DSP_FUNCTIONS + float phaseRad = PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode + rampupFactor = ( 1.0f + arm_sin_f32(phaseRad ) ) / 2.0f; // sin value is -1..1 range, we translate it to 0..2 and we scale it by 2 +#else + float phaseRad = M_PI * ((fabsf (speed) / speedRampupCeil) - 0.5f);// we start to compute the normalized angle (speed / normalizedSpeed@5%) and translate it of -1/2PI to translate sin on 1/2 periode + rampupFactor = ( 1.0f + sinf(phaseRad ) ) / 2.0f; // sin value is -1..1 range, we translate it to 0..2 and we scale it by 2 +#endif } int8_t sign = speed >= 0 ? 1 : -1; @@ -447,12 +483,12 @@ int32_t EffectsCalculator::calcConditionEffectForce(FFB_Effect *effect, float m * until the fade time where the strength changes to the fade level until the stop time of the effect. * Infinite effects can't have an envelope and return the normal magnitude. */ -int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect) +int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect, int32_t baseMagnitude) { if(effect->duration == FFB_EFFECT_DURATION_INFINITE || effect->duration == 0){ return effect->magnitude; // Effect is infinite. envelope is invalid } - int32_t scaler = abs(effect->magnitude); + int32_t scaler = abs(baseMagnitude); uint32_t elapsed_time = HAL_GetTick() - effect->startTime; if (elapsed_time < effect->attackTime && effect->attackTime != 0) { @@ -466,7 +502,7 @@ int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect) scaler /= (int32_t)effect->fadeTime; scaler += effect->fadeLevel; } - scaler = signbit(effect->magnitude) ? -scaler : scaler; // Follow original sign of magnitude because envelope has no sign (important for constant force) + scaler = signbit(baseMagnitude) ? -scaler : scaler; // Follow original sign of magnitude because envelope has no sign (important for constant force) return scaler; } @@ -516,6 +552,199 @@ void EffectsCalculator::setFilters(FFB_Effect *effect){ } } +// ============================================================================== +// --- Reconstruction Filter --- +// ============================================================================== + +void EffectsCalculator::updateEffectReconstruction(FFB_Effect* effect, float new_magnitude, float new_offset, bool is_periodic) +{ + // Update target variables (for NONE mode and fallback) + effect->magnitude = (int16_t)new_magnitude; + + // Push new magnitude into recon structure + pushReconstructionSample(&effect->recon_magnitude, new_magnitude); + + // Push new offset into recon structure if periodic + if (is_periodic) { + effect->offset = (int16_t)new_offset; + pushReconstructionSample(&effect->recon_offset, new_offset); + } +} + +void EffectsCalculator::pushReconstructionSample(ReconFilterState* state, float newValue) +{ + uint32_t now_us = micros(); + + // Shift the spline points + for (int i = 0; i < 3; ++i) { + state->spline_x[i] = state->spline_x[i + 1]; + state->spline_y[i] = state->spline_y[i + 1]; + } + + // Add the new point + state->spline_x[3] = (float)now_us; + state->spline_y[3] = newValue; + + // FIll in initial points if not ready yet at 60Hz, so 16.66ms intervals + if (!state->isSplineReady) { + float fake_time_step = 16666.0f; // 60Hz + state->spline_x[0] = state->spline_x[3] - 3.0f * fake_time_step; + state->spline_y[0] = state->spline_y[3]; + state->spline_x[1] = state->spline_x[3] - 2.0f * fake_time_step; + state->spline_y[1] = state->spline_y[3]; + state->spline_x[2] = state->spline_x[3] - 1.0f * fake_time_step; + state->spline_y[2] = state->spline_y[3]; + state->isSplineReady = true; // Ready after first fill + } + + // Initialize spline if needed + if (reconFilterMode == ReconFilterMode::SPLINE_CUBIC_NATURAL) { +#ifdef USE_DSP_FUNCTIONS + arm_spline_init_f32( + &state->spline_instance, + ARM_SPLINE_NATURAL, + state->spline_x, + state->spline_y, + 4, + state->spline_y2, + state->spline_scratch + ); + + state->spline_arm_initialized = true; + +#endif + } +} + +float EffectsCalculator::evaluateReconstructionFilter(ReconFilterState* state, float fallbackValue) +{ + // 'reconFilterMode' is a member variable of the class + + float resulting_value = fallbackValue; // Default value, "NONE" + uint32_t now_us = micros(); + float last_known_time = state->spline_x[3]; // The most recent point + + // 50ms timeout (if the game is paused, etc.) + if (!state->isSplineReady || (now_us > last_known_time + 50000)) { + return state->spline_y[3]; // Maintain the last value + } + + switch(reconFilterMode) { + case ReconFilterMode::NO_RECONSTRUCTION: + resulting_value = state->spline_y[3]; // The most recent (no filter) + break; + + case ReconFilterMode::LINEAR_INTERPOLATION: + { + // "Safe" Linear Interpolation (Delayed by 1 sample) + // To absolutely guarantee NO OVERSHOOT, we must not extrapolate. + // We accept a strictly defined transport delay of 1 sample. + // + // Logic: We are currently at time 'now_us', which is AFTER the latest update 't1'. + // We don't know the future, so we replay the ramp from V0 to V1, but we start it NOW. + // We use the previous interval duration (t1 - t0) as our best guess for when the + // NEXT packet (t2) will arrive, ensuring we reach V1 exactly when t2 is expected. + + float t0 = state->spline_x[2]; // Time of penultimate sample (T-1) + float t1 = state->spline_x[3]; // Time of latest sample (T) + float v0 = state->spline_y[2]; // Value of penultimate sample (V0) + float v1 = state->spline_y[3]; // Value of latest sample (V1) + + // Estimate the expected duration until the next packet arrives. + float expected_interval = t1 - t0; + + // Sanity check: avoid division by zero if updates are too fast (e.g. double-send bugs) + if (expected_interval < 1.0f) { + resulting_value = v1; + break; + } + + // Calculate progress based on time elapsed SINCE t1. + float time_since_t1 = (float)(now_us - t1); + float progress = time_since_t1 / expected_interval; + + // Clamp progress to [0.0, 1.0] to eliminate ANY overshoot. + progress = clip(progress, 0.0f, 1.0f); + + // Interpolate between the two KNOWN, SAFE past values. + resulting_value = v0 + progress * (v1 - v0); + break; + } + +#ifdef USE_DSP_FUNCTIONS + // if DSP is enabled we have SPLINE available, else use Hermite + case ReconFilterMode::SPLINE_CUBIC_NATURAL: + { + // If spline is not ready, return last known value + if (!state->spline_arm_initialized) { + resulting_value = state->spline_y[3]; + break; + } + + float32_t interpolated_torque_f = 0.0f; + float32_t now_f = (float)now_us; + + // Clamp time to known range to avoid extrapolation + float32_t interp_time = clip(now_f, state->spline_x[0], state->spline_x[3]); + + // Perform the spline interpolation using CMSIS-DSP + arm_spline_f32(&state->spline_instance, &interp_time, &interpolated_torque_f, 1); + + // Return the interpolated value + resulting_value = interpolated_torque_f; + break; + } +#endif + + case ReconFilterMode::SPLINE_CUBIC_HERMITE: +#ifndef USE_DSP_FUNCTIONS + // if the DSP is not available, we use Hermite for spline + case ReconFilterMode::SPLINE_CUBIC_NATURAL: +#endif + { + // Hermite interpolation is made between P1 (idx 1) and P2 (idx 2) + const float p1 = state->spline_y[1]; + const float p2 = state->spline_y[2]; + const float t1 = state->spline_x[1]; + const float t2 = state->spline_x[2]; + + // sanity check to not divide by zero and clamp for not overshoot + float interval = t2 - t1; + if (interval <= 0) { + resulting_value = p1; + break; + } + float t = ((float)now_us - t1) / interval; + t = clip(t, 0.0f, 1.0f); + + // Tangents (Catmull-Rom) + float m1, m2; + float dt_m1 = state->spline_x[2] - state->spline_x[0]; + float dt_m2 = state->spline_x[3] - state->spline_x[1]; + + // Compute tangents safely + if (dt_m1 > 0) m1 = (state->spline_y[2] - state->spline_y[0]) / dt_m1; else m1 = 0; + if (dt_m2 > 0) m2 = (state->spline_y[3] - state->spline_y[1]) / dt_m2; else m2 = 0; + + // Scale tangents by interval + m1 *= interval; + m2 *= interval; + + // Hermite basis functions + float tSq = t * t; + float tCub = tSq * t; + float h_00 = 2*tCub - 3*tSq + 1; + float h_10 = tCub - 2*tSq + t; + float h_01 = -2*tCub + 3*tSq; + float h_11 = tCub - tSq; + + // Final interpolation + resulting_value = h_00 * p1 + h_10 * m1 + h_01 * p2 + h_11 * m2; + break; + } + } + return resulting_value; +} void EffectsCalculator::setGain(uint8_t gain) { @@ -524,8 +753,6 @@ void EffectsCalculator::setGain(uint8_t gain) uint8_t EffectsCalculator::getGain() { return global_gain; } - - /* * Read parameters from flash and restore settings */ @@ -578,6 +805,11 @@ void EffectsCalculator::restoreFlash() frictionPctSpeedToRampup = (effects & 0xff); } + // Read reconstruction parameters + if(Flash_Read(ADR_FFB_RECONSTRUCTION_FILTER, &effects)){ + reconFilterMode = (ReconFilterMode)(effects & 0x03); + } + } // Saves parameters to flash @@ -618,6 +850,9 @@ void EffectsCalculator::saveFlash() effects = frictionPctSpeedToRampup | (filterProfileId << 8); Flash_Write(ADR_FFB_EFFECTS3, effects); + // Save reconstruction parameters + effects = (uint16_t)(reconFilterMode); + Flash_Write(ADR_FFB_RECONSTRUCTION_FILTER, effects); } void EffectsCalculator::checkFilterCoeff(biquad_constant_t *filter, uint32_t freq,uint8_t q) @@ -943,6 +1178,15 @@ CommandStatus EffectsCalculator::command(const ParsedCommand& cmd,std::vector(cmd.val, 0, 3); + reconFilterMode = (ReconFilterMode)mode; + } + break; + default: return CommandStatus::NOT_FOUND; } diff --git a/Firmware/FFBoard/Src/Filters.cpp b/Firmware/FFBoard/Src/Filters.cpp index a9c59330a..1eb2644d6 100644 --- a/Firmware/FFBoard/Src/Filters.cpp +++ b/Firmware/FFBoard/Src/Filters.cpp @@ -2,20 +2,33 @@ * Filters.cpp * * Created on: Feb 13, 2020 - * Author: Yannick + * Author: Yannick, Vincent * * Based on http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/ */ #include "Filters.h" - #include +#ifdef USE_DSP_FUNCTIONS +#include "arm_math.h" +#endif Biquad::Biquad(){ - z1 = z2 = 0.0; +#ifdef USE_DSP_FUNCTIONS + // Clear state + memset(pState, 0, sizeof(pState)); + // Initialize the CMSIS-DSP biquad instance + arm_biquad_cascade_df1_init_f32(&S, 1, pCoeffs, pState); +#else + z1 = 0.0f; + z2 = 0.0f; +#endif } Biquad::Biquad(BiquadType type, float Fc, float Q, float peakGainDB) { +#ifdef USE_DSP_FUNCTIONS + arm_biquad_cascade_df1_init_f32(&S, 1, pCoeffs, pState); +#endif setBiquad(type, Fc, Q, peakGainDB); } @@ -49,13 +62,23 @@ float Biquad::getQ() const { return this->Q; } +void Biquad::setPeakGain(float peakGainDB) { + this->peakGain = peakGainDB; + calcBiquad(); +} + /** * Calculates one step of the filter and returns the output */ float Biquad::process(float in) { - float out = in * a0 + z1; + float out; +#ifdef USE_DSP_FUNCTIONS + arm_biquad_cascade_df1_f32(&S, &in, &out, 1); +#else + out = in * a0 + z1; z1 = in * a1 + z2 - b1 * out; z2 = in * a2 - b2 * out; +#endif return out; } @@ -72,11 +95,18 @@ void Biquad::setBiquad(BiquadType type, float Fc, float Q, float peakGainDB) { * Updates parameters and resets the biquad filter */ void Biquad::calcBiquad(void) { + float norm; + float K, V; +#ifdef USE_DSP_FUNCTIONS + float a0 = 0.0f, a1 = 0.0f, a2 = 0.0f, b1 = 0.0f, b2 = 0.0f; + V = powf(10, fabsf(peakGain) / 20.0f); + K = arm_sin_f32(PI * Fc) / arm_cos_f32(PI * Fc); +#else z1 = 0.0; z2 = 0.0; - float norm; - float V = pow(10, fabs(peakGain) / 20.0); - float K = tan(M_PI * Fc); + V = pow(10, fabs(peakGain) / 20.0); + K = tan(M_PI * Fc); +#endif switch (this->type) { case BiquadType::lowpass: norm = 1 / (1 + K / Q + K * K); @@ -134,41 +164,84 @@ void Biquad::calcBiquad(void) { break; case BiquadType::lowshelf: if (peakGain >= 0) { // boost - norm = 1 / (1 + sqrt(2) * K + K * K); - a0 = (1 + sqrt(2*V) * K + V * K * K) * norm; + float sqrt2, sqrt2V; +#ifdef USE_DSP_FUNCTIONS + arm_sqrt_f32(2, &sqrt2); + arm_sqrt_f32(2*V, &sqrt2V); +#else + sqrt2 = sqrt(2); + sqrt2V = sqrt(2*V); +#endif + norm = 1 / (1 + sqrt2 * K + K * K); + a0 = (1 + sqrt2V * K + V * K * K) * norm; a1 = 2 * (V * K * K - 1) * norm; - a2 = (1 - sqrt(2*V) * K + V * K * K) * norm; + a2 = (1 - sqrt2V * K + V * K * K) * norm; b1 = 2 * (K * K - 1) * norm; - b2 = (1 - sqrt(2) * K + K * K) * norm; + b2 = (1 - sqrt2 * K + K * K) * norm; } else { // cut - norm = 1 / (1 + sqrt(2*V) * K + V * K * K); - a0 = (1 + sqrt(2) * K + K * K) * norm; + float sqrt2, sqrt2V; +#ifdef USE_DSP_FUNCTIONS + arm_sqrt_f32(2, &sqrt2); + arm_sqrt_f32(2*V, &sqrt2V); +#else + sqrt2 = sqrt(2); + sqrt2V = sqrt(2*V); +#endif + norm = 1 / (1 + sqrt2V * K + V * K * K); + a0 = (1 + sqrt2 * K + K * K) * norm; a1 = 2 * (K * K - 1) * norm; - a2 = (1 - sqrt(2) * K + K * K) * norm; + a2 = (1 - sqrt2 * K + K * K) * norm; b1 = 2 * (V * K * K - 1) * norm; - b2 = (1 - sqrt(2*V) * K + V * K * K) * norm; + b2 = (1 - sqrt2V * K + V * K * K) * norm; } break; case BiquadType::highshelf: if (peakGain >= 0) { // boost - norm = 1 / (1 + sqrt(2) * K + K * K); - a0 = (V + sqrt(2*V) * K + K * K) * norm; + float sqrt2, sqrt2V; +#ifdef USE_DSP_FUNCTIONS + arm_sqrt_f32(2, &sqrt2); + arm_sqrt_f32(2*V, &sqrt2V); +#else + sqrt2 = sqrt(2); + sqrt2V = sqrt(2*V); +#endif + norm = 1 / (1 + sqrt2 * K + K * K); + a0 = (V + sqrt2V * K + K * K) * norm; a1 = 2 * (K * K - V) * norm; - a2 = (V - sqrt(2*V) * K + K * K) * norm; + a2 = (V - sqrt2V * K + K * K) * norm; b1 = 2 * (K * K - 1) * norm; - b2 = (1 - sqrt(2) * K + K * K) * norm; + b2 = (1 - sqrt2 * K + K * K) * norm; } else { // cut - norm = 1 / (V + sqrt(2*V) * K + K * K); - a0 = (1 + sqrt(2) * K + K * K) * norm; + float sqrt2, sqrt2V; +#ifdef USE_DSP_FUNCTIONS + arm_sqrt_f32(2, &sqrt2); + arm_sqrt_f32(2*V, &sqrt2V); +#else + sqrt2 = sqrt(2); + sqrt2V = sqrt(2*V); +#endif + norm = 1 / (V + sqrt2V * K + K * K); + a0 = (1 + sqrt2 * K + K * K) * norm; a1 = 2 * (K * K - 1) * norm; - a2 = (1 - sqrt(2) * K + K * K) * norm; + a2 = (1 - sqrt2 * K + K * K) * norm; b1 = 2 * (K * K - V) * norm; - b2 = (V - sqrt(2*V) * K + K * K) * norm; + b2 = (V - sqrt2V * K + K * K) * norm; } break; } - return; +#ifdef USE_DSP_FUNCTIONS + // Store coefficients in the format required by CMSIS-DSP: {b0, b1, b2, -a1, -a2} + // Note the negated feedback coefficients a1 and a2. + pCoeffs[0] = a0; + pCoeffs[1] = a1; + pCoeffs[2] = a2; + pCoeffs[3] = -b1; + pCoeffs[4] = -b2; + + // Reset state + memset(pState, 0, sizeof(pState)); +#endif } diff --git a/Firmware/FFBoard/Src/HidCommandInterface.cpp b/Firmware/FFBoard/Src/HidCommandInterface.cpp index 418c3a032..9dbbbc6ca 100644 --- a/Firmware/FFBoard/Src/HidCommandInterface.cpp +++ b/Firmware/FFBoard/Src/HidCommandInterface.cpp @@ -193,7 +193,7 @@ void HID_CommandInterface::hidCmdCallback(HID_CMD_Data_t* data){ for(CommandHandler* handler : handlers){ ParsedCommand newCmd = cmd; newCmd.target = handler; - if(newCmd.target == nullptr || !(cmd.target->isValidCommandId(cmd.cmdId, CMDFLAG_STR_ONLY))){ + if(newCmd.target == nullptr || !(newCmd.target->isValidCommandId(cmd.cmdId, CMDFLAG_STR_ONLY))){ data->type = HidCmdType::notFound; //sendHidCmd(data); // Send back error this->outBuffer.push_back(*data); diff --git a/Firmware/FFBoard/Src/HidFFB.cpp b/Firmware/FFBoard/Src/HidFFB.cpp index 5b12a41f7..4d74c3bf8 100644 --- a/Firmware/FFBoard/Src/HidFFB.cpp +++ b/Firmware/FFBoard/Src/HidFFB.cpp @@ -12,6 +12,10 @@ #include "cppmain.h" #include +#ifdef USE_DSP_FUNCTIONS +#include "arm_math.h" +#endif + HidFFB::HidFFB(std::shared_ptr ec,uint8_t axisCount) : effects_calc(ec), effects(ec->effects),axisCount(axisCount) { directionEnableMask = 1 << axisCount; // Direction enable bit is last bit after axis enable bits @@ -235,7 +239,8 @@ void HidFFB::set_constant_effect(FFB_SetConstantForce_Data_t* data){ cfUpdateEvent(); FFB_Effect& effect_p = effects[data->effectBlockIndex-1]; - effect_p.magnitude = data->magnitude; + //effect_p.magnitude = data->magnitude; + effects_calc->updateEffectReconstruction(&effect_p, (float)data->magnitude, 0.0f, false); // if(effect_p.state == 0){ // effect_p.state = 1; // Force start effect // } @@ -315,10 +320,16 @@ void HidFFB::set_effect(FFB_SetEffect_t* effect){ if(!overridesCondition){ - float phaseX = M_PI*2.0 * (effect->directionX/36000.0f); +#ifdef USE_DSP_FUNCTIONS + float phaseX = PI*2.0f * (effect->directionX/36000.0f); + effect_p->axisMagnitudes[0] = directionEnable ? arm_sin_f32(phaseX) : (effect->enableAxis & X_AXIS_ENABLE ? (effect->directionX - 18000.0f) / 18000.0f : 0); // Angular vector if dirEnable used otherwise linear or 0 if axis enabled + effect_p->axisMagnitudes[1] = directionEnable ? -arm_cos_f32(phaseX) : (effect->enableAxis & Y_AXIS_ENABLE ? -(effect->directionY - 18000.0f) / 18000.0f : 0); +#else + float phaseX = M_PI*2.0f * (effect->directionX/36000.0); effect_p->axisMagnitudes[0] = directionEnable ? sin(phaseX) : (effect->enableAxis & X_AXIS_ENABLE ? (effect->directionX - 18000.0f) / 18000.0f : 0); // Angular vector if dirEnable used otherwise linear or 0 if axis enabled effect_p->axisMagnitudes[1] = directionEnable ? -cos(phaseX) : (effect->enableAxis & Y_AXIS_ENABLE ? -(effect->directionY - 18000.0f) / 18000.0f : 0); +#endif } #if MAX_AXIS == 3 @@ -453,9 +464,11 @@ void HidFFB::set_periodic(FFB_SetPeriodic_Data_t* report){ FFB_Effect* effect = &effects[report->effectBlockIndex-1]; effect->period = clip(report->period,1,0x7fff); // Period is never 0 - effect->magnitude = report->magnitude; - effect->offset = report->offset; effect->phase = report->phase; + + effects_calc->updateEffectReconstruction(effect, (float)report->magnitude, (float)report->offset, true); + //effect->magnitude = report->magnitude; + //effect->offset = report->offset; //effect->counter = 0; } diff --git a/Firmware/FFBoard/Src/MotorDriver.cpp b/Firmware/FFBoard/Src/MotorDriver.cpp index b7325de9d..cd27754cc 100644 --- a/Firmware/FFBoard/Src/MotorDriver.cpp +++ b/Firmware/FFBoard/Src/MotorDriver.cpp @@ -81,6 +81,12 @@ const ClassIdentifier MotorDriver::getInfo(){ return info; } +/** + * Setup driver when is selected + */ +void MotorDriver::setupDriver(){ + +} /** * Turn the motor with positive/negative power. diff --git a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h index fdde1fa2b..222da9e6b 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h +++ b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h @@ -2,7 +2,7 @@ * TMC4671.h * * Created on: Feb 1, 2020 - * Author: Yannick + * Author: Yannick, Vincent */ #ifndef TMC4671_H_ @@ -235,7 +235,7 @@ struct TMC4671PIDConf{ struct TMC4671Limits{ uint16_t pid_torque_flux_ddt = 32767; uint16_t pid_uq_ud = 30000; - uint16_t pid_torque_flux = 32767; + uint16_t pid_torque_flux = 30000; uint32_t pid_acc_lim = 2147483647; uint32_t pid_vel_lim = 2147483647; int32_t pid_pos_low = -2147483647; @@ -345,6 +345,17 @@ class TMC4671Biquad{ } TMC4671Biquad(const TMC4671Biquad_t bq) : params(bq){} TMC4671Biquad(const Biquad& bq,bool enable = true){ +#ifdef USE_DSP_FUNCTIONS + const float* coeffs = bq.getCoeffs(); + // coeffs is in order {b0, b1, b2, -a1, -a2} + // TMC expects {b0, b1, b2, a1, a2} with a1 and a2 negated + this->params.b0 = (int32_t)(coeffs[0] * (float)(1 << 29)); + this->params.b1 = (int32_t)(coeffs[1] * (float)(1 << 29)); + this->params.b2 = (int32_t)(coeffs[2] * (float)(1 << 29)); + this->params.a1 = (int32_t)(coeffs[3] * (float)(1 << 29)); + this->params.a2 = (int32_t)(coeffs[4] * (float)(1 << 29)); + this->params.enable = bq.getFc() > 0 ? enable : false; +#else // Note: trinamic swapped the naming of b and a from the regular convention in the datasheet and a and b are possibly inverse to b in our filter class this->params.a1 = -(int32_t)(bq.b1 * (float)(1 << 29)); this->params.a2 = -(int32_t)(bq.b2 * (float)(1 << 29)); @@ -352,6 +363,7 @@ class TMC4671Biquad{ this->params.b1 = (int32_t)(bq.a1 * (float)(1 << 29)); this->params.b2 = (int32_t)(bq.a2 * (float)(1 << 29)); this->params.enable = bq.getFc() > 0 ? enable : false; +#endif } void enable(bool enable){ params.enable = enable; @@ -412,6 +424,7 @@ friend class TMCDebugBridge; TMC4671MainConfig conf; + void setupDriver(); bool initialize(); void initializeWithPower(); @@ -485,15 +498,20 @@ friend class TMCDebugBridge; void stopMotor(); void startMotor(); + void setPowerLimit(uint16_t power) override; + void emergencyStop(bool reset); bool emergency = false; bool estopTriggered = false; void turn(int16_t power); + + int16_t getVelocityControllerTorque(); int16_t nextFlux = 0; int16_t idleFlux = 0; uint16_t maxOffsetFlux = 0; int16_t bangInitPower = 5000; // Default current in setup routines + uint16_t maxPowerAxis = 0; int16_t controlFluxDissipate(); const float fluxDissipationLimit = 1000; @@ -629,6 +647,7 @@ friend class TMCDebugBridge; static std::span tmc4671_hw_configs; // Can override in external target file private: + uint8_t drv_address = 0; OutputPin enablePin = OutputPin(*DRV_ENABLE_GPIO_Port,DRV_ENABLE_Pin); const Error indexNotHitError = Error(ErrorCode::encoderIndexMissed,ErrorType::critical,"Encoder index missed"); const Error lowVoltageError = Error(ErrorCode::undervoltage,ErrorType::warning,"Low motor voltage"); @@ -737,5 +756,3 @@ class TMC_2 : public TMC4671 { }; #endif #endif /* TMC4671_H_ */ - - diff --git a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h index d05638476..6c70a395f 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h +++ b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h @@ -13,11 +13,11 @@ #include "main.h" // Change this to the amount of currently registered variables -#define NB_OF_VAR 164 +#define NB_OF_VAR 183 extern const uint16_t VirtAddVarTab[NB_OF_VAR]; // Amount of variables in exportable list -#define NB_EXPORTABLE_ADR 149 +#define NB_EXPORTABLE_ADR 168 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR]; @@ -88,6 +88,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_FFB_EFFECTS1 0x284 // 0-7 inertia, 8-15 friction #define ADR_FFB_EFFECTS2 0x285 // 0-7 spring, 8-15 damper #define ADR_FFB_EFFECTS3 0x286 // 0-7 friction ramp up zone, 8-9 filterProfile +#define ADR_FFB_RECONSTRUCTION_FILTER 0x287 // 0-1 recon filter mode // Button Sources: #define ADR_ADS111X_CONF1 0x290 // How many axis configured 1-3 @@ -98,12 +99,18 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_AXIS1_DEGREES 0x303 #define ADR_AXIS1_MAX_SPEED 0x304 // Store the max speed #define ADR_AXIS1_MAX_ACCEL 0x305 // Store the max accel +#define ADR_AXIS1_MAX_SLEWRATE_DRV 0x306 // Max slew rate for drv #define ADR_AXIS1_ENDSTOP 0x307 // 0-7 endstop margin, 8-15 endstop stiffness #define ADR_AXIS1_EFFECTS1 0x308 // 0-7 idlespring, 8-15 damper #define ADR_AXIS1_SPEEDACCEL_FILTER 0x309 // Speed/Accel filter Lowpass profile #define ADR_AXIS1_ENC_RATIO 0x30A // Accel filter Lowpass #define ADR_AXIS1_EFFECTS2 0x30B // 0-7 Friction, 8-15 Inertia #define ADR_AXIS1_POSTPROCESS1 0x30C // 0-7 expo curve +#define ADR_AXIS1_EQ1 0x30D // Equalizer band 1,2 gains +#define ADR_AXIS1_EQ2 0x30E // Equalizer band 3,4 gains +#define ADR_AXIS1_EQ3 0x30F // Equalizer band 5 gain +#define ADR_AXIS1_HANDSOFF_CONF 0x310 // Hands-off config +#define ADR_AXIS1_HANDSOFF_ACCEL 0x311 // Hands-off accel threshold // TMC1 #define ADR_TMC1_MOTCONF 0x320 // 0-2: MotType 3-5: PhiE source 6-15: Poles #define ADR_TMC1_CPR 0x321 @@ -124,12 +131,18 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_AXIS2_DEGREES 0x343 #define ADR_AXIS2_MAX_SPEED 0x344 // Store the max speed #define ADR_AXIS2_MAX_ACCEL 0x345 // Store the max accel +#define ADR_AXIS2_MAX_SLEWRATE_DRV 0x346 // Max slew rate for drv #define ADR_AXIS2_ENDSTOP 0x347 // 0-7 endstop margin, 8-15 endstop stiffness #define ADR_AXIS2_EFFECTS1 0x348 // 0-7 idlespring, 8-15 damper #define ADR_AXIS2_SPEEDACCEL_FILTER 0x349 // Speed/Accel filter Lowpass profile #define ADR_AXIS2_ENC_RATIO 0x34A // Store the encoder ratio for an axis #define ADR_AXIS2_EFFECTS2 0x34B // 0-7 Friction, 8-15 Inertia #define ADR_AXIS2_POSTPROCESS1 0x34C // 0-7 expo curve +#define ADR_AXIS2_EQ1 0x34D // Equalizer band 1,2 gains +#define ADR_AXIS2_EQ2 0x34E // Equalizer band 3,4 gains +#define ADR_AXIS2_EQ3 0x34F // Equalizer band 5 gain +#define ADR_AXIS2_HANDSOFF_CONF 0x350 // Hands-off config +#define ADR_AXIS2_HANDSOFF_ACCEL 0x351 // Hands-off accel threshold // TMC2 #define ADR_TMC2_MOTCONF 0x360 // 0-2: MotType 3-5: PhiE source 6-15: Poles #define ADR_TMC2_CPR 0x361 @@ -150,12 +163,18 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if #define ADR_AXIS3_DEGREES 0x383 #define ADR_AXIS3_MAX_SPEED 0x384 // Store the max speed #define ADR_AXIS3_MAX_ACCEL 0x385 // Store the max accel +#define ADR_AXIS3_MAX_SLEWRATE_DRV 0x386 // Max slew rate for drv #define ADR_AXIS3_ENDSTOP 0x387 // 0-7 endstop margin, 8-15 endstop stiffness #define ADR_AXIS3_EFFECTS1 0x388 // 0-7 idlespring, 8-15 damper #define ADR_AXIS3_SPEEDACCEL_FILTER 0x389 // Speed/Accel filter Lowpass profile #define ADR_AXIS3_ENC_RATIO 0x38A // Store the encoder ratio for an axis #define ADR_AXIS3_EFFECTS2 0x38B // 0-7 Friction, 8-15 Inertia #define ADR_AXIS3_POSTPROCESS1 0x38C // 0-7 expo curve +#define ADR_AXIS3_EQ1 0x38D // Equalizer band 1,2 gains +#define ADR_AXIS3_EQ2 0x38E // Equalizer band 3,4 gains +#define ADR_AXIS3_EQ3 0x38F // Equalizer band 5 gain +#define ADR_AXIS3_HANDSOFF_CONF 0x390 // Hands-off config +#define ADR_AXIS3_HANDSOFF_ACCEL 0x391 // Hands-off accel threshold // TMC3 #define ADR_TMC3_MOTCONF 0x3A0 // 0-2: MotType 3-5: PhiE source 6-15: Poles #define ADR_TMC3_CPR 0x3A1 diff --git a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp index 13934f2e9..031b769e0 100644 --- a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp @@ -2,7 +2,7 @@ * FFBWheel.cpp * * Created on: 31.01.2020 - * Author: Yannick / Lidders + * Author: Yannick / Lidders / Vincent */ #include @@ -29,7 +29,7 @@ const FFBHIDMain::FFB_update_rates FFBHIDMain::ffbrates; // Default rates * setFFBEffectsCalc must be called in constructor of derived class to finish the setup */ FFBHIDMain::FFBHIDMain(uint8_t axisCount,bool hidAxis32b) : - Thread("FFBMAIN", 256, 30), + Thread("FFBMAIN", 312, 30), SelectableInputs(ButtonSource::all_buttonsources,AnalogSource::all_analogsources), axisCount(axisCount),hidAxis32b(hidAxis32b) { diff --git a/Firmware/FFBoard/UserExtensions/Src/FFBoardMain.cpp b/Firmware/FFBoard/UserExtensions/Src/FFBoardMain.cpp index 83c42dd0e..a3c9b559f 100644 --- a/Firmware/FFBoard/UserExtensions/Src/FFBoardMain.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/FFBoardMain.cpp @@ -2,7 +2,7 @@ * FFBoardMain.cpp * * Created on: 23.01.2020 - * Author: Yannick + * Author: Yannick, Vincent */ #include "FFBoardMain.h" diff --git a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp index 1d7be6823..55c824792 100644 --- a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp @@ -2,7 +2,7 @@ * TMC4671.cpp * * Created on: Feb 1, 2020 - * Author: Yannick + * Author: Yannick, Vincent */ #include "TMC4671.h" @@ -49,6 +49,7 @@ ClassIdentifier TMC4671::info = { TMC4671::TMC4671(SPIPort& spiport,OutputPin cspin,uint8_t address) : CommandHandler("tmc", CLSID_MOT_TMC0,address-1), SPIDevice{motor_spi,cspin},Thread("TMC", TMC_THREAD_MEM, TMC_THREAD_PRIO) { + this->drv_address = address; CommandHandler::setCommandsEnabled(false); setAddress(address); registerCommands(); @@ -239,6 +240,18 @@ int32_t TMC4671::getTmcVM(){ return ((float)agpiA_VM * conf.hwconf.vmScaler) * 1000; } +void TMC4671::setupDriver() { + // Configuration specific to TMC4671 upon starting the motor + // The power limit is set by the Axis class via setPowerLimit during initialization. + + this->setExternalEncoderAllowed(true); + this->restoreFlash(); + this->setLimits(curLimits); + + // Start driver + this->setMotionMode(MotionMode::torque); + this->Start(); // Start thread +} /** * Sets all parameters of the driver at startup. Only has to be called once when the driver is detected @@ -2046,8 +2059,11 @@ void TMC4671::setTorque(int16_t torque){ if(curMotionMode != MotionMode::torque){ setMotionMode(MotionMode::torque,true); } - updateReg(0x64,torque,0xffff,16); + + // Update main torque setpoint + updateReg(0x64, torque, 0xffff, 16); } + int16_t TMC4671::getTorque(){ return readReg(0x64) >> 16; } @@ -2068,10 +2084,16 @@ void TMC4671::setFluxTorque(int16_t flux, int16_t torque){ #ifdef TMC4671_TORQUE_USE_ASYNC writeRegAsync(0x64, (flux & 0xffff) | (torque << 16)); #else + // Update main flux and torque setpoints writeReg(0x64, (flux & 0xffff) | (torque << 16)); #endif } +int16_t TMC4671::getVelocityControllerTorque(){ + writeReg(0x6F, 4); // INTERIM_ADDR = 4 for PIDOUT_TARGET_TORQUE + return (int16_t)readReg(0x6E); // Read from INTERIM_DATA +} + void TMC4671::setFluxTorqueFF(int16_t flux, int16_t torque){ if(curMotionMode != MotionMode::torque){ setMotionMode(MotionMode::torque,true); @@ -2123,6 +2145,11 @@ void TMC4671::setUqUdLimit(uint16_t limit){ writeReg(0x5D, limit); } +void TMC4671::setPowerLimit(uint16_t power) { + maxPowerAxis = power; + setTorqueLimit(power); +} + void TMC4671::setTorqueLimit(uint16_t limit){ this->curLimits.pid_torque_flux = limit; bangInitPower = (float)limit*0.75; @@ -3105,7 +3132,6 @@ CommandStatus TMC4671::command(const ParsedCommand& cmd,std::vector