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