Branch data Line data Source code
1 : : #include "FFBEngine.h"
2 : : #include "Config.h"
3 : : #include "RestApiProvider.h"
4 : : #include <iostream>
5 : : #include <mutex>
6 : :
7 : : extern std::recursive_mutex g_engine_mutex;
8 : : #include <algorithm>
9 : : #include <cmath>
10 : :
11 : : using namespace ffb_math;
12 : :
13 [ + + + + : 6900 : FFBEngine::FFBEngine() {
+ + + + +
+ + + +
- ]
14 : 345 : last_log_time = std::chrono::steady_clock::now();
15 [ + - ]: 345 : Preset::ApplyDefaultsToEngine(*this);
16 : 345 : }
17 : :
18 : : // v0.7.34: Safety Check for Issue #79
19 : : // Determines if FFB should be active based on vehicle scoring state.
20 : : // v0.7.49: Modified for #126 to allow cool-down laps after finish.
21 : 2315 : bool FFBEngine::IsFFBAllowed(const VehicleScoringInfoV01& scoring, unsigned char gamePhase) const {
22 : : // mControl: 0 = local player, 1 = AI, 2 = Remote, -1 = None
23 : : // mFinishStatus: 0 = none, 1 = finished, 2 = DNF, 3 = DQ
24 : :
25 : : // 1. Core control check
26 [ + + + + ]: 2315 : if (!scoring.mIsPlayer || scoring.mControl != 0) return false;
27 : :
28 : : // 2. DO NOT use gamePhase to determine if IsFFBAllowed (eg. session over would cause no FFB after finish line, or time up)
29 : : // (gamePhase, Game Phases: 7=Stopped, 8=Session Over)
30 : :
31 : : // 3. Individual status safety
32 : : // Allow Finished (1) and DNF (2) as long as player is in control.
33 : : // Mute for Disqualified (3).
34 [ + + ]: 9 : if (scoring.mFinishStatus == 3) return false;
35 : :
36 : : // 4. Garage Safety: Mute FFB when in garage stall
37 [ + + ]: 7 : if (scoring.mInGarageStall) return false;
38 : :
39 : 5 : return true;
40 : : }
41 : :
42 : : // v0.7.49: Slew rate limiter for safety (#79)
43 : : // Clamps the rate of change of the output force to prevent violent jolts.
44 : : // If restricted is true (e.g. after finish or lost control), limit is tighter.
45 : 4771 : double FFBEngine::ApplySafetySlew(double target_force, double dt, bool restricted) {
46 [ + + ]: 4771 : if (!std::isfinite(target_force)) return 0.0;
47 [ + + ]: 4768 : double max_slew = restricted ? (double)SAFETY_SLEW_RESTRICTED : (double)SAFETY_SLEW_NORMAL;
48 : 4768 : double max_change = max_slew * dt;
49 : 4768 : double delta = target_force - m_last_output_force;
50 : 4768 : delta = std::clamp(delta, -max_change, max_change);
51 : 4768 : m_last_output_force += delta;
52 : 4768 : return m_last_output_force;
53 : : }
54 : :
55 : : // Helper to retrieve data (Consumer)
56 : 44 : std::vector<FFBSnapshot> FFBEngine::GetDebugBatch() {
57 : 44 : std::vector<FFBSnapshot> batch;
58 : : {
59 [ + - ]: 44 : std::lock_guard<std::mutex> lock(m_debug_mutex);
60 [ + + ]: 44 : if (!m_debug_buffer.empty()) {
61 : 42 : batch.swap(m_debug_buffer);
62 : : }
63 : 44 : }
64 : 44 : return batch;
65 : 0 : }
66 : :
67 : : // ---------------------------------------------------------------------------
68 : : // Grip & Load Estimation methods have been moved to GripLoadEstimation.cpp.
69 : : // See docs/dev_docs/reports/FFBEngine_refactoring_analysis.md for rationale.
70 : : // Functions moved:
71 : : // update_static_load_reference, InitializeLoadReference,
72 : : // calculate_raw_slip_angle_pair, calculate_slip_angle,
73 : : // calculate_grip, approximate_load, approximate_rear_load,
74 : : // calculate_kinematic_load, calculate_manual_slip_ratio,
75 : : // calculate_slope_grip, calculate_slope_confidence,
76 : : // calculate_wheel_slip_ratio
77 : : // ---------------------------------------------------------------------------
78 : :
79 : :
80 : : // Signal Conditioning: Applies idle smoothing and notch filters to raw torque
81 : : // Returns the conditioned force value ready for effect processing
82 : 14674 : double FFBEngine::apply_signal_conditioning(double raw_torque, const TelemInfoV01* data, FFBCalculationContext& ctx) {
83 : 14674 : double game_force_proc = raw_torque;
84 : :
85 : : // Idle Smoothing
86 : 14674 : double effective_shaft_smoothing = (double)m_steering_shaft_smoothing;
87 : 14674 : double idle_speed_threshold = (double)m_speed_gate_upper;
88 [ + + ]: 14674 : if (idle_speed_threshold < (double)IDLE_SPEED_MIN_M_S) idle_speed_threshold = (double)IDLE_SPEED_MIN_M_S;
89 [ + + ]: 14674 : if (ctx.car_speed < idle_speed_threshold) {
90 : 3319 : double idle_blend = (idle_speed_threshold - ctx.car_speed) / idle_speed_threshold;
91 : 3319 : double dynamic_smooth = (double)IDLE_BLEND_FACTOR * idle_blend;
92 : 3319 : effective_shaft_smoothing = (std::max)(effective_shaft_smoothing, dynamic_smooth);
93 : : }
94 : :
95 [ + + ]: 14674 : if (effective_shaft_smoothing > MIN_TAU_S) {
96 : 3340 : double alpha_shaft = ctx.dt / (effective_shaft_smoothing + ctx.dt);
97 : 3340 : alpha_shaft = (std::min)(ALPHA_MAX, (std::max)(ALPHA_MIN, alpha_shaft));
98 : 3340 : m_steering_shaft_torque_smoothed += alpha_shaft * (game_force_proc - m_steering_shaft_torque_smoothed);
99 : 3340 : game_force_proc = m_steering_shaft_torque_smoothed;
100 : : } else {
101 : 11334 : m_steering_shaft_torque_smoothed = game_force_proc;
102 : : }
103 : :
104 : : // Frequency Estimator Logic
105 : 14674 : double alpha_hpf = ctx.dt / (HPF_TIME_CONSTANT_S + ctx.dt);
106 : 14674 : m_torque_ac_smoothed += alpha_hpf * (game_force_proc - m_torque_ac_smoothed);
107 : 14674 : double ac_torque = game_force_proc - m_torque_ac_smoothed;
108 : :
109 [ + + + + ]: 14674 : if ((m_prev_ac_torque < -ZERO_CROSSING_EPSILON && ac_torque > ZERO_CROSSING_EPSILON) ||
110 [ + + + + ]: 14307 : (m_prev_ac_torque > ZERO_CROSSING_EPSILON && ac_torque < -ZERO_CROSSING_EPSILON)) {
111 : :
112 : 731 : double now = data->mElapsedTime;
113 : 731 : double period = now - m_last_crossing_time;
114 : :
115 [ + + + + ]: 731 : if (period > MIN_FREQ_PERIOD && period < MAX_FREQ_PERIOD) {
116 : 41 : double inst_freq = 1.0 / (period * DUAL_DIVISOR);
117 : 41 : m_debug_freq = m_debug_freq * DEBUG_FREQ_SMOOTHING + inst_freq * (1.0 - DEBUG_FREQ_SMOOTHING);
118 : : }
119 : 731 : m_last_crossing_time = now;
120 : : }
121 : 14674 : m_prev_ac_torque = ac_torque;
122 : :
123 : 14674 : const TelemWheelV01& fl_ref = data->mWheel[0];
124 : 14674 : double radius = (double)fl_ref.mStaticUndeflectedRadius / UNIT_CM_TO_M;
125 [ + + ]: 14674 : if (radius < RADIUS_FALLBACK_MIN_M) radius = RADIUS_FALLBACK_DEFAULT_M;
126 : 14674 : double circumference = TWO_PI * radius;
127 [ + - ]: 14674 : double wheel_freq = (circumference > 0.0) ? (ctx.car_speed / circumference) : 0.0;
128 : 14674 : m_theoretical_freq = wheel_freq;
129 : :
130 : : // Dynamic Notch Filter
131 [ + + ]: 14674 : if (m_flatspot_suppression) {
132 [ + + ]: 3 : if (wheel_freq > 1.0) {
133 [ + - ]: 1 : m_notch_filter.Update(wheel_freq, 1.0/ctx.dt, (double)m_notch_q);
134 : 1 : double input_force = game_force_proc;
135 : 1 : double filtered_force = m_notch_filter.Process(input_force);
136 : 1 : game_force_proc = input_force * (1.0f - m_flatspot_strength) + filtered_force * m_flatspot_strength;
137 : : } else {
138 : 2 : m_notch_filter.Reset();
139 : : }
140 : : }
141 : :
142 : : // Static Notch Filter
143 [ + + ]: 14674 : if (m_static_notch_enabled) {
144 : 1500 : double bw = (double)m_static_notch_width;
145 [ + + ]: 1500 : if (bw < MIN_NOTCH_WIDTH_HZ) bw = MIN_NOTCH_WIDTH_HZ;
146 : 1500 : double q = (double)m_static_notch_freq / bw;
147 [ + - ]: 1500 : m_static_notch_filter.Update((double)m_static_notch_freq, 1.0/ctx.dt, q);
148 : 1500 : game_force_proc = m_static_notch_filter.Process(game_force_proc);
149 : : } else {
150 : 13174 : m_static_notch_filter.Reset();
151 : : }
152 : :
153 : 14674 : return game_force_proc;
154 : : }
155 : :
156 : : // Refactored calculate_force
157 : 14667 : double FFBEngine::calculate_force(const TelemInfoV01* data, const char* vehicleClass, const char* vehicleName, float genFFBTorque, bool allowed, double override_dt) {
158 [ + + ]: 14667 : if (!data) return 0.0;
159 [ + - ]: 14665 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
160 : :
161 : : // --- 0. UP-SAMPLING (Issue #216) ---
162 : : // If override_dt is provided (e.g. from main.cpp), we are in 400Hz upsampling mode.
163 : : // Otherwise (override_dt <= 0), we are in legacy/test mode: every call is a new frame.
164 : 14665 : bool upsampling_active = (override_dt > 0.0);
165 [ + + + + ]: 14665 : bool is_new_frame = !upsampling_active || (data->mElapsedTime != m_last_telemetry_time);
166 : :
167 [ + + ]: 14665 : if (is_new_frame) m_last_telemetry_time = data->mElapsedTime;
168 : :
169 [ + + ]: 14665 : double ffb_dt = upsampling_active ? override_dt : (double)data->mDeltaTime;
170 [ + + ]: 14665 : if (ffb_dt < 0.0001) ffb_dt = 0.0025;
171 : :
172 : : // Synchronize persistent working copy
173 : 14665 : m_working_info = *data;
174 : :
175 : : // Upsample Steering Shaft Torque (Holt-Winters)
176 : 14665 : double shaft_torque = m_upsample_shaft_torque.Process(data->mSteeringShaftTorque, ffb_dt, is_new_frame);
177 : 14665 : m_working_info.mSteeringShaftTorque = shaft_torque;
178 : :
179 : : // Update wheels in working_info (Channels used for derivatives)
180 [ + + ]: 73325 : for (int i = 0; i < 4; i++) {
181 : 58660 : m_working_info.mWheel[i].mLateralPatchVel = m_upsample_lat_patch_vel[i].Process(data->mWheel[i].mLateralPatchVel, ffb_dt, is_new_frame);
182 : 58660 : m_working_info.mWheel[i].mLongitudinalPatchVel = m_upsample_long_patch_vel[i].Process(data->mWheel[i].mLongitudinalPatchVel, ffb_dt, is_new_frame);
183 : 58660 : m_working_info.mWheel[i].mVerticalTireDeflection = m_upsample_vert_deflection[i].Process(data->mWheel[i].mVerticalTireDeflection, ffb_dt, is_new_frame);
184 : 58660 : m_working_info.mWheel[i].mSuspForce = m_upsample_susp_force[i].Process(data->mWheel[i].mSuspForce, ffb_dt, is_new_frame);
185 : 58660 : m_working_info.mWheel[i].mBrakePressure = m_upsample_brake_pressure[i].Process(data->mWheel[i].mBrakePressure, ffb_dt, is_new_frame);
186 : 58660 : m_working_info.mWheel[i].mRotation = m_upsample_rotation[i].Process(data->mWheel[i].mRotation, ffb_dt, is_new_frame);
187 : : }
188 : :
189 : : // Upsample other derivative sources
190 : 14665 : m_working_info.mUnfilteredSteering = m_upsample_steering.Process(data->mUnfilteredSteering, ffb_dt, is_new_frame);
191 : 14665 : m_working_info.mUnfilteredThrottle = m_upsample_throttle.Process(data->mUnfilteredThrottle, ffb_dt, is_new_frame);
192 : 14665 : m_working_info.mUnfilteredBrake = m_upsample_brake.Process(data->mUnfilteredBrake, ffb_dt, is_new_frame);
193 : 14665 : m_working_info.mLocalAccel.x = m_upsample_local_accel_x.Process(data->mLocalAccel.x, ffb_dt, is_new_frame);
194 : 14665 : m_working_info.mLocalAccel.y = m_upsample_local_accel_y.Process(data->mLocalAccel.y, ffb_dt, is_new_frame);
195 : 14665 : m_working_info.mLocalAccel.z = m_upsample_local_accel_z.Process(data->mLocalAccel.z, ffb_dt, is_new_frame);
196 : 14665 : m_working_info.mLocalRotAccel.y = m_upsample_local_rot_accel_y.Process(data->mLocalRotAccel.y, ffb_dt, is_new_frame);
197 : :
198 : : // Use upsampled data pointer for all calculations
199 : 14665 : const TelemInfoV01* upsampled_data = &m_working_info;
200 : :
201 : : // Select Torque Source
202 : : // v0.7.63 Fix: genFFBTorque (Direct Torque 400Hz) is normalized [-1.0, 1.0].
203 : : // It must be scaled by m_wheelbase_max_nm to match the engine's internal Nm-based pipeline.
204 [ + + ]: 14665 : double raw_torque_input = (m_torque_source == 1) ? (double)genFFBTorque * (double)m_wheelbase_max_nm : shaft_torque;
205 : :
206 : : // RELIABILITY FIX: Sanitize input torque
207 [ + + ]: 14665 : if (!std::isfinite(raw_torque_input)) return 0.0;
208 : :
209 : : // --- 0. DYNAMIC NORMALIZATION (Issue #152) ---
210 : : // 1. Contextual Spike Rejection (Lightweight MAD alternative)
211 : 14664 : double current_abs_torque = std::abs(raw_torque_input);
212 : 14664 : double alpha_slow = ffb_dt / (TORQUE_ROLL_AVG_TAU + ffb_dt); // 1-second rolling average
213 : 14664 : m_rolling_average_torque += alpha_slow * (current_abs_torque - m_rolling_average_torque);
214 : :
215 : 14664 : double lat_g_abs = std::abs(upsampled_data->mLocalAccel.x / (double)GRAVITY_MS2);
216 : 14664 : double torque_slew = std::abs(raw_torque_input - m_last_raw_torque) / (ffb_dt + (double)EPSILON_DIV);
217 : 14664 : m_last_raw_torque = raw_torque_input;
218 : :
219 : : // Flag as spike if torque jumps > 3x the rolling average (with a 15Nm floor to prevent low-speed false positives)
220 [ + + + + ]: 14664 : bool is_contextual_spike = (current_abs_torque > (m_rolling_average_torque * TORQUE_SPIKE_RATIO)) && (current_abs_torque > TORQUE_SPIKE_MIN_NM);
221 : :
222 : : // Safety check for clean state
223 [ + + + + : 14664 : bool is_clean_state = (lat_g_abs < LAT_G_CLEAN_LIMIT) && (torque_slew < TORQUE_SLEW_CLEAN_LIMIT) && !is_contextual_spike;
+ - ]
224 : :
225 : : // 2. Leaky Integrator (Exponential Decay + Floor)
226 [ + + + + : 14664 : if (is_clean_state && m_torque_source == 0 && m_dynamic_normalization_enabled) {
+ + ]
227 [ + + ]: 802 : if (current_abs_torque > m_session_peak_torque) {
228 : 401 : m_session_peak_torque = current_abs_torque; // Fast attack
229 : : } else {
230 : : // Exponential decay (0.5% reduction per second)
231 : 401 : double decay_factor = 1.0 - ((double)SESSION_PEAK_DECAY_RATE * ffb_dt);
232 : 401 : m_session_peak_torque *= decay_factor;
233 : : }
234 : : // Absolute safety floor and ceiling
235 : 802 : m_session_peak_torque = std::clamp(m_session_peak_torque, (double)PEAK_TORQUE_FLOOR, (double)PEAK_TORQUE_CEILING);
236 : : }
237 : :
238 : : // 3. EMA Filtering on the Gain Multiplier (Zero-latency physics)
239 : : // v0.7.71: For In-Game FFB (1), we normalize against the wheelbase max since the signal is already normalized [-1, 1].
240 : : double target_structural_mult;
241 [ + + ]: 14664 : if (m_torque_source == 1) {
242 : 19 : target_structural_mult = 1.0 / ((double)m_wheelbase_max_nm + (double)EPSILON_DIV);
243 [ + + ]: 14645 : } else if (m_dynamic_normalization_enabled) {
244 : 803 : target_structural_mult = 1.0 / (m_session_peak_torque + (double)EPSILON_DIV);
245 : : } else {
246 : 13842 : target_structural_mult = 1.0 / ((double)m_target_rim_nm + (double)EPSILON_DIV);
247 : : }
248 : 14664 : double alpha_gain = ffb_dt / ((double)STRUCT_MULT_SMOOTHING_TAU + ffb_dt); // 250ms smoothing
249 : 14664 : m_smoothed_structural_mult += alpha_gain * (target_structural_mult - m_smoothed_structural_mult);
250 : :
251 : : // Class Seeding
252 : 14664 : bool seeded = false;
253 [ + + + - : 14664 : if (vehicleClass && (m_current_class_name != vehicleClass || (vehicleName && strcmp(m_vehicle_name, vehicleName) != 0))) {
+ + + + +
+ + + ]
254 [ + - ]: 660 : m_current_class_name = vehicleClass;
255 [ + - ]: 660 : InitializeLoadReference(vehicleClass, vehicleName);
256 : 660 : m_warned_invalid_range = false; // Reset warning on car change
257 : 660 : seeded = true;
258 : :
259 : : // Trigger REST API Fallback if enabled and range is invalid (Issue #221)
260 [ + + + - ]: 660 : if (m_rest_api_enabled && data->mPhysicalSteeringWheelRange <= 0.0f) {
261 [ + - + - ]: 1 : RestApiProvider::Get().RequestSteeringRange(m_rest_api_port);
262 : : }
263 : : }
264 : :
265 : : // --- 1. INITIALIZE CONTEXT ---
266 : 14664 : FFBCalculationContext ctx;
267 : 14664 : ctx.dt = ffb_dt; // Use our constant FFB loop time for all internal physics
268 : :
269 : : // Sanity Check: Delta Time (Keep legacy warning if raw dt is broken)
270 [ + + ]: 14664 : if (data->mDeltaTime <= DT_EPSILON) {
271 [ + + ]: 208 : if (!m_warned_dt) {
272 [ + - + - : 18 : std::cout << "[WARNING] Invalid DeltaTime (<=0). Using default " << DEFAULT_DT << "s." << std::endl;
+ - + - ]
273 : 18 : m_warned_dt = true;
274 : : }
275 : 208 : ctx.frame_warn_dt = true;
276 : : }
277 : :
278 : 14664 : ctx.car_speed_long = upsampled_data->mLocalVel.z;
279 : 14664 : ctx.car_speed = std::abs(ctx.car_speed_long);
280 : :
281 : : // Steering Range Diagnostic (Issue #218)
282 [ + + ]: 14664 : if (upsampled_data->mPhysicalSteeringWheelRange <= 0.0f) {
283 [ + + ]: 14647 : if (!m_warned_invalid_range) {
284 [ + - + - ]: 807 : float fallback = RestApiProvider::Get().GetFallbackRangeDeg();
285 [ + + - + ]: 807 : if (m_rest_api_enabled && fallback > 0.0f) {
286 [ # # ]: 0 : std::cout << "[FFB] Invalid Shared Memory Steering Range. Using REST API fallback: "
287 [ # # # # : 0 : << fallback << " deg" << std::endl;
# # ]
288 : : } else {
289 : 807 : std::cout << "[WARNING] Invalid PhysicalSteeringWheelRange (<=0) for " << upsampled_data->mVehicleName
290 [ + - + - : 807 : << ". Soft Lock and Steering UI may be incorrect." << std::endl;
+ - + - ]
291 : : }
292 : 807 : m_warned_invalid_range = true;
293 : : }
294 : : }
295 : : // Update Context strings (for UI/Logging)
296 : : // Only update if first char differs to avoid redundant copies
297 [ + + - + ]: 14664 : if (m_vehicle_name[0] != upsampled_data->mVehicleName[0] || m_vehicle_name[VEHICLE_NAME_CHECK_IDX] != upsampled_data->mVehicleName[VEHICLE_NAME_CHECK_IDX]) {
298 : : #ifdef _WIN32
299 : : strncpy_s(m_vehicle_name, sizeof(m_vehicle_name), upsampled_data->mVehicleName, _TRUNCATE);
300 : : strncpy_s(m_track_name, sizeof(m_track_name), upsampled_data->mTrackName, _TRUNCATE);
301 : : #else
302 : 192 : strncpy(m_vehicle_name, upsampled_data->mVehicleName, STR_MAX_64);
303 : 192 : m_vehicle_name[STR_MAX_64] = '\0';
304 : 192 : strncpy(m_track_name, upsampled_data->mTrackName, STR_MAX_64);
305 : 192 : m_track_name[STR_MAX_64] = '\0';
306 : : #endif
307 : : }
308 : :
309 : : // --- 2. SIGNAL CONDITIONING (STATE UPDATES) ---
310 : :
311 : : // Chassis Inertia Simulation
312 : 14664 : double chassis_tau = (double)m_chassis_inertia_smoothing;
313 [ + - ]: 14664 : if (chassis_tau < MIN_TAU_S) chassis_tau = MIN_TAU_S;
314 : 14664 : double alpha_chassis = ctx.dt / (chassis_tau + ctx.dt);
315 : 14664 : m_accel_x_smoothed += alpha_chassis * (upsampled_data->mLocalAccel.x - m_accel_x_smoothed);
316 : 14664 : m_accel_z_smoothed += alpha_chassis * (upsampled_data->mLocalAccel.z - m_accel_z_smoothed);
317 : :
318 : : // --- 3. TELEMETRY PROCESSING ---
319 : : // Front Wheels
320 : 14664 : const TelemWheelV01& fl = upsampled_data->mWheel[0];
321 : 14664 : const TelemWheelV01& fr = upsampled_data->mWheel[1];
322 : :
323 : : // Raw Inputs
324 : 14664 : double raw_torque = raw_torque_input;
325 : 14664 : double raw_load = (fl.mTireLoad + fr.mTireLoad) / DUAL_DIVISOR;
326 : 14664 : double raw_grip = (fl.mGripFract + fr.mGripFract) / DUAL_DIVISOR;
327 : :
328 : : // Update Stats
329 : 14664 : s_torque.Update(raw_torque);
330 : 14664 : s_load.Update(raw_load);
331 : 14664 : s_grip.Update(raw_grip);
332 : 14664 : s_lat_g.Update(upsampled_data->mLocalAccel.x);
333 : :
334 : : // Stats Latching
335 : 14664 : auto now = std::chrono::steady_clock::now();
336 [ + - + - : 14664 : if (std::chrono::duration_cast<std::chrono::seconds>(now - last_log_time).count() >= 1) {
+ + ]
337 : 8 : s_torque.ResetInterval();
338 : 8 : s_load.ResetInterval();
339 : 8 : s_grip.ResetInterval();
340 : 8 : s_lat_g.ResetInterval();
341 : 8 : last_log_time = now;
342 : : }
343 : :
344 : : // --- 4. PRE-CALCULATIONS ---
345 : :
346 : : // Average Load & Fallback Logic
347 : 14664 : ctx.avg_load = raw_load;
348 : :
349 : : // Hysteresis for missing load
350 [ + + + + ]: 14664 : if (ctx.avg_load < 1.0 && ctx.car_speed > SPEED_EPSILON) {
351 : 2041 : m_missing_load_frames++;
352 : : } else {
353 : 12623 : m_missing_load_frames = (std::max)(0, m_missing_load_frames - 1);
354 : : }
355 : :
356 [ + + ]: 14664 : if (m_missing_load_frames > MISSING_LOAD_WARN_THRESHOLD) {
357 : : // Fallback Logic
358 [ + + ]: 1307 : if (fl.mSuspForce > MIN_VALID_SUSP_FORCE) {
359 [ + - ]: 129 : double calc_load_fl = approximate_load(fl);
360 [ + - ]: 129 : double calc_load_fr = approximate_load(fr);
361 : 129 : ctx.avg_load = (calc_load_fl + calc_load_fr) / DUAL_DIVISOR;
362 : : } else {
363 [ + - ]: 1178 : double kin_load_fl = calculate_kinematic_load(data, 0);
364 [ + - ]: 1178 : double kin_load_fr = calculate_kinematic_load(data, 1);
365 : 1178 : ctx.avg_load = (kin_load_fl + kin_load_fr) / DUAL_DIVISOR;
366 : : }
367 [ + + ]: 1307 : if (!m_warned_load) {
368 [ + - + - : 18 : std::cout << "Warning: Data for mTireLoad from the game seems to be missing for this car (" << data->mVehicleName << "). (Likely Encrypted/DLC Content). Using Kinematic Fallback." << std::endl;
+ - + - ]
369 : 18 : m_warned_load = true;
370 : : }
371 : 1307 : ctx.frame_warn_load = true;
372 : : }
373 : :
374 : : // Sanity Checks (Missing Data)
375 : :
376 : : // 1. Suspension Force (mSuspForce)
377 : 14664 : double avg_susp_f = (fl.mSuspForce + fr.mSuspForce) / DUAL_DIVISOR;
378 [ + + + + : 14664 : if (avg_susp_f < MIN_VALID_SUSP_FORCE && std::abs(data->mLocalVel.z) > SPEED_EPSILON) {
+ + ]
379 : 2890 : m_missing_susp_force_frames++;
380 : : } else {
381 : 11774 : m_missing_susp_force_frames = (std::max)(0, m_missing_susp_force_frames - 1);
382 : : }
383 [ + + + + ]: 14664 : if (m_missing_susp_force_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_susp_force) {
384 [ + - + - : 11 : std::cout << "Warning: Data for mSuspForce from the game seems to be missing for this car (" << data->mVehicleName << "). (Likely Encrypted/DLC Content). A fallback estimation will be used." << std::endl;
+ - + - ]
385 : 11 : m_warned_susp_force = true;
386 : : }
387 : :
388 : : // 2. Suspension Deflection (mSuspensionDeflection)
389 : 14664 : double avg_susp_def = (std::abs(fl.mSuspensionDeflection) + std::abs(fr.mSuspensionDeflection)) / DUAL_DIVISOR;
390 [ + + + + : 14664 : if (avg_susp_def < DEFLECTION_NEAR_ZERO_M && std::abs(data->mLocalVel.z) > SPEED_HIGH_THRESHOLD) {
+ + ]
391 : 10524 : m_missing_susp_deflection_frames++;
392 : : } else {
393 : 4140 : m_missing_susp_deflection_frames = (std::max)(0, m_missing_susp_deflection_frames - 1);
394 : : }
395 [ + + + + ]: 14664 : if (m_missing_susp_deflection_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_susp_deflection) {
396 [ + - + - : 30 : std::cout << "Warning: Data for mSuspensionDeflection from the game seems to be missing for this car (" << data->mVehicleName << "). (Likely Encrypted/DLC Content). A fallback estimation will be used." << std::endl;
+ - + - ]
397 : 30 : m_warned_susp_deflection = true;
398 : : }
399 : :
400 : : // 3. Front Lateral Force (mLateralForce)
401 : 14664 : double avg_lat_force_front = (std::abs(fl.mLateralForce) + std::abs(fr.mLateralForce)) / DUAL_DIVISOR;
402 [ + - + + : 14664 : if (avg_lat_force_front < MIN_VALID_LAT_FORCE_N && std::abs(data->mLocalAccel.x) > G_FORCE_THRESHOLD) {
+ + ]
403 : 4547 : m_missing_lat_force_front_frames++;
404 : : } else {
405 : 10117 : m_missing_lat_force_front_frames = (std::max)(0, m_missing_lat_force_front_frames - 1);
406 : : }
407 [ + + + + ]: 14664 : if (m_missing_lat_force_front_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_lat_force_front) {
408 [ + - + - : 13 : std::cout << "Warning: Data for mLateralForce (Front) from the game seems to be missing for this car (" << data->mVehicleName << "). (Likely Encrypted/DLC Content). A fallback estimation will be used." << std::endl;
+ - + - ]
409 : 13 : m_warned_lat_force_front = true;
410 : : }
411 : :
412 : : // 4. Rear Lateral Force (mLateralForce)
413 : 14664 : double avg_lat_force_rear = (std::abs(data->mWheel[2].mLateralForce) + std::abs(data->mWheel[3].mLateralForce)) / DUAL_DIVISOR;
414 [ + + + + : 14664 : if (avg_lat_force_rear < MIN_VALID_LAT_FORCE_N && std::abs(data->mLocalAccel.x) > G_FORCE_THRESHOLD) {
+ + ]
415 : 4487 : m_missing_lat_force_rear_frames++;
416 : : } else {
417 : 10177 : m_missing_lat_force_rear_frames = (std::max)(0, m_missing_lat_force_rear_frames - 1);
418 : : }
419 [ + + + + ]: 14664 : if (m_missing_lat_force_rear_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_lat_force_rear) {
420 [ + - + - : 12 : std::cout << "Warning: Data for mLateralForce (Rear) from the game seems to be missing for this car (" << data->mVehicleName << "). (Likely Encrypted/DLC Content). A fallback estimation will be used." << std::endl;
+ - + - ]
421 : 12 : m_warned_lat_force_rear = true;
422 : : }
423 : :
424 : : // 5. Vertical Tire Deflection (mVerticalTireDeflection)
425 : 14664 : double avg_vert_def = (std::abs(fl.mVerticalTireDeflection) + std::abs(fr.mVerticalTireDeflection)) / DUAL_DIVISOR;
426 [ + + + + : 14664 : if (avg_vert_def < DEFLECTION_NEAR_ZERO_M && std::abs(data->mLocalVel.z) > SPEED_HIGH_THRESHOLD) {
+ + ]
427 : 1855 : m_missing_vert_deflection_frames++;
428 : : } else {
429 : 12809 : m_missing_vert_deflection_frames = (std::max)(0, m_missing_vert_deflection_frames - 1);
430 : : }
431 [ + + + + ]: 14664 : if (m_missing_vert_deflection_frames > MISSING_TELEMETRY_WARN_THRESHOLD && !m_warned_vert_deflection) {
432 : 8 : std::cout << "[WARNING] mVerticalTireDeflection is missing for car: " << data->mVehicleName
433 [ + - + - : 8 : << ". (Likely Encrypted/DLC Content). Road Texture fallback active." << std::endl;
+ - + - ]
434 : 8 : m_warned_vert_deflection = true;
435 : : }
436 : :
437 : : // Peak Hold Logic
438 [ + + + + ]: 14664 : if (m_auto_load_normalization_enabled && !seeded) {
439 [ + + ]: 202 : if (ctx.avg_load > m_auto_peak_load) {
440 : 52 : m_auto_peak_load = ctx.avg_load; // Fast Attack
441 : : } else {
442 : 150 : m_auto_peak_load -= (LOAD_DECAY_RATE * ctx.dt); // Slow Decay (~100N/s)
443 : : }
444 : : }
445 : 14664 : m_auto_peak_load = (std::max)(LOAD_SAFETY_FLOOR, m_auto_peak_load); // Safety Floor
446 : :
447 : : // Load Factors (Stage 3: Giannoulis Soft-Knee Compression)
448 : : // 1. Calculate raw load multiplier based on static weight
449 : : // Safety clamp: Ensure load factor is non-negative even with telemetry noise
450 : 14664 : double x = (std::max)(0.0, ctx.avg_load / m_static_front_load);
451 : :
452 : : // 2. Giannoulis Soft-Knee Parameters
453 : 14664 : double T = COMPRESSION_KNEE_THRESHOLD; // Threshold (Start compressing at 1.5x static weight)
454 : 14664 : double W = COMPRESSION_KNEE_WIDTH; // Knee Width (Transition from 1.25x to 1.75x)
455 : 14664 : double R = COMPRESSION_RATIO; // Compression Ratio (4:1 above the knee)
456 : :
457 : 14664 : double lower_bound = T - (W / DUAL_DIVISOR);
458 : 14664 : double upper_bound = T + (W / DUAL_DIVISOR);
459 : 14664 : double compressed_load_factor = x;
460 : :
461 : : // 3. Apply Compression
462 [ + + ]: 14664 : if (x > upper_bound) {
463 : : // Linear compressed region
464 : 9562 : compressed_load_factor = T + ((x - T) / R);
465 [ + + ]: 5102 : } else if (x > lower_bound) {
466 : : // Quadratic soft-knee transition
467 : 554 : double diff = x - lower_bound;
468 : 554 : compressed_load_factor = x + (((1.0 / R) - 1.0) * (diff * diff)) / (DUAL_DIVISOR * W);
469 : : }
470 : :
471 : : // 4. EMA Smoothing on the vibration multiplier (100ms time constant)
472 : 14664 : double alpha_vibration = ctx.dt / (VIBRATION_EMA_TAU + ctx.dt);
473 : 14664 : m_smoothed_vibration_mult += alpha_vibration * (compressed_load_factor - m_smoothed_vibration_mult);
474 : :
475 : : // 5. Apply to context with user caps
476 : 14664 : double texture_safe_max = (std::min)(USER_CAP_MAX, (double)m_texture_load_cap);
477 : 14664 : ctx.texture_load_factor = (std::min)(texture_safe_max, m_smoothed_vibration_mult);
478 : :
479 : 14664 : double brake_safe_max = (std::min)(USER_CAP_MAX, (double)m_brake_load_cap);
480 : 14664 : ctx.brake_load_factor = (std::min)(brake_safe_max, m_smoothed_vibration_mult);
481 : :
482 : : // Hardware Scaling Safeties
483 : 14664 : double wheelbase_max_safe = (double)m_wheelbase_max_nm;
484 [ - + ]: 14664 : if (wheelbase_max_safe < 1.0) wheelbase_max_safe = 1.0;
485 : :
486 : : // Speed Gate - v0.7.2 Smoothstep S-curve
487 : 29328 : ctx.speed_gate = smoothstep(
488 : 14664 : (double)m_speed_gate_lower,
489 [ + - ]: 14664 : (double)m_speed_gate_upper,
490 : : ctx.car_speed
491 : : );
492 : :
493 : : // --- 5. EFFECT CALCULATIONS ---
494 : :
495 : : // A. Understeer (Base Torque + Grip Loss)
496 : :
497 : : // Grip Estimation (v0.4.5 FIX)
498 : 14664 : GripResult front_grip_res = calculate_grip(fl, fr, ctx.avg_load, m_warned_grip,
499 : 14664 : m_prev_slip_angle[0], m_prev_slip_angle[1],
500 [ + - ]: 14664 : ctx.car_speed, ctx.dt, data->mVehicleName, data, true /* is_front */);
501 : 14664 : ctx.avg_grip = front_grip_res.value;
502 : 14664 : m_grip_diag.front_original = front_grip_res.original;
503 : 14664 : m_grip_diag.front_approximated = front_grip_res.approximated;
504 : 14664 : m_grip_diag.front_slip_angle = front_grip_res.slip_angle;
505 [ + + ]: 14664 : if (front_grip_res.approximated) ctx.frame_warn_grip = true;
506 : :
507 : : // 2. Signal Conditioning (Smoothing, Notch Filters)
508 [ + - ]: 14664 : double game_force_proc = apply_signal_conditioning(raw_torque_input, upsampled_data, ctx);
509 : :
510 : : // Base Steering Force (Issue #178)
511 : 14664 : double base_input = game_force_proc;
512 : :
513 : : // Apply Grip Modulation
514 : 14664 : double grip_loss = (1.0 - ctx.avg_grip) * m_understeer_effect;
515 : 14664 : ctx.grip_factor = (std::max)(0.0, 1.0 - grip_loss);
516 : :
517 : : // v0.7.63: Passthrough Logic for Direct Torque (TIC mode)
518 [ + + ]: 14664 : double grip_factor_applied = m_torque_passthrough ? 1.0 : ctx.grip_factor;
519 : :
520 : : // v0.7.46: Dynamic Weight logic
521 [ + + ]: 14664 : if (m_auto_load_normalization_enabled) {
522 [ + - ]: 218 : update_static_load_reference(ctx.avg_load, ctx.car_speed, ctx.dt);
523 : : }
524 : 14664 : double dynamic_weight_factor = 1.0;
525 : :
526 : : // Only apply if enabled AND we have real load data (no warnings)
527 [ + + + + ]: 14664 : if (m_dynamic_weight_gain > 0.0 && !ctx.frame_warn_load) {
528 : 133 : double load_ratio = ctx.avg_load / m_static_front_load;
529 : : // Blend: 1.0 + (Ratio - 1.0) * Gain
530 : 133 : dynamic_weight_factor = 1.0 + (load_ratio - 1.0) * (double)m_dynamic_weight_gain;
531 : 133 : dynamic_weight_factor = std::clamp(dynamic_weight_factor, DYNAMIC_WEIGHT_MIN, DYNAMIC_WEIGHT_MAX);
532 : : }
533 : :
534 : : // Apply Smoothing to Dynamic Weight (v0.7.47)
535 : 14664 : double dw_alpha = ctx.dt / ((double)m_dynamic_weight_smoothing + ctx.dt + EPSILON_DIV);
536 : 14664 : dw_alpha = (std::max)(0.0, (std::min)(1.0, dw_alpha));
537 : 14664 : m_dynamic_weight_smoothed += dw_alpha * (dynamic_weight_factor - m_dynamic_weight_smoothed);
538 : 14664 : dynamic_weight_factor = m_dynamic_weight_smoothed;
539 : :
540 : : // v0.7.63: Final factor application
541 [ + + ]: 14664 : double dw_factor_applied = m_torque_passthrough ? 1.0 : dynamic_weight_factor;
542 : :
543 [ + + ]: 14664 : double gain_to_apply = (m_torque_source == 1) ? (double)m_ingame_ffb_gain : (double)m_steering_shaft_gain;
544 : 14664 : double output_force = (base_input * gain_to_apply) * dw_factor_applied * grip_factor_applied;
545 : 14664 : output_force *= ctx.speed_gate;
546 : :
547 : : // B. SoP Lateral (Oversteer)
548 [ + - ]: 14664 : calculate_sop_lateral(upsampled_data, ctx);
549 : :
550 : : // C. Gyro Damping
551 [ + - ]: 14664 : calculate_gyro_damping(upsampled_data, ctx);
552 : :
553 : : // D. Effects
554 : 14664 : calculate_abs_pulse(upsampled_data, ctx);
555 [ + - ]: 14664 : calculate_lockup_vibration(upsampled_data, ctx);
556 [ + - ]: 14664 : calculate_wheel_spin(upsampled_data, ctx);
557 : 14664 : calculate_slide_texture(upsampled_data, ctx);
558 : 14664 : calculate_road_texture(upsampled_data, ctx);
559 : 14664 : calculate_suspension_bottoming(upsampled_data, ctx);
560 [ + - ]: 14664 : calculate_soft_lock(upsampled_data, ctx);
561 : :
562 : : // v0.7.78 FIX: Support stationary/garage soft lock (Issue #184)
563 : : // If not allowed (e.g. in garage or AI driving), mute all forces EXCEPT Soft Lock.
564 [ + + ]: 14664 : if (!allowed) {
565 : 2309 : output_force = 0.0;
566 : 2309 : ctx.sop_base_force = 0.0;
567 : 2309 : ctx.rear_torque = 0.0;
568 : 2309 : ctx.yaw_force = 0.0;
569 : 2309 : ctx.gyro_force = 0.0;
570 : 2309 : ctx.scrub_drag_force = 0.0;
571 : 2309 : ctx.road_noise = 0.0;
572 : 2309 : ctx.slide_noise = 0.0;
573 : 2309 : ctx.spin_rumble = 0.0;
574 : 2309 : ctx.bottoming_crunch = 0.0;
575 : 2309 : ctx.abs_pulse_force = 0.0;
576 : 2309 : ctx.lockup_rumble = 0.0;
577 : : // NOTE: ctx.soft_lock_force is PRESERVED.
578 : :
579 : : // Also zero out base_input for snapshot clarity
580 : 2309 : base_input = 0.0;
581 : : }
582 : :
583 : : // --- 6. SUMMATION (Issue #152 & #153 Split Scaling) ---
584 : : // Split into Structural (Dynamic Normalization) and Texture (Absolute Nm) groups
585 : : // v0.7.77 FIX: Soft Lock moved to Texture group to maintain absolute Nm scaling (Issue #181)
586 : 14664 : double structural_sum = output_force + ctx.sop_base_force + ctx.rear_torque + ctx.yaw_force + ctx.gyro_force +
587 : 14664 : ctx.scrub_drag_force;
588 : :
589 : : // Apply Torque Drop (from Spin/Traction Loss) only to structural physics
590 : 14664 : structural_sum *= ctx.gain_reduction_factor;
591 : :
592 : : // Apply Dynamic Normalization to structural forces
593 : 14664 : double norm_structural = structural_sum * m_smoothed_structural_mult;
594 : :
595 : : // Vibration Effects are calculated in absolute Nm
596 : : // v0.7.110: Apply m_vibration_gain to textures, but NOT to Soft Lock (Issue #206)
597 : 14664 : double vibration_sum_nm = ctx.road_noise + ctx.slide_noise + ctx.spin_rumble + ctx.bottoming_crunch + ctx.abs_pulse_force + ctx.lockup_rumble;
598 : 14664 : double final_texture_nm = (vibration_sum_nm * (double)m_vibration_gain) + ctx.soft_lock_force;
599 : :
600 : : // --- 7. OUTPUT SCALING (Physical Target Model) ---
601 : : // Map structural to the target rim torque, then divide by wheelbase max to get DirectInput %
602 : 14664 : double di_structural = norm_structural * ((double)m_target_rim_nm / wheelbase_max_safe);
603 : :
604 : : // Map absolute texture Nm directly to the wheelbase max
605 : 14664 : double di_texture = final_texture_nm / wheelbase_max_safe;
606 : :
607 : 14664 : double norm_force = (di_structural + di_texture) * m_gain;
608 : :
609 : : // Min Force
610 : : // v0.7.85 FIX: Bypass min_force if NOT allowed (e.g. in garage) unless soft lock is significant.
611 : : // This prevents the "grinding" feel from tiny residuals when FFB should be muted.
612 : 14664 : bool significant_soft_lock = std::abs(ctx.soft_lock_force) > SOFT_LOCK_MUTE_THRESHOLD_NM; // > 0.1 Nm
613 [ + + + + ]: 14664 : if (allowed || significant_soft_lock) {
614 [ + + + + : 12363 : if (std::abs(norm_force) > FFB_EPSILON && std::abs(norm_force) < m_min_force) {
+ + ]
615 [ + + ]: 161 : double sign = (norm_force > 0.0) ? 1.0 : -1.0;
616 : 161 : norm_force = sign * m_min_force;
617 : : }
618 : : }
619 : :
620 [ + + ]: 14664 : if (m_invert_force) {
621 : 3364 : norm_force *= -1.0;
622 : : }
623 : :
624 : : // --- 8. STATE UPDATES (POST-CALC) ---
625 : : // CRITICAL: These updates must run UNCONDITIONALLY every frame to prevent
626 : : // stale state issues when effects are toggled on/off.
627 : : // Use upsampled_data to ensure derivatives (current - prev) / dt
628 : : // are calculated correctly over the 400Hz 2.5ms interval.
629 [ + + ]: 73320 : for (int i = 0; i < 4; i++) {
630 : 58656 : m_prev_vert_deflection[i] = upsampled_data->mWheel[i].mVerticalTireDeflection;
631 : 58656 : m_prev_rotation[i] = upsampled_data->mWheel[i].mRotation;
632 : 58656 : m_prev_brake_pressure[i] = upsampled_data->mWheel[i].mBrakePressure;
633 : : }
634 : 14664 : m_prev_susp_force[0] = upsampled_data->mWheel[0].mSuspForce;
635 : 14664 : m_prev_susp_force[1] = upsampled_data->mWheel[1].mSuspForce;
636 : :
637 : : // v0.6.36 FIX: Move m_prev_vert_accel to unconditional section
638 : : // Previously only updated inside calculate_road_texture when enabled.
639 : : // Now always updated to prevent stale data if other effects use it.
640 : 14664 : m_prev_vert_accel = data->mLocalAccel.y;
641 : :
642 : : // --- 9. SNAPSHOT ---
643 : : // This block captures the current state of the FFB Engine (inputs, outputs, intermediate calculations)
644 : : // into a thread-safe buffer. These snapshots are retrieved by the GUI layer (or other consumers)
645 : : // to visualize real-time telemetry graphs, FFB clipping, and effect contributions.
646 : : // It uses a mutex to protect the shared circular buffer.
647 : : {
648 [ + - ]: 14664 : std::lock_guard<std::mutex> lock(m_debug_mutex);
649 [ + + ]: 14664 : if (m_debug_buffer.size() < DEBUG_BUFFER_CAP) {
650 : : FFBSnapshot snap;
651 : 5350 : snap.total_output = (float)norm_force;
652 : 5350 : snap.base_force = (float)base_input;
653 : 5350 : snap.sop_force = (float)ctx.sop_unboosted_force; // Use unboosted for snapshot
654 : 5350 : snap.understeer_drop = (float)((base_input * m_steering_shaft_gain) * (1.0 - grip_factor_applied));
655 : 5350 : snap.oversteer_boost = (float)(ctx.sop_base_force - ctx.sop_unboosted_force); // Exact boost amount
656 : :
657 : 5350 : snap.ffb_rear_torque = (float)ctx.rear_torque;
658 : 5350 : snap.ffb_scrub_drag = (float)ctx.scrub_drag_force;
659 : 5350 : snap.ffb_yaw_kick = (float)ctx.yaw_force;
660 : 5350 : snap.ffb_gyro_damping = (float)ctx.gyro_force;
661 : 5350 : snap.texture_road = (float)ctx.road_noise;
662 : 5350 : snap.texture_slide = (float)ctx.slide_noise;
663 : 5350 : snap.texture_lockup = (float)ctx.lockup_rumble;
664 : 5350 : snap.texture_spin = (float)ctx.spin_rumble;
665 : 5350 : snap.texture_bottoming = (float)ctx.bottoming_crunch;
666 : 5350 : snap.ffb_abs_pulse = (float)ctx.abs_pulse_force;
667 : 5350 : snap.ffb_soft_lock = (float)ctx.soft_lock_force;
668 : 5350 : snap.session_peak_torque = (float)m_session_peak_torque;
669 [ + + ]: 5350 : snap.clipping = (std::abs(norm_force) > (double)CLIPPING_THRESHOLD) ? 1.0f : 0.0f;
670 : :
671 : : // Physics
672 : 5350 : snap.calc_front_load = (float)ctx.avg_load;
673 : 5350 : snap.calc_rear_load = (float)ctx.avg_rear_load;
674 : 5350 : snap.calc_rear_lat_force = (float)ctx.calc_rear_lat_force;
675 : 5350 : snap.calc_front_grip = (float)ctx.avg_grip;
676 : 5350 : snap.calc_rear_grip = (float)ctx.avg_rear_grip;
677 : 5350 : snap.calc_front_slip_angle_smoothed = (float)m_grip_diag.front_slip_angle;
678 : 5350 : snap.calc_rear_slip_angle_smoothed = (float)m_grip_diag.rear_slip_angle;
679 : :
680 [ + - ]: 5350 : snap.raw_front_slip_angle = (float)calculate_raw_slip_angle_pair(fl, fr);
681 [ + - ]: 5350 : snap.raw_rear_slip_angle = (float)calculate_raw_slip_angle_pair(data->mWheel[2], data->mWheel[3]);
682 : :
683 : : // Telemetry
684 : 5350 : snap.steer_force = (float)raw_torque;
685 : 5350 : snap.raw_shaft_torque = (float)data->mSteeringShaftTorque;
686 : 5350 : snap.raw_gen_torque = (float)genFFBTorque;
687 : 5350 : snap.raw_input_steering = (float)data->mUnfilteredSteering;
688 : 5350 : snap.raw_front_tire_load = (float)raw_load;
689 : 5350 : snap.raw_front_grip_fract = (float)raw_grip;
690 : 5350 : snap.raw_rear_grip = (float)((data->mWheel[2].mGripFract + data->mWheel[3].mGripFract) / DUAL_DIVISOR);
691 : 5350 : snap.raw_front_susp_force = (float)((fl.mSuspForce + fr.mSuspForce) / DUAL_DIVISOR);
692 : 5350 : snap.raw_front_ride_height = (float)((std::min)(fl.mRideHeight, fr.mRideHeight));
693 : 5350 : snap.raw_rear_lat_force = (float)((data->mWheel[2].mLateralForce + data->mWheel[3].mLateralForce) / DUAL_DIVISOR);
694 : 5350 : snap.raw_car_speed = (float)ctx.car_speed_long;
695 : 5350 : snap.raw_input_throttle = (float)data->mUnfilteredThrottle;
696 : 5350 : snap.raw_input_brake = (float)data->mUnfilteredBrake;
697 : 5350 : snap.accel_x = (float)data->mLocalAccel.x;
698 : 5350 : snap.raw_front_lat_patch_vel = (float)((std::abs(fl.mLateralPatchVel) + std::abs(fr.mLateralPatchVel)) / DUAL_DIVISOR);
699 : 5350 : snap.raw_front_deflection = (float)((fl.mVerticalTireDeflection + fr.mVerticalTireDeflection) / DUAL_DIVISOR);
700 : 5350 : snap.raw_front_long_patch_vel = (float)((fl.mLongitudinalPatchVel + fr.mLongitudinalPatchVel) / DUAL_DIVISOR);
701 : 5350 : snap.raw_rear_lat_patch_vel = (float)((std::abs(data->mWheel[2].mLateralPatchVel) + std::abs(data->mWheel[3].mLateralPatchVel)) / DUAL_DIVISOR);
702 : 5350 : snap.raw_rear_long_patch_vel = (float)((data->mWheel[2].mLongitudinalPatchVel + data->mWheel[3].mLongitudinalPatchVel) / DUAL_DIVISOR);
703 : :
704 : 5350 : float sm_range_rad = data->mPhysicalSteeringWheelRange;
705 : 5350 : float range_deg = sm_range_rad * (180.0f / (float)PI);
706 : :
707 : : // Fallback to REST API if enabled and SM range is invalid (Issue #221)
708 [ + + + - ]: 5350 : if (m_rest_api_enabled && sm_range_rad <= 0.0f) {
709 [ + - + - ]: 1 : float fallback = RestApiProvider::Get().GetFallbackRangeDeg();
710 [ - + ]: 1 : if (fallback > 0.0f) {
711 : 0 : range_deg = fallback;
712 : : }
713 : : }
714 : :
715 : 5350 : snap.steering_range_deg = range_deg;
716 : 5350 : snap.steering_angle_deg = (float)data->mUnfilteredSteering * (range_deg / 2.0f);
717 : :
718 : 5350 : snap.warn_load = ctx.frame_warn_load;
719 [ + + + + ]: 5350 : snap.warn_grip = ctx.frame_warn_grip || ctx.frame_warn_rear_grip;
720 : 5350 : snap.warn_dt = ctx.frame_warn_dt;
721 : 5350 : snap.debug_freq = (float)m_debug_freq;
722 : 5350 : snap.tire_radius = (float)fl.mStaticUndeflectedRadius / 100.0f;
723 : 5350 : snap.slope_current = (float)m_slope_current; // v0.7.1: Slope detection diagnostic
724 : :
725 : 5350 : snap.ffb_rate = (float)m_ffb_rate;
726 : 5350 : snap.telemetry_rate = (float)m_telemetry_rate;
727 : 5350 : snap.hw_rate = (float)m_hw_rate;
728 : 5350 : snap.torque_rate = (float)m_torque_rate;
729 : 5350 : snap.gen_torque_rate = (float)m_gen_torque_rate;
730 : :
731 [ + - ]: 5350 : m_debug_buffer.push_back(snap);
732 : : }
733 : 14664 : }
734 : :
735 : : // Telemetry Logging (v0.7.x)
736 [ + - + + ]: 14664 : if (AsyncLogger::Get().IsLogging()) {
737 : : LogFrame frame;
738 : 2534 : frame.timestamp = data->mElapsedTime;
739 : 2534 : frame.delta_time = data->mDeltaTime;
740 : :
741 : : // Inputs
742 : 2534 : frame.steering = (float)data->mUnfilteredSteering;
743 : 2534 : frame.throttle = (float)data->mUnfilteredThrottle;
744 : 2534 : frame.brake = (float)data->mUnfilteredBrake;
745 : :
746 : : // Vehicle state
747 : 2534 : frame.speed = (float)ctx.car_speed;
748 : 2534 : frame.lat_accel = (float)data->mLocalAccel.x;
749 : 2534 : frame.long_accel = (float)data->mLocalAccel.z;
750 : 2534 : frame.yaw_rate = (float)data->mLocalRot.y;
751 : :
752 : : // Front Axle raw
753 : 2534 : frame.slip_angle_fl = (float)fl.mLateralPatchVel / (float)(std::max)(1.0, ctx.car_speed);
754 : 2534 : frame.slip_angle_fr = (float)fr.mLateralPatchVel / (float)(std::max)(1.0, ctx.car_speed);
755 [ + - ]: 2534 : frame.slip_ratio_fl = (float)calculate_wheel_slip_ratio(fl);
756 [ + - ]: 2534 : frame.slip_ratio_fr = (float)calculate_wheel_slip_ratio(fr);
757 : 2534 : frame.grip_fl = (float)fl.mGripFract;
758 : 2534 : frame.grip_fr = (float)fr.mGripFract;
759 : 2534 : frame.load_fl = (float)fl.mTireLoad;
760 : 2534 : frame.load_fr = (float)fr.mTireLoad;
761 : :
762 : : // Calculated values
763 : 2534 : frame.calc_slip_angle_front = (float)m_grip_diag.front_slip_angle;
764 : 2534 : frame.calc_grip_front = (float)ctx.avg_grip;
765 : :
766 : : // Slope detection
767 : 2534 : frame.dG_dt = (float)m_slope_dG_dt;
768 : 2534 : frame.dAlpha_dt = (float)m_slope_dAlpha_dt;
769 : 2534 : frame.slope_current = (float)m_slope_current;
770 : 2534 : frame.slope_raw_unclamped = (float)m_debug_slope_raw;
771 : 2534 : frame.slope_numerator = (float)m_debug_slope_num;
772 : 2534 : frame.slope_denominator = (float)m_debug_slope_den;
773 : 2534 : frame.hold_timer = (float)m_slope_hold_timer;
774 : 2534 : frame.input_slip_smoothed = (float)m_slope_slip_smoothed;
775 : 2534 : frame.slope_smoothed = (float)m_slope_smoothed_output;
776 : :
777 : : // Use extracted confidence calculation
778 [ + - ]: 2534 : frame.confidence = (float)calculate_slope_confidence(m_slope_dAlpha_dt);
779 : :
780 : : // Surface types (Accuracy Tools)
781 : 2534 : frame.surface_type_fl = (float)fl.mSurfaceType;
782 : 2534 : frame.surface_type_fr = (float)fr.mSurfaceType;
783 : :
784 : : // Advanced Slope Detection (v0.7.40)
785 : 2534 : frame.slope_torque = (float)m_slope_torque_current;
786 : 2534 : frame.slew_limited_g = (float)m_debug_lat_g_slew;
787 : :
788 : : // Rear axle
789 : 2534 : frame.calc_grip_rear = (float)ctx.avg_rear_grip;
790 : 2534 : frame.grip_delta = (float)(ctx.avg_grip - ctx.avg_rear_grip);
791 : :
792 : : // FFB output
793 : 2534 : frame.ffb_total = (float)norm_force;
794 : 2534 : frame.ffb_shaft_torque = (float)data->mSteeringShaftTorque;
795 : 2534 : frame.ffb_gen_torque = (float)genFFBTorque;
796 : 2534 : frame.ffb_grip_factor = (float)ctx.grip_factor;
797 : 2534 : frame.ffb_sop = (float)ctx.sop_base_force;
798 : 2534 : frame.speed_gate = (float)ctx.speed_gate;
799 : 2534 : frame.load_peak_ref = (float)m_auto_peak_load;
800 : 2534 : frame.clipping = (std::abs(norm_force) > CLIPPING_THRESHOLD);
801 : 2534 : frame.ffb_base = (float)base_input; // Plan included ffb_base
802 : :
803 [ + - + - ]: 2534 : AsyncLogger::Get().Log(frame);
804 : : }
805 : :
806 : 14664 : return (std::max)(-1.0, (std::min)(1.0, norm_force));
807 : 14665 : }
808 : :
809 : : // Helper: Calculate Seat-of-the-Pants (SoP) Lateral & Oversteer Boost
810 : 14665 : void FFBEngine::calculate_sop_lateral(const TelemInfoV01* data, FFBCalculationContext& ctx) {
811 : : // 1. Raw Lateral G (Chassis-relative X)
812 : : // Clamp to 5G to prevent numeric instability in crashes
813 : 14665 : double raw_g = (std::max)(-G_LIMIT_5G * GRAVITY_MS2, (std::min)(G_LIMIT_5G * GRAVITY_MS2, data->mLocalAccel.x));
814 : 14665 : double lat_g = (raw_g / GRAVITY_MS2);
815 : :
816 : : // Smoothing: Map 0.0-1.0 slider to 0.1-0.0001s tau
817 : 14665 : double smoothness = 1.0 - (double)m_sop_smoothing_factor;
818 : 14665 : smoothness = (std::max)(0.0, (std::min)(SMOOTHNESS_LIMIT_0999, smoothness));
819 : 14665 : double tau = smoothness * SOP_SMOOTHING_MAX_TAU;
820 : 14665 : double alpha = ctx.dt / (tau + ctx.dt);
821 : 14665 : alpha = (std::max)(MIN_LFM_ALPHA, (std::min)(1.0, alpha));
822 : 14665 : m_sop_lat_g_smoothed += alpha * (lat_g - m_sop_lat_g_smoothed);
823 : :
824 : : // Base SoP Force
825 : 14665 : double sop_base = m_sop_lat_g_smoothed * m_sop_effect * (double)m_sop_scale;
826 : 14665 : ctx.sop_unboosted_force = sop_base; // Store for snapshot
827 : :
828 : : // 2. Oversteer Boost (Grip Differential)
829 : : // Calculate Rear Grip
830 : 14665 : GripResult rear_grip_res = calculate_grip(data->mWheel[2], data->mWheel[3], ctx.avg_load, m_warned_rear_grip,
831 : 14665 : m_prev_slip_angle[2], m_prev_slip_angle[3],
832 [ + - ]: 14665 : ctx.car_speed, ctx.dt, data->mVehicleName, data, false /* is_front */);
833 : 14665 : ctx.avg_rear_grip = rear_grip_res.value;
834 : 14665 : m_grip_diag.rear_original = rear_grip_res.original;
835 : 14665 : m_grip_diag.rear_approximated = rear_grip_res.approximated;
836 : 14665 : m_grip_diag.rear_slip_angle = rear_grip_res.slip_angle;
837 [ + + ]: 14665 : if (rear_grip_res.approximated) ctx.frame_warn_rear_grip = true;
838 : :
839 [ + + ]: 14665 : if (!m_slope_detection_enabled) {
840 : 12624 : double grip_delta = ctx.avg_grip - ctx.avg_rear_grip;
841 [ + + ]: 12624 : if (grip_delta > 0.0) {
842 : 940 : sop_base *= (1.0 + (grip_delta * m_oversteer_boost * OVERSTEER_BOOST_MULT));
843 : : }
844 : : }
845 : 14665 : ctx.sop_base_force = sop_base;
846 : :
847 : : // 3. Rear Aligning Torque (v0.4.9)
848 : : // Calculate load for rear wheels (for tire stiffness scaling)
849 [ + - ]: 14665 : double calc_load_rl = approximate_rear_load(data->mWheel[2]);
850 [ + - ]: 14665 : double calc_load_rr = approximate_rear_load(data->mWheel[3]);
851 : 14665 : ctx.avg_rear_load = (calc_load_rl + calc_load_rr) / DUAL_DIVISOR;
852 : :
853 : : // Rear lateral force estimation: F = Alpha * k * TireLoad
854 : 14665 : double rear_slip_angle = m_grip_diag.rear_slip_angle;
855 : 14665 : ctx.calc_rear_lat_force = rear_slip_angle * ctx.avg_rear_load * REAR_TIRE_STIFFNESS_COEFFICIENT;
856 : 14665 : ctx.calc_rear_lat_force = (std::max)(-MAX_REAR_LATERAL_FORCE, (std::min)(MAX_REAR_LATERAL_FORCE, ctx.calc_rear_lat_force));
857 : :
858 : : // Torque = Force * Aligning_Lever
859 : : // Note negative sign: Oversteer (Rear Slide) pushes wheel TOWARDS slip direction
860 : 14665 : ctx.rear_torque = -ctx.calc_rear_lat_force * REAR_ALIGN_TORQUE_COEFFICIENT * m_rear_align_effect;
861 : :
862 : : // 4. Yaw Kick (Inertial Oversteer)
863 : 14665 : double raw_yaw_accel = data->mLocalRotAccel.y;
864 : : // v0.4.16: Reject yaw at low speeds and below threshold
865 [ + + + + : 14665 : if (ctx.car_speed < MIN_YAW_KICK_SPEED_MS || std::abs(raw_yaw_accel) < (double)m_yaw_kick_threshold) {
+ + ]
866 : 3323 : raw_yaw_accel = 0.0;
867 : : }
868 : :
869 : : // Alpha Smoothing (v0.4.16)
870 : 14665 : double tau_yaw = (double)m_yaw_accel_smoothing;
871 [ + + ]: 14665 : if (tau_yaw < MIN_TAU_S) tau_yaw = MIN_TAU_S;
872 : 14665 : double alpha_yaw = ctx.dt / (tau_yaw + ctx.dt);
873 : 14665 : m_yaw_accel_smoothed += alpha_yaw * (raw_yaw_accel - m_yaw_accel_smoothed);
874 : :
875 : 14665 : ctx.yaw_force = -1.0 * m_yaw_accel_smoothed * m_sop_yaw_gain * (double)BASE_NM_YAW_KICK;
876 : :
877 : : // Apply speed gate to all lateral effects
878 : 14665 : ctx.sop_base_force *= ctx.speed_gate;
879 : 14665 : ctx.rear_torque *= ctx.speed_gate;
880 : 14665 : ctx.yaw_force *= ctx.speed_gate;
881 : 14665 : }
882 : :
883 : : // Helper: Calculate Gyroscopic Damping (v0.4.17)
884 : 14670 : void FFBEngine::calculate_gyro_damping(const TelemInfoV01* data, FFBCalculationContext& ctx) {
885 : : // 1. Calculate Steering Velocity (rad/s)
886 : 14670 : float range = data->mPhysicalSteeringWheelRange;
887 : :
888 : : // Fallback to REST API if enabled and SM range is invalid (Issue #221)
889 [ + + + - ]: 14670 : if (m_rest_api_enabled && range <= 0.0f) {
890 : 1 : float fallback_deg = RestApiProvider::Get().GetFallbackRangeDeg();
891 [ - + ]: 1 : if (fallback_deg > 0.0f) {
892 : 0 : range = fallback_deg * ((float)PI / 180.0f);
893 : : }
894 : : }
895 : :
896 [ + + ]: 14670 : if (range <= 0.0f) range = (float)DEFAULT_STEERING_RANGE_RAD;
897 : 14670 : double steer_angle = data->mUnfilteredSteering * (range / DUAL_DIVISOR);
898 : 14670 : double steer_vel = (steer_angle - m_prev_steering_angle) / ctx.dt;
899 : 14670 : m_prev_steering_angle = steer_angle;
900 : :
901 : : // 2. Alpha Smoothing
902 : 14670 : double tau_gyro = (double)m_gyro_smoothing;
903 [ + + ]: 14670 : if (tau_gyro < MIN_TAU_S) tau_gyro = MIN_TAU_S;
904 : 14670 : double alpha_gyro = ctx.dt / (tau_gyro + ctx.dt);
905 : 14670 : m_steering_velocity_smoothed += alpha_gyro * (steer_vel - m_steering_velocity_smoothed);
906 : :
907 : : // 3. Force = -Vel * Gain * Speed_Scaling
908 : : // Speed scaling: Gyro effect increases with wheel RPM (car speed)
909 : 14670 : ctx.gyro_force = -1.0 * m_steering_velocity_smoothed * m_gyro_gain * (ctx.car_speed / GYRO_SPEED_SCALE);
910 : 14670 : }
911 : :
912 : : // Helper: Calculate ABS Pulse (v0.7.53)
913 : 15672 : void FFBEngine::calculate_abs_pulse(const TelemInfoV01* data, FFBCalculationContext& ctx) {
914 [ + + ]: 15672 : if (!m_abs_pulse_enabled) return;
915 : :
916 : 1074 : bool abs_active = false;
917 [ + + ]: 1322 : for (int i = 0; i < 4; i++) {
918 : : // Detection: Sudden pressure oscillation + high brake pedal
919 : 1260 : double pressure_delta = (data->mWheel[i].mBrakePressure - m_prev_brake_pressure[i]) / ctx.dt;
920 [ + + + + : 1260 : if (data->mUnfilteredBrake > ABS_PEDAL_THRESHOLD && std::abs(pressure_delta) > ABS_PRESSURE_RATE_THRESHOLD) {
+ + ]
921 : 1012 : abs_active = true;
922 : 1012 : break;
923 : : }
924 : : }
925 : :
926 [ + + ]: 1074 : if (abs_active) {
927 : : // Generate sine pulse
928 : 1012 : m_abs_phase += (double)m_abs_freq_hz * ctx.dt * TWO_PI;
929 : 1012 : m_abs_phase = std::fmod(m_abs_phase, TWO_PI);
930 : 1012 : ctx.abs_pulse_force = (double)(std::sin(m_abs_phase) * m_abs_gain * ABS_PULSE_MAGNITUDE_SCALER * ctx.speed_gate);
931 : : }
932 : : }
933 : :
934 : : // Helper: Calculate Lockup Vibration (v0.4.36 - REWRITTEN as dedicated method)
935 : 14664 : void FFBEngine::calculate_lockup_vibration(const TelemInfoV01* data, FFBCalculationContext& ctx) {
936 [ + + ]: 14664 : if (!m_lockup_enabled) return;
937 : :
938 : 3896 : double worst_severity = 0.0;
939 : 3896 : double chosen_freq_multiplier = 1.0;
940 : 3896 : double chosen_pressure_factor = 0.0;
941 : :
942 : : // Calculate reference slip for front wheels (v0.4.38)
943 [ + - ]: 3896 : double slip_fl = calculate_wheel_slip_ratio(data->mWheel[0]);
944 [ + - ]: 3896 : double slip_fr = calculate_wheel_slip_ratio(data->mWheel[1]);
945 : 3896 : double worst_front = (std::min)(slip_fl, slip_fr);
946 : :
947 [ + + ]: 19480 : for (int i = 0; i < 4; i++) {
948 : 15584 : const auto& w = data->mWheel[i];
949 [ + - ]: 15584 : double slip = calculate_wheel_slip_ratio(w);
950 : 15584 : double slip_abs = std::abs(slip);
951 : :
952 : : // 1. Predictive Lockup (v0.4.38)
953 : : // Detects rapidly decelerating wheels BEFORE they reach full lock
954 : 15584 : double wheel_accel = (w.mRotation - m_prev_rotation[i]) / ctx.dt;
955 : 15584 : double radius = (double)w.mStaticUndeflectedRadius / UNIT_CM_TO_M;
956 [ + + ]: 15584 : if (radius < RADIUS_FALLBACK_MIN_M) radius = RADIUS_FALLBACK_DEFAULT_M;
957 : 15584 : double car_dec_ang = -std::abs(data->mLocalAccel.z / radius);
958 : :
959 : : // Signal Quality Check (Reject surface bumps)
960 : 15584 : double susp_vel = std::abs(w.mVerticalTireDeflection - m_prev_vert_deflection[i]) / ctx.dt;
961 : 15584 : bool is_bumpy = (susp_vel > (double)m_lockup_bump_reject);
962 : :
963 : : // Pre-conditions
964 : 15584 : bool brake_active = (data->mUnfilteredBrake > PREDICTION_BRAKE_THRESHOLD);
965 : 15584 : bool is_grounded = (w.mSuspForce > PREDICTION_LOAD_THRESHOLD);
966 : :
967 : 15584 : double start_threshold = (double)m_lockup_start_pct / PERCENT_TO_DECIMAL;
968 : 15584 : double full_threshold = (double)m_lockup_full_pct / PERCENT_TO_DECIMAL;
969 : 15584 : double trigger_threshold = full_threshold;
970 : :
971 [ + + + + : 15584 : if (brake_active && is_grounded && !is_bumpy) {
+ + ]
972 : : // Predictive Trigger: Wheel decelerating significantly faster than chassis
973 : 242 : double sensitivity_threshold = -1.0 * (double)m_lockup_prediction_sens;
974 [ + + + - ]: 242 : if (wheel_accel < car_dec_ang * LOCKUP_ACCEL_MARGIN && wheel_accel < sensitivity_threshold) {
975 : 2 : trigger_threshold = start_threshold; // Ease into effect earlier
976 : : }
977 : : }
978 : :
979 : : // 2. Intensity Calculation
980 [ + + ]: 15584 : if (slip_abs > trigger_threshold) {
981 : 283 : double window = full_threshold - start_threshold;
982 [ - + ]: 283 : if (window < MIN_SLIP_WINDOW) window = MIN_SLIP_WINDOW;
983 : :
984 : 283 : double normalized = (slip_abs - start_threshold) / window;
985 : 283 : double severity = (std::min)(1.0, (std::max)(0.0, normalized));
986 : :
987 : : // Apply gamma for curve control
988 : 283 : severity = std::pow(severity, (double)m_lockup_gamma);
989 : :
990 : : // Frequency calculation
991 : 283 : double freq_mult = 1.0;
992 [ + + ]: 283 : if (i >= 2) {
993 : : // v0.4.38: Rear wheels use a different frequency to distinguish front/rear lockup
994 [ + + ]: 34 : if (slip < (worst_front - AXLE_DIFF_HYSTERESIS)) {
995 : 2 : freq_mult = LOCKUP_FREQ_MULTIPLIER_REAR;
996 : : }
997 : : }
998 : :
999 : : // Pressure weighting (v0.4.38)
1000 : 283 : double pressure_factor = w.mBrakePressure;
1001 [ + + + + ]: 283 : if (pressure_factor < LOW_PRESSURE_LOCKUP_THRESHOLD && slip_abs > LOW_PRESSURE_LOCKUP_FIX) pressure_factor = LOW_PRESSURE_LOCKUP_FIX; // Catch low-pressure lockups
1002 : :
1003 [ + + ]: 283 : if (severity > worst_severity) {
1004 : 134 : worst_severity = severity;
1005 : 134 : chosen_freq_multiplier = freq_mult;
1006 : 134 : chosen_pressure_factor = pressure_factor;
1007 : : }
1008 : : }
1009 : : }
1010 : :
1011 : : // 3. Vibration Synthesis
1012 [ + + ]: 3896 : if (worst_severity > 0.0) {
1013 : 134 : double base_freq = LOCKUP_BASE_FREQ + (ctx.car_speed * LOCKUP_FREQ_SPEED_MULT);
1014 : 134 : double final_freq = base_freq * chosen_freq_multiplier * (double)m_lockup_freq_scale;
1015 : :
1016 : 134 : m_lockup_phase += final_freq * ctx.dt * TWO_PI;
1017 : 134 : m_lockup_phase = std::fmod(m_lockup_phase, TWO_PI);
1018 : :
1019 : 134 : double amp = worst_severity * chosen_pressure_factor * m_lockup_gain * (double)BASE_NM_LOCKUP_VIBRATION * ctx.brake_load_factor;
1020 : :
1021 : : // v0.4.38: Boost rear lockup volume
1022 [ + + ]: 134 : if (chosen_freq_multiplier < 1.0) amp *= (double)m_lockup_rear_boost;
1023 : :
1024 : 134 : ctx.lockup_rumble = std::sin(m_lockup_phase) * amp * ctx.speed_gate;
1025 : : }
1026 : : }
1027 : :
1028 : : // Helper: Calculate Wheel Spin Vibration (v0.6.36)
1029 : 14666 : void FFBEngine::calculate_wheel_spin(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1030 [ + + + + ]: 14666 : if (m_spin_enabled && data->mUnfilteredThrottle > SPIN_THROTTLE_THRESHOLD) {
1031 [ + - ]: 26 : double slip_rl = calculate_wheel_slip_ratio(data->mWheel[2]);
1032 [ + - ]: 26 : double slip_rr = calculate_wheel_slip_ratio(data->mWheel[3]);
1033 : 26 : double max_slip = (std::max)(slip_rl, slip_rr);
1034 : :
1035 [ + - ]: 26 : if (max_slip > SPIN_SLIP_THRESHOLD) {
1036 : 26 : double severity = (max_slip - SPIN_SLIP_THRESHOLD) / SPIN_SEVERITY_RANGE;
1037 : 26 : severity = (std::min)(1.0, severity);
1038 : :
1039 : : // Attenuate primary torque when spinning (Torque Drop)
1040 : : // v0.6.43: Blunted effect (0.6 multiplier) to prevent complete loss of feel
1041 : 26 : ctx.gain_reduction_factor = (1.0 - (severity * m_spin_gain * SPIN_TORQUE_DROP_FACTOR));
1042 : :
1043 : : // Generate vibration based on spin velocity (RPM delta)
1044 : 26 : double slip_speed_ms = ctx.car_speed * max_slip;
1045 : 26 : double freq = (SPIN_BASE_FREQ + (slip_speed_ms * SPIN_FREQ_SLIP_MULT)) * (double)m_spin_freq_scale;
1046 [ + + ]: 26 : if (freq > SPIN_MAX_FREQ) freq = SPIN_MAX_FREQ; // Human sensory limit for gross vibration
1047 : :
1048 : 26 : m_spin_phase += freq * ctx.dt * TWO_PI;
1049 : 26 : m_spin_phase = std::fmod(m_spin_phase, TWO_PI);
1050 : :
1051 : 26 : double amp = severity * m_spin_gain * (double)BASE_NM_SPIN_VIBRATION;
1052 : 26 : ctx.spin_rumble = std::sin(m_spin_phase) * amp;
1053 : : }
1054 : : }
1055 : 14666 : }
1056 : :
1057 : : // Helper: Calculate Slide Texture (Friction Vibration)
1058 : 14667 : void FFBEngine::calculate_slide_texture(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1059 [ + + ]: 14667 : if (!m_slide_texture_enabled) return;
1060 : :
1061 : : // Use average lateral patch velocity of front wheels
1062 : 1157 : double lat_vel_fl = std::abs(data->mWheel[0].mLateralPatchVel);
1063 : 1157 : double lat_vel_fr = std::abs(data->mWheel[1].mLateralPatchVel);
1064 : 1157 : double front_slip_avg = (lat_vel_fl + lat_vel_fr) / DUAL_DIVISOR;
1065 : :
1066 : : // Use average lateral patch velocity of rear wheels
1067 : 1157 : double lat_vel_rl = std::abs(data->mWheel[2].mLateralPatchVel);
1068 : 1157 : double lat_vel_rr = std::abs(data->mWheel[3].mLateralPatchVel);
1069 : 1157 : double rear_slip_avg = (lat_vel_rl + lat_vel_rr) / DUAL_DIVISOR;
1070 : :
1071 : : // Use the max slide velocity between axles
1072 : 1157 : double effective_slip_vel = (std::max)(front_slip_avg, rear_slip_avg);
1073 : :
1074 [ + + ]: 1157 : if (effective_slip_vel > SLIDE_VEL_THRESHOLD) {
1075 : : // High-frequency sawtooth noise for localized friction feel
1076 : 1143 : double base_freq = SLIDE_BASE_FREQ + (effective_slip_vel * SLIDE_FREQ_VEL_MULT);
1077 : 1143 : double freq = base_freq * (double)m_slide_freq_scale;
1078 : :
1079 [ + + ]: 1143 : if (freq > SLIDE_MAX_FREQ) freq = SLIDE_MAX_FREQ; // Hard clamp for hardware safety
1080 : :
1081 : 1143 : m_slide_phase += freq * ctx.dt * TWO_PI;
1082 : 1143 : m_slide_phase = std::fmod(m_slide_phase, TWO_PI);
1083 : :
1084 : : // Sawtooth generator (0 to 1 range across TWO_PI) -> (-1 to 1)
1085 : 1143 : double sawtooth = (m_slide_phase / TWO_PI) * SAWTOOTH_SCALE - SAWTOOTH_OFFSET;
1086 : :
1087 : : // Intensity scaling (Grip based)
1088 : 1143 : double grip_scale = (std::max)(0.0, 1.0 - ctx.avg_grip);
1089 : :
1090 : 1143 : ctx.slide_noise = sawtooth * m_slide_texture_gain * (double)BASE_NM_SLIDE_TEXTURE * ctx.texture_load_factor * grip_scale;
1091 : : }
1092 : : }
1093 : :
1094 : : // Helper: Calculate Road Texture & Scrub Drag
1095 : 14666 : void FFBEngine::calculate_road_texture(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1096 : : // 1. Scrub Drag (Longitudinal resistive force from lateral sliding)
1097 [ + + ]: 14666 : if (m_scrub_drag_gain > 0.0) {
1098 : 1085 : double avg_lat_vel = (data->mWheel[0].mLateralPatchVel + data->mWheel[1].mLateralPatchVel) / DUAL_DIVISOR;
1099 : 1085 : double abs_lat_vel = std::abs(avg_lat_vel);
1100 : :
1101 [ + - ]: 1085 : if (abs_lat_vel > SCRUB_VEL_THRESHOLD) {
1102 : 1085 : double fade = (std::min)(1.0, abs_lat_vel / SCRUB_FADE_RANGE); // Fade in over 0.5m/s
1103 [ + + ]: 1085 : double drag_dir = (avg_lat_vel > 0.0) ? -1.0 : 1.0;
1104 : 1085 : ctx.scrub_drag_force = drag_dir * m_scrub_drag_gain * (double)BASE_NM_SCRUB_DRAG * fade;
1105 : : }
1106 : : }
1107 : :
1108 [ + + ]: 14666 : if (!m_road_texture_enabled) return;
1109 : :
1110 : : // 2. Road Texture (Delta Deflection Method)
1111 : : // Measures the rate of change in tire vertical compression
1112 : 3861 : double delta_l = data->mWheel[0].mVerticalTireDeflection - m_prev_vert_deflection[0];
1113 : 3861 : double delta_r = data->mWheel[1].mVerticalTireDeflection - m_prev_vert_deflection[1];
1114 : :
1115 : : // Outlier rejection (crashes/jumps)
1116 : 3861 : delta_l = (std::max)(-DEFLECTION_DELTA_LIMIT, (std::min)(DEFLECTION_DELTA_LIMIT, delta_l));
1117 : 3861 : delta_r = (std::max)(-DEFLECTION_DELTA_LIMIT, (std::min)(DEFLECTION_DELTA_LIMIT, delta_r));
1118 : :
1119 : 3861 : double road_noise_val = 0.0;
1120 : :
1121 : : // FALLBACK (v0.6.36): If mVerticalTireDeflection is missing (Encrypted DLC),
1122 : : // use Chassis Vertical Acceleration delta as a secondary source.
1123 [ + + - + ]: 3861 : bool deflection_active = (std::abs(delta_l) > DEFLECTION_ACTIVE_THRESHOLD || std::abs(delta_r) > DEFLECTION_ACTIVE_THRESHOLD);
1124 : :
1125 [ + + + + ]: 3861 : if (deflection_active || ctx.car_speed < ROAD_TEXTURE_SPEED_THRESHOLD) {
1126 : 3329 : road_noise_val = (delta_l + delta_r) * DEFLECTION_NM_SCALE; // Scale to NM
1127 : : } else {
1128 : : // Fallback to vertical acceleration rate-of-change (jerk-like scaling)
1129 : 532 : double vert_accel = data->mLocalAccel.y;
1130 : 532 : double delta_accel = vert_accel - m_prev_vert_accel;
1131 : 532 : road_noise_val = delta_accel * ACCEL_ROAD_TEXTURE_SCALE * DEFLECTION_NM_SCALE; // Blend into similar range
1132 : : }
1133 : :
1134 : 3861 : ctx.road_noise = road_noise_val * m_road_texture_gain * ctx.texture_load_factor;
1135 : 3861 : ctx.road_noise *= ctx.speed_gate;
1136 : : }
1137 : :
1138 : 666 : void FFBEngine::ResetNormalization() {
1139 [ + - ]: 666 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
1140 : :
1141 : 666 : m_warned_invalid_range = false; // Issue #218: Reset warning on manual normalization reset
1142 : :
1143 : : // 1. Structural Normalization Reset (Stage 1)
1144 : : // If disabled, we return to the user's manual target.
1145 : : // If enabled, we reset to the target to restart the learning process.
1146 : 666 : m_session_peak_torque = (std::max)(1.0, (double)m_target_rim_nm);
1147 : 666 : m_smoothed_structural_mult = 1.0 / (m_session_peak_torque + EPSILON_DIV);
1148 : 666 : m_rolling_average_torque = m_session_peak_torque;
1149 : :
1150 : : // 2. Vibration Normalization Reset (Stage 3)
1151 : : // Always return to the class-default seed load.
1152 [ + - ]: 666 : ParsedVehicleClass vclass = ParseVehicleClass(m_current_class_name.c_str(), m_vehicle_name);
1153 [ + - ]: 666 : m_auto_peak_load = GetDefaultLoadForClass(vclass);
1154 : :
1155 : : // Reset static load reference
1156 : 666 : m_static_front_load = m_auto_peak_load * 0.5;
1157 : 666 : m_static_load_latched = false;
1158 : :
1159 : : // If we have a saved static load, restore it (v0.7.70 logic)
1160 : 666 : double saved_load = 0.0;
1161 [ + - + - : 1332 : if (Config::GetSavedStaticLoad(m_vehicle_name, saved_load)) {
- + ]
1162 : 0 : m_static_front_load = saved_load;
1163 : 0 : m_static_load_latched = true;
1164 : : }
1165 : :
1166 : 666 : m_smoothed_vibration_mult = 1.0;
1167 : :
1168 [ + - + - ]: 666 : std::cout << "[FFB] Normalization state reset. Structural Peak: " << m_session_peak_torque
1169 [ + - + - : 666 : << " Nm | Load Peak: " << m_auto_peak_load << " N" << std::endl;
+ - + - ]
1170 : 666 : }
1171 : :
1172 : : // Helper: Calculate Suspension Bottoming (v0.6.22)
1173 : : // NOTE: calculate_soft_lock has been moved to SteeringUtils.cpp.
1174 : 14670 : void FFBEngine::calculate_suspension_bottoming(const TelemInfoV01* data, FFBCalculationContext& ctx) {
1175 [ + + ]: 14670 : if (!m_bottoming_enabled) return;
1176 : 3650 : bool triggered = false;
1177 : 3650 : double intensity = 0.0;
1178 : :
1179 : : // Method 0: Direct Ride Height Monitoring
1180 [ + + ]: 3650 : if (m_bottoming_method == 0) {
1181 : 3641 : double min_rh = (std::min)(data->mWheel[0].mRideHeight, data->mWheel[1].mRideHeight);
1182 [ + + + - ]: 3641 : if (min_rh < BOTTOMING_RH_THRESHOLD_M && min_rh > -1.0) { // < 2mm
1183 : 3640 : triggered = true;
1184 : 3640 : intensity = (BOTTOMING_RH_THRESHOLD_M - min_rh) / BOTTOMING_RH_THRESHOLD_M; // Map 2mm->0mm to 0.0->1.0
1185 : : }
1186 : : } else {
1187 : : // Method 1: Suspension Force Impulse (Rate of Change)
1188 : 9 : double dForceL = (data->mWheel[0].mSuspForce - m_prev_susp_force[0]) / ctx.dt;
1189 : 9 : double dForceR = (data->mWheel[1].mSuspForce - m_prev_susp_force[1]) / ctx.dt;
1190 : 9 : double max_dForce = (std::max)(dForceL, dForceR);
1191 : :
1192 [ + + ]: 9 : if (max_dForce > BOTTOMING_IMPULSE_THRESHOLD_N_S) { // 100kN/s impulse
1193 : 5 : triggered = true;
1194 : 5 : intensity = (max_dForce - BOTTOMING_IMPULSE_THRESHOLD_N_S) / BOTTOMING_IMPULSE_RANGE_N_S;
1195 : : }
1196 : : }
1197 : :
1198 : : // Safety Trigger: Raw Load Peak (Catches Method 0/1 failures)
1199 [ + + ]: 3650 : if (!triggered) {
1200 : 5 : double max_load = (std::max)(data->mWheel[0].mTireLoad, data->mWheel[1].mTireLoad);
1201 : 5 : double bottoming_threshold = m_static_front_load * BOTTOMING_LOAD_MULT;
1202 [ + + ]: 5 : if (max_load > bottoming_threshold) {
1203 : 2 : triggered = true;
1204 : 2 : double excess = max_load - bottoming_threshold;
1205 : 2 : intensity = std::sqrt(excess) * BOTTOMING_INTENSITY_SCALE; // Non-linear response
1206 : : }
1207 : : }
1208 : :
1209 [ + + ]: 3650 : if (triggered) {
1210 : : // Generate high-intensity low-frequency "thump"
1211 : 3647 : double bump_magnitude = intensity * m_bottoming_gain * (double)BASE_NM_BOTTOMING;
1212 : 3647 : double freq = BOTTOMING_FREQ_HZ;
1213 : :
1214 : 3647 : m_bottoming_phase += freq * ctx.dt * TWO_PI;
1215 : 3647 : m_bottoming_phase = std::fmod(m_bottoming_phase, TWO_PI);
1216 : :
1217 : 3647 : ctx.bottoming_crunch = std::sin(m_bottoming_phase) * bump_magnitude * ctx.speed_gate;
1218 : : }
1219 : : }
|
