Branch data Line data Source code
1 : : #include "FFBEngine.h"
2 : : #include "Config.h"
3 : : #include "StringUtils.h"
4 : : #include "RestApiProvider.h"
5 : : #include "Logger.h"
6 : : #include "io/lmu_sm_interface/LmuSharedMemoryWrapper.h"
7 : : #include <iostream>
8 : : #include <mutex>
9 : :
10 : : extern std::recursive_mutex g_engine_mutex;
11 : : #include <algorithm>
12 : : #include <cmath>
13 : :
14 : : using namespace ffb_math;
15 : :
16 [ + + + + : 8816 : FFBEngine::FFBEngine() {
+ + + + +
+ + + +
- ]
17 : 464 : last_log_time = std::chrono::steady_clock::now();
18 [ + - ]: 464 : Preset::ApplyDefaultsToEngine(*this);
19 : 464 : m_safety.SetTimePtr(&m_working_info.mElapsedTime);
20 : 464 : }
21 : :
22 : : // ---------------------------------------------------------------------------
23 : : // Grip & Load Estimation methods have been moved to GripLoadEstimation.cpp.
24 : : // See docs/dev_docs/reports/FFBEngine_refactoring_analysis.md for rationale.
25 : : // Functions moved:
26 : : // update_static_load_reference, InitializeLoadReference,
27 : : // calculate_raw_slip_angle_pair, calculate_slip_angle,
28 : : // calculate_axle_grip, approximate_load, approximate_rear_load,
29 : : // calculate_manual_slip_ratio,
30 : : // calculate_slope_grip, calculate_slope_confidence,
31 : : // calculate_wheel_slip_ratio
32 : : // ---------------------------------------------------------------------------
33 : :
34 : :
35 : : // Signal Conditioning: Applies idle smoothing and notch filters to raw torque
36 : : // Returns the conditioned force value ready for effect processing
37 : 15854 : double FFBEngine::apply_signal_conditioning(double raw_torque, const TelemInfoV01* data, FFBCalculationContext& ctx) {
38 : 15854 : double game_force_proc = raw_torque;
39 : :
40 : : // Idle Smoothing
41 : 15854 : double effective_shaft_smoothing = (double)m_steering_shaft_smoothing;
42 : 15854 : double idle_speed_threshold = (double)m_speed_gate_upper;
43 [ + + ]: 15854 : if (idle_speed_threshold < (double)IDLE_SPEED_MIN_M_S) idle_speed_threshold = (double)IDLE_SPEED_MIN_M_S;
44 [ + + ]: 15854 : if (ctx.car_speed < idle_speed_threshold) {
45 : 1146 : double idle_blend = (idle_speed_threshold - ctx.car_speed) / idle_speed_threshold;
46 : 1146 : double dynamic_smooth = (double)IDLE_BLEND_FACTOR * idle_blend;
47 : 1146 : effective_shaft_smoothing = (std::max)(effective_shaft_smoothing, dynamic_smooth);
48 : : }
49 : :
50 [ + + ]: 15854 : if (effective_shaft_smoothing > MIN_TAU_S) {
51 : 1169 : double alpha_shaft = ctx.dt / (effective_shaft_smoothing + ctx.dt);
52 : 1169 : alpha_shaft = (std::min)(ALPHA_MAX, (std::max)(ALPHA_MIN, alpha_shaft));
53 : 1169 : m_steering_shaft_torque_smoothed += alpha_shaft * (game_force_proc - m_steering_shaft_torque_smoothed);
54 : 1169 : game_force_proc = m_steering_shaft_torque_smoothed;
55 : : } else {
56 : 14685 : m_steering_shaft_torque_smoothed = game_force_proc;
57 : : }
58 : :
59 : : // Frequency Estimator Logic
60 : 15854 : double alpha_hpf = ctx.dt / (HPF_TIME_CONSTANT_S + ctx.dt);
61 : 15854 : m_torque_ac_smoothed += alpha_hpf * (game_force_proc - m_torque_ac_smoothed);
62 : 15854 : double ac_torque = game_force_proc - m_torque_ac_smoothed;
63 : :
64 [ + + + + ]: 15854 : if ((m_prev_ac_torque < -ZERO_CROSSING_EPSILON && ac_torque > ZERO_CROSSING_EPSILON) ||
65 [ + + + + ]: 15543 : (m_prev_ac_torque > ZERO_CROSSING_EPSILON && ac_torque < -ZERO_CROSSING_EPSILON)) {
66 : :
67 : 635 : double now = data->mElapsedTime;
68 : 635 : double period = now - m_last_crossing_time;
69 : :
70 [ + + + + ]: 635 : if (period > MIN_FREQ_PERIOD && period < MAX_FREQ_PERIOD) {
71 : 37 : double inst_freq = 1.0 / (period * DUAL_DIVISOR);
72 : 37 : m_debug_freq = m_debug_freq * DEBUG_FREQ_SMOOTHING + inst_freq * (1.0 - DEBUG_FREQ_SMOOTHING);
73 : : }
74 : 635 : m_last_crossing_time = now;
75 : : }
76 : 15854 : m_prev_ac_torque = ac_torque;
77 : :
78 : 15854 : const TelemWheelV01& fl_ref = data->mWheel[0];
79 : 15854 : double radius = (double)fl_ref.mStaticUndeflectedRadius / UNIT_CM_TO_M;
80 [ + + ]: 15854 : if (radius < RADIUS_FALLBACK_MIN_M) radius = RADIUS_FALLBACK_DEFAULT_M;
81 : 15854 : double circumference = TWO_PI * radius;
82 [ + - ]: 15854 : double wheel_freq = (circumference > 0.0) ? (ctx.car_speed / circumference) : 0.0;
83 : 15854 : m_theoretical_freq = wheel_freq;
84 : :
85 : : // Dynamic Notch Filter
86 [ + + ]: 15854 : if (m_flatspot_suppression) {
87 [ + + ]: 8 : if (wheel_freq > 1.0) {
88 [ + - ]: 3 : m_notch_filter.Update(wheel_freq, 1.0/ctx.dt, (double)m_notch_q);
89 : 3 : double input_force = game_force_proc;
90 : 3 : double filtered_force = m_notch_filter.Process(input_force);
91 : 3 : game_force_proc = input_force * (1.0f - m_flatspot_strength) + filtered_force * m_flatspot_strength;
92 : : } else {
93 : 5 : m_notch_filter.Reset();
94 : : }
95 : : }
96 : :
97 : : // Static Notch Filter
98 [ + + ]: 15854 : if (m_static_notch_enabled) {
99 : 1502 : double bw = (double)m_static_notch_width;
100 [ + + ]: 1502 : if (bw < MIN_NOTCH_WIDTH_HZ) bw = MIN_NOTCH_WIDTH_HZ;
101 : 1502 : double q = (double)m_static_notch_freq / bw;
102 [ + - ]: 1502 : m_static_notch_filter.Update((double)m_static_notch_freq, 1.0/ctx.dt, q);
103 : 1502 : game_force_proc = m_static_notch_filter.Process(game_force_proc);
104 : : } else {
105 : 14352 : m_static_notch_filter.Reset();
106 : : }
107 : :
108 : 15854 : return game_force_proc;
109 : : }
110 : :
111 : : // Refactored calculate_force
112 : 15850 : double FFBEngine::calculate_force(const TelemInfoV01* data, const char* vehicleClass, const char* vehicleName, float genFFBTorque, bool allowed, double override_dt, signed char mControl) {
113 [ + + ]: 15850 : if (!data) return 0.0;
114 [ + - ]: 15845 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
115 : :
116 : : // --- 1. CORE PHYSICS CRASH DETECTION ---
117 : : // If the chassis or steering itself is NaN, the car is in the void or the session is dead.
118 : : // We must abort to prevent math explosions.
119 : 15845 : if (!std::isfinite(data->mUnfilteredSteering) ||
120 [ + - ]: 15837 : !std::isfinite(data->mLocalRot.y) ||
121 [ + - ]: 15837 : !std::isfinite(data->mLocalAccel.x) ||
122 [ + - ]: 15837 : !std::isfinite(data->mLocalAccel.z) ||
123 [ + + + + : 47518 : !std::isfinite(data->mSteeringShaftTorque) ||
+ + ]
124 [ - + ]: 15836 : !std::isfinite(genFFBTorque)) {
125 : :
126 : : // Rate-limited logging (once every 5 seconds)
127 [ + + ]: 9 : if (data->mElapsedTime > m_last_core_nan_log_time + 5.0) {
128 [ + - + - ]: 6 : Logger::Get().LogFile("[Diag] Core Physics NaN/Inf detected! (Steering, Accel, or Torque). FFB muted for this frame.");
129 : 6 : m_last_core_nan_log_time = data->mElapsedTime;
130 : : }
131 : 9 : return 0.0;
132 : : }
133 : :
134 : : // --- 0. METADATA & CLASS SEEDING (Issue #379) ---
135 : : // Moved to top to ensure car changes trigger resets before physics loop
136 [ + - ]: 15836 : bool seeded = m_metadata.UpdateInternal(vehicleClass, vehicleName, data->mTrackName);
137 [ + + ]: 15836 : if (seeded) {
138 [ + - ]: 111 : InitializeLoadReference(m_metadata.GetCurrentClassName(), m_metadata.GetVehicleName());
139 : : }
140 : :
141 : : // --- 1. UP-SAMPLING (Issue #216) ---
142 : : // If override_dt is provided (e.g. from main.cpp), we are in 400Hz upsampling mode.
143 : : // Otherwise (override_dt <= 0), we are in legacy/test mode: every call is a new frame.
144 : 15836 : bool upsampling_active = (override_dt > 0.0);
145 [ + + + + ]: 15836 : bool is_new_frame = !upsampling_active || (data->mElapsedTime != m_last_telemetry_time);
146 : :
147 [ + + ]: 15836 : if (is_new_frame) m_last_telemetry_time = data->mElapsedTime;
148 : :
149 [ + + ]: 15836 : double ffb_dt = upsampling_active ? override_dt : (double)data->mDeltaTime;
150 [ + + ]: 15836 : if (ffb_dt < 0.0001) ffb_dt = 0.0025;
151 : :
152 : : // Synchronize persistent working copy
153 : 15836 : m_working_info = *data;
154 : :
155 : : // --- 2. AUXILIARY DATA SANITIZATION ---
156 : : // Sanitize non-core chassis data
157 [ - + ]: 15836 : if (!std::isfinite(m_working_info.mUnfilteredThrottle)) m_working_info.mUnfilteredThrottle = 0.0;
158 [ - + ]: 15836 : if (!std::isfinite(m_working_info.mUnfilteredBrake)) m_working_info.mUnfilteredBrake = 0.0;
159 [ - + ]: 15836 : if (!std::isfinite(m_working_info.mLocalRotAccel.y)) m_working_info.mLocalRotAccel.y = 0.0;
160 [ - + ]: 15836 : if (!std::isfinite(m_working_info.mLocalVel.x)) m_working_info.mLocalVel.x = 0.0;
161 [ - + ]: 15836 : if (!std::isfinite(m_working_info.mLocalVel.y)) m_working_info.mLocalVel.y = 0.0;
162 [ - + ]: 15836 : if (!std::isfinite(m_working_info.mLocalVel.z)) m_working_info.mLocalVel.z = 0.0;
163 : :
164 : : // Replace NaN/Infinity in wheel channels with 0.0.
165 : : // This protects the filters AND seamlessly triggers our existing fallback logic
166 : : // (e.g., approximate_load) if the data is encrypted or missing.
167 : 15836 : bool aux_nan_detected = false;
168 [ + + ]: 79180 : for (int i = 0; i < 4; i++) {
169 [ + + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mTireLoad)) { m_working_info.mWheel[i].mTireLoad = 0.0; aux_nan_detected = true; }
170 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mGripFract)) { m_working_info.mWheel[i].mGripFract = 0.0; aux_nan_detected = true; }
171 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mSuspForce)) { m_working_info.mWheel[i].mSuspForce = 0.0; aux_nan_detected = true; }
172 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mVerticalTireDeflection)) { m_working_info.mWheel[i].mVerticalTireDeflection = 0.0; aux_nan_detected = true; }
173 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mLateralPatchVel)) { m_working_info.mWheel[i].mLateralPatchVel = 0.0; aux_nan_detected = true; }
174 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mLongitudinalPatchVel)) { m_working_info.mWheel[i].mLongitudinalPatchVel = 0.0; aux_nan_detected = true; }
175 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mRotation)) { m_working_info.mWheel[i].mRotation = 0.0; aux_nan_detected = true; }
176 [ - + ]: 63344 : if (!std::isfinite(m_working_info.mWheel[i].mBrakePressure)) { m_working_info.mWheel[i].mBrakePressure = 0.0; aux_nan_detected = true; }
177 : : }
178 : :
179 [ + + + + ]: 15836 : if (aux_nan_detected && data->mElapsedTime > m_last_aux_nan_log_time + 5.0) {
180 [ + - + - ]: 2 : Logger::Get().LogFile("[Diag] Auxiliary Wheel NaN/Inf detected and sanitized to 0.0.");
181 : 2 : m_last_aux_nan_log_time = data->mElapsedTime;
182 : : }
183 : :
184 : : // Upsample Steering Shaft Torque (Holt-Winters)
185 : 15836 : double shaft_torque = m_upsample_shaft_torque.Process(m_working_info.mSteeringShaftTorque, ffb_dt, is_new_frame);
186 : 15836 : m_working_info.mSteeringShaftTorque = shaft_torque;
187 : :
188 : : // Update wheels in working_info (Channels used for derivatives)
189 : : // Use sanitized m_working_info as input for upsamplers
190 [ + + ]: 79180 : for (int i = 0; i < 4; i++) {
191 : 63344 : m_working_info.mWheel[i].mLateralPatchVel = m_upsample_lat_patch_vel[i].Process(m_working_info.mWheel[i].mLateralPatchVel, ffb_dt, is_new_frame);
192 : 63344 : m_working_info.mWheel[i].mLongitudinalPatchVel = m_upsample_long_patch_vel[i].Process(m_working_info.mWheel[i].mLongitudinalPatchVel, ffb_dt, is_new_frame);
193 : 63344 : m_working_info.mWheel[i].mVerticalTireDeflection = m_upsample_vert_deflection[i].Process(m_working_info.mWheel[i].mVerticalTireDeflection, ffb_dt, is_new_frame);
194 : 63344 : m_working_info.mWheel[i].mSuspForce = m_upsample_susp_force[i].Process(m_working_info.mWheel[i].mSuspForce, ffb_dt, is_new_frame);
195 : 63344 : m_working_info.mWheel[i].mBrakePressure = m_upsample_brake_pressure[i].Process(m_working_info.mWheel[i].mBrakePressure, ffb_dt, is_new_frame);
196 : 63344 : m_working_info.mWheel[i].mRotation = m_upsample_rotation[i].Process(m_working_info.mWheel[i].mRotation, ffb_dt, is_new_frame);
197 : : }
198 : :
199 : : // Upsample other derivative sources
200 : 15836 : m_working_info.mUnfilteredSteering = m_upsample_steering.Process(m_working_info.mUnfilteredSteering, ffb_dt, is_new_frame);
201 : 15836 : m_working_info.mUnfilteredThrottle = m_upsample_throttle.Process(m_working_info.mUnfilteredThrottle, ffb_dt, is_new_frame);
202 : 15836 : m_working_info.mUnfilteredBrake = m_upsample_brake.Process(m_working_info.mUnfilteredBrake, ffb_dt, is_new_frame);
203 : 15836 : m_working_info.mLocalAccel.x = m_upsample_local_accel_x.Process(m_working_info.mLocalAccel.x, ffb_dt, is_new_frame);
204 : :
205 : : // --- DERIVED ACCELERATION (Issue #278) ---
206 : : // Recalculate acceleration from velocity at 100Hz ticks to avoid raw sensor spikes.
207 [ + + ]: 15836 : if (is_new_frame) {
208 [ + + ]: 15820 : if (!m_local_vel_seeded) {
209 : 310 : m_prev_local_vel = m_working_info.mLocalVel;
210 : 310 : m_local_vel_seeded = true;
211 : : }
212 : :
213 [ + + ]: 15820 : double game_dt = (data->mDeltaTime > 1e-6) ? data->mDeltaTime : 0.01;
214 : 15820 : m_derived_accel_y_100hz = (m_working_info.mLocalVel.y - m_prev_local_vel.y) / game_dt;
215 : 15820 : m_derived_accel_z_100hz = (m_working_info.mLocalVel.z - m_prev_local_vel.z) / game_dt;
216 : 15820 : m_prev_local_vel = m_working_info.mLocalVel;
217 : : }
218 : :
219 : : // --- 1.1 DERIVED ACCELERATION APPLICATION ---
220 : : // Recalculate acceleration from velocity to avoid raw sensor spikes.
221 : : // We apply this even in legacy mode (non-upsampled) if the user provides
222 : : // velocity data but no acceleration, or to ensure consistency across modes.
223 : 15836 : m_working_info.mLocalAccel.y = m_upsample_local_accel_y.Process(m_derived_accel_y_100hz, ffb_dt, is_new_frame);
224 : 15836 : m_working_info.mLocalAccel.z = m_upsample_local_accel_z.Process(m_derived_accel_z_100hz, ffb_dt, is_new_frame);
225 : :
226 : 15836 : m_working_info.mLocalRotAccel.y = m_upsample_local_rot_accel_y.Process(m_working_info.mLocalRotAccel.y, ffb_dt, is_new_frame);
227 : 15836 : m_working_info.mLocalRot.y = m_upsample_local_rot_y.Process(m_working_info.mLocalRot.y, ffb_dt, is_new_frame);
228 : :
229 : : // Use upsampled data pointer for all calculations
230 : 15836 : const TelemInfoV01* upsampled_data = &m_working_info;
231 : :
232 : : // --- Stats Latching (Issue #379) ---
233 : : // Moved to top of frame to ensure resets happen BEFORE any updates
234 : : // within the same frame in unit tests.
235 : : {
236 : 15836 : auto now_stats = std::chrono::steady_clock::now();
237 [ + - + - : 15836 : if (std::chrono::duration_cast<std::chrono::seconds>(now_stats - last_log_time).count() >= 1) {
+ + ]
238 : 3 : s_torque.ResetInterval();
239 : 3 : s_front_load.ResetInterval();
240 : 3 : s_front_grip.ResetInterval();
241 : 3 : s_lat_g.ResetInterval();
242 : 3 : last_log_time = now_stats;
243 : : }
244 : : }
245 : :
246 : : // --- SAFETY & TRANSITION LOGIC ---
247 [ + + + + : 15836 : if (m_safety.GetLastAllowed() && !allowed) {
+ + ]
248 [ + - + + : 18 : Logger::Get().LogFile("[Safety] FFB Muted (Reason: %s)", upsampled_data->mElapsedTime > 0 ? "Game/State Mute" : "Initialization");
+ - ]
249 [ + + + + : 15818 : } else if (!m_safety.GetLastAllowed() && allowed) {
+ + ]
250 [ + - + - ]: 6 : Logger::Get().LogFile("[Safety] FFB Unmuted");
251 [ + - ]: 6 : m_safety.TriggerSafetyWindow("FFB Unmuted", upsampled_data->mElapsedTime);
252 : : }
253 : 15836 : m_safety.SetLastAllowed(allowed);
254 : :
255 [ + + ]: 15836 : if (mControl != m_safety.GetLastControl()) {
256 [ + + ]: 276 : if (m_safety.GetLastControl() != -2) { // Skip first frame
257 [ + - + - ]: 4 : Logger::Get().LogFile("[Safety] mControl Transition: %d -> %d", (int)m_safety.GetLastControl(), (int)mControl);
258 [ + - ]: 4 : m_safety.TriggerSafetyWindow("Control Transition", upsampled_data->mElapsedTime);
259 : : }
260 : 276 : m_safety.SetLastControl(mControl);
261 : : }
262 : :
263 : : // Transition Logic: Reset filters when entering OR exiting "Muted" state (e.g. Garage/AI)
264 : : // to clear out high-frequency residuals and prevent stale state from infecting new sessions.
265 [ + + ]: 15836 : if (m_was_allowed != allowed) {
266 : 25 : m_kerb_timer = 0.0;
267 : :
268 : : // --- NEW: Reset diagnostic timers ---
269 : 25 : m_last_core_nan_log_time = -999.0;
270 : 25 : m_last_aux_nan_log_time = -999.0;
271 : 25 : m_last_math_nan_log_time = -999.0;
272 : :
273 : 25 : m_upsample_shaft_torque.Reset();
274 : 25 : m_upsample_steering.Reset();
275 : 25 : m_upsample_throttle.Reset();
276 : 25 : m_upsample_brake.Reset();
277 : 25 : m_upsample_local_accel_x.Reset();
278 : 25 : m_upsample_local_accel_y.Reset();
279 : 25 : m_upsample_local_accel_z.Reset();
280 : 25 : m_upsample_local_rot_accel_y.Reset();
281 : 25 : m_upsample_local_rot_y.Reset();
282 [ + + ]: 125 : for (int i = 0; i < 4; i++) {
283 : 100 : m_upsample_lat_patch_vel[i].Reset();
284 : 100 : m_upsample_long_patch_vel[i].Reset();
285 : 100 : m_upsample_vert_deflection[i].Reset();
286 : 100 : m_upsample_susp_force[i].Reset();
287 : 100 : m_upsample_brake_pressure[i].Reset();
288 : 100 : m_upsample_rotation[i].Reset();
289 : : }
290 : 25 : m_steering_velocity_smoothed = 0.0;
291 : 25 : m_steering_shaft_torque_smoothed = 0.0;
292 : 25 : m_accel_x_smoothed = 0.0;
293 : 25 : m_accel_z_smoothed = 0.0;
294 : 25 : m_sop_lat_g_smoothed = 0.0;
295 : 25 : m_long_load_smoothed = 1.0;
296 : 25 : m_yaw_accel_smoothed = 0.0;
297 : 25 : m_prev_yaw_rate = 0.0;
298 : 25 : m_yaw_rate_seeded = false;
299 : 25 : m_fast_yaw_accel_smoothed = 0.0;
300 : 25 : m_prev_fast_yaw_accel = 0.0;
301 : 25 : m_yaw_accel_seeded = false;
302 : 25 : m_unloaded_vulnerability_smoothed = 0.0;
303 : 25 : m_power_vulnerability_smoothed = 0.0;
304 : 25 : m_prev_local_vel = {};
305 : 25 : m_local_vel_seeded = false;
306 : 25 : m_derivatives_seeded = false;
307 : : }
308 : 15836 : m_was_allowed = allowed;
309 : :
310 : : // SEEDING GATE (Issue #379): Prevent teleport spikes from Garage -> Track
311 [ + + + + ]: 15836 : if (!m_derivatives_seeded && allowed) {
312 : : // 1. Update all "prev" states used for derivatives to current values
313 [ + + ]: 600 : for (int i = 0; i < 4; i++) {
314 : 480 : m_prev_vert_deflection[i] = data->mWheel[i].mVerticalTireDeflection;
315 : 480 : m_prev_rotation[i] = data->mWheel[i].mRotation;
316 : 480 : m_prev_brake_pressure[i] = data->mWheel[i].mBrakePressure;
317 : 480 : m_prev_susp_force[i] = data->mWheel[i].mSuspForce;
318 : : }
319 : 120 : m_prev_steering_angle = upsampled_data->mUnfilteredSteering;
320 : 120 : m_prev_yaw_rate = upsampled_data->mLocalRot.y;
321 : 120 : m_prev_vert_accel = upsampled_data->mLocalAccel.y;
322 : :
323 : : // 2. Warm up LPFs to current values to prevent ramp-up transients
324 : 120 : m_accel_x_smoothed = upsampled_data->mLocalAccel.x;
325 : 120 : m_accel_z_smoothed = upsampled_data->mLocalAccel.z;
326 : 120 : m_sop_lat_g_smoothed = upsampled_data->mLocalAccel.x / GRAVITY_MS2;
327 : :
328 : : // Use approximate loads for SoP seeding if necessary
329 : 120 : double fl_l = upsampled_data->mWheel[0].mTireLoad;
330 : 120 : double fr_l = upsampled_data->mWheel[1].mTireLoad;
331 : 120 : double rl_l = upsampled_data->mWheel[2].mTireLoad;
332 : 120 : double rr_l = upsampled_data->mWheel[3].mTireLoad;
333 [ + + ]: 120 : if (fl_l < 1.0) {
334 [ + - ]: 32 : fl_l = approximate_load(upsampled_data->mWheel[0]);
335 [ + - ]: 32 : fr_l = approximate_load(upsampled_data->mWheel[1]);
336 [ + - ]: 32 : rl_l = approximate_rear_load(upsampled_data->mWheel[2]);
337 [ + - ]: 32 : rr_l = approximate_rear_load(upsampled_data->mWheel[3]);
338 : : }
339 : 120 : double t_load = fl_l + fr_l + rl_l + rr_l;
340 [ + + ]: 120 : m_sop_load_smoothed = (t_load > 1.0) ? (fr_l + rr_l - fl_l - rl_l) / t_load : 0.0;
341 : :
342 : 120 : m_steering_velocity_smoothed = 0.0;
343 [ + + ]: 120 : m_steering_shaft_torque_smoothed = (m_torque_source == 1) ? (double)genFFBTorque * (double)m_wheelbase_max_nm : shaft_torque;
344 : 120 : m_last_raw_torque = m_steering_shaft_torque_smoothed;
345 : 120 : m_rolling_average_torque = std::abs(m_steering_shaft_torque_smoothed);
346 : :
347 : 120 : m_derivatives_seeded = true;
348 : : // NOTE: We do NOT return early. By seeding 'prev' to 'current',
349 : : // the deltas calculated later in this same frame will be zero,
350 : : // naturally muting derivative effects while allowing the rest of
351 : : // the pipeline (and snapshots) to proceed normally.
352 : : }
353 : :
354 : : // Select Torque Source
355 : : // v0.7.63 Fix: genFFBTorque (Direct Torque 400Hz) is normalized [-1.0, 1.0].
356 : : // It must be scaled by m_wheelbase_max_nm to match the engine's internal Nm-based pipeline.
357 [ + + ]: 15836 : double raw_torque_input = (m_torque_source == 1) ? (double)genFFBTorque * (double)m_wheelbase_max_nm : shaft_torque;
358 : :
359 : : // RELIABILITY FIX: Sanitize input torque
360 [ - + ]: 15836 : if (!std::isfinite(raw_torque_input)) return 0.0;
361 : :
362 : : // --- 0. DYNAMIC NORMALIZATION (Issue #152) ---
363 : : // 1. Contextual Spike Rejection (Lightweight MAD alternative)
364 : 15836 : double current_abs_torque = std::abs(raw_torque_input);
365 : 15836 : double alpha_slow = ffb_dt / (TORQUE_ROLL_AVG_TAU + ffb_dt); // 1-second rolling average
366 : 15836 : m_rolling_average_torque += alpha_slow * (current_abs_torque - m_rolling_average_torque);
367 : :
368 : 15836 : double lat_g_abs = std::abs(upsampled_data->mLocalAccel.x / (double)GRAVITY_MS2);
369 : 15836 : double torque_slew = std::abs(raw_torque_input - m_last_raw_torque) / (ffb_dt + (double)EPSILON_DIV);
370 : 15836 : m_last_raw_torque = raw_torque_input;
371 : :
372 : : // Flag as spike if torque jumps > 3x the rolling average (with a 15Nm floor to prevent low-speed false positives)
373 [ + + + + ]: 15836 : bool is_contextual_spike = (current_abs_torque > (m_rolling_average_torque * TORQUE_SPIKE_RATIO)) && (current_abs_torque > TORQUE_SPIKE_MIN_NM);
374 : :
375 : : // Safety check for clean state
376 [ + + + + : 15836 : bool is_clean_state = (lat_g_abs < LAT_G_CLEAN_LIMIT) && (torque_slew < TORQUE_SLEW_CLEAN_LIMIT) && !is_contextual_spike;
+ + ]
377 : :
378 : : // 2. Leaky Integrator (Exponential Decay + Floor)
379 [ + + + + : 15836 : if (is_clean_state && m_torque_source == 0 && m_dynamic_normalization_enabled) {
+ + ]
380 [ + + ]: 815 : if (current_abs_torque > m_session_peak_torque) {
381 : 402 : m_session_peak_torque = current_abs_torque; // Fast attack
382 : : } else {
383 : : // Exponential decay (0.5% reduction per second)
384 : 413 : double decay_factor = 1.0 - ((double)SESSION_PEAK_DECAY_RATE * ffb_dt);
385 : 413 : m_session_peak_torque *= decay_factor;
386 : : }
387 : : // Absolute safety floor and ceiling
388 : 815 : m_session_peak_torque = std::clamp(m_session_peak_torque, (double)PEAK_TORQUE_FLOOR, (double)PEAK_TORQUE_CEILING);
389 : : }
390 : :
391 : : // 3. EMA Filtering on the Gain Multiplier (Zero-latency physics)
392 : : // v0.7.71: For In-Game FFB (1), we normalize against the wheelbase max since the signal is already normalized [-1, 1].
393 : : double target_structural_mult;
394 [ + + ]: 15836 : if (m_torque_source == 1) {
395 : 23 : target_structural_mult = 1.0 / ((double)m_wheelbase_max_nm + (double)EPSILON_DIV);
396 [ + + ]: 15813 : } else if (m_dynamic_normalization_enabled) {
397 : 820 : target_structural_mult = 1.0 / (m_session_peak_torque + (double)EPSILON_DIV);
398 : : } else {
399 : 14993 : target_structural_mult = 1.0 / ((double)m_target_rim_nm + (double)EPSILON_DIV);
400 : : }
401 : 15836 : double alpha_gain = ffb_dt / ((double)STRUCT_MULT_SMOOTHING_TAU + ffb_dt); // 250ms smoothing
402 : 15836 : m_smoothed_structural_mult += alpha_gain * (target_structural_mult - m_smoothed_structural_mult);
403 : :
404 : : // Trigger REST API Fallback if enabled and range is invalid (Issue #221)
405 [ + + + + : 15836 : if (seeded && m_rest_api_enabled && data->mPhysicalSteeringWheelRange <= 0.0f) {
+ - ]
406 [ + - + - ]: 3 : RestApiProvider::Get().RequestSteeringRange(m_rest_api_port);
407 : : }
408 : :
409 : : // --- 1. INITIALIZE CONTEXT ---
410 : 15836 : FFBCalculationContext ctx;
411 : 15836 : ctx.dt = ffb_dt; // Use our constant FFB loop time for all internal physics
412 : :
413 : : // Sanity Check: Delta Time (Keep legacy warning if raw dt is broken)
414 [ + + ]: 15836 : if (data->mDeltaTime <= DT_EPSILON) {
415 [ + + ]: 191 : if (!m_warned_dt) {
416 [ + - + - ]: 15 : Logger::Get().LogFile("[WARNING] Invalid DeltaTime (<=0). Using default %.4fs.", DEFAULT_DT);
417 : 15 : m_warned_dt = true;
418 : : }
419 : 191 : ctx.frame_warn_dt = true;
420 : : }
421 : :
422 : 15836 : ctx.car_speed_long = upsampled_data->mLocalVel.z;
423 : 15836 : ctx.car_speed = std::abs(ctx.car_speed_long);
424 : :
425 : : // Steering Range Diagnostic (Issue #218)
426 [ + + ]: 15836 : if (upsampled_data->mPhysicalSteeringWheelRange <= 0.0f) {
427 [ + + ]: 15815 : if (!m_metadata.HasWarnedInvalidRange()) {
428 [ + - + - ]: 290 : float fallback = RestApiProvider::Get().GetFallbackRangeDeg();
429 [ + + - + ]: 290 : if (m_rest_api_enabled && fallback > 0.0f) {
430 [ # # # # ]: 0 : Logger::Get().LogFile("[FFB] Invalid Shared Memory Steering Range. Using REST API fallback: %.1f deg", fallback);
431 : : } else {
432 [ + - + - ]: 290 : Logger::Get().LogFile("[WARNING] Invalid PhysicalSteeringWheelRange (<=0) for %s. Soft Lock and Steering UI may be incorrect.", upsampled_data->mVehicleName);
433 : : }
434 : 290 : m_metadata.SetWarnedInvalidRange(true);
435 : : }
436 : : }
437 : :
438 : : // --- 2. SIGNAL CONDITIONING (STATE UPDATES) ---
439 : :
440 : : // Chassis Inertia Simulation
441 : 15836 : double chassis_tau = (double)m_chassis_inertia_smoothing;
442 [ + + ]: 15836 : if (chassis_tau < MIN_TAU_S) chassis_tau = MIN_TAU_S;
443 : 15836 : double alpha_chassis = ctx.dt / (chassis_tau + ctx.dt);
444 [ + + + - : 15836 : if (m_derivatives_seeded && m_was_allowed && allowed) {
+ - ]
445 : 15715 : m_accel_x_smoothed += alpha_chassis * (upsampled_data->mLocalAccel.x - m_accel_x_smoothed);
446 : 15715 : m_accel_z_smoothed += alpha_chassis * (upsampled_data->mLocalAccel.z - m_accel_z_smoothed);
447 : : }
448 : :
449 : : // --- 3. TELEMETRY PROCESSING ---
450 : : // Front Wheels
451 : 15836 : const TelemWheelV01& fl = upsampled_data->mWheel[0];
452 : 15836 : const TelemWheelV01& fr = upsampled_data->mWheel[1];
453 : :
454 : : // Raw Inputs
455 : 15836 : double raw_torque = raw_torque_input;
456 : 15836 : double raw_front_load = (fl.mTireLoad + fr.mTireLoad) / DUAL_DIVISOR;
457 : 15836 : double raw_front_grip = (fl.mGripFract + fr.mGripFract) / DUAL_DIVISOR;
458 : :
459 : : // Update Stats
460 : 15836 : s_torque.Update(raw_torque);
461 : 15836 : s_front_load.Update(raw_front_load);
462 : 15836 : s_front_grip.Update(raw_front_grip);
463 : 15836 : s_lat_g.Update(upsampled_data->mLocalAccel.x);
464 : :
465 : : // --- 4. PRE-CALCULATIONS ---
466 : :
467 : : // Average Load & Fallback Logic
468 : 15836 : ctx.avg_front_load = raw_front_load;
469 : :
470 : : // Hysteresis for missing load
471 [ + + + + ]: 15836 : if (ctx.avg_front_load < 1.0 && ctx.car_speed > SPEED_EPSILON) {
472 : 2370 : m_missing_load_frames++;
473 : : } else {
474 : 13466 : m_missing_load_frames = (std::max)(0, m_missing_load_frames - 1);
475 : : }
476 : :
477 [ + + ]: 15836 : if (m_missing_load_frames > MISSING_LOAD_WARN_THRESHOLD) {
478 : : // Fallback Logic: Use suspension-based approximation (more accurate than kinematic)
479 [ + - ]: 1490 : double calc_load_fl = approximate_load(fl);
480 [ + - ]: 1490 : double calc_load_fr = approximate_load(fr);
481 : 1490 : ctx.avg_front_load = (calc_load_fl + calc_load_fr) / DUAL_DIVISOR;
482 : :
483 [ + + ]: 1490 : if (!m_warned_load) {
484 [ + - + - ]: 26 : Logger::Get().LogFile("Warning: Data for mTireLoad from the game seems to be missing for this car (%s). (Likely Encrypted/DLC Content). Using Suspension-based Fallback.", data->mVehicleName);
485 : 26 : m_warned_load = true;
486 : : }
487 : 1490 : ctx.frame_warn_load = true;
488 : : }
489 : :
490 : : // Sanity Checks (Missing Data)
491 : :
492 : : // 1. Suspension Force (mSuspForce)
493 : 15836 : double avg_susp_f = (fl.mSuspForce + fr.mSuspForce) / DUAL_DIVISOR;
494 [ + + + + : 15836 : if (avg_susp_f < MIN_VALID_SUSP_FORCE && std::abs(data->mLocalVel.z) > SPEED_EPSILON) {
+ + ]
495 : 2832 : m_missing_susp_force_frames++;
496 : : } else {
497 : 13004 : m_missing_susp_force_frames = (std::max)(0, m_missing_susp_force_frames - 1);
498 : : }
499 [ + + + + ]: 15836 : if (m_missing_susp_force_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_susp_force) {
500 [ + - + - ]: 12 : Logger::Get().LogFile("Warning: Data for mSuspForce from the game seems to be missing for this car (%s). (Likely Encrypted/DLC Content). A fallback estimation will be used.", data->mVehicleName);
501 : 12 : m_warned_susp_force = true;
502 : : }
503 : :
504 : : // 2. Suspension Deflection (mSuspensionDeflection)
505 : 15836 : double avg_susp_def = (std::abs(fl.mSuspensionDeflection) + std::abs(fr.mSuspensionDeflection)) / DUAL_DIVISOR;
506 [ + + + + : 15836 : if (avg_susp_def < DEFLECTION_NEAR_ZERO_M && std::abs(data->mLocalVel.z) > SPEED_HIGH_THRESHOLD) {
+ + ]
507 : 12261 : m_missing_susp_deflection_frames++;
508 : : } else {
509 : 3575 : m_missing_susp_deflection_frames = (std::max)(0, m_missing_susp_deflection_frames - 1);
510 : : }
511 [ + + + + ]: 15836 : if (m_missing_susp_deflection_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_susp_deflection) {
512 [ + - + - ]: 44 : Logger::Get().LogFile("Warning: Data for mSuspensionDeflection from the game seems to be missing for this car (%s). (Likely Encrypted/DLC Content). A fallback estimation will be used.", data->mVehicleName);
513 : 44 : m_warned_susp_deflection = true;
514 : : }
515 : :
516 : : // 3. Front Lateral Force (mLateralForce)
517 : 15836 : double avg_lat_force_front = (std::abs(fl.mLateralForce) + std::abs(fr.mLateralForce)) / DUAL_DIVISOR;
518 [ + - + + : 15836 : if (avg_lat_force_front < MIN_VALID_LAT_FORCE_N && std::abs(data->mLocalAccel.x) > G_FORCE_THRESHOLD) {
+ + ]
519 : 4763 : m_missing_lat_force_front_frames++;
520 : : } else {
521 : 11073 : m_missing_lat_force_front_frames = (std::max)(0, m_missing_lat_force_front_frames - 1);
522 : : }
523 [ + + + + ]: 15836 : if (m_missing_lat_force_front_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_lat_force_front) {
524 [ + - + - ]: 14 : Logger::Get().LogFile("Warning: Data for mLateralForce (Front) from the game seems to be missing for this car (%s). (Likely Encrypted/DLC Content). A fallback estimation will be used.", data->mVehicleName);
525 : 14 : m_warned_lat_force_front = true;
526 : : }
527 : :
528 : : // 4. Rear Lateral Force (mLateralForce)
529 : 15836 : double avg_lat_force_rear = (std::abs(data->mWheel[2].mLateralForce) + std::abs(data->mWheel[3].mLateralForce)) / DUAL_DIVISOR;
530 [ + + + + : 15836 : if (avg_lat_force_rear < MIN_VALID_LAT_FORCE_N && std::abs(data->mLocalAccel.x) > G_FORCE_THRESHOLD) {
+ + ]
531 : 4703 : m_missing_lat_force_rear_frames++;
532 : : } else {
533 : 11133 : m_missing_lat_force_rear_frames = (std::max)(0, m_missing_lat_force_rear_frames - 1);
534 : : }
535 [ + + + + ]: 15836 : if (m_missing_lat_force_rear_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_lat_force_rear) {
536 [ + - + - ]: 13 : Logger::Get().LogFile("Warning: Data for mLateralForce (Rear) from the game seems to be missing for this car (%s). (Likely Encrypted/DLC Content). A fallback estimation will be used.", data->mVehicleName);
537 : 13 : m_warned_lat_force_rear = true;
538 : : }
539 : :
540 : : // 5. Vertical Tire Deflection (mVerticalTireDeflection)
541 : 15836 : double avg_vert_def = (std::abs(fl.mVerticalTireDeflection) + std::abs(fr.mVerticalTireDeflection)) / DUAL_DIVISOR;
542 [ + + + + : 15836 : if (avg_vert_def < DEFLECTION_NEAR_ZERO_M && std::abs(data->mLocalVel.z) > SPEED_HIGH_THRESHOLD) {
+ + ]
543 : 2000 : m_missing_vert_deflection_frames++;
544 : : } else {
545 : 13836 : m_missing_vert_deflection_frames = (std::max)(0, m_missing_vert_deflection_frames - 1);
546 : : }
547 [ + + + + ]: 15836 : if (m_missing_vert_deflection_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_vert_deflection) {
548 [ + - + - ]: 9 : Logger::Get().LogFile("[WARNING] mVerticalTireDeflection is missing for car: %s. (Likely Encrypted/DLC Content). Road Texture fallback active.", data->mVehicleName);
549 : 9 : m_warned_vert_deflection = true;
550 : : }
551 : :
552 : : // Calculate Rear Load early for learning (v0.7.164)
553 : 15836 : double calc_load_rl = upsampled_data->mWheel[2].mTireLoad;
554 : 15836 : double calc_load_rr = upsampled_data->mWheel[3].mTireLoad;
555 [ + + ]: 15836 : if (ctx.frame_warn_load) {
556 [ + - ]: 1490 : calc_load_rl = approximate_rear_load(upsampled_data->mWheel[2]);
557 [ + - ]: 1490 : calc_load_rr = approximate_rear_load(upsampled_data->mWheel[3]);
558 : : }
559 : 15836 : ctx.avg_rear_load = (calc_load_rl + calc_load_rr) / DUAL_DIVISOR;
560 : :
561 : : // ALWAYS learn static load reference (used by Longitudinal Load, Bottoming, and Normalization).
562 : : // v0.7.164: Expanded to include rear load for context-aware oversteer effects.
563 [ + - ]: 15836 : update_static_load_reference(ctx.avg_front_load, ctx.avg_rear_load, ctx.car_speed, ctx.dt);
564 : :
565 : : // Peak Hold Logic
566 [ + + + + ]: 15836 : if (m_auto_load_normalization_enabled && !seeded) {
567 [ + + ]: 205 : if (ctx.avg_front_load > m_auto_peak_front_load) {
568 : 53 : m_auto_peak_front_load = ctx.avg_front_load; // Fast Attack
569 : : } else {
570 : 152 : m_auto_peak_front_load -= (LOAD_DECAY_RATE * ctx.dt); // Slow Decay (~100N/s)
571 : : }
572 : : }
573 : 15836 : m_auto_peak_front_load = (std::max)(LOAD_SAFETY_FLOOR, m_auto_peak_front_load); // Safety Floor
574 : :
575 : : // Load Factors (Stage 3: Giannoulis Soft-Knee Compression)
576 : : // 1. Calculate raw load multiplier based on static weight
577 : : // Safety clamp: Ensure load factor is non-negative even with telemetry noise
578 : 15836 : double x = (std::max)(0.0, ctx.avg_front_load / m_static_front_load);
579 : :
580 : : // 2. Giannoulis Soft-Knee Parameters
581 : 15836 : double T = COMPRESSION_KNEE_THRESHOLD; // Threshold (Start compressing at 1.5x static weight)
582 : 15836 : double W = COMPRESSION_KNEE_WIDTH; // Knee Width (Transition from 1.25x to 1.75x)
583 : 15836 : double R = COMPRESSION_RATIO; // Compression Ratio (4:1 above the knee)
584 : :
585 : 15836 : double lower_bound = T - (W / DUAL_DIVISOR);
586 : 15836 : double upper_bound = T + (W / DUAL_DIVISOR);
587 : 15836 : double compressed_load_factor = x;
588 : :
589 : : // 3. Apply Compression
590 [ + + ]: 15836 : if (x > upper_bound) {
591 : : // Linear compressed region
592 : 1171 : compressed_load_factor = T + ((x - T) / R);
593 [ + + ]: 14665 : } else if (x > lower_bound) {
594 : : // Quadratic soft-knee transition
595 : 2882 : double diff = x - lower_bound;
596 : 2882 : compressed_load_factor = x + (((1.0 / R) - 1.0) * (diff * diff)) / (DUAL_DIVISOR * W);
597 : : }
598 : :
599 : : // 4. EMA Smoothing on the vibration multiplier (100ms time constant)
600 : 15836 : double alpha_vibration = ctx.dt / (VIBRATION_EMA_TAU + ctx.dt);
601 : 15836 : m_smoothed_vibration_mult += alpha_vibration * (compressed_load_factor - m_smoothed_vibration_mult);
602 : :
603 : : // 5. Apply to context with user caps
604 : 15836 : double texture_safe_max = (std::min)(USER_CAP_MAX, (double)m_texture_load_cap);
605 : 15836 : ctx.texture_load_factor = (std::min)(texture_safe_max, m_smoothed_vibration_mult);
606 : :
607 : 15836 : double brake_safe_max = (std::min)(USER_CAP_MAX, (double)m_brake_load_cap);
608 : 15836 : ctx.brake_load_factor = (std::min)(brake_safe_max, m_smoothed_vibration_mult);
609 : :
610 : : // Hardware Scaling Safeties
611 : 15836 : double wheelbase_max_safe = (double)m_wheelbase_max_nm;
612 [ + + ]: 15836 : if (wheelbase_max_safe < 1.0) wheelbase_max_safe = 1.0;
613 : :
614 : : // Speed Gate - v0.7.2 Smoothstep S-curve
615 : 31672 : ctx.speed_gate = smoothstep(
616 : 15836 : (double)m_speed_gate_lower,
617 [ + - ]: 15836 : (double)m_speed_gate_upper,
618 : : ctx.car_speed
619 : : );
620 : :
621 : : // --- 5. EFFECT CALCULATIONS ---
622 : :
623 : : // A. Understeer (Base Torque + Grip Loss)
624 : :
625 : : // Grip Estimation (v0.4.5 FIX)
626 : 15836 : GripResult front_grip_res = calculate_axle_grip(fl, fr, ctx.avg_front_load, m_warned_grip,
627 : 15836 : m_prev_slip_angle[0], m_prev_slip_angle[1],
628 : 15836 : m_prev_load[0], m_prev_load[1], // NEW
629 [ + - ]: 15836 : ctx.car_speed, ctx.dt, data->mVehicleName, data, true /* is_front */);
630 : 15836 : ctx.avg_front_grip = front_grip_res.value;
631 : 15836 : m_grip_diag.front_original = front_grip_res.original;
632 : 15836 : m_grip_diag.front_approximated = front_grip_res.approximated;
633 : 15836 : m_grip_diag.front_slip_angle = front_grip_res.slip_angle;
634 [ + + ]: 15836 : if (front_grip_res.approximated) ctx.frame_warn_grip = true;
635 : :
636 : : // 2. Signal Conditioning (Smoothing, Notch Filters)
637 [ + - ]: 15836 : double game_force_proc = apply_signal_conditioning(raw_torque_input, upsampled_data, ctx);
638 : :
639 : : // Base Steering Force (Issue #178)
640 : 15836 : double base_input = game_force_proc;
641 : :
642 : : // --- REWRITTEN: Gamma-Shaped Grip Modulation ---
643 : 15836 : double raw_loss = std::clamp(1.0 - ctx.avg_front_grip, 0.0, 1.0);
644 : :
645 : : // Apply Gamma curve (pow)
646 : 15836 : double shaped_loss = std::pow(raw_loss, (double)m_understeer_gamma);
647 : :
648 : 15836 : double grip_loss = shaped_loss * m_understeer_effect;
649 : 15836 : ctx.grip_factor = (std::max)(0.0, 1.0 - grip_loss);
650 : :
651 : : // v0.7.63: Passthrough Logic for Direct Torque (TIC mode)
652 [ + + ]: 15836 : double grip_factor_applied = m_torque_passthrough ? 1.0 : ctx.grip_factor;
653 : :
654 : : // v0.7.46: Longitudinal Load logic (#301)
655 : : // if (m_auto_load_normalization_enabled) {
656 : : // update_static_load_reference(ctx.avg_front_load, ctx.car_speed, ctx.dt);
657 : : // }
658 : 15836 : double long_load_factor = 1.0;
659 : :
660 : : // Apply if enabled (Uses chassis G-force, completely immune to aero and missing telemetry)
661 [ + + ]: 15836 : if (m_long_load_effect > 0.0) {
662 : : // Use Derived Longitudinal Acceleration (Z-axis) to isolate weight transfer.
663 : : // LMU Coordinate System: +Z is rearward (deceleration/braking). -Z is forward (acceleration).
664 : : // Normalize: 1G braking = +1.0, 1G acceleration = -1.0
665 : 50 : double long_g = m_accel_z_smoothed / GRAVITY_MS2;
666 : :
667 : : // Domain Scaling: We want to capture up to 5G of dynamic range for high-downforce cars.
668 : 50 : const double MAX_G_RANGE = 5.0;
669 : 50 : double long_load_norm = std::clamp(long_g, -MAX_G_RANGE, MAX_G_RANGE);
670 : :
671 [ + + ]: 50 : if (m_long_load_transform != LoadTransform::LINEAR) {
672 : : // 1. Map the [-5.0, 5.0] range into the [-1.0, 1.0] domain required by the polynomials
673 : 12 : double x = long_load_norm / MAX_G_RANGE;
674 : :
675 : : // 2. Apply the mathematical transformation safely
676 [ + + + - ]: 12 : switch (m_long_load_transform) {
677 : 8 : case LoadTransform::CUBIC: x = apply_load_transform_cubic(x); break;
678 : 2 : case LoadTransform::QUADRATIC: x = apply_load_transform_quadratic(x); break;
679 : 2 : case LoadTransform::HERMITE: x = apply_load_transform_hermite(x); break;
680 : 0 : default: break;
681 : : }
682 : :
683 : : // 3. Map the result back to the [-5.0, 5.0] dynamic range
684 : 12 : long_load_norm = x * MAX_G_RANGE;
685 : : }
686 : :
687 : : // Blend: 1.0 + (Ratio * Gain)
688 : 50 : long_load_factor = 1.0 + long_load_norm * (double)m_long_load_effect;
689 : 50 : long_load_factor = std::clamp(long_load_factor, LONG_LOAD_MIN, LONG_LOAD_MAX);
690 : : }
691 : :
692 : : // Apply Smoothing to Longitudinal Load (v0.7.47)
693 : 15836 : double dw_alpha = ctx.dt / ((double)m_long_load_smoothing + ctx.dt + EPSILON_DIV);
694 : 15836 : dw_alpha = (std::max)(0.0, (std::min)(1.0, dw_alpha));
695 : 15836 : m_long_load_smoothed += dw_alpha * (long_load_factor - m_long_load_smoothed);
696 : 15836 : long_load_factor = m_long_load_smoothed;
697 : :
698 : : // v0.7.63: Final factor application
699 [ + + ]: 15836 : double dw_factor_applied = m_torque_passthrough ? 1.0 : long_load_factor;
700 : :
701 [ + + ]: 15836 : double gain_to_apply = (m_torque_source == 1) ? (double)m_ingame_ffb_gain : (double)m_steering_shaft_gain;
702 : :
703 : : // Formula Refactor (#301): Longitudinal Load MUST remain a multiplier to maintain
704 : : // physical aligning torque correctness (zero torque in straight line despite weight shift).
705 : 15836 : double base_steer_force = (base_input * gain_to_apply) * grip_factor_applied;
706 : 15836 : double output_force = base_steer_force * dw_factor_applied;
707 : :
708 : : // Capture isolated force component for diagnostics ONLY
709 : 15836 : ctx.long_load_force = base_steer_force * (dw_factor_applied - 1.0);
710 : :
711 : 15836 : output_force *= ctx.speed_gate;
712 : 15836 : ctx.long_load_force *= ctx.speed_gate;
713 : :
714 : : // B. SoP Lateral (Oversteer)
715 [ + - ]: 15836 : calculate_sop_lateral(upsampled_data, ctx);
716 : :
717 : : // C. Gyro Damping
718 [ + - ]: 15836 : calculate_gyro_damping(upsampled_data, ctx);
719 : :
720 : : // D. Effects
721 : 15836 : calculate_abs_pulse(upsampled_data, ctx);
722 [ + - ]: 15836 : calculate_lockup_vibration(upsampled_data, ctx);
723 [ + - ]: 15836 : calculate_wheel_spin(upsampled_data, ctx);
724 : 15836 : calculate_slide_texture(upsampled_data, ctx);
725 : 15836 : calculate_road_texture(upsampled_data, ctx);
726 [ + - ]: 15836 : calculate_suspension_bottoming(upsampled_data, ctx);
727 [ + - ]: 15836 : calculate_soft_lock(upsampled_data, ctx);
728 : :
729 : : // v0.7.78 FIX: Support stationary/garage soft lock (Issue #184)
730 : : // If not allowed (e.g. in garage or AI driving), mute all forces EXCEPT Soft Lock.
731 [ + + ]: 15836 : if (!allowed) {
732 : 121 : output_force = 0.0;
733 : 121 : ctx.sop_base_force = 0.0;
734 : 121 : ctx.rear_torque = 0.0;
735 : 121 : ctx.yaw_force = 0.0;
736 : 121 : ctx.gyro_force = 0.0;
737 : 121 : ctx.scrub_drag_force = 0.0;
738 : 121 : ctx.road_noise = 0.0;
739 : 121 : ctx.slide_noise = 0.0;
740 : 121 : ctx.spin_rumble = 0.0;
741 : 121 : ctx.bottoming_crunch = 0.0;
742 : 121 : ctx.abs_pulse_force = 0.0;
743 : 121 : ctx.lockup_rumble = 0.0;
744 : : // NOTE: ctx.soft_lock_force is PRESERVED.
745 : :
746 : : // Also zero out base_input for snapshot clarity
747 : 121 : base_input = 0.0;
748 : : }
749 : :
750 : : // --- 6. SUMMATION (Issue #152 & #153 Split Scaling) ---
751 : : // Split into Structural (Dynamic Normalization) and Texture (Absolute Nm) groups
752 : : // v0.7.77 FIX: Soft Lock moved to Texture group to maintain absolute Nm scaling (Issue #181)
753 : : // Note: long_load_force is ALREADY included in output_force as a multiplier.
754 : 15836 : double structural_sum = output_force + ctx.sop_base_force + ctx.lat_load_force + ctx.rear_torque + ctx.yaw_force + ctx.gyro_force +
755 : 15836 : ctx.scrub_drag_force;
756 : :
757 : : // Apply Torque Drop (from Spin/Traction Loss) only to structural physics
758 : 15836 : structural_sum *= ctx.gain_reduction_factor;
759 : :
760 : : // Apply Dynamic Normalization to structural forces
761 : 15836 : double norm_structural = structural_sum * m_smoothed_structural_mult;
762 : :
763 : : // Vibration Effects are calculated in absolute Nm
764 : : // v0.7.110: Apply m_vibration_gain to textures, but NOT to Soft Lock (Issue #206)
765 : : // v0.7.150: Decouple ABS and Lockup from global vibration gain (Issue #290)
766 : 15836 : double surface_vibs_nm = ctx.road_noise + ctx.slide_noise + ctx.spin_rumble + ctx.bottoming_crunch;
767 : 15836 : double critical_vibs_nm = ctx.abs_pulse_force + ctx.lockup_rumble;
768 : 15836 : double final_texture_nm = (surface_vibs_nm * (double)m_vibration_gain) + critical_vibs_nm + ctx.soft_lock_force;
769 : :
770 : : // --- 7. OUTPUT SCALING (Physical Target Model) ---
771 : : // Map structural to the target rim torque, then divide by wheelbase max to get DirectInput %
772 : 15836 : double di_structural = norm_structural * ((double)m_target_rim_nm / wheelbase_max_safe);
773 : :
774 : : // Map absolute texture Nm directly to the wheelbase max
775 : 15836 : double di_texture = final_texture_nm / wheelbase_max_safe;
776 : :
777 : 15836 : double norm_force = (di_structural + di_texture) * m_gain;
778 : :
779 : : // --- SAFETY MITIGATION (Stage 2) ---
780 [ + - ]: 15836 : norm_force = m_safety.ProcessSafetyMitigation(norm_force, ctx.dt);
781 : :
782 : : // Min Force
783 : : // v0.7.85 FIX: Bypass min_force if NOT allowed (e.g. in garage) unless soft lock is significant.
784 : : // This prevents the "grinding" feel from tiny residuals when FFB should be muted.
785 : : // v0.7.118: Tighten gate to require BOTH steering beyond limit AND significant force.
786 [ + + + + ]: 15969 : bool significant_soft_lock = (std::abs(upsampled_data->mUnfilteredSteering) > 1.0) &&
787 : 133 : (std::abs(ctx.soft_lock_force) > 0.5); // > 0.5 Nm
788 : :
789 [ + + + + ]: 15836 : if (allowed || significant_soft_lock) {
790 [ + + + + : 15825 : if (std::abs(norm_force) > FFB_EPSILON && std::abs(norm_force) < m_min_force) {
+ + ]
791 [ + + ]: 161 : double sign = (norm_force > 0.0) ? 1.0 : -1.0;
792 : 161 : norm_force = sign * m_min_force;
793 : : }
794 : : }
795 : :
796 [ + + ]: 15836 : if (m_invert_force) {
797 : 1191 : norm_force *= -1.0;
798 : : }
799 : :
800 : : // --- FULL TOCK DETECTION (Issue #303) ---
801 [ + - ]: 15836 : m_safety.UpdateTockDetection(upsampled_data->mUnfilteredSteering, norm_force, ctx.dt);
802 : :
803 : : // --- 8. STATE UPDATES (POST-CALC) ---
804 : : // CRITICAL: These updates must run UNCONDITIONALLY every frame to prevent
805 : : // stale state issues when effects are toggled on/off.
806 : : // v0.7.116: Use upsampled_data to ensure derivatives (current - prev) / dt
807 : : // are calculated correctly over the 400Hz 2.5ms interval.
808 [ + + ]: 79180 : for (int i = 0; i < 4; i++) {
809 : 63344 : m_prev_vert_deflection[i] = upsampled_data->mWheel[i].mVerticalTireDeflection;
810 : 63344 : m_prev_rotation[i] = upsampled_data->mWheel[i].mRotation;
811 : 63344 : m_prev_brake_pressure[i] = upsampled_data->mWheel[i].mBrakePressure;
812 : : }
813 : : // FIX (Issue #355): Update m_prev_susp_force at the END of the calculate_force loop
814 : : // to ensure correct dForce calculation for Method 1 next frame.
815 : 15836 : m_prev_susp_force[0] = upsampled_data->mWheel[0].mSuspForce;
816 : 15836 : m_prev_susp_force[1] = upsampled_data->mWheel[1].mSuspForce;
817 : :
818 : : // v0.6.36 FIX: Move m_prev_vert_accel to unconditional section
819 : : // Previously only updated inside calculate_road_texture when enabled.
820 : : // Now always updated to prevent stale data if other effects use it.
821 : : // v0.7.145 (Issue #278): Use upsampled derived acceleration for smoother Jerk calculation.
822 : 15836 : m_prev_vert_accel = upsampled_data->mLocalAccel.y;
823 : :
824 : : // --- 9. DERIVE LOGGABLE DIAGNOSTICS ---
825 : 15836 : float sm_range_rad = data->mPhysicalSteeringWheelRange;
826 : 15836 : float range_deg = sm_range_rad * (180.0f / (float)PI);
827 : :
828 : : // Fallback to REST API if enabled and SM range is invalid (Issue #221)
829 [ + + + - ]: 15836 : if (m_rest_api_enabled && sm_range_rad <= 0.0f) {
830 [ + - + - ]: 5 : float fallback = RestApiProvider::Get().GetFallbackRangeDeg();
831 [ + + ]: 5 : if (fallback > 0.0f) {
832 : 1 : range_deg = fallback;
833 : : }
834 : : }
835 : :
836 : 15836 : float steering_angle_deg = (float)data->mUnfilteredSteering * (range_deg / 2.0f);
837 : 15836 : float understeer_drop = (float)((base_input * m_steering_shaft_gain) * (1.0 - grip_factor_applied));
838 : 15836 : float oversteer_boost = (float)(ctx.sop_base_force - ctx.sop_unboosted_force); // Exact boost amount
839 : :
840 : : // --- 10. SNAPSHOT ---
841 : : // This block captures the current state of the FFB Engine (inputs, outputs, intermediate calculations)
842 : : // into a thread-safe buffer. These snapshots are retrieved by the GUI layer (or other consumers)
843 : : // to visualize real-time telemetry graphs, FFB clipping, and effect contributions.
844 : : // It uses a mutex to protect the shared circular buffer.
845 : : {
846 : : FFBSnapshot snap;
847 : 15836 : snap.total_output = (float)norm_force;
848 : 15836 : snap.base_force = (float)base_input;
849 : 15836 : snap.sop_force = (float)ctx.sop_unboosted_force; // Use unboosted for snapshot
850 : 15836 : snap.lat_load_force = (float)ctx.lat_load_force;
851 : 15836 : snap.long_load_force = (float)ctx.long_load_force;
852 : 15836 : snap.understeer_drop = understeer_drop;
853 : 15836 : snap.oversteer_boost = oversteer_boost;
854 : :
855 : 15836 : snap.ffb_rear_torque = (float)ctx.rear_torque;
856 : 15836 : snap.ffb_scrub_drag = (float)ctx.scrub_drag_force;
857 : 15836 : snap.ffb_yaw_kick = (float)ctx.yaw_force;
858 : 15836 : snap.ffb_gyro_damping = (float)ctx.gyro_force;
859 : 15836 : snap.texture_road = (float)ctx.road_noise;
860 : 15836 : snap.texture_slide = (float)ctx.slide_noise;
861 : 15836 : snap.texture_lockup = (float)ctx.lockup_rumble;
862 : 15836 : snap.texture_spin = (float)ctx.spin_rumble;
863 : 15836 : snap.texture_bottoming = (float)ctx.bottoming_crunch;
864 : 15836 : snap.ffb_abs_pulse = (float)ctx.abs_pulse_force;
865 : 15836 : snap.ffb_soft_lock = (float)ctx.soft_lock_force;
866 : 15836 : snap.session_peak_torque = (float)m_session_peak_torque;
867 [ + + ]: 15836 : snap.clipping = (std::abs(norm_force) > (double)CLIPPING_THRESHOLD) ? 1.0f : 0.0f;
868 : :
869 : : // Physics
870 : 15836 : snap.calc_front_load = (float)ctx.avg_front_load;
871 : 15836 : snap.calc_rear_load = (float)ctx.avg_rear_load;
872 : 15836 : snap.calc_rear_lat_force = (float)ctx.calc_rear_lat_force;
873 : 15836 : snap.calc_front_grip = (float)ctx.avg_front_grip;
874 : 15836 : snap.calc_rear_grip = (float)ctx.avg_rear_grip;
875 : 15836 : snap.calc_front_slip_angle_smoothed = (float)m_grip_diag.front_slip_angle;
876 : 15836 : snap.calc_rear_slip_angle_smoothed = (float)m_grip_diag.rear_slip_angle;
877 : :
878 [ + - ]: 15836 : snap.raw_front_slip_angle = (float)calculate_raw_slip_angle_pair(fl, fr);
879 [ + - ]: 15836 : snap.raw_rear_slip_angle = (float)calculate_raw_slip_angle_pair(data->mWheel[2], data->mWheel[3]);
880 : :
881 : : // Telemetry
882 : 15836 : snap.steer_force = (float)raw_torque;
883 : 15836 : snap.raw_shaft_torque = (float)data->mSteeringShaftTorque;
884 : 15836 : snap.raw_gen_torque = (float)genFFBTorque;
885 : 15836 : snap.raw_input_steering = (float)data->mUnfilteredSteering;
886 : 15836 : snap.raw_front_tire_load = (float)raw_front_load;
887 : 15836 : snap.raw_front_grip_fract = (float)raw_front_grip;
888 : 15836 : snap.raw_rear_grip = (float)((data->mWheel[2].mGripFract + data->mWheel[3].mGripFract) / DUAL_DIVISOR);
889 : 15836 : snap.raw_front_susp_force = (float)((fl.mSuspForce + fr.mSuspForce) / DUAL_DIVISOR);
890 : 15836 : snap.raw_front_ride_height = (float)((std::min)(fl.mRideHeight, fr.mRideHeight));
891 : 15836 : snap.raw_rear_lat_force = (float)((data->mWheel[2].mLateralForce + data->mWheel[3].mLateralForce) / DUAL_DIVISOR);
892 : 15836 : snap.raw_car_speed = (float)ctx.car_speed_long;
893 : 15836 : snap.raw_input_throttle = (float)data->mUnfilteredThrottle;
894 : 15836 : snap.raw_input_brake = (float)data->mUnfilteredBrake;
895 : 15836 : snap.accel_x = (float)data->mLocalAccel.x;
896 : 15836 : snap.raw_front_lat_patch_vel = (float)((std::abs(fl.mLateralPatchVel) + std::abs(fr.mLateralPatchVel)) / DUAL_DIVISOR);
897 : 15836 : snap.raw_front_deflection = (float)((fl.mVerticalTireDeflection + fr.mVerticalTireDeflection) / DUAL_DIVISOR);
898 : 15836 : snap.raw_front_long_patch_vel = (float)((fl.mLongitudinalPatchVel + fr.mLongitudinalPatchVel) / DUAL_DIVISOR);
899 : 15836 : snap.raw_rear_lat_patch_vel = (float)((std::abs(data->mWheel[2].mLateralPatchVel) + std::abs(data->mWheel[3].mLateralPatchVel)) / DUAL_DIVISOR);
900 : 15836 : snap.raw_rear_long_patch_vel = (float)((data->mWheel[2].mLongitudinalPatchVel + data->mWheel[3].mLongitudinalPatchVel) / DUAL_DIVISOR);
901 : :
902 : 15836 : snap.steering_range_deg = range_deg;
903 : 15836 : snap.steering_angle_deg = steering_angle_deg;
904 : :
905 : 15836 : snap.warn_load = ctx.frame_warn_load;
906 [ + + + + ]: 15836 : snap.warn_grip = ctx.frame_warn_grip || ctx.frame_warn_rear_grip;
907 : 15836 : snap.warn_dt = ctx.frame_warn_dt;
908 : 15836 : snap.debug_freq = (float)m_debug_freq;
909 : 15836 : snap.tire_radius = (float)fl.mStaticUndeflectedRadius / 100.0f;
910 : 15836 : snap.slope_current = (float)m_slope_current; // v0.7.1: Slope detection diagnostic
911 : :
912 : 15836 : snap.ffb_rate = (float)m_ffb_rate;
913 : 15836 : snap.telemetry_rate = (float)m_telemetry_rate;
914 : 15836 : snap.hw_rate = (float)m_hw_rate;
915 : 15836 : snap.torque_rate = (float)m_torque_rate;
916 : 15836 : snap.gen_torque_rate = (float)m_gen_torque_rate;
917 : 15836 : snap.physics_rate = (float)m_physics_rate;
918 [ + - ]: 15836 : m_debug_buffer.Push(snap);
919 : : }
920 : :
921 : : // Telemetry Logging (v0.7.129: Augmented Binary & 400Hz)
922 [ + - + + ]: 15836 : if (AsyncLogger::Get().IsLogging()) {
923 : : LogFrame frame;
924 : 41 : frame.timestamp = upsampled_data->mElapsedTime;
925 : 41 : frame.delta_time = ctx.dt;
926 : :
927 : : // --- PROCESSED 400Hz DATA (Smooth) ---
928 : 41 : frame.speed = (float)ctx.car_speed;
929 : 41 : frame.lat_accel = (float)upsampled_data->mLocalAccel.x;
930 : 41 : frame.long_accel = (float)upsampled_data->mLocalAccel.z;
931 : 41 : frame.yaw_rate = (float)upsampled_data->mLocalRot.y;
932 : :
933 : 41 : frame.steering = (float)upsampled_data->mUnfilteredSteering;
934 : 41 : frame.throttle = (float)upsampled_data->mUnfilteredThrottle;
935 : 41 : frame.brake = (float)upsampled_data->mUnfilteredBrake;
936 : :
937 : : // --- RAW 100Hz GAME DATA (Step-function) ---
938 : 41 : frame.raw_steering = (float)data->mUnfilteredSteering;
939 : 41 : frame.raw_throttle = (float)data->mUnfilteredThrottle;
940 : 41 : frame.raw_brake = (float)data->mUnfilteredBrake;
941 : 41 : frame.raw_lat_accel = (float)data->mLocalAccel.x;
942 : 41 : frame.raw_long_accel = (float)data->mLocalAccel.z;
943 : 41 : frame.raw_game_yaw_accel = (float)data->mLocalRotAccel.y;
944 : 41 : frame.raw_game_shaft_torque = (float)data->mSteeringShaftTorque;
945 : 41 : frame.raw_game_gen_torque = (float)genFFBTorque;
946 : :
947 : 41 : frame.raw_load_fl = (float)data->mWheel[0].mTireLoad;
948 : 41 : frame.raw_load_fr = (float)data->mWheel[1].mTireLoad;
949 : 41 : frame.raw_load_rl = (float)data->mWheel[2].mTireLoad;
950 : 41 : frame.raw_load_rr = (float)data->mWheel[3].mTireLoad;
951 : :
952 : 41 : frame.raw_slip_vel_lat_fl = (float)data->mWheel[0].mLateralPatchVel;
953 : 41 : frame.raw_slip_vel_lat_fr = (float)data->mWheel[1].mLateralPatchVel;
954 : 41 : frame.raw_slip_vel_lat_rl = (float)data->mWheel[2].mLateralPatchVel;
955 : 41 : frame.raw_slip_vel_lat_rr = (float)data->mWheel[3].mLateralPatchVel;
956 : :
957 : 41 : frame.raw_slip_vel_long_fl = (float)data->mWheel[0].mLongitudinalPatchVel;
958 : 41 : frame.raw_slip_vel_long_fr = (float)data->mWheel[1].mLongitudinalPatchVel;
959 : 41 : frame.raw_slip_vel_long_rl = (float)data->mWheel[2].mLongitudinalPatchVel;
960 : 41 : frame.raw_slip_vel_long_rr = (float)data->mWheel[3].mLongitudinalPatchVel;
961 : :
962 : 41 : frame.raw_ride_height_fl = (float)data->mWheel[0].mRideHeight;
963 : 41 : frame.raw_ride_height_fr = (float)data->mWheel[1].mRideHeight;
964 : 41 : frame.raw_ride_height_rl = (float)data->mWheel[2].mRideHeight;
965 : 41 : frame.raw_ride_height_rr = (float)data->mWheel[3].mRideHeight;
966 : :
967 : 41 : frame.raw_susp_deflection_fl = (float)data->mWheel[0].mSuspensionDeflection;
968 : 41 : frame.raw_susp_deflection_fr = (float)data->mWheel[1].mSuspensionDeflection;
969 : 41 : frame.raw_susp_deflection_rl = (float)data->mWheel[2].mSuspensionDeflection;
970 : 41 : frame.raw_susp_deflection_rr = (float)data->mWheel[3].mSuspensionDeflection;
971 : :
972 : 41 : frame.raw_susp_force_fl = (float)data->mWheel[0].mSuspForce;
973 : 41 : frame.raw_susp_force_fr = (float)data->mWheel[1].mSuspForce;
974 : 41 : frame.raw_susp_force_rl = (float)data->mWheel[2].mSuspForce;
975 : 41 : frame.raw_susp_force_rr = (float)data->mWheel[3].mSuspForce;
976 : :
977 : 41 : frame.raw_brake_pressure_fl = (float)data->mWheel[0].mBrakePressure;
978 : 41 : frame.raw_brake_pressure_fr = (float)data->mWheel[1].mBrakePressure;
979 : 41 : frame.raw_brake_pressure_rl = (float)data->mWheel[2].mBrakePressure;
980 : 41 : frame.raw_brake_pressure_rr = (float)data->mWheel[3].mBrakePressure;
981 : :
982 : 41 : frame.raw_rotation_fl = (float)data->mWheel[0].mRotation;
983 : 41 : frame.raw_rotation_fr = (float)data->mWheel[1].mRotation;
984 : 41 : frame.raw_rotation_rl = (float)data->mWheel[2].mRotation;
985 : 41 : frame.raw_rotation_rr = (float)data->mWheel[3].mRotation;
986 : :
987 : : // --- ALGORITHM STATE (400Hz) ---
988 : 41 : frame.slip_angle_fl = (float)fl.mLateralPatchVel / (float)(std::max)(1.0, ctx.car_speed);
989 : 41 : frame.slip_angle_fr = (float)fr.mLateralPatchVel / (float)(std::max)(1.0, ctx.car_speed);
990 : 41 : frame.slip_angle_rl = (float)upsampled_data->mWheel[2].mLateralPatchVel / (float)(std::max)(1.0, ctx.car_speed);
991 : 41 : frame.slip_angle_rr = (float)upsampled_data->mWheel[3].mLateralPatchVel / (float)(std::max)(1.0, ctx.car_speed);
992 : :
993 [ + - ]: 41 : frame.slip_ratio_fl = (float)calculate_wheel_slip_ratio(fl);
994 [ + - ]: 41 : frame.slip_ratio_fr = (float)calculate_wheel_slip_ratio(fr);
995 [ + - ]: 41 : frame.slip_ratio_rl = (float)calculate_wheel_slip_ratio(upsampled_data->mWheel[2]);
996 [ + - ]: 41 : frame.slip_ratio_rr = (float)calculate_wheel_slip_ratio(upsampled_data->mWheel[3]);
997 : :
998 : 41 : frame.grip_fl = (float)fl.mGripFract;
999 : 41 : frame.grip_fr = (float)fr.mGripFract;
1000 : 41 : frame.grip_rl = (float)upsampled_data->mWheel[2].mGripFract;
1001 : 41 : frame.grip_rr = (float)upsampled_data->mWheel[3].mGripFract;
1002 : :
1003 : 41 : frame.load_fl = (float)fl.mTireLoad;
1004 : 41 : frame.load_fr = (float)fr.mTireLoad;
1005 : 41 : frame.load_rl = (float)upsampled_data->mWheel[2].mTireLoad;
1006 : 41 : frame.load_rr = (float)upsampled_data->mWheel[3].mTireLoad;
1007 : :
1008 : 41 : frame.ride_height_fl = (float)fl.mRideHeight;
1009 : 41 : frame.ride_height_fr = (float)fr.mRideHeight;
1010 : 41 : frame.ride_height_rl = (float)upsampled_data->mWheel[2].mRideHeight;
1011 : 41 : frame.ride_height_rr = (float)upsampled_data->mWheel[3].mRideHeight;
1012 : :
1013 : 41 : frame.susp_deflection_fl = (float)fl.mSuspensionDeflection;
1014 : 41 : frame.susp_deflection_fr = (float)fr.mSuspensionDeflection;
1015 : 41 : frame.susp_deflection_rl = (float)upsampled_data->mWheel[2].mSuspensionDeflection;
1016 : 41 : frame.susp_deflection_rr = (float)upsampled_data->mWheel[3].mSuspensionDeflection;
1017 : :
1018 : 41 : frame.calc_slip_angle_front = (float)m_grip_diag.front_slip_angle;
1019 : 41 : frame.calc_slip_angle_rear = (float)m_grip_diag.rear_slip_angle;
1020 : 41 : frame.calc_grip_front = (float)ctx.avg_front_grip;
1021 : 41 : frame.calc_grip_rear = (float)ctx.avg_rear_grip;
1022 : 41 : frame.grip_delta = (float)(ctx.avg_front_grip - ctx.avg_rear_grip);
1023 : 41 : frame.calc_rear_lat_force = (float)ctx.calc_rear_lat_force;
1024 : :
1025 : 41 : frame.smoothed_yaw_accel = (float)m_yaw_accel_smoothed;
1026 : 41 : frame.lat_load_norm = (float)m_sop_load_smoothed;
1027 : :
1028 : 41 : frame.dG_dt = (float)m_slope_dG_dt;
1029 : 41 : frame.dAlpha_dt = (float)m_slope_dAlpha_dt;
1030 : 41 : frame.slope_current = (float)m_slope_current;
1031 : 41 : frame.slope_raw_unclamped = (float)m_debug_slope_raw;
1032 : 41 : frame.slope_numerator = (float)m_debug_slope_num;
1033 : 41 : frame.slope_denominator = (float)m_debug_slope_den;
1034 : 41 : frame.hold_timer = (float)m_slope_hold_timer;
1035 : 41 : frame.input_slip_smoothed = (float)m_slope_slip_smoothed;
1036 : 41 : frame.slope_smoothed = (float)m_slope_smoothed_output;
1037 [ + - ]: 41 : frame.confidence = (float)calculate_slope_confidence(m_slope_dAlpha_dt);
1038 : :
1039 : 41 : frame.surface_type_fl = (float)fl.mSurfaceType;
1040 : 41 : frame.surface_type_fr = (float)fr.mSurfaceType;
1041 : 41 : frame.slope_torque = (float)m_slope_torque_current;
1042 : 41 : frame.slew_limited_g = (float)m_debug_lat_g_slew;
1043 : :
1044 : 41 : frame.session_peak_torque = (float)m_session_peak_torque;
1045 : 41 : frame.long_load_factor = (float)long_load_factor;
1046 : 41 : frame.structural_mult = (float)m_smoothed_structural_mult;
1047 : 41 : frame.vibration_mult = (float)m_smoothed_vibration_mult;
1048 : 41 : frame.steering_angle_deg = steering_angle_deg;
1049 : 41 : frame.steering_range_deg = range_deg;
1050 : 41 : frame.debug_freq = (float)m_debug_freq;
1051 : 41 : frame.tire_radius = (float)fl.mStaticUndeflectedRadius / 100.0f;
1052 : :
1053 : : // --- FFB COMPONENTS (400Hz) ---
1054 : 41 : frame.ffb_total = (float)norm_force;
1055 : 41 : frame.ffb_base = (float)base_input;
1056 : 41 : frame.ffb_understeer_drop = understeer_drop;
1057 : 41 : frame.ffb_oversteer_boost = oversteer_boost;
1058 : 41 : frame.ffb_sop = (float)ctx.sop_base_force;
1059 : 41 : frame.ffb_rear_torque = (float)ctx.rear_torque;
1060 : 41 : frame.ffb_scrub_drag = (float)ctx.scrub_drag_force;
1061 : 41 : frame.ffb_yaw_kick = (float)ctx.yaw_force;
1062 : 41 : frame.ffb_gyro_damping = (float)ctx.gyro_force;
1063 : 41 : frame.ffb_road_texture = (float)ctx.road_noise;
1064 : 41 : frame.ffb_slide_texture = (float)ctx.slide_noise;
1065 : 41 : frame.ffb_lockup_vibration = (float)ctx.lockup_rumble;
1066 : 41 : frame.ffb_spin_vibration = (float)ctx.spin_rumble;
1067 : 41 : frame.ffb_bottoming_crunch = (float)ctx.bottoming_crunch;
1068 : 41 : frame.ffb_abs_pulse = (float)ctx.abs_pulse_force;
1069 : 41 : frame.ffb_soft_lock = (float)ctx.soft_lock_force;
1070 : :
1071 : 41 : frame.extrapolated_yaw_accel = (float)upsampled_data->mLocalRotAccel.y;
1072 : :
1073 : : // Passive test: calculate yaw accel from velocity derivative
1074 : : // Note: mLocalRot.y is angular velocity (rad/s), so its derivative is angular acceleration (rad/s^2).
1075 : 41 : double current_yaw_rate = upsampled_data->mLocalRot.y;
1076 [ + + ]: 41 : if (!m_yaw_rate_log_seeded) {
1077 : 2 : m_prev_yaw_rate_log = current_yaw_rate;
1078 : 2 : m_yaw_rate_log_seeded = true;
1079 : : }
1080 [ + - ]: 41 : frame.derived_yaw_accel = (ctx.dt > 1e-6) ? (float)((current_yaw_rate - m_prev_yaw_rate_log) / ctx.dt) : 0.0f;
1081 : 41 : m_prev_yaw_rate_log = current_yaw_rate;
1082 : :
1083 : 41 : frame.ffb_shaft_torque = (float)upsampled_data->mSteeringShaftTorque;
1084 : 41 : frame.ffb_gen_torque = (float)genFFBTorque;
1085 : 41 : frame.ffb_grip_factor = (float)ctx.grip_factor;
1086 : 41 : frame.speed_gate = (float)ctx.speed_gate;
1087 : 41 : frame.front_load_peak_ref = (float)m_auto_peak_front_load;
1088 : :
1089 [ + - ]: 41 : frame.approx_load_fl = (float)approximate_load(fl);
1090 [ + - ]: 41 : frame.approx_load_fr = (float)approximate_load(fr);
1091 [ + - ]: 41 : frame.approx_load_rl = (float)approximate_rear_load(upsampled_data->mWheel[2]);
1092 [ + - ]: 41 : frame.approx_load_rr = (float)approximate_rear_load(upsampled_data->mWheel[3]);
1093 : :
1094 : : // --- SYSTEM (400Hz) ---
1095 : 41 : frame.physics_rate = (float)m_physics_rate;
1096 : 41 : frame.clipping = (uint8_t)(std::abs(norm_force) > CLIPPING_THRESHOLD);
1097 : 41 : frame.warn_bits = 0;
1098 [ - + ]: 41 : if (ctx.frame_warn_load) frame.warn_bits |= 0x01;
1099 [ - + - - ]: 41 : if (ctx.frame_warn_grip || ctx.frame_warn_rear_grip) frame.warn_bits |= 0x02;
1100 [ - + ]: 41 : if (ctx.frame_warn_dt) frame.warn_bits |= 0x04;
1101 : 41 : frame.marker = 0; // Handled inside Log()
1102 : :
1103 [ + - + - ]: 41 : AsyncLogger::Get().Log(frame);
1104 : : }
1105 : :
1106 : : // --- NEW: Final NaN catch-all ---
1107 [ - + ]: 15836 : if (!std::isfinite(norm_force)) {
1108 [ # # ]: 0 : if (data->mElapsedTime > m_last_math_nan_log_time + 5.0) {
1109 [ # # # # ]: 0 : Logger::Get().LogFile("[Diag] Final output force is NaN/Inf! Internal math instability detected. Muting FFB.");
1110 : 0 : m_last_math_nan_log_time = data->mElapsedTime;
1111 : : }
1112 : 0 : norm_force = 0.0;
1113 : : }
1114 : :
1115 : 15836 : return (std::max)(-1.0, (std::min)(1.0, norm_force));
1116 : 15845 : }
1117 : :
1118 : : // Helper: Calculate Seat-of-the-Pants (SoP) Lateral & Oversteer Boost
1119 : 15842 : void FFBEngine::calculate_sop_lateral(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1120 : : // 1. Raw Lateral G (Chassis-relative X)
1121 : : // Clamp to 5G to prevent numeric instability in crashes
1122 : 15842 : double raw_g = (std::max)(-G_LIMIT_5G * GRAVITY_MS2, (std::min)(G_LIMIT_5G * GRAVITY_MS2, data->mLocalAccel.x));
1123 : 15842 : double lat_g_accel = (raw_g / GRAVITY_MS2);
1124 : :
1125 : : // 2. Global Normalized Lateral Load Transfer (Chassis Roll) - Issue #306
1126 : 15842 : double fl_load = data->mWheel[0].mTireLoad;
1127 : 15842 : double fr_load = data->mWheel[1].mTireLoad;
1128 : 15842 : double rl_load = data->mWheel[2].mTireLoad;
1129 : 15842 : double rr_load = data->mWheel[3].mTireLoad;
1130 : :
1131 [ + + ]: 15842 : if (ctx.frame_warn_load) {
1132 [ + - ]: 1490 : fl_load = approximate_load(data->mWheel[0]);
1133 [ + - ]: 1490 : fr_load = approximate_load(data->mWheel[1]);
1134 [ + - ]: 1490 : rl_load = approximate_rear_load(data->mWheel[2]);
1135 [ + - ]: 1490 : rr_load = approximate_rear_load(data->mWheel[3]);
1136 : : }
1137 : :
1138 : 15842 : double left_load = fl_load + rl_load;
1139 : 15842 : double right_load = fr_load + rr_load;
1140 : 15842 : double total_load = left_load + right_load;
1141 : :
1142 : : // Issue #321: Use (Right - Left) for global roll feel to avoid inverted sensation and notchiness
1143 [ + + ]: 15842 : double lat_load_norm = (total_load > 1.0) ? (right_load - left_load) / total_load : 0.0;
1144 : :
1145 : : // Safety clamp before transformation
1146 : 15842 : lat_load_norm = std::clamp(lat_load_norm, -1.0, 1.0);
1147 : :
1148 : : // Apply Transformation (Issue #282)
1149 [ + + + + ]: 15842 : switch (m_lat_load_transform) {
1150 : 3 : case LoadTransform::CUBIC:
1151 : 3 : lat_load_norm = apply_load_transform_cubic(lat_load_norm);
1152 : 3 : break;
1153 : 3 : case LoadTransform::QUADRATIC:
1154 : 3 : lat_load_norm = apply_load_transform_quadratic(lat_load_norm);
1155 : 3 : break;
1156 : 3 : case LoadTransform::HERMITE:
1157 : 3 : lat_load_norm = apply_load_transform_hermite(lat_load_norm);
1158 : 3 : break;
1159 : 15833 : case LoadTransform::LINEAR:
1160 : : default:
1161 : 15833 : break;
1162 : : }
1163 : :
1164 : : // Smoothing: Map 0.0-1.0 slider to 0.1-0.0001s tau
1165 : 15842 : double smoothness = (double)m_sop_smoothing_factor;
1166 : 15842 : smoothness = (std::max)(0.0, (std::min)(SMOOTHNESS_LIMIT_0999, smoothness));
1167 : 15842 : double tau = smoothness * SOP_SMOOTHING_MAX_TAU;
1168 : 15842 : double alpha = ctx.dt / (tau + ctx.dt);
1169 : 15842 : alpha = (std::max)(MIN_LFM_ALPHA, (std::min)(1.0, alpha));
1170 : :
1171 : 15842 : m_sop_lat_g_smoothed += alpha * (lat_g_accel - m_sop_lat_g_smoothed);
1172 : 15842 : m_sop_load_smoothed += alpha * (lat_load_norm - m_sop_load_smoothed);
1173 : :
1174 : : // Base SoP Force (G-based only - Issue #282)
1175 : 15842 : double sop_base = (m_sop_lat_g_smoothed * (double)m_sop_effect) * (double)m_sop_scale;
1176 : 15842 : ctx.sop_unboosted_force = sop_base; // Store for snapshot
1177 : :
1178 : : // Independent Lateral Load Force (Issue #282)
1179 : 15842 : ctx.lat_load_force = (m_sop_load_smoothed * (double)m_lat_load_effect) * (double)m_sop_scale;
1180 : :
1181 : : // 2. Oversteer Boost (Grip Differential)
1182 : : // Calculate Rear Grip
1183 : 15842 : GripResult rear_grip_res = calculate_axle_grip(data->mWheel[2], data->mWheel[3], ctx.avg_front_load, m_warned_rear_grip,
1184 : 15842 : m_prev_slip_angle[2], m_prev_slip_angle[3],
1185 : 15842 : m_prev_load[2], m_prev_load[3], // NEW
1186 [ + - ]: 15842 : ctx.car_speed, ctx.dt, data->mVehicleName, data, false /* is_front */);
1187 : 15842 : ctx.avg_rear_grip = rear_grip_res.value;
1188 : 15842 : m_grip_diag.rear_original = rear_grip_res.original;
1189 : 15842 : m_grip_diag.rear_approximated = rear_grip_res.approximated;
1190 : 15842 : m_grip_diag.rear_slip_angle = rear_grip_res.slip_angle;
1191 [ + + ]: 15842 : if (rear_grip_res.approximated) ctx.frame_warn_rear_grip = true;
1192 : :
1193 [ + + ]: 15842 : if (!m_slope_detection_enabled) {
1194 : 13781 : double grip_delta = ctx.avg_front_grip - ctx.avg_rear_grip;
1195 [ + + ]: 13781 : if (grip_delta > 0.0) {
1196 : 1002 : sop_base *= (1.0 + (grip_delta * m_oversteer_boost * OVERSTEER_BOOST_MULT));
1197 : : }
1198 : : }
1199 : 15842 : ctx.sop_base_force = sop_base;
1200 : :
1201 : : // 3. Rear Aligning Torque (v0.4.9)
1202 : : // Load for rear wheels already calculated earlier for update_static_load_reference
1203 : :
1204 : : // Rear lateral force estimation: F = Alpha * k * TireLoad
1205 : 15842 : double rear_slip_angle = m_grip_diag.rear_slip_angle;
1206 : :
1207 : : // --- FIX 1: Physics Saturation (Always On) ---
1208 : : // Cap dynamic load to 1.5x static weight to prevent vertical kerb spikes
1209 : 15842 : double max_effective_load = m_static_rear_load * KERB_LOAD_CAP_MULT;
1210 : 15842 : double effective_rear_load = std::min(ctx.avg_rear_load, (std::max)(1.0, max_effective_load));
1211 : :
1212 : : // Soft-clip slip angle (Simulates Pneumatic Trail falloff)
1213 : : // Critical: Ensure division-by-zero protection if optimal slip angle is not yet latched
1214 : 15842 : double optimal_slip_ref = (std::max)(0.01f, m_optimal_slip_angle);
1215 : 15842 : double normalized_slip = rear_slip_angle / (optimal_slip_ref + 0.001);
1216 : 15842 : double effective_slip = optimal_slip_ref * std::tanh(normalized_slip);
1217 : :
1218 : : // --- FIX 2: Hybrid Kerb Strike Rejection (GUI Controlled) ---
1219 : 15842 : double kerb_attenuation = 1.0;
1220 : :
1221 [ + + ]: 15842 : if (m_kerb_strike_rejection > 0.0) {
1222 : : // A. Surface Type Detection (Works on ALL cars)
1223 [ + + - + ]: 5 : bool on_kerb = (data->mWheel[2].mSurfaceType == 5) || (data->mWheel[3].mSurfaceType == 5);
1224 : :
1225 : : // B. Suspension Velocity Detection (Works on unencrypted cars)
1226 : 5 : bool violent_bump = false;
1227 [ + - ]: 5 : if (m_missing_vert_deflection_frames <= MISSING_TELEMETRY_WARN_THRESHOLD) {
1228 : 5 : double susp_vel_rl = std::abs(data->mWheel[2].mVerticalTireDeflection - m_prev_vert_deflection[2]) / ctx.dt;
1229 : 5 : double susp_vel_rr = std::abs(data->mWheel[3].mVerticalTireDeflection - m_prev_vert_deflection[3]) / ctx.dt;
1230 : 5 : violent_bump = std::max(susp_vel_rl, susp_vel_rr) > KERB_DETECTION_THRESHOLD_M_S;
1231 : : }
1232 : :
1233 : : // Trigger the timer (Hold the attenuation for 100ms after leaving the kerb)
1234 [ + + + + ]: 5 : if (on_kerb || violent_bump) {
1235 : 3 : m_kerb_timer = KERB_HOLD_TIME_S;
1236 : : } else {
1237 : 2 : m_kerb_timer = std::max(0.0, m_kerb_timer - ctx.dt);
1238 : : }
1239 : :
1240 : : // Apply the attenuation
1241 [ + + ]: 5 : if (m_kerb_timer > 0.0) {
1242 : : // If slider is 1.0, attenuation drops to 0.0 (100% muted)
1243 : : // If slider is 0.5, attenuation drops to 0.5 (50% muted)
1244 : 4 : kerb_attenuation = 1.0 - (double)m_kerb_strike_rejection;
1245 : : }
1246 : : }
1247 : :
1248 : : // Calculate final force with the sanitized, zero-latency variables
1249 : 15842 : ctx.calc_rear_lat_force = effective_slip * effective_rear_load * REAR_TIRE_STIFFNESS_COEFFICIENT;
1250 : 15842 : ctx.calc_rear_lat_force = std::clamp(ctx.calc_rear_lat_force, -MAX_REAR_LATERAL_FORCE, MAX_REAR_LATERAL_FORCE);
1251 : :
1252 : : // Torque = Force * Aligning_Lever * Kerb_Attenuation
1253 : : // Note negative sign: Oversteer (Rear Slide) pushes wheel TOWARDS slip direction
1254 : 15842 : ctx.rear_torque = -ctx.calc_rear_lat_force * REAR_ALIGN_TORQUE_COEFFICIENT * m_rear_align_effect * kerb_attenuation;
1255 : :
1256 : : // 4. Yaw Kicks (Context-Aware Oversteer - Issue #322)
1257 : :
1258 : : // Shared leading indicators: Derive Yaw Acceleration from Yaw Rate (mLocalRot.y)
1259 : 15842 : double current_yaw_rate = data->mLocalRot.y;
1260 [ + + ]: 15842 : if (!m_yaw_rate_seeded) {
1261 : 319 : m_prev_yaw_rate = current_yaw_rate;
1262 : 319 : m_yaw_rate_seeded = true;
1263 : : }
1264 [ + + ]: 15842 : double derived_yaw_accel = (ctx.dt > 1e-6) ? (current_yaw_rate - m_prev_yaw_rate) / ctx.dt : 0.0;
1265 : 15842 : m_prev_yaw_rate = current_yaw_rate;
1266 : :
1267 : : // NEW: Fast smoothing for the base signal (15ms tau) to remove 400Hz noise before Gamma amplification
1268 : 15842 : double tau_fast = 0.015;
1269 : 15842 : double alpha_fast = ctx.dt / (tau_fast + ctx.dt);
1270 : 15842 : m_fast_yaw_accel_smoothed += alpha_fast * (derived_yaw_accel - m_fast_yaw_accel_smoothed);
1271 : :
1272 : : // Calculate Yaw Jerk (Rate of change of Yaw Acceleration) for transient shaping
1273 [ + + ]: 15842 : if (!m_yaw_accel_seeded) {
1274 : 310 : m_prev_fast_yaw_accel = m_fast_yaw_accel_smoothed;
1275 : 310 : m_yaw_accel_seeded = true;
1276 : : }
1277 [ + + ]: 15842 : double yaw_jerk = (ctx.dt > 1e-6) ? (m_fast_yaw_accel_smoothed - m_prev_fast_yaw_accel) / ctx.dt : 0.0;
1278 : 15842 : m_prev_fast_yaw_accel = m_fast_yaw_accel_smoothed;
1279 : :
1280 : : // Clamp raw jerk to prevent insane spikes from collisions/telemetry glitches
1281 : 15842 : yaw_jerk = std::clamp(yaw_jerk, -100.0, 100.0);
1282 : :
1283 : : // --- A. General Yaw Kick (Baseline Rotation Feel) ---
1284 : 15842 : double tau_yaw = (double)m_yaw_accel_smoothing;
1285 [ + + ]: 15842 : if (tau_yaw < MIN_TAU_S) tau_yaw = MIN_TAU_S;
1286 : 15842 : double alpha_yaw = ctx.dt / (tau_yaw + ctx.dt);
1287 : 15842 : m_yaw_accel_smoothed += alpha_yaw * (derived_yaw_accel - m_yaw_accel_smoothed);
1288 : :
1289 : 15842 : double general_yaw_force = 0.0;
1290 [ + + ]: 15842 : if (ctx.car_speed >= MIN_YAW_KICK_SPEED_MS) {
1291 [ + + ]: 14702 : if (std::abs(m_yaw_accel_smoothed) > (double)m_yaw_kick_threshold) {
1292 : 568 : double processed_yaw = m_yaw_accel_smoothed - std::copysign((double)m_yaw_kick_threshold, m_yaw_accel_smoothed);
1293 : 568 : general_yaw_force = -1.0 * processed_yaw * m_sop_yaw_gain * (double)BASE_NM_YAW_KICK;
1294 : : }
1295 : : }
1296 : :
1297 : : // --- B. Unloaded Yaw Kick (Braking / Lift-off) ---
1298 : 15842 : double unloaded_yaw_force = 0.0;
1299 [ + + + + ]: 15842 : if (ctx.car_speed >= MIN_YAW_KICK_SPEED_MS && m_unloaded_yaw_gain > 0.001f) {
1300 [ + - ]: 138 : double load_ratio = (m_static_rear_load > 1.0) ? ctx.avg_rear_load / m_static_rear_load : 1.0;
1301 : 138 : double rear_load_drop = (std::max)(0.0, 1.0 - load_ratio);
1302 : 138 : double raw_unloaded_vuln = (std::min)(1.0, rear_load_drop * (double)m_unloaded_yaw_sens);
1303 : :
1304 : : // ASYMMETRIC SMOOTHING: 2ms attack (instant), 50ms decay (prevents chatter)
1305 [ + + ]: 138 : double tau_unloaded = (raw_unloaded_vuln > m_unloaded_vulnerability_smoothed) ? 0.002 : 0.050;
1306 : 138 : m_unloaded_vulnerability_smoothed += (ctx.dt / (tau_unloaded + ctx.dt)) * (raw_unloaded_vuln - m_unloaded_vulnerability_smoothed);
1307 : :
1308 [ + + ]: 138 : if (m_unloaded_vulnerability_smoothed > 0.01) {
1309 : : // Attack Phase Gate: Only apply punch if jerk is amplifying the current acceleration
1310 : 86 : double punch_addition = 0.0;
1311 [ + + ]: 86 : if ((yaw_jerk * m_fast_yaw_accel_smoothed) > 0.0) {
1312 : 54 : punch_addition = std::clamp(yaw_jerk * (double)m_unloaded_yaw_punch, -10.0, 10.0);
1313 : : }
1314 : :
1315 : : // CRITICAL FIX: Use the 15ms smoothed yaw, NOT the raw derived yaw
1316 : 86 : double punchy_yaw = m_fast_yaw_accel_smoothed + punch_addition;
1317 : :
1318 [ + + ]: 86 : if (std::abs(punchy_yaw) > (double)m_unloaded_yaw_threshold) {
1319 : 54 : double processed_yaw = punchy_yaw - std::copysign((double)m_unloaded_yaw_threshold, punchy_yaw);
1320 [ + + ]: 54 : double sign = (processed_yaw >= 0.0) ? 1.0 : -1.0;
1321 : 54 : double yaw_norm = (std::min)(1.0, std::abs(processed_yaw) / 10.0);
1322 : 54 : double shaped_yaw = std::pow(yaw_norm, (double)m_unloaded_yaw_gamma) * 10.0 * sign;
1323 : 54 : unloaded_yaw_force = -1.0 * shaped_yaw * (double)m_unloaded_yaw_gain * (double)BASE_NM_YAW_KICK * m_unloaded_vulnerability_smoothed;
1324 : : }
1325 : : }
1326 : : }
1327 : :
1328 : : // --- C. Power Yaw Kick (Acceleration / Traction Loss) ---
1329 : 15842 : double power_yaw_force = 0.0;
1330 [ + + + + ]: 15842 : if (ctx.car_speed >= MIN_YAW_KICK_SPEED_MS && m_power_yaw_gain > 0.001f) {
1331 [ + - ]: 163 : double slip_rl = calculate_wheel_slip_ratio(data->mWheel[2]);
1332 [ + - ]: 163 : double slip_rr = calculate_wheel_slip_ratio(data->mWheel[3]);
1333 [ + - ]: 163 : double max_rear_spin = (std::max)({ 0.0, slip_rl, slip_rr });
1334 : :
1335 : 163 : double slip_start = (double)m_power_slip_threshold * 0.5;
1336 [ + - ]: 163 : double slip_vulnerability = inverse_lerp(slip_start, (double)m_power_slip_threshold, max_rear_spin);
1337 : 163 : double throttle = std::clamp((double)data->mUnfilteredThrottle, 0.0, 1.0);
1338 : 163 : double raw_power_vuln = slip_vulnerability * throttle;
1339 : :
1340 : : // ASYMMETRIC SMOOTHING: 2ms attack (instant), 50ms decay (prevents chatter)
1341 [ + + ]: 163 : double tau_power = (raw_power_vuln > m_power_vulnerability_smoothed) ? 0.002 : 0.050;
1342 : 163 : m_power_vulnerability_smoothed += (ctx.dt / (tau_power + ctx.dt)) * (raw_power_vuln - m_power_vulnerability_smoothed);
1343 : :
1344 [ + + ]: 163 : if (m_power_vulnerability_smoothed > 0.01) {
1345 : : // Attack Phase Gate: Only apply punch if jerk is amplifying the current acceleration
1346 : 159 : double punch_addition = 0.0;
1347 [ + + ]: 159 : if ((yaw_jerk * m_fast_yaw_accel_smoothed) > 0.0) {
1348 : 152 : punch_addition = std::clamp(yaw_jerk * (double)m_power_yaw_punch, -10.0, 10.0);
1349 : : }
1350 : :
1351 : : // CRITICAL FIX: Use the 15ms smoothed yaw, NOT the raw derived yaw
1352 : 159 : double punchy_yaw = m_fast_yaw_accel_smoothed + punch_addition;
1353 : :
1354 [ + + ]: 159 : if (std::abs(punchy_yaw) > (double)m_power_yaw_threshold) {
1355 : 154 : double processed_yaw = punchy_yaw - std::copysign((double)m_power_yaw_threshold, punchy_yaw);
1356 [ + - ]: 154 : double sign = (processed_yaw >= 0.0) ? 1.0 : -1.0;
1357 : 154 : double yaw_norm = (std::min)(1.0, std::abs(processed_yaw) / 10.0);
1358 : 154 : double shaped_yaw = std::pow(yaw_norm, (double)m_power_yaw_gamma) * 10.0 * sign;
1359 : 154 : power_yaw_force = -1.0 * shaped_yaw * (double)m_power_yaw_gain * (double)BASE_NM_YAW_KICK * m_power_vulnerability_smoothed;
1360 : : }
1361 : : }
1362 : : }
1363 : :
1364 : : // Blending Logic: Use sign-preserving max absolute value
1365 : 15842 : ctx.yaw_force = general_yaw_force;
1366 [ + + ]: 15842 : if (std::abs(unloaded_yaw_force) > std::abs(ctx.yaw_force)) ctx.yaw_force = unloaded_yaw_force;
1367 [ + + ]: 15842 : if (std::abs(power_yaw_force) > std::abs(ctx.yaw_force)) ctx.yaw_force = power_yaw_force;
1368 : :
1369 : : // Apply speed gate to all lateral effects
1370 : 15842 : ctx.sop_base_force *= ctx.speed_gate;
1371 : 15842 : ctx.lat_load_force *= ctx.speed_gate;
1372 : 15842 : ctx.rear_torque *= ctx.speed_gate;
1373 : 15842 : ctx.yaw_force *= ctx.speed_gate;
1374 : 15842 : }
1375 : :
1376 : : // Helper: Calculate Gyroscopic Damping (v0.4.17)
1377 : 15842 : void FFBEngine::calculate_gyro_damping(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1378 : : // 1. Calculate Steering Velocity (rad/s)
1379 : 15842 : float range = data->mPhysicalSteeringWheelRange;
1380 : :
1381 : : // Fallback to REST API if enabled and SM range is invalid (Issue #221)
1382 [ + + + - ]: 15842 : if (m_rest_api_enabled && range <= 0.0f) {
1383 : 5 : float fallback_deg = RestApiProvider::Get().GetFallbackRangeDeg();
1384 [ + + ]: 5 : if (fallback_deg > 0.0f) {
1385 : 1 : range = fallback_deg * ((float)PI / 180.0f);
1386 : : }
1387 : : }
1388 : :
1389 [ + + ]: 15842 : if (range <= 0.0f) range = (float)DEFAULT_STEERING_RANGE_RAD;
1390 : 15842 : double steer_angle = data->mUnfilteredSteering * (range / DUAL_DIVISOR);
1391 : 15842 : double steer_vel = (steer_angle - m_prev_steering_angle) / ctx.dt;
1392 : 15842 : m_prev_steering_angle = steer_angle;
1393 : :
1394 : : // 2. Alpha Smoothing
1395 : 15842 : double tau_gyro = (double)m_gyro_smoothing;
1396 [ + + ]: 15842 : if (tau_gyro < MIN_TAU_S) tau_gyro = MIN_TAU_S;
1397 : 15842 : double alpha_gyro = ctx.dt / (tau_gyro + ctx.dt);
1398 : 15842 : m_steering_velocity_smoothed += alpha_gyro * (steer_vel - m_steering_velocity_smoothed);
1399 : :
1400 : : // 3. Force = -Vel * Gain * Speed_Scaling
1401 : : // Speed scaling: Gyro effect increases with wheel RPM (car speed)
1402 : 15842 : ctx.gyro_force = -1.0 * m_steering_velocity_smoothed * m_gyro_gain * (ctx.car_speed / GYRO_SPEED_SCALE);
1403 : 15842 : }
1404 : :
1405 : : // Helper: Calculate ABS Pulse (v0.7.53)
1406 : 16844 : void FFBEngine::calculate_abs_pulse(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1407 [ + + ]: 16844 : if (!m_abs_pulse_enabled) return;
1408 : :
1409 : 1077 : bool abs_active = false;
1410 [ + + ]: 1329 : for (int i = 0; i < 4; i++) {
1411 : : // Detection: Sudden pressure oscillation + high brake pedal
1412 : 1266 : double pressure_delta = (data->mWheel[i].mBrakePressure - m_prev_brake_pressure[i]) / ctx.dt;
1413 [ + + + + : 1266 : if (data->mUnfilteredBrake > ABS_PEDAL_THRESHOLD && std::abs(pressure_delta) > ABS_PRESSURE_RATE_THRESHOLD) {
+ + ]
1414 : 1014 : abs_active = true;
1415 : 1014 : break;
1416 : : }
1417 : : }
1418 : :
1419 [ + + ]: 1077 : if (abs_active) {
1420 : : // Generate sine pulse
1421 : 1014 : m_abs_phase += (double)m_abs_freq_hz * ctx.dt * TWO_PI;
1422 : 1014 : m_abs_phase = std::fmod(m_abs_phase, TWO_PI);
1423 : 1014 : ctx.abs_pulse_force = (double)(std::sin(m_abs_phase) * m_abs_gain * ABS_PULSE_MAGNITUDE_SCALER * ctx.speed_gate);
1424 : : }
1425 : : }
1426 : :
1427 : : // Helper: Calculate Lockup Vibration (v0.4.36 - REWRITTEN as dedicated method)
1428 : 15842 : void FFBEngine::calculate_lockup_vibration(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1429 [ + + ]: 15842 : if (!m_lockup_enabled) return;
1430 : :
1431 : 1724 : double worst_severity = 0.0;
1432 : 1724 : double chosen_freq_multiplier = 1.0;
1433 : 1724 : double chosen_pressure_factor = 0.0;
1434 : :
1435 : : // Calculate reference slip for front wheels (v0.4.38)
1436 [ + - ]: 1724 : double slip_fl = calculate_wheel_slip_ratio(data->mWheel[0]);
1437 [ + - ]: 1724 : double slip_fr = calculate_wheel_slip_ratio(data->mWheel[1]);
1438 : 1724 : double worst_front = (std::min)(slip_fl, slip_fr);
1439 : :
1440 [ + + ]: 8620 : for (int i = 0; i < 4; i++) {
1441 : 6896 : const auto& w = data->mWheel[i];
1442 [ + - ]: 6896 : double slip = calculate_wheel_slip_ratio(w);
1443 : 6896 : double slip_abs = std::abs(slip);
1444 : :
1445 : : // 1. Predictive Lockup (v0.4.38)
1446 : : // Detects rapidly decelerating wheels BEFORE they reach full lock
1447 : 6896 : double wheel_accel = (w.mRotation - m_prev_rotation[i]) / ctx.dt;
1448 : 6896 : double radius = (double)w.mStaticUndeflectedRadius / UNIT_CM_TO_M;
1449 [ + + ]: 6896 : if (radius < RADIUS_FALLBACK_MIN_M) radius = RADIUS_FALLBACK_DEFAULT_M;
1450 : 6896 : double car_dec_ang = -std::abs(data->mLocalAccel.z / radius);
1451 : :
1452 : : // Signal Quality Check (Reject surface bumps)
1453 : 6896 : double susp_vel = std::abs(w.mVerticalTireDeflection - m_prev_vert_deflection[i]) / ctx.dt;
1454 : 6896 : bool is_bumpy = (susp_vel > (double)m_lockup_bump_reject);
1455 : :
1456 : : // Pre-conditions
1457 : 6896 : bool brake_active = (data->mUnfilteredBrake > PREDICTION_BRAKE_THRESHOLD);
1458 : :
1459 : : // FIX (Issue #355): Use actual tire load (or accurate approximation) for grounding check.
1460 : : // mSuspForce is pushrod load, which can be zero/negative when airborne.
1461 [ + + + - ]: 6896 : double current_load = ctx.frame_warn_load ? approximate_load(w) : w.mTireLoad;
1462 : 6896 : bool is_grounded = (current_load > PREDICTION_LOAD_THRESHOLD);
1463 : :
1464 : 6896 : double start_threshold = (double)m_lockup_start_pct / PERCENT_TO_DECIMAL;
1465 : 6896 : double full_threshold = (double)m_lockup_full_pct / PERCENT_TO_DECIMAL;
1466 : 6896 : double trigger_threshold = full_threshold;
1467 : :
1468 [ + + + + : 6896 : if (brake_active && is_grounded && !is_bumpy) {
+ + ]
1469 : : // Predictive Trigger: Wheel decelerating significantly faster than chassis
1470 : 629 : double sensitivity_threshold = -1.0 * (double)m_lockup_prediction_sens;
1471 [ + + + - ]: 629 : if (wheel_accel < car_dec_ang * LOCKUP_ACCEL_MARGIN && wheel_accel < sensitivity_threshold) {
1472 : 5 : trigger_threshold = start_threshold; // Ease into effect earlier
1473 : : }
1474 : : }
1475 : :
1476 : : // 2. Intensity Calculation
1477 [ + + ]: 6896 : if (slip_abs > trigger_threshold) {
1478 : 557 : double window = full_threshold - start_threshold;
1479 [ + + ]: 557 : if (window < MIN_SLIP_WINDOW) window = MIN_SLIP_WINDOW;
1480 : :
1481 : 557 : double normalized = (slip_abs - start_threshold) / window;
1482 : 557 : double severity = (std::min)(1.0, (std::max)(0.0, normalized));
1483 : :
1484 : : // Apply gamma for curve control
1485 : 557 : severity = std::pow(severity, (double)m_lockup_gamma);
1486 : :
1487 : : // Frequency calculation
1488 : 557 : double freq_mult = 1.0;
1489 [ + + ]: 557 : if (i >= 2) {
1490 : : // v0.4.38: Rear wheels use a different frequency to distinguish front/rear lockup
1491 [ + + ]: 300 : if (slip < (worst_front - AXLE_DIFF_HYSTERESIS)) {
1492 : 2 : freq_mult = LOCKUP_FREQ_MULTIPLIER_REAR;
1493 : : }
1494 : : }
1495 : :
1496 : : // Pressure weighting (v0.4.38)
1497 : 557 : double pressure_factor = w.mBrakePressure;
1498 [ + + + + ]: 557 : if (pressure_factor < LOW_PRESSURE_LOCKUP_THRESHOLD && slip_abs > LOW_PRESSURE_LOCKUP_FIX) pressure_factor = LOW_PRESSURE_LOCKUP_FIX; // Catch low-pressure lockups
1499 : :
1500 [ + + ]: 557 : if (severity > worst_severity) {
1501 : 299 : worst_severity = severity;
1502 : 299 : chosen_freq_multiplier = freq_mult;
1503 : 299 : chosen_pressure_factor = pressure_factor;
1504 : : }
1505 : : }
1506 : : }
1507 : :
1508 : : // 3. Vibration Synthesis
1509 [ + + ]: 1724 : if (worst_severity > 0.0) {
1510 : 299 : double base_freq = LOCKUP_BASE_FREQ + (ctx.car_speed * LOCKUP_FREQ_SPEED_MULT);
1511 : 299 : double final_freq = base_freq * chosen_freq_multiplier * (double)m_lockup_freq_scale;
1512 : :
1513 : 299 : m_lockup_phase += final_freq * ctx.dt * TWO_PI;
1514 : 299 : m_lockup_phase = std::fmod(m_lockup_phase, TWO_PI);
1515 : :
1516 : 299 : double amp = worst_severity * chosen_pressure_factor * m_lockup_gain * (double)BASE_NM_LOCKUP_VIBRATION * ctx.brake_load_factor;
1517 : :
1518 : : // v0.4.38: Boost rear lockup volume
1519 [ + + ]: 299 : if (chosen_freq_multiplier < 1.0) amp *= (double)m_lockup_rear_boost;
1520 : :
1521 : 299 : ctx.lockup_rumble = std::sin(m_lockup_phase) * amp * ctx.speed_gate;
1522 : : }
1523 : : }
1524 : :
1525 : : // Helper: Calculate Wheel Spin Vibration (v0.6.36)
1526 : 15838 : void FFBEngine::calculate_wheel_spin(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1527 [ + + + + ]: 15838 : if (m_spin_enabled && data->mUnfilteredThrottle > SPIN_THROTTLE_THRESHOLD) {
1528 [ + - ]: 226 : double slip_rl = calculate_wheel_slip_ratio(data->mWheel[2]);
1529 [ + - ]: 226 : double slip_rr = calculate_wheel_slip_ratio(data->mWheel[3]);
1530 : 226 : double max_slip = (std::max)(slip_rl, slip_rr);
1531 : :
1532 [ + + ]: 226 : if (max_slip > SPIN_SLIP_THRESHOLD) {
1533 : 67 : double severity = (max_slip - SPIN_SLIP_THRESHOLD) / SPIN_SEVERITY_RANGE;
1534 : 67 : severity = (std::min)(1.0, severity);
1535 : :
1536 : : // Attenuate primary torque when spinning (Torque Drop)
1537 : : // v0.6.43: Blunted effect (0.6 multiplier) to prevent complete loss of feel
1538 : 67 : ctx.gain_reduction_factor = (1.0 - (severity * m_spin_gain * SPIN_TORQUE_DROP_FACTOR));
1539 : :
1540 : : // Generate vibration based on spin velocity (RPM delta)
1541 : 67 : double slip_speed_ms = ctx.car_speed * max_slip;
1542 : 67 : double freq = (SPIN_BASE_FREQ + (slip_speed_ms * SPIN_FREQ_SLIP_MULT)) * (double)m_spin_freq_scale;
1543 [ + + ]: 67 : if (freq > SPIN_MAX_FREQ) freq = SPIN_MAX_FREQ; // Human sensory limit for gross vibration
1544 : :
1545 : 67 : m_spin_phase += freq * ctx.dt * TWO_PI;
1546 : 67 : m_spin_phase = std::fmod(m_spin_phase, TWO_PI);
1547 : :
1548 : : // Issue #306: Scale vibration amplitude by rear load factor
1549 : 67 : double current_rear_load = (data->mWheel[2].mTireLoad + data->mWheel[3].mTireLoad) / DUAL_DIVISOR;
1550 : 67 : double rear_load_factor = std::clamp(current_rear_load / (m_static_front_load + 1.0), 0.2, 2.0);
1551 : :
1552 : 67 : double amp = severity * m_spin_gain * (double)BASE_NM_SPIN_VIBRATION * rear_load_factor;
1553 : 67 : ctx.spin_rumble = std::sin(m_spin_phase) * amp;
1554 : : }
1555 : : }
1556 : 15838 : }
1557 : :
1558 : : // Helper: Calculate Slide Texture (Friction Vibration)
1559 : 15839 : void FFBEngine::calculate_slide_texture(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1560 [ + + ]: 15839 : if (!m_slide_texture_enabled) return;
1561 : :
1562 : : // Use average lateral patch velocity of front wheels
1563 : 1144 : double lat_vel_fl = std::abs(data->mWheel[0].mLateralPatchVel);
1564 : 1144 : double lat_vel_fr = std::abs(data->mWheel[1].mLateralPatchVel);
1565 : 1144 : double front_slip_avg = (lat_vel_fl + lat_vel_fr) / DUAL_DIVISOR;
1566 : :
1567 : : // Use average lateral patch velocity of rear wheels
1568 : 1144 : double lat_vel_rl = std::abs(data->mWheel[2].mLateralPatchVel);
1569 : 1144 : double lat_vel_rr = std::abs(data->mWheel[3].mLateralPatchVel);
1570 : 1144 : double rear_slip_avg = (lat_vel_rl + lat_vel_rr) / DUAL_DIVISOR;
1571 : :
1572 : : // Use the max slide velocity between axles
1573 : 1144 : double effective_slip_vel = (std::max)(front_slip_avg, rear_slip_avg);
1574 : :
1575 [ + + ]: 1144 : if (effective_slip_vel > SLIDE_VEL_THRESHOLD) {
1576 : : // High-frequency sawtooth noise for localized friction feel
1577 : 1130 : double base_freq = SLIDE_BASE_FREQ + (effective_slip_vel * SLIDE_FREQ_VEL_MULT);
1578 : 1130 : double freq = base_freq * (double)m_slide_freq_scale;
1579 : :
1580 [ + + ]: 1130 : if (freq > SLIDE_MAX_FREQ) freq = SLIDE_MAX_FREQ; // Hard clamp for hardware safety
1581 : :
1582 : 1130 : m_slide_phase += freq * ctx.dt * TWO_PI;
1583 : 1130 : m_slide_phase = std::fmod(m_slide_phase, TWO_PI);
1584 : :
1585 : : // Sawtooth generator (0 to 1 range across TWO_PI) -> (-1 to 1)
1586 : 1130 : double sawtooth = (m_slide_phase / TWO_PI) * SAWTOOTH_SCALE - SAWTOOTH_OFFSET;
1587 : :
1588 : : // Intensity scaling (Grip based)
1589 : 1130 : double grip_scale = (std::max)(0.0, 1.0 - ctx.avg_front_grip);
1590 : :
1591 : 1130 : ctx.slide_noise = sawtooth * m_slide_texture_gain * (double)BASE_NM_SLIDE_TEXTURE * ctx.texture_load_factor * grip_scale;
1592 : : }
1593 : : }
1594 : :
1595 : : // Helper: Calculate Road Texture & Scrub Drag
1596 : 15838 : void FFBEngine::calculate_road_texture(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1597 : : // 1. Scrub Drag (Longitudinal resistive force from lateral sliding)
1598 [ + + ]: 15838 : if (m_scrub_drag_gain > 0.0) {
1599 : 1108 : double avg_lat_vel = (data->mWheel[0].mLateralPatchVel + data->mWheel[1].mLateralPatchVel) / DUAL_DIVISOR;
1600 : 1108 : double abs_lat_vel = std::abs(avg_lat_vel);
1601 : :
1602 [ + + ]: 1108 : if (abs_lat_vel > SCRUB_VEL_THRESHOLD) {
1603 : 1107 : double fade = (std::min)(1.0, abs_lat_vel / SCRUB_FADE_RANGE); // Fade in over 0.5m/s
1604 [ + + ]: 1107 : double drag_dir = (avg_lat_vel > 0.0) ? -1.0 : 1.0;
1605 : : // Issue #306: Scale by load factor
1606 : 1107 : ctx.scrub_drag_force = drag_dir * m_scrub_drag_gain * (double)BASE_NM_SCRUB_DRAG * fade * ctx.texture_load_factor;
1607 : : }
1608 : : }
1609 : :
1610 [ + + ]: 15838 : if (!m_road_texture_enabled) return;
1611 : :
1612 : : // 2. Road Texture (Delta Deflection Method)
1613 : : // Measures the rate of change in tire vertical compression
1614 : 1706 : double delta_l = data->mWheel[0].mVerticalTireDeflection - m_prev_vert_deflection[0];
1615 : 1706 : double delta_r = data->mWheel[1].mVerticalTireDeflection - m_prev_vert_deflection[1];
1616 : :
1617 : : // Outlier rejection (crashes/jumps)
1618 : 1706 : delta_l = (std::max)(-DEFLECTION_DELTA_LIMIT, (std::min)(DEFLECTION_DELTA_LIMIT, delta_l));
1619 : 1706 : delta_r = (std::max)(-DEFLECTION_DELTA_LIMIT, (std::min)(DEFLECTION_DELTA_LIMIT, delta_r));
1620 : :
1621 : 1706 : double road_noise_val = 0.0;
1622 : :
1623 : : // FALLBACK (v0.6.36): If mVerticalTireDeflection is missing (Encrypted DLC),
1624 : : // use Chassis Vertical Acceleration delta as a secondary source.
1625 [ + + - + ]: 1706 : bool deflection_active = (std::abs(delta_l) > DEFLECTION_ACTIVE_THRESHOLD || std::abs(delta_r) > DEFLECTION_ACTIVE_THRESHOLD);
1626 : :
1627 [ + + + + ]: 1706 : if (deflection_active || ctx.car_speed < ROAD_TEXTURE_SPEED_THRESHOLD) {
1628 : 1024 : road_noise_val = (delta_l + delta_r) * DEFLECTION_NM_SCALE; // Scale to NM
1629 : : } else {
1630 : : // Fallback to vertical acceleration rate-of-change (jerk-like scaling)
1631 : 682 : double vert_accel = data->mLocalAccel.y;
1632 : 682 : double delta_accel = vert_accel - m_prev_vert_accel;
1633 : 682 : road_noise_val = delta_accel * ACCEL_ROAD_TEXTURE_SCALE * DEFLECTION_NM_SCALE; // Blend into similar range
1634 : : }
1635 : :
1636 : 1706 : ctx.road_noise = road_noise_val * m_road_texture_gain * ctx.texture_load_factor;
1637 : 1706 : ctx.road_noise *= ctx.speed_gate;
1638 : : }
1639 : :
1640 : 133 : void FFBEngine::ResetNormalization() {
1641 [ + - ]: 133 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
1642 : :
1643 : 133 : m_metadata.ResetWarnings(); // Issue #218: Reset warning on manual normalization reset
1644 : :
1645 : : // 1. Structural Normalization Reset (Stage 1)
1646 : : // If disabled, we return to the user's manual target.
1647 : : // If enabled, we reset to the target to restart the learning process.
1648 : 133 : m_session_peak_torque = (std::max)(1.0, (double)m_target_rim_nm);
1649 : 133 : m_smoothed_structural_mult = 1.0 / (m_session_peak_torque + EPSILON_DIV);
1650 : 133 : m_rolling_average_torque = m_session_peak_torque;
1651 : 133 : m_last_raw_torque = 0.0;
1652 : :
1653 : : // 2. Vibration Normalization Reset (Stage 3)
1654 : : // Always return to the class-default seed load.
1655 [ + - ]: 133 : ParsedVehicleClass vclass = ParseVehicleClass(m_metadata.GetCurrentClassName(), m_metadata.GetVehicleName());
1656 [ + - ]: 133 : m_auto_peak_front_load = GetDefaultLoadForClass(vclass);
1657 : :
1658 : : // Reset static load reference
1659 : 133 : m_static_front_load = m_auto_peak_front_load * 0.5;
1660 : 133 : m_static_rear_load = m_auto_peak_front_load * 0.5;
1661 : 133 : m_static_load_latched = false;
1662 : :
1663 : : // If we have a saved static load, restore it (v0.7.70 logic)
1664 : 133 : double saved_front_load = 0.0;
1665 : 133 : double saved_rear_load = 0.0;
1666 [ + - ]: 133 : std::string vName = m_metadata.GetVehicleName();
1667 : :
1668 [ + - + + ]: 133 : if (Config::GetSavedStaticLoad(vName, saved_front_load)) {
1669 : 60 : m_static_front_load = saved_front_load;
1670 : :
1671 [ + - + - : 60 : if (Config::GetSavedStaticLoad(vName + "_rear", saved_rear_load)) {
+ + ]
1672 : 53 : m_static_rear_load = saved_rear_load;
1673 : : } else {
1674 : 7 : m_static_rear_load = m_auto_peak_front_load * 0.5;
1675 : : }
1676 : :
1677 : 60 : m_static_load_latched = true;
1678 : : }
1679 : :
1680 : 133 : m_smoothed_vibration_mult = 1.0;
1681 : :
1682 [ + - + - ]: 133 : Logger::Get().LogFile("[FFB] Normalization state reset. Structural Peak: %.2f Nm | Load Peak: %.2f N",
1683 : : m_session_peak_torque, m_auto_peak_front_load);
1684 : 133 : }
1685 : :
1686 : : // Helper: Calculate Suspension Bottoming (v0.6.22)
1687 : : // NOTE: calculate_soft_lock has been moved to SteeringUtils.cpp.
1688 : 15848 : void FFBEngine::calculate_suspension_bottoming(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1689 [ + + ]: 15848 : if (!m_bottoming_enabled) return;
1690 : 1370 : bool triggered = false;
1691 : 1370 : double intensity = 0.0;
1692 : :
1693 : : // Method 0: Direct Ride Height Monitoring
1694 [ + + ]: 1370 : if (m_bottoming_method == 0) {
1695 : 1349 : double min_rh = (std::min)(data->mWheel[0].mRideHeight, data->mWheel[1].mRideHeight);
1696 [ + + + - ]: 1349 : if (min_rh < BOTTOMING_RH_THRESHOLD_M && min_rh > -1.0) { // < 2mm
1697 : 1348 : triggered = true;
1698 : 1348 : intensity = (BOTTOMING_RH_THRESHOLD_M - min_rh) / BOTTOMING_RH_THRESHOLD_M; // Map 2mm->0mm to 0.0->1.0
1699 : : }
1700 : : } else {
1701 : : // Method 1: Suspension Force Impulse (Rate of Change)
1702 : :
1703 : : // CRITICAL: mSuspForce is the pushrod load, not the wheel load.
1704 : : // We must multiply by the Motion Ratio to normalize the impulse back to the wheel.
1705 : : // Otherwise, prototypes (MR ~0.5) will trigger bottoming 2x as often as GTs (MR ~0.65)
1706 : : // for the exact same physical bump.
1707 [ + - ]: 21 : double mr = GetMotionRatioForClass(m_metadata.GetCurrentClass());
1708 : :
1709 : 21 : double dForceL = ((data->mWheel[0].mSuspForce - m_prev_susp_force[0]) * mr) / ctx.dt;
1710 : 21 : double dForceR = ((data->mWheel[1].mSuspForce - m_prev_susp_force[1]) * mr) / ctx.dt;
1711 : 21 : double max_dForce = (std::max)(dForceL, dForceR);
1712 : :
1713 [ + + ]: 21 : if (max_dForce > BOTTOMING_IMPULSE_THRESHOLD_N_S) { // 100kN/s impulse at the WHEEL
1714 : 11 : triggered = true;
1715 : 11 : intensity = (max_dForce - BOTTOMING_IMPULSE_THRESHOLD_N_S) / BOTTOMING_IMPULSE_RANGE_N_S;
1716 : : }
1717 : : }
1718 : :
1719 : : // Safety Trigger: Raw Load Peak (Catches Method 0/1 failures)
1720 [ + + ]: 1370 : if (!triggered) {
1721 : : // FIX (Issue #355): Support encrypted cars by using the approximation fallback
1722 [ - + - - ]: 11 : double load_l = ctx.frame_warn_load ? approximate_load(data->mWheel[0]) : data->mWheel[0].mTireLoad;
1723 [ - + - - ]: 11 : double load_r = ctx.frame_warn_load ? approximate_load(data->mWheel[1]) : data->mWheel[1].mTireLoad;
1724 : 11 : double max_load = (std::max)(load_l, load_r);
1725 : :
1726 : 11 : double bottoming_threshold = m_static_front_load * BOTTOMING_LOAD_MULT;
1727 [ + + ]: 11 : if (max_load > bottoming_threshold) {
1728 : 1 : triggered = true;
1729 : 1 : double excess = max_load - bottoming_threshold;
1730 : 1 : intensity = std::sqrt(excess) * BOTTOMING_INTENSITY_SCALE; // Non-linear response
1731 : : }
1732 : : }
1733 : :
1734 [ + + ]: 1370 : if (triggered) {
1735 : : // Generate high-intensity low-frequency "thump"
1736 : 1360 : double bump_magnitude = intensity * m_bottoming_gain * (double)BASE_NM_BOTTOMING;
1737 : 1360 : double freq = BOTTOMING_FREQ_HZ;
1738 : :
1739 : 1360 : m_bottoming_phase += freq * ctx.dt * TWO_PI;
1740 : 1360 : m_bottoming_phase = std::fmod(m_bottoming_phase, TWO_PI);
1741 : :
1742 : 1360 : ctx.bottoming_crunch = std::sin(m_bottoming_phase) * bump_magnitude * ctx.speed_gate;
1743 : : }
1744 : : }
|
