Basic expo curve implementation

Ultrawipf · Ultrawipf · commit ea15ea5f3879 · 2025-05-27T16:22:58.000+02:00
diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
@@ -59,6 +59,7 @@ struct AxisFlashAddrs
 	uint16_t encoderRatio = ADR_AXIS1_ENC_RATIO;
 
 	uint16_t speedAccelFilter = ADR_AXIS1_SPEEDACCEL_FILTER;
+	uint16_t postprocess1 = ADR_AXIS1_POSTPROCESS1;
 };
 
 struct AxisConfig
@@ -92,7 +93,8 @@ struct GearRatio_t{
 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,
-	filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia
+	filterSpeed, filterAccel, filterProfileId,cpr,axisfriction,axisinertia,
+	expo,exposcale
 };
 
 class Axis : public PersistentStorage, public CommandHandler, public ErrorHandler
@@ -159,13 +161,16 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	int32_t getTorque(); // current torque scaled as a 32 bit signed value
 	int16_t updateEndstop();
 
+	int32_t calculateExpoTorque(int32_t torque);
+
 	void startForceFadeIn(float start = 0,float fadeTime = 0.5);
 
 	metric_t* getMetrics();
 
 	void setEffectTorque(int32_t torque);
 	bool updateTorque(int32_t* totalTorque);
 
+
 	void setGearRatio(uint8_t numerator,uint8_t denominator);
 
 	static const std::vector<class_entry<MotorDriver>> axis1_drivers;
@@ -271,9 +276,15 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	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
+
 };
 
 #endif /* SRC_AXIS_H_ */
diff --git a/Firmware/FFBoard/Inc/constants.h b/Firmware/FFBoard/Inc/constants.h
@@ -8,7 +8,7 @@
  * For more settings see target_constants.h in a target specific folder
  */
 
-static const uint8_t SW_VERSION_INT[3] = {1,16,3}; // Version as array. 8 bit each!
+static const uint8_t SW_VERSION_INT[3] = {1,16,4}; // Version as array. 8 bit each!
 #ifndef MAX_AXIS
 #define MAX_AXIS 2 // ONLY USE 2 for now else screws HID Reports
 #endif
diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
@@ -95,22 +95,22 @@ Axis::Axis(char axis,volatile Control_t* control) :CommandHandler("axis", CLSID_
 		setInstance(0);
 		this->flashAddrs = AxisFlashAddrs({ADR_AXIS1_CONFIG, ADR_AXIS1_MAX_SPEED, ADR_AXIS1_MAX_ACCEL,
 										   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_SPEEDACCEL_FILTER,ADR_AXIS1_POSTPROCESS1});
 	}
 	else if (axis == 'Y')
 	{
 		drv_chooser = ClassChooser<MotorDriver>(axis2_drivers);
 		setInstance(1);
 		this->flashAddrs = AxisFlashAddrs({ADR_AXIS2_CONFIG, ADR_AXIS2_MAX_SPEED, ADR_AXIS2_MAX_ACCEL,
 										   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_SPEEDACCEL_FILTER,ADR_AXIS2_POSTPROCESS1});
 	}
 	else if (axis == 'Z')
 	{
 		setInstance(2);
 		this->flashAddrs = AxisFlashAddrs({ADR_AXIS3_CONFIG, ADR_AXIS3_MAX_SPEED, ADR_AXIS3_MAX_ACCEL,
 										   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_SPEEDACCEL_FILTER,ADR_AXIS3_POSTPROCESS1});
 	}
 
 
@@ -157,6 +157,8 @@ void Axis::registerCommands(){
 	registerCommand("filterAccel", Axis_commands::filterAccel, "Biquad filter freq and q*100 for accel", CMDFLAG_GET);
 
 	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);
 }
 
 /*
@@ -235,6 +237,11 @@ void Axis::restoreFlash(){
 		accelFilter.setQ(filterAccelCst[this->filterProfileId].q / 100.0);
 	}
 
+	uint16_t pp1;
+	if(Flash_Read(flashAddrs.postprocess1, &pp1)){
+		setExpo(pp1 & 0xff);
+	}
+
 }
 // Saves parameters to flash.
 void Axis::saveFlash(){
@@ -253,6 +260,10 @@ void Axis::saveFlash(){
 	// save CF biquad
 	uint16_t filterStorage = (uint16_t)this->filterProfileId & 0xFF;
 	Flash_Write(flashAddrs.speedAccelFilter, filterStorage);
+
+	// Postprocessing
+	Flash_Write(flashAddrs.postprocess1, expoValInt & 0xff);
+
 }
 
 
@@ -598,7 +609,7 @@ void Axis::calculateAxisEffects(bool ffb_on){
 		float speed = metric.current.speed * INTERNAL_SCALER_FRICTION;
 		float speedRampupCeil = 4096;
 		float rampupFactor = 1.0;
-		if (fabs (speed) < speedRampupCeil) {								// if speed in the range to rampup we apply a sinus curbe to ramup
+		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
 		}
@@ -655,6 +666,18 @@ uint16_t Axis::getPower(){
 	return power;
 }
 
+/**
+ * Calculates an exponential torque correction curve
+ */
+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;
+	}
+}
+
 void  Axis::updateTorqueScaler() {
 	effect_margin_scaler = ((float)fx_ratio_i/255.0);
 	torqueScaler = ((float)power / (float)0x7fff);
@@ -702,11 +725,15 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 
 	// Scale effect torque
 	int32_t torque = effectTorque; // Game effects
+	if(expo != 1){
+		torque = calculateExpoTorque(torque);
+	}
 	torque *= effect_margin_scaler;
 	torque += axisEffectTorque; // Independent effects
 	torque += updateEndstop();
 	torque *= torqueScaler; // Scale to power
 
+
 	// TODO speed and accel limiters
 	if(maxSpeedDegS > 0){
 
@@ -785,6 +812,21 @@ void Axis::setDegrees(uint16_t degrees){
 }
 
 
+void Axis::setExpo(int val){
+	val = clip(val, -127, 127);
+	expoValInt = val;
+	if(val == 0){
+		expo = 1; // Explicitly force expo off
+		return;
+	}
+	float valF = abs((float)val / expoScaler);
+	if(val < 0){
+		expo = 1.0f/(1.0f+valF);
+	}else{
+		expo = 1+valF;
+	}
+}
+
 CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>& replies){
 
 	switch(static_cast<Axis_commands>(cmd.cmdId)){
@@ -998,6 +1040,14 @@ CommandStatus Axis::command(const ParsedCommand& cmd,std::vector<CommandReply>&
 		}
 		break;
 
+	case Axis_commands::expo:
+		handleGetSetFunc(cmd, replies, expoValInt, &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;
+
 	default:
 		return CommandStatus::NOT_FOUND;
 	}
diff --git 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 161
+#define NB_OF_VAR 164
 extern const uint16_t VirtAddVarTab[NB_OF_VAR];
 
 // Amount of variables in exportable list
-#define NB_EXPORTABLE_ADR 146
+#define NB_EXPORTABLE_ADR 149
 extern const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR];
 
 
@@ -103,6 +103,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #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
 // TMC1
 #define ADR_TMC1_MOTCONF 0x320 // 0-2: MotType 3-5: PhiE source 6-15: Poles
 #define ADR_TMC1_CPR 0x321
@@ -128,6 +129,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #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
 // TMC2
 #define ADR_TMC2_MOTCONF 0x360 // 0-2: MotType 3-5: PhiE source 6-15: Poles
 #define ADR_TMC2_CPR 0x361
@@ -153,6 +155,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #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
 // TMC3
 #define ADR_TMC3_MOTCONF 0x3A0 // 0-2: MotType 3-5: PhiE source 6-15: Poles
 #define ADR_TMC3_CPR 0x3A1
@@ -216,6 +219,7 @@ uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data) will return 1 if
 #define ADR_LOCALANALOG_MAX_6 0x50D
 #define ADR_LOCALANALOG_MIN_7 0x50E
 #define ADR_LOCALANALOG_MAX_7 0x50F
+// ADS111X
 #define ADR_ADS111X_MIN_0 0x510
 #define ADR_ADS111X_MAX_0 0x511
 #define ADR_ADS111X_MIN_1 0x512
diff --git a/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c b/Firmware/FFBoard/UserExtensions/Src/eeprom_addresses.c
@@ -29,6 +29,9 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_FFBWHEEL_BUTTONCONF,
 	ADR_FFBWHEEL_ANALOGCONF,
 	ADR_FFBWHEEL_CONF1,
+// CAN remote
+	ADR_CANREMOTE_CONF1,
+	ADR_CANREMOTE_CONF2,
 // Button Sources:
 	ADR_SPI_BTN_1_CONF,
 	ADR_SHIFTERANALOG_CONF,
@@ -82,6 +85,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_AXIS1_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
 	ADR_AXIS1_ENC_RATIO, // Accel filter Lowpass
 	ADR_AXIS1_EFFECTS2, // 0-7 Friction, 8-15 Inertia
+	ADR_AXIS1_POSTPROCESS1, // 0-7 expo curve
 // TMC1
 	ADR_TMC1_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles
 	ADR_TMC1_CPR,
@@ -107,6 +111,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_AXIS2_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
 	ADR_AXIS2_ENC_RATIO, // Store the encoder ratio for an axis
 	ADR_AXIS2_EFFECTS2, // 0-7 Friction, 8-15 Inertia
+	ADR_AXIS2_POSTPROCESS1, // 0-7 expo curve
 // TMC2
 	ADR_TMC2_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles
 	ADR_TMC2_CPR,
@@ -132,6 +137,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_AXIS3_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
 	ADR_AXIS3_ENC_RATIO, // Store the encoder ratio for an axis
 	ADR_AXIS3_EFFECTS2, // 0-7 Friction, 8-15 Inertia
+	ADR_AXIS3_POSTPROCESS1, // 0-7 expo curve
 // TMC3
 	ADR_TMC3_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles
 	ADR_TMC3_CPR,
@@ -195,6 +201,7 @@ const uint16_t VirtAddVarTab[NB_OF_VAR] =
 	ADR_LOCALANALOG_MAX_6,
 	ADR_LOCALANALOG_MIN_7,
 	ADR_LOCALANALOG_MAX_7,
+// ADS111X
 	ADR_ADS111X_MIN_0,
 	ADR_ADS111X_MAX_0,
 	ADR_ADS111X_MIN_1,
@@ -222,6 +229,9 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_FFBWHEEL_BUTTONCONF,
 	ADR_FFBWHEEL_ANALOGCONF,
 	ADR_FFBWHEEL_CONF1,
+// CAN remote
+	ADR_CANREMOTE_CONF1,
+	ADR_CANREMOTE_CONF2,
 // Button Sources:
 	ADR_SPI_BTN_1_CONF,
 	ADR_SHIFTERANALOG_CONF,
@@ -275,6 +285,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_AXIS1_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
 	ADR_AXIS1_ENC_RATIO, // Accel filter Lowpass
 	ADR_AXIS1_EFFECTS2, // 0-7 Friction, 8-15 Inertia
+	ADR_AXIS1_POSTPROCESS1, // 0-7 expo curve
 // TMC1
 	ADR_TMC1_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles
 	ADR_TMC1_CPR,
@@ -300,6 +311,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_AXIS2_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
 	ADR_AXIS2_ENC_RATIO, // Store the encoder ratio for an axis
 	ADR_AXIS2_EFFECTS2, // 0-7 Friction, 8-15 Inertia
+	ADR_AXIS2_POSTPROCESS1, // 0-7 expo curve
 // TMC2
 	ADR_TMC2_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles
 	ADR_TMC2_CPR,
@@ -325,6 +337,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_AXIS3_SPEEDACCEL_FILTER, // Speed/Accel filter Lowpass profile
 	ADR_AXIS3_ENC_RATIO, // Store the encoder ratio for an axis
 	ADR_AXIS3_EFFECTS2, // 0-7 Friction, 8-15 Inertia
+	ADR_AXIS3_POSTPROCESS1, // 0-7 expo curve
 // TMC3
 	ADR_TMC3_MOTCONF, // 0-2: MotType 3-5: PhiE source 6-15: Poles
 	ADR_TMC3_CPR,
@@ -388,6 +401,7 @@ const uint16_t exportableFlashAddresses[NB_EXPORTABLE_ADR] =
 	ADR_LOCALANALOG_MAX_6,
 	ADR_LOCALANALOG_MIN_7,
 	ADR_LOCALANALOG_MAX_7,
+// ADS111X
 	ADR_ADS111X_MIN_0,
 	ADR_ADS111X_MAX_0,
 	ADR_ADS111X_MIN_1,
diff --git a/Firmware/scripts/memory_map.csv b/Firmware/scripts/memory_map.csv
@@ -67,6 +67,7 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS1_SPEEDACCEL_FILTER,0x309,// Speed/Accel filter Lowpass profile,1,1
 ,ADR_AXIS1_ENC_RATIO,0x30A,// Accel filter Lowpass,1,1
 ,ADR_AXIS1_EFFECTS2,0x30B,"// 0-7 Friction, 8-15 Inertia",1,1
+,ADR_AXIS1_POSTPROCESS1,0x30C,// 0-7 expo curve,1,1
 // TMC1,,,,,
 ,ADR_TMC1_MOTCONF,0x320,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC1_CPR,0x321,,1,1
@@ -92,6 +93,13 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS2_SPEEDACCEL_FILTER,0x349,// Speed/Accel filter Lowpass profile,1,1
 ,ADR_AXIS2_ENC_RATIO,0x34A,// Store the encoder ratio for an axis,1,1
 ,ADR_AXIS2_EFFECTS2,0x34B,"// 0-7 Friction, 8-15 Inertia",1,1
+,ADR_AXIS2_POSTPROCESS1,0x34C,// 0-7 expo curve,1,1
+,,,,,
+,,,,,
+,,,,,
+,,,,,
+,,,,,
+,,,,,
 // TMC2,,,,,
 ,ADR_TMC2_MOTCONF,0x360,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC2_CPR,0x361,,1,1
@@ -117,6 +125,9 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_AXIS3_SPEEDACCEL_FILTER,0x389,// Speed/Accel filter Lowpass profile,1,1
 ,ADR_AXIS3_ENC_RATIO,0x38A,// Store the encoder ratio for an axis,1,1
 ,ADR_AXIS3_EFFECTS2,0x38B,"// 0-7 Friction, 8-15 Inertia",1,1
+,ADR_AXIS3_POSTPROCESS1,0x38C,// 0-7 expo curve,1,1
+,,,,,
+,,,,,
 // TMC3,,,,,
 ,ADR_TMC3_MOTCONF,0x3A0,// 0-2: MotType 3-5: PhiE source 6-15: Poles,1,1
 ,ADR_TMC3_CPR,0x3A1,,1,1
@@ -183,6 +194,7 @@ Section comment,Name,Value,Comment on key,VirtAddVarTab,exportableFlashAddresses
 ,ADR_LOCALANALOG_MAX_6,0x50D,,1,1
 ,ADR_LOCALANALOG_MIN_7,0x50E,,1,1
 ,ADR_LOCALANALOG_MAX_7,0x50F,,1,1
+// ADS111X,,,,,
 ,ADR_ADS111X_MIN_0,0x510,,1,1
 ,ADR_ADS111X_MAX_0,0x511,,1,1
 ,ADR_ADS111X_MIN_1,0x512,,1,1
