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