Added Lerp filter to Constant Force Effect

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;

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;

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_ */

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
 

Firmware/FFBoard/Inc/HidFFB.h

@@ -14,18 +14,21 @@
 #include "Filters.h"
 #include "EffectsCalculator.h"
 #include "FastAvg.h"
+#include "ExponentialWeightedMovingAverage.h"
 
 
 class HidFFB: public UsbHidHandler {
 public:
 	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<float,20> hidPeriodAvg;
-	FastAvg<float,20> cfUpdatePeriodAvg;
 
 	uint32_t lastOut = 0;
-	uint32_t lastCfUpdate = 0;
+	uint16_t lastCfUpdate = 0;
+	uint16_t cfUpdateMicros = 0;
 };
 
 #endif /* HIDFFB_H_ */

Firmware/FFBoard/Inc/ffb_defs.h

@@ -289,6 +289,7 @@ typedef struct
 	uint32_t attackTime = 0, fadeTime = 0;	 // Envelope effect
 
 	std::unique_ptr<Biquad> filter[MAX_AXIS] = { nullptr };  // Optional filter
+	std::unique_ptr<InterpFFB> 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;

Firmware/FFBoard/Src/AxesManager.cpp

@@ -73,6 +73,9 @@ void AxesManager::updateTorque() {
 	}
 }
 
+void AxesManager::cfEffectsFreqToEffectsCalc(uint16_t freq){
+	effects_calc->setCfEffectsFreq(freq);
+}
 
 std::vector<int32_t>* AxesManager::getAxisValues(){
 	this->axisValues.clear(); // Empty axes

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<void(std::unique_ptr<Biquad> &)> fnptr = [=](std::unique_ptr<Biquad> &filter){};
+	std::function<void(std::unique_ptr<InterpFFB> &)> fnptr2 = [=](std::unique_ptr<InterpFFB> &interp){};
 
 	switch (effect->type)
 	{
@@ -557,12 +574,19 @@ void EffectsCalculator::setFilters(FFB_Effect *effect){
 			else
 				filter = std::make_unique<Biquad>(BiquadType::lowpass, this->filter[0].constant.freq / (float)calcfrequency, this->filter[0].constant.q * qfloatScaler, (float)0.0);
 		};
+		fnptr2 = [=](std::unique_ptr<InterpFFB> &interp){
+			if (interp != nullptr)
+				interp->setInterpFactor(this->interp.factor);
+			else
+				interp = std::make_unique<InterpFFB>(this->interp.factor);
+		};
 		break;
 	}
 
 
 	for (int i=0; i<MAX_AXIS; i++) {
 		fnptr(effect->filter[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<CommandReply>& replies){
 	switch(static_cast<EffectsCalculator_commands>(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)
 		{

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;
+}

Firmware/FFBoard/Src/Filters.cpp

@@ -9,6 +9,54 @@
 
 #include <math.h>
 
+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;

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){