diff --git a/Configurator b/Configurator index a534fbf11..a6935c67c 160000 --- a/Configurator +++ b/Configurator @@ -1 +1 @@ -Subproject commit a534fbf1198d0a7bdc5fb120e631aa9df7ce7993 +Subproject commit a6935c67c954edcf89f0d88b4a7c166b54693a1f diff --git a/Firmware/FFBoard/Inc/AxesManager.h b/Firmware/FFBoard/Inc/AxesManager.h index b715a3dca..9378010d5 100644 --- a/Firmware/FFBoard/Inc/AxesManager.h +++ b/Firmware/FFBoard/Inc/AxesManager.h @@ -46,6 +46,7 @@ class AxesManager void emergencyStop(bool reset); void resetPosZero(); + void cfEffectsFreqToEffectsCalc(uint16_t freq); private: volatile Control_t* control; diff --git a/Firmware/FFBoard/Inc/EffectsCalculator.h b/Firmware/FFBoard/Inc/EffectsCalculator.h index 3e9cd5e52..22ae77219 100644 --- a/Firmware/FFBoard/Inc/EffectsCalculator.h +++ b/Firmware/FFBoard/Inc/EffectsCalculator.h @@ -49,6 +49,10 @@ struct effect_biquad_t { biquad_constant_t inertia = { 15, 20 }; }; +struct effect_interp_t { + uint16_t factor = 0; +}; + struct effect_stat_t { int16_t current=0; int16_t max=0; @@ -59,7 +63,7 @@ enum class EffectsCalculator_commands : uint32_t { ffbfiltercf,ffbfiltercf_q,effects,spring,friction,damper,inertia, damper_f, damper_q, friction_f, friction_q, inertia_f, inertia_q, filterProfileId, frictionPctSpeedToRampup, - monitorEffect, effectsDetails, effectsForces, + monitorEffect, effectsDetails, effectsForces,ffbinterpf }; class EffectsCalculator: public PersistentStorage, @@ -76,6 +80,8 @@ class EffectsCalculator: public PersistentStorage, void saveFlash(); void restoreFlash(); + void setCfEffectsFreq(uint16_t freq); + // virtual bool processHidCommand(HID_Custom_Data_t* data); bool isActive(); void setActive(bool active); @@ -103,9 +109,12 @@ class EffectsCalculator: public PersistentStorage, protected: private: + uint16_t cf_effects_freq; + 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 + effect_interp_t interp; uint8_t filterProfileId = 0; const uint32_t calcfrequency = 1000; // HID frequency 1khz const float qfloatScaler = 0.01; diff --git a/Firmware/FFBoard/Inc/ExponentialWeightedMovingAverage.h b/Firmware/FFBoard/Inc/ExponentialWeightedMovingAverage.h new file mode 100644 index 000000000..9f1666c4f --- /dev/null +++ b/Firmware/FFBoard/Inc/ExponentialWeightedMovingAverage.h @@ -0,0 +1,17 @@ +#ifndef EXPONENTIALWEIGHTEDMOVINGAVERAGE_H_ +#define EXPONENTIALWEIGHTEDMOVINGAVERAGE_H_ + +class ExponentialWeightedMovingAverage { +public: + ExponentialWeightedMovingAverage(float factor); + + void clear(); + + float process(float input); + +private: + float _factor = 0; + float _result = 0; +}; + +#endif /* EXPONENTIALWEIGHTEDMOVINGAVERAGE_H_ */ diff --git a/Firmware/FFBoard/Inc/Filters.h b/Firmware/FFBoard/Inc/Filters.h index d548397c7..bf45c66dc 100644 --- a/Firmware/FFBoard/Inc/Filters.h +++ b/Firmware/FFBoard/Inc/Filters.h @@ -49,6 +49,27 @@ class Biquad{ float z1, z2; }; +class InterpFFB { +public: + + InterpFFB(); + InterpFFB(int16_t interp_f); + ~InterpFFB(); + void setInterpFactor(int16_t interp_f); + int16_t getInterpFactor(); + int16_t getEffectiveInterpFactor(); + float interpFloat(int32_t input, uint32_t auto_interp_factor); + float lerp(float a, float b, float c); + +protected: + + int16_t interp_f = 0; //interp factor + float cd = 0.0; //current + int32_t input_backup = 0.0; //to track goal from a step behind + float dd = 0.0; //delta + int16_t effective_interp_f = 0; + int8_t lerpCount = 0; +}; #endif diff --git a/Firmware/FFBoard/Inc/HidFFB.h b/Firmware/FFBoard/Inc/HidFFB.h index a768d4867..374305b4c 100644 --- a/Firmware/FFBoard/Inc/HidFFB.h +++ b/Firmware/FFBoard/Inc/HidFFB.h @@ -14,6 +14,7 @@ #include "Filters.h" #include "EffectsCalculator.h" #include "FastAvg.h" +#include "ExponentialWeightedMovingAverage.h" class HidFFB: public UsbHidHandler { @@ -21,11 +22,13 @@ class HidFFB: public UsbHidHandler { HidFFB(); virtual ~HidFFB(); + ExponentialWeightedMovingAverage cfEwma = ExponentialWeightedMovingAverage(0.0005); //EWMA Filter for CF Freq + void hidOut(uint8_t report_id, hid_report_type_t report_type,const uint8_t* buffer, uint16_t bufsize) override; uint16_t hidGet(uint8_t report_id, hid_report_type_t report_type,uint8_t* buffer, uint16_t reqlen) override; uint32_t getRate(); // Returns an estimate of the hid effect update speed in hz - uint32_t getConstantForceRate(); // Returns an estimate of the constant force effect update rate in hz + uint16_t getConstantForceRate(); // Returns an estimate of the constant force effect update rate in hz bool getFfbActive(); static bool HID_SendReport(uint8_t *report,uint16_t len); @@ -65,10 +68,10 @@ class HidFFB: public UsbHidHandler { reportFFB_status_t reportFFBStatus; FastAvg hidPeriodAvg; - FastAvg cfUpdatePeriodAvg; uint32_t lastOut = 0; - uint32_t lastCfUpdate = 0; + uint16_t lastCfUpdate = 0; + uint16_t cfUpdateMicros = 0; }; #endif /* HIDFFB_H_ */ diff --git a/Firmware/FFBoard/Inc/ffb_defs.h b/Firmware/FFBoard/Inc/ffb_defs.h index 75119ca84..1b11bd156 100644 --- a/Firmware/FFBoard/Inc/ffb_defs.h +++ b/Firmware/FFBoard/Inc/ffb_defs.h @@ -289,6 +289,7 @@ typedef struct uint32_t attackTime = 0, fadeTime = 0; // Envelope effect std::unique_ptr filter[MAX_AXIS] = { nullptr }; // Optional filter + std::unique_ptr interp[MAX_AXIS] = { nullptr }; // Optional Interpolation uint16_t startDelay = 0; uint32_t startTime = 0; // Elapsed time in ms before effect starts uint16_t samplePeriod = 0; diff --git a/Firmware/FFBoard/Src/AxesManager.cpp b/Firmware/FFBoard/Src/AxesManager.cpp index e25472e95..342758183 100644 --- a/Firmware/FFBoard/Src/AxesManager.cpp +++ b/Firmware/FFBoard/Src/AxesManager.cpp @@ -73,6 +73,9 @@ void AxesManager::updateTorque() { } } +void AxesManager::cfEffectsFreqToEffectsCalc(uint16_t freq){ + effects_calc->setCfEffectsFreq(freq); +} std::vector* AxesManager::getAxisValues(){ this->axisValues.clear(); // Empty axes diff --git a/Firmware/FFBoard/Src/EffectsCalculator.cpp b/Firmware/FFBoard/Src/EffectsCalculator.cpp index 8e2b3ef06..399a20f5d 100644 --- a/Firmware/FFBoard/Src/EffectsCalculator.cpp +++ b/Firmware/FFBoard/Src/EffectsCalculator.cpp @@ -32,6 +32,7 @@ EffectsCalculator::EffectsCalculator() : CommandHandler("fx", CLSID_EFFECTSCALC) restoreFlash(); CommandHandler::registerCommands(); + registerCommand("interpFactor", EffectsCalculator_commands::ffbinterpf, "Constant force interpolation factor", CMDFLAG_GET | CMDFLAG_SET); registerCommand("filterCfFreq", EffectsCalculator_commands::ffbfiltercf, "Constant force filter frequency", CMDFLAG_GET | CMDFLAG_SET); registerCommand("filterCfQ", EffectsCalculator_commands::ffbfiltercf_q, "Constant force filter Q-factor", CMDFLAG_GET | CMDFLAG_SET); registerCommand("spring", EffectsCalculator_commands::spring, "Spring gain", CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING); @@ -304,7 +305,9 @@ int32_t EffectsCalculator::calcNonConditionEffectForce(FFB_Effect *effect) { return (force_vector * effect->gain) / 255; } - +void EffectsCalculator::setCfEffectsFreq(uint16_t freq){ + this->cf_effects_freq = freq; +} /* * If the number of Condition report blocks is equal to the number of axes for the effect, then the first report @@ -358,6 +361,19 @@ int32_t EffectsCalculator::calcComponentForce(FFB_Effect *effect, int32_t forceV forceVector = effect->filter[con_idx]->process(forceVector); } } + // Optional interpolation to smooth ffb + if(effect->interp[con_idx] != nullptr) { + // if the filter is enabled we apply it + uint8_t auto_interp_factor = floor(calcfrequency / this->cf_effects_freq); + if (effect->interp[con_idx]->getInterpFactor() == 1 && auto_interp_factor >= 2) //IF 1 or 0, Filter Disabled + { + forceVector = effect->interp[con_idx]->interpFloat(forceVector, auto_interp_factor); + } + else if(effect->interp[con_idx]->getInterpFactor() > 1) + { + forceVector = effect->interp[con_idx]->interpFloat(forceVector, 1); + } + } } case FFB_EFFECT_RAMP: case FFB_EFFECT_SQUARE: @@ -523,6 +539,7 @@ int32_t EffectsCalculator::getEnvelopeMagnitude(FFB_Effect *effect) void EffectsCalculator::setFilters(FFB_Effect *effect){ std::function &)> fnptr = [=](std::unique_ptr &filter){}; + std::function &)> fnptr2 = [=](std::unique_ptr &interp){}; switch (effect->type) { @@ -557,12 +574,19 @@ void EffectsCalculator::setFilters(FFB_Effect *effect){ else filter = std::make_unique(BiquadType::lowpass, this->filter[0].constant.freq / (float)calcfrequency, this->filter[0].constant.q * qfloatScaler, (float)0.0); }; + fnptr2 = [=](std::unique_ptr &interp){ + if (interp != nullptr) + interp->setInterpFactor(this->interp.factor); + else + interp = std::make_unique(this->interp.factor); + }; break; } for (int i=0; ifilter[i]); + fnptr2(effect->interp[i]); } } @@ -587,6 +611,12 @@ void EffectsCalculator::setEffectsArray(FFB_Effect *pEffects) void EffectsCalculator::restoreFlash() { uint16_t filterStorage; + + if(Flash_Read(ADR_FFB_INTERP_FILTER, &filterStorage)){ + this->interp.factor = filterStorage; + updateFilterSettingsForEffects(FFB_EFFECT_CONSTANT); + } + if (Flash_Read(ADR_FFB_CF_FILTER, &filterStorage)) { uint32_t freq = filterStorage & 0x1FF; @@ -640,6 +670,9 @@ void EffectsCalculator::saveFlash() { uint16_t filterStorage; + //save CF interpolation + Flash_Write(ADR_FFB_INTERP_FILTER, (uint16_t)interp.factor); + // save CF biquad filterStorage = (uint16_t)filter[0].constant.freq & 0x1FF; filterStorage |= ( (uint16_t)filter[0].constant.q & 0x7F ) << 9 ; @@ -797,6 +830,17 @@ void EffectsCalculator::resetLoggedActiveEffects(bool reinit){ CommandStatus EffectsCalculator::command(const ParsedCommand& cmd,std::vector& replies){ switch(static_cast(cmd.cmdId)){ + case EffectsCalculator_commands::ffbinterpf: + if (cmd.type == CMDtype::get) + { + replies.emplace_back(interp.factor); + } + else if (cmd.type == CMDtype::set) + { + interp.factor = cmd.val; + updateFilterSettingsForEffects(FFB_EFFECT_CONSTANT); + } + break; case EffectsCalculator_commands::ffbfiltercf: if (cmd.type == CMDtype::get) { diff --git a/Firmware/FFBoard/Src/ExponentialWeightedMovingAverage.cpp b/Firmware/FFBoard/Src/ExponentialWeightedMovingAverage.cpp new file mode 100644 index 000000000..5ae83f90f --- /dev/null +++ b/Firmware/FFBoard/Src/ExponentialWeightedMovingAverage.cpp @@ -0,0 +1,14 @@ +#include "ExponentialWeightedMovingAverage.h" + +ExponentialWeightedMovingAverage::ExponentialWeightedMovingAverage(float factor) { + this->_factor = factor; +} + +void ExponentialWeightedMovingAverage::clear() { + this->_result = 0; +} + +float ExponentialWeightedMovingAverage::process(float input) { + _result = (_factor * input) - (_factor * _result) + _result; + return _result; +} diff --git a/Firmware/FFBoard/Src/Filters.cpp b/Firmware/FFBoard/Src/Filters.cpp index f25d18d0d..994b73f9d 100644 --- a/Firmware/FFBoard/Src/Filters.cpp +++ b/Firmware/FFBoard/Src/Filters.cpp @@ -9,6 +9,54 @@ #include +InterpFFB::InterpFFB(){ + interp_f = 0.0; +} + +InterpFFB::InterpFFB(int16_t ifactor){ + setInterpFactor(ifactor); +} + +InterpFFB::~InterpFFB() { +} + +void InterpFFB::setInterpFactor(int16_t ifactor){ + this->interp_f = ifactor; +} + +int16_t InterpFFB::getInterpFactor(){ + return this->interp_f; +} + +int16_t InterpFFB::getEffectiveInterpFactor(){ + return this->effective_interp_f; +} +float InterpFFB::lerp(float a, float b, float c){ + return a + (b - a) * c; +} + +//Note: This function is called in a way that effective_interp_f can never be 1 or 0. +//It minimum can be 2. If it is 1 or 0, this filter is not activated. +float InterpFFB::interpFloat(int32_t input, uint32_t auto_interp_factor){ + if(auto_interp_factor != 1){ + effective_interp_f = auto_interp_factor; + } + else { + effective_interp_f = interp_f; + } + + if(input_backup != input) { + cd = input_backup; + input_backup = input; + lerpCount = 0; + } + if(lerpCount < effective_interp_f) { + lerpCount++; + return lerp((float)cd,(float)input, ((float)lerpCount / (float)effective_interp_f)); + } else { + return (float)input; + } +} Biquad::Biquad(){ z1 = z2 = 0.0; diff --git a/Firmware/FFBoard/Src/HidFFB.cpp b/Firmware/FFBoard/Src/HidFFB.cpp index 60878c013..12c182178 100644 --- a/Firmware/FFBoard/Src/HidFFB.cpp +++ b/Firmware/FFBoard/Src/HidFFB.cpp @@ -10,7 +10,7 @@ #include "flash_helpers.h" #include "hid_device.h" #include "cppmain.h" - +#include "math.h" HidFFB::HidFFB() { @@ -63,14 +63,14 @@ uint32_t HidFFB::getRate(){ /** * Calculates the frequency of the CF effect only */ -uint32_t HidFFB::getConstantForceRate(){ - float periodAvg = cfUpdatePeriodAvg.getAverage(); - if((HAL_GetTick() - lastCfUpdate) > 1000 || periodAvg == 0){ +uint16_t HidFFB::getConstantForceRate() { + float periodAvg = cfEwma.process(cfUpdateMicros); + if(periodAvg < 0.5){ // Reset average - cfUpdatePeriodAvg.clear(); + cfEwma.clear(); return 0; - }else{ - return (1000.0/periodAvg); + }else { + return (round(1000000.0 / periodAvg)); } } @@ -263,17 +263,19 @@ void HidFFB::ffb_control(uint8_t cmd){ void HidFFB::set_constant_effect(FFB_SetConstantForce_Data_t* data){ + cfUpdateMicros = ((uint16_t)(micros() - lastCfUpdate)); + if(data->effectBlockIndex == 0 || data->effectBlockIndex > MAX_EFFECTS){ return; } - cfUpdatePeriodAvg.addValue((uint32_t)(HAL_GetTick() - lastCfUpdate)); + lastCfUpdate = micros(); + FFB_Effect& effect_p = effects[data->effectBlockIndex-1]; effect_p.magnitude = data->magnitude; // if(effect_p.state == 0){ // effect_p.state = 1; // Force start effect // } - lastCfUpdate = HAL_GetTick(); } void HidFFB::new_effect(FFB_CreateNewEffect_Feature_Data_t* effect){ diff --git a/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h b/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h index 2677988a7..8367d1238 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h +++ b/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h @@ -72,6 +72,7 @@ class FFBHIDMain: public FFBoardMain, public cpp_freertos::Thread, PersistentSto void emergencyStop(bool reset); uint32_t getRate(); + uint16_t getConstantForceRate(); bool getFfbActive(); //void timerElapsed(TIM_HandleTypeDef* htim); @@ -128,6 +129,9 @@ class FFBHIDMain: public FFBoardMain, public cpp_freertos::Thread, PersistentSto cpp_freertos::BinarySemaphore sourcesSem = cpp_freertos::BinarySemaphore(true); volatile uint32_t lastEstop = 0; + + //To compare CF effect Freq and determine to be send again to effectsCalculator? + uint16_t backupCfFreq = 0; }; #endif /* SRC_FFBWHEEL_H_ */ diff --git a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h index 2288914b9..3becba9a0 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h +++ b/Firmware/FFBoard/UserExtensions/Inc/eeprom_addresses.h @@ -82,6 +82,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_INTERP_FILTER 0x287 // Constant Force Interpolation // Button Sources: #define ADR_ADS111X_CONF1 0x290 // How many axis configured 1-3 diff --git a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp index 7a7dad495..47e685063 100644 --- a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp @@ -170,6 +170,11 @@ void FFBHIDMain::updateControl(){ //debugpin.set(); axes_manager->update(); + // CF Effects Frequency to effects calculator + if(this->backupCfFreq != (backupCfFreq = this->getConstantForceRate())){ + axes_manager->cfEffectsFreqToEffectsCalc(backupCfFreq); + } + if(++report_rate_cnt >= usb_report_rate){ report_rate_cnt = 0; this->send_report(); @@ -253,6 +258,10 @@ uint32_t FFBHIDMain::getRate() { return this->ffb->getRate(); } +uint16_t FFBHIDMain::getConstantForceRate() { + return this->ffb->getConstantForceRate(); +} + bool FFBHIDMain::getFfbActive(){ return this->ffb->getFfbActive(); }