Branch data Line data Source code
1 : : // GripLoadEstimation.cpp
2 : : // ---------------------------------------------------------------------------
3 : : // Grip and Load Estimation methods for FFBEngine.
4 : : //
5 : : // This file is a source-level split of FFBEngine.cpp (Approach A).
6 : : // All functions here are FFBEngine member methods; FFBEngine.h is unchanged.
7 : : //
8 : : // Rationale: These functions form a self-contained "data preparation" layer —
9 : : // they take raw (possibly broken/encrypted) telemetry and produce the best
10 : : // available grip and load values. The FFB engine then consumes those values
11 : : // without needing to know how they were estimated.
12 : : //
13 : : // See docs/dev_docs/reports/FFBEngine_refactoring_analysis.md for full details.
14 : : // ---------------------------------------------------------------------------
15 : :
16 : : #include "FFBEngine.h"
17 : : #include "Config.h"
18 : : #include "Logger.h"
19 : : #include "Logger.h"
20 : : #include <iostream>
21 : : #include <mutex>
22 : :
23 : : extern std::recursive_mutex g_engine_mutex;
24 : : #include <cmath>
25 : : #include "StringUtils.h"
26 : :
27 : : using namespace ffb_math;
28 : :
29 : : // Helper: Learn static front and rear load reference (v0.7.46, expanded v0.7.164)
30 : 15893 : void FFBEngine::update_static_load_reference(double current_front_load, double current_rear_load, double speed, double dt) {
31 [ + - ]: 15893 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
32 [ + + ]: 15893 : if (m_static_load_latched) return; // Do not update if latched
33 : :
34 [ + + + + ]: 3583 : if (speed > 2.0 && speed < 15.0) {
35 : : // Front Load Learning
36 [ + + ]: 2165 : if (m_static_front_load < 100.0) {
37 : 3 : m_static_front_load = current_front_load;
38 : : } else {
39 : 2162 : m_static_front_load += (dt / 5.0) * (current_front_load - m_static_front_load);
40 : : }
41 : : // Rear Load Learning
42 [ + + ]: 2165 : if (m_static_rear_load < 100.0) {
43 : 10 : m_static_rear_load = current_rear_load;
44 : : } else {
45 : 2155 : m_static_rear_load += (dt / 5.0) * (current_rear_load - m_static_rear_load);
46 : : }
47 [ + + + + : 1418 : } else if (speed >= 15.0 && m_static_front_load > 1000.0 && m_static_rear_load > 1000.0) {
+ + ]
48 : : // Latch the value once we exceed 15 m/s (aero begins to take over)
49 : 147 : m_static_load_latched = true;
50 : :
51 : : // Save to config map (v0.7.70)
52 [ + - ]: 147 : std::string vName = m_metadata.GetVehicleName();
53 [ + - + + : 147 : if (vName != "Unknown" && vName != "") {
+ - + - +
+ ]
54 [ + - ]: 20 : Config::SetSavedStaticLoad(vName, m_static_front_load);
55 [ + - + - ]: 20 : Config::SetSavedStaticLoad(vName + "_rear", m_static_rear_load);
56 : 20 : Config::m_needs_save = true; // Flag main thread to write to disk
57 [ + - + - ]: 20 : Logger::Get().LogFile("[FFB] Latched and saved static loads for %s: F=%.2fN, R=%.2fN", vName.c_str(), m_static_front_load, m_static_rear_load);
58 : : }
59 : 147 : }
60 : :
61 : : // Safety fallback: Ensure we have a sane baseline if learning failed
62 [ + + ]: 3583 : if (m_static_front_load < 1000.0) {
63 : 5 : m_static_front_load = m_auto_peak_front_load * 0.5;
64 : : }
65 [ + + ]: 3583 : if (m_static_rear_load < 1000.0) {
66 : 177 : m_static_rear_load = m_auto_peak_front_load * 0.5;
67 : : }
68 [ + + ]: 15893 : }
69 : :
70 : : // Initialize the load reference based on vehicle class and name seeding
71 : 114 : void FFBEngine::InitializeLoadReference(const char* className, const char* vehicleName) {
72 [ + - ]: 114 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
73 : :
74 : : // v0.7.109: Perform a full normalization reset on car change
75 : : // This ensures that session-learned peaks from a previous car don't pollute the new session.
76 [ + - ]: 114 : ResetNormalization();
77 : :
78 : : // --- FIX #374: Reset all missing telemetry counters and warning flags ---
79 : 114 : m_metadata.ResetWarnings();
80 : 114 : m_missing_load_frames = 0;
81 : 114 : m_missing_lat_force_front_frames = 0;
82 : 114 : m_missing_lat_force_rear_frames = 0;
83 : 114 : m_missing_susp_force_frames = 0;
84 : 114 : m_missing_susp_deflection_frames = 0;
85 : 114 : m_missing_vert_deflection_frames = 0;
86 : :
87 : 114 : m_warned_load = false;
88 : 114 : m_warned_grip = false;
89 : 114 : m_warned_rear_grip = false;
90 : 114 : m_warned_lat_force_front = false;
91 : 114 : m_warned_lat_force_rear = false;
92 : 114 : m_warned_susp_force = false;
93 : 114 : m_warned_susp_deflection = false;
94 : 114 : m_warned_vert_deflection = false;
95 : : // -----------------------------------------------------------------------
96 : :
97 [ + - ]: 114 : ParsedVehicleClass vclass = ParseVehicleClass(className, vehicleName);
98 : :
99 : : // Stage 3 Reset: Ensure peak load starts at class baseline
100 [ + - ]: 114 : m_auto_peak_front_load = GetDefaultLoadForClass(vclass);
101 : :
102 [ + - + - ]: 114 : std::string vName = vehicleName ? vehicleName : "Unknown";
103 : :
104 : : // Check if we already have a saved static load for this specific car (v0.7.70)
105 : 114 : double saved_front_load = 0.0;
106 : 114 : double saved_rear_load = 0.0;
107 [ + - + + ]: 114 : if (Config::GetSavedStaticLoad(vName, saved_front_load)) {
108 : 51 : m_static_front_load = saved_front_load;
109 : :
110 [ + - + - : 51 : if (Config::GetSavedStaticLoad(vName + "_rear", saved_rear_load)) {
+ + ]
111 : 42 : m_static_rear_load = saved_rear_load;
112 : : } else {
113 : : // Migration: If we have front but no rear, estimate rear based on class default
114 : 9 : m_static_rear_load = m_auto_peak_front_load * 0.5;
115 : : }
116 : :
117 : 51 : m_static_load_latched = true; // Skip the 2-15 m/s learning phase
118 [ + - + - ]: 51 : Logger::Get().LogFile("[FFB] Loaded persistent static loads for %s: F=%.2fN, R=%.2fN", vName.c_str(), m_static_front_load, m_static_rear_load);
119 : : } else {
120 : : // Reset static load reference for new car class
121 : 63 : m_static_front_load = m_auto_peak_front_load * 0.5;
122 : 63 : m_static_rear_load = m_auto_peak_front_load * 0.5;
123 : 63 : m_static_load_latched = false;
124 [ + - + - ]: 63 : Logger::Get().LogFile("[FFB] No saved load for %s. Learning required.", vName.c_str());
125 : : }
126 : :
127 : 114 : m_smoothed_vibration_mult = 1.0;
128 : :
129 : : // v0.7.119: Update engine's car name immediately to prevent seeding loop (Issue #238)
130 [ + - ]: 114 : m_metadata.m_last_handled_vehicle_name = vName;
131 : :
132 : : // Sync metadata manager state
133 [ + - ]: 114 : m_metadata.UpdateInternal(className, vehicleName, nullptr);
134 : :
135 : : // Issue #368: Log strings with quotes to reveal hidden spaces if detection fails
136 [ + - + + : 114 : Logger::Get().LogFile("[FFB] Vehicle Identification -> Detected Class: %s | Seed Load: %.2fN (Raw -> Class: '%s', Name: '%s')",
+ - + - ]
137 : : VehicleClassToString(vclass), m_auto_peak_front_load, (className ? className : "Unknown"), vName.c_str());
138 : 114 : }
139 : :
140 : : // Helper: Calculate Raw Slip Angle for a pair of wheels (v0.4.9 Refactor)
141 : : // Returns the average slip angle of two wheels using atan2(lateral_vel, longitudinal_vel)
142 : : // v0.4.19: Removed abs() from lateral velocity to preserve sign for debug visualization
143 : 31455 : double FFBEngine::calculate_raw_slip_angle_pair(const TelemWheelV01& w1, const TelemWheelV01& w2) {
144 : 31455 : double v_long_1 = std::abs(w1.mLongitudinalGroundVel);
145 : 31455 : double v_long_2 = std::abs(w2.mLongitudinalGroundVel);
146 [ + + ]: 31455 : if (v_long_1 < MIN_SLIP_ANGLE_VELOCITY) v_long_1 = MIN_SLIP_ANGLE_VELOCITY;
147 [ + + ]: 31455 : if (v_long_2 < MIN_SLIP_ANGLE_VELOCITY) v_long_2 = MIN_SLIP_ANGLE_VELOCITY;
148 : 31455 : double raw_angle_1 = std::atan2(w1.mLateralPatchVel, v_long_1);
149 : 31455 : double raw_angle_2 = std::atan2(w2.mLateralPatchVel, v_long_2);
150 : 31455 : return (raw_angle_1 + raw_angle_2) / 2.0;
151 : : }
152 : :
153 : 62939 : double FFBEngine::calculate_slip_angle(const TelemWheelV01& w, double& prev_state, double dt) {
154 : 62939 : double v_long = std::abs(w.mLongitudinalGroundVel);
155 [ + + ]: 62939 : if (v_long < MIN_SLIP_ANGLE_VELOCITY) v_long = MIN_SLIP_ANGLE_VELOCITY;
156 : :
157 : : // v0.4.19: PRESERVE SIGN - Do NOT use abs() on lateral velocity
158 : : // Positive lateral vel (+X = left) → Positive slip angle
159 : : // Negative lateral vel (-X = right) → Negative slip angle
160 : : // This sign is critical for directional counter-steering
161 : 62939 : double raw_angle = std::atan2(w.mLateralPatchVel, v_long); // SIGN PRESERVED
162 : :
163 : : // LPF: Time Corrected Alpha (v0.4.37)
164 : : // Target: Alpha 0.1 at 400Hz (dt = 0.0025)
165 : : // Formula: alpha = dt / (tau + dt) -> 0.1 = 0.0025 / (tau + 0.0025) -> tau approx 0.0225s
166 : : // v0.4.40: Using configurable m_slip_angle_smoothing
167 : 62939 : double tau = (double)m_slip_angle_smoothing;
168 [ + + ]: 62939 : if (tau < 0.0001) tau = 0.0001; // Safety clamp
169 : :
170 : 62939 : double alpha = dt / (tau + dt);
171 : :
172 : : // Safety clamp
173 : 62939 : alpha = (std::min)(1.0, (std::max)(0.001, alpha));
174 : 62939 : prev_state = prev_state + alpha * (raw_angle - prev_state);
175 : 62939 : return prev_state;
176 : : }
177 : :
178 : : // Helper: Calculate Axle Grip with Fallback (v0.4.6 Hardening)
179 : : // This function calculates the average grip for a pair of wheels (axle).
180 : : // If the primary telemetry grip is missing, it reconstructs it from slip angle and ratio.
181 : : // The avg_axle_load parameter is used as a threshold for triggering the reconstruction fallback.
182 : 31469 : GripResult FFBEngine::calculate_axle_grip(const TelemWheelV01& w1,
183 : : const TelemWheelV01& w2,
184 : : double avg_axle_load,
185 : : bool& warned_flag,
186 : : double& prev_slip1,
187 : : double& prev_slip2,
188 : : double& prev_load1, // NEW: State for load smoothing
189 : : double& prev_load2, // NEW: State for load smoothing
190 : : double car_speed,
191 : : double dt,
192 : : const char* vehicleName,
193 : : const TelemInfoV01* data,
194 : : bool is_front) {
195 : : // Note on mGripFract: The LMU InternalsPlugin.hpp comments state this is the
196 : : // "fraction of the contact patch that is sliding". This is poorly phrased.
197 : : // In actual telemetry output, 1.0 = Full Adhesion (Gripping) and 0.0 = Fully Sliding.
198 : : GripResult result;
199 : 31469 : double total_load = w1.mTireLoad + w2.mTireLoad;
200 [ + + ]: 31469 : if (total_load > 1.0) {
201 : 25037 : result.original = (w1.mGripFract * w1.mTireLoad + w2.mGripFract * w2.mTireLoad) / total_load;
202 : : } else {
203 : : // Fallback for zero load (e.g. jump/missing data)
204 : 6432 : result.original = (w1.mGripFract + w2.mGripFract) / 2.0;
205 : : }
206 : :
207 : 31469 : result.value = result.original;
208 : 31469 : result.approximated = false;
209 : 31469 : result.slip_angle = 0.0;
210 : :
211 : : // ==================================================================================
212 : : // CRITICAL LOGIC FIX (v0.4.14) - DO NOT MOVE INSIDE CONDITIONAL BLOCK
213 : : // ==================================================================================
214 : : // We MUST calculate slip angle every single frame, regardless of whether the
215 : : // grip fallback is triggered or not.
216 : : //
217 : : // Reason 1 (Physics State): The Low Pass Filter (LPF) inside calculate_slip_angle
218 : : // relies on continuous execution. If we skip frames (because telemetry
219 : : // is good), the 'prev_slip' state becomes stale. When telemetry eventually
220 : : // fails, the LPF will smooth against ancient history, causing a math spike.
221 : : //
222 : : // Reason 2 (Dependency): The 'Rear Aligning Torque' effect (calculated later)
223 : : // reads 'result.slip_angle'. If we only calculate this when grip is
224 : : // missing, the Rear Torque effect will toggle ON/OFF randomly based on
225 : : // telemetry health, causing violent kicks and "reverse FFB" sensations.
226 : : // ==================================================================================
227 : 31469 : double slip1 = calculate_slip_angle(w1, prev_slip1, dt);
228 : 31469 : double slip2 = calculate_slip_angle(w2, prev_slip2, dt);
229 : 31469 : result.slip_angle = (slip1 + slip2) / 2.0;
230 : :
231 : : // ==================================================================================
232 : : // SHADOW MODE: Always calculate slope grip if enabled (for diagnostics and logging)
233 : : // This allows us to compare our math against unencrypted cars to tune the algorithm.
234 : : // ==================================================================================
235 : 31469 : double slope_grip_estimate = 1.0;
236 [ + + + + : 31469 : if (is_front && data && car_speed >= 5.0) {
+ + ]
237 : 14605 : slope_grip_estimate = calculate_slope_grip(
238 : 14605 : data->mLocalAccel.x / 9.81,
239 : : result.slip_angle,
240 : : dt,
241 : : data
242 : : );
243 : : }
244 : :
245 : : // Fallback condition: Grip is essentially zero BUT car has significant load
246 [ + + + + ]: 31469 : if (result.value < 0.0001 && avg_axle_load > 100.0) {
247 : 27422 : result.approximated = true;
248 : :
249 [ + + ]: 27422 : if (car_speed < 5.0) {
250 : : // Note: We still keep the calculated slip_angle in result.slip_angle
251 : : // for visualization/rear torque, even if we force grip to 1.0 here.
252 : 859 : result.value = 1.0;
253 : : } else {
254 [ + + + + : 26563 : if (m_slope_detection_enabled && is_front && data) {
+ - ]
255 : 2042 : result.value = slope_grip_estimate;
256 : : } else {
257 : : // --- REFINED: Load-Sensitive Continuous Friction Circle ---
258 : :
259 : 49042 : auto calc_wheel_grip = [&](const TelemWheelV01& w, double slip_angle, double& prev_load) {
260 : : // 1. Dynamic Load Sensitivity with Slew/Smoothing
261 [ + + + - ]: 49042 : double raw_load = warned_flag ? approximate_load(w) : w.mTireLoad;
262 : :
263 : : // Smooth the load to prevent curb strikes from causing threshold jitter (50ms tau)
264 : 49042 : double load_alpha = dt / (0.050 + dt);
265 : 49042 : prev_load += load_alpha * (raw_load - prev_load);
266 : 49042 : double current_load = prev_load;
267 : :
268 : : // Calculate how loaded the tire is compared to the car's static weight
269 [ + + ]: 49042 : double static_ref = is_front ? m_static_front_load : m_static_rear_load;
270 : 49042 : double load_ratio = current_load / (static_ref + 1.0);
271 : 49042 : load_ratio = std::clamp(load_ratio, 0.25, 4.0); // Safety bounds
272 : :
273 : : // Tire physics: Optimal slip angle increases with load (Hertzian cube root)
274 : : // Note: Future thermal/pressure multipliers would be applied here
275 : 49042 : double dynamic_slip_angle = m_optimal_slip_angle * std::pow(load_ratio, 0.333);
276 : :
277 : : // 2. Lateral Component
278 : 49042 : double lat_metric = std::abs(slip_angle) / dynamic_slip_angle;
279 : :
280 : : // 3. Longitudinal Component
281 : 49042 : double long_ratio = calculate_manual_slip_ratio(w, car_speed);
282 : 49042 : double long_metric = std::abs(long_ratio) / (double)m_optimal_slip_ratio;
283 : :
284 : : // 4. Combined Vector (Friction Circle)
285 : 49042 : double combined_slip = std::sqrt((lat_metric * lat_metric) + (long_metric * long_metric));
286 : :
287 : : // 5. Continuous Falloff Curve with Sliding Friction Asymptote
288 : 49042 : double cs2 = combined_slip * combined_slip;
289 : 49042 : double cs4 = cs2 * cs2;
290 : :
291 : 49042 : const double MIN_SLIDING_GRIP = 0.05;
292 : 49042 : return MIN_SLIDING_GRIP + ((1.0 - MIN_SLIDING_GRIP) / (1.0 + cs4));
293 : 24521 : };
294 : :
295 : : // Calculate grip for each wheel independently, passing the state reference
296 [ + - ]: 24521 : double grip1 = calc_wheel_grip(w1, slip1, prev_load1);
297 [ + - ]: 24521 : double grip2 = calc_wheel_grip(w2, slip2, prev_load2);
298 : :
299 : : // Average the resulting grip fractions
300 : 24521 : result.value = (grip1 + grip2) / 2.0;
301 : : }
302 : : }
303 : :
304 : : // Clamp to standard 0.0 - 1.0 bounds
305 : 27422 : result.value = std::clamp(result.value, 0.0, 1.0);
306 : :
307 [ + + ]: 27422 : if (!warned_flag) {
308 : 470 : Logger::Get().LogFile("Warning: Data for mGripFract from the game seems to be missing for this car (%s). (Likely Encrypted/DLC Content). A fallback estimation will be used.", vehicleName);
309 : 470 : warned_flag = true;
310 : : }
311 : : }
312 : :
313 : : // Apply Adaptive Smoothing (v0.7.47)
314 [ + + ]: 31469 : double& state = is_front ? m_front_grip_smoothed_state : m_rear_grip_smoothed_state;
315 : 62938 : result.value = apply_adaptive_smoothing(result.value, state, dt,
316 : 31469 : (double)m_grip_smoothing_steady,
317 : 31469 : (double)m_grip_smoothing_fast,
318 : 31469 : (double)m_grip_smoothing_sensitivity);
319 : :
320 : 31469 : result.value = (std::max)(0.0, (std::min)(1.0, result.value));
321 : 31469 : return result;
322 : : }
323 : :
324 : : // ========================================================================================
325 : : // CRITICAL VEHICLE DYNAMICS NOTE: mSuspForce vs Wheel Load
326 : : // ========================================================================================
327 : : // The LMU telemetry channel `mSuspForce` represents the load on the internal PUSHROD,
328 : : // NOT the load at the tire contact patch.
329 : : // Because race cars use bellcranks, the pushrod has a mechanical advantage (Motion Ratio).
330 : : // For example, a Hypercar with a Motion Ratio of 0.5 means the pushrod moves half as far
331 : : // as the wheel, and therefore carries TWICE the force of the wheel.
332 : : // To approximate the actual Tire Load, we MUST multiply mSuspForce by the Motion Ratio,
333 : : // and then add the unsprung mass (the weight of the wheel, tire, and brakes).
334 : : // ========================================================================================
335 : :
336 : : // Helper: Approximate Load (v0.4.5 / Improved v0.7.171 / Refactored v0.7.175)
337 : : // Uses class-specific motion ratios to convert pushrod force (mSuspForce) to wheel load,
338 : : // and adds an estimate for front unsprung mass.
339 : 56365 : double FFBEngine::approximate_load(const TelemWheelV01& w) {
340 : 56365 : ParsedVehicleClass vclass = m_metadata.GetCurrentClass();
341 : 56365 : double motion_ratio = GetMotionRatioForClass(vclass);
342 : 56365 : double unsprung_weight = GetUnsprungWeightForClass(vclass, false /* is_rear */);
343 : :
344 : 56365 : return (std::max)(0.0, (w.mSuspForce * motion_ratio) + unsprung_weight);
345 : : }
346 : :
347 : : // Helper: Approximate Rear Load (v0.4.10 / Improved v0.7.171 / Refactored v0.7.175)
348 : : // Similar to approximate_load, but uses rear-specific unsprung mass estimates.
349 : 5998 : double FFBEngine::approximate_rear_load(const TelemWheelV01& w) {
350 : 5998 : ParsedVehicleClass vclass = m_metadata.GetCurrentClass();
351 : 5998 : double motion_ratio = GetMotionRatioForClass(vclass);
352 : 5998 : double unsprung_weight = GetUnsprungWeightForClass(vclass, true /* is_rear */);
353 : :
354 : 5998 : return (std::max)(0.0, (w.mSuspForce * motion_ratio) + unsprung_weight);
355 : : }
356 : :
357 : : // Helper: Calculate Manual Slip Ratio (v0.4.6)
358 : 49047 : double FFBEngine::calculate_manual_slip_ratio(const TelemWheelV01& w, double car_speed_ms) {
359 : : // Safety Trap: Force 0 slip at very low speeds (v0.4.6)
360 [ + + ]: 49047 : if (std::abs(car_speed_ms) < 2.0) return 0.0;
361 : :
362 : : // Radius in meters (stored as cm unsigned char)
363 : : // Explicit cast to double before division (v0.4.6)
364 : 49044 : double radius_m = (double)w.mStaticUndeflectedRadius / 100.0;
365 [ + + ]: 49044 : if (radius_m < 0.1) radius_m = 0.33; // Fallback if 0 or invalid
366 : :
367 : 49044 : double wheel_vel = w.mRotation * radius_m;
368 : :
369 : : // Avoid div-by-zero at standstill
370 : 49044 : double denom = std::abs(car_speed_ms);
371 [ - + ]: 49044 : if (denom < 1.0) denom = 1.0;
372 : :
373 : : // Ratio = (V_wheel - V_car) / V_car
374 : : // Lockup: V_wheel < V_car -> Ratio < 0
375 : : // Spin: V_wheel > V_car -> Ratio > 0
376 : 49044 : return (wheel_vel - car_speed_ms) / denom;
377 : : }
378 : :
379 : : // Helper: Calculate Grip Factor from Slope - v0.7.40 REWRITE
380 : : // Robust projected slope estimation with hold-and-decay logic and torque-based anticipation.
381 : 15110 : double FFBEngine::calculate_slope_grip(double lateral_g, double slip_angle, double dt, const TelemInfoV01* data) {
382 : : // 1. Signal Hygiene (Slew Limiter & Pre-Smoothing)
383 : :
384 : : // Initialize slew limiter and smoothing state on first sample to avoid ramp-up transients
385 [ + + ]: 15110 : if (m_slope_buffer_count == 0) {
386 : 243 : m_slope_lat_g_prev = std::abs(lateral_g);
387 : 243 : m_slope_lat_g_smoothed = std::abs(lateral_g);
388 : 243 : m_slope_slip_smoothed = std::abs(slip_angle);
389 [ + + ]: 243 : if (data) {
390 : 232 : m_slope_torque_smoothed = std::abs(data->mSteeringShaftTorque);
391 : 232 : m_slope_steer_smoothed = std::abs(data->mUnfilteredSteering);
392 : : }
393 : : }
394 : :
395 [ + - ]: 15110 : double lat_g_slew = apply_slew_limiter(std::abs(lateral_g), m_slope_lat_g_prev, (double)m_slope_g_slew_limit, dt);
396 : 15110 : m_debug_lat_g_slew = lat_g_slew;
397 : :
398 : 15110 : double alpha_smooth = dt / (0.01 + dt);
399 : :
400 [ + + ]: 15110 : if (m_slope_buffer_count > 0) {
401 : 14867 : m_slope_lat_g_smoothed += alpha_smooth * (lat_g_slew - m_slope_lat_g_smoothed);
402 : 14867 : m_slope_slip_smoothed += alpha_smooth * (std::abs(slip_angle) - m_slope_slip_smoothed);
403 [ + + ]: 14867 : if (data) {
404 : 14380 : m_slope_torque_smoothed += alpha_smooth * (std::abs(data->mSteeringShaftTorque) - m_slope_torque_smoothed);
405 : 14380 : m_slope_steer_smoothed += alpha_smooth * (std::abs(data->mUnfilteredSteering) - m_slope_steer_smoothed);
406 : : }
407 : : }
408 : :
409 : : // 2. Update Buffers with smoothed values
410 : 15110 : m_slope_lat_g_buffer[m_slope_buffer_index] = m_slope_lat_g_smoothed;
411 : 15110 : m_slope_slip_buffer[m_slope_buffer_index] = m_slope_slip_smoothed;
412 [ + + ]: 15110 : if (data) {
413 : 14612 : m_slope_torque_buffer[m_slope_buffer_index] = m_slope_torque_smoothed;
414 : 14612 : m_slope_steer_buffer[m_slope_buffer_index] = m_slope_steer_smoothed;
415 : : }
416 : :
417 : 15110 : m_slope_buffer_index = (m_slope_buffer_index + 1) % SLOPE_BUFFER_MAX;
418 [ + + ]: 15110 : if (m_slope_buffer_count < SLOPE_BUFFER_MAX) m_slope_buffer_count++;
419 : :
420 : : // 3. Calculate G-based Derivatives (Savitzky-Golay)
421 : 15110 : double dG_dt = calculate_sg_derivative(m_slope_lat_g_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
422 : 15110 : double dAlpha_dt = calculate_sg_derivative(m_slope_slip_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
423 : :
424 : 15110 : m_slope_dG_dt = dG_dt;
425 : 15110 : m_slope_dAlpha_dt = dAlpha_dt;
426 : :
427 : : // 4. Projected Slope Logic (G-based)
428 [ + + ]: 15110 : if (std::abs(dAlpha_dt) > (double)m_slope_alpha_threshold) {
429 : 1607 : m_slope_hold_timer = SLOPE_HOLD_TIME;
430 : 1607 : m_debug_slope_num = dG_dt * dAlpha_dt;
431 : 1607 : m_debug_slope_den = (dAlpha_dt * dAlpha_dt) + 0.000001;
432 : 1607 : m_debug_slope_raw = m_debug_slope_num / m_debug_slope_den;
433 : 1607 : m_slope_current = std::clamp(m_debug_slope_raw, -20.0, 20.0);
434 : : } else {
435 : 13503 : m_slope_hold_timer -= dt;
436 [ + + ]: 13503 : if (m_slope_hold_timer <= 0.0) {
437 : 13095 : m_slope_hold_timer = 0.0;
438 : 13095 : m_slope_current += (double)m_slope_decay_rate * dt * (0.0 - m_slope_current);
439 : : }
440 : : }
441 : :
442 : : // 5. Calculate Torque-based Slope (Pneumatic Trail Anticipation)
443 [ + + + + ]: 15110 : volatile bool can_calc_torque = (m_slope_use_torque && data != nullptr);
444 [ + + ]: 15110 : if (can_calc_torque) {
445 : 14609 : double dTorque_dt = calculate_sg_derivative(m_slope_torque_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
446 : 14609 : double dSteer_dt = calculate_sg_derivative(m_slope_steer_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
447 : :
448 [ + + ]: 14609 : if (std::abs(dSteer_dt) > (double)m_slope_alpha_threshold) { // Unified threshold for steering movement
449 : 146 : m_debug_slope_torque_num = dTorque_dt * dSteer_dt;
450 : 146 : m_debug_slope_torque_den = (dSteer_dt * dSteer_dt) + 0.000001;
451 : 146 : m_slope_torque_current = std::clamp(m_debug_slope_torque_num / m_debug_slope_torque_den, -50.0, 50.0);
452 : : } else {
453 : 14463 : m_slope_torque_current += (double)m_slope_decay_rate * dt * (0.0 - m_slope_torque_current);
454 : : }
455 : : } else {
456 : 501 : m_slope_torque_current = 20.0; // Positive value means no grip loss detected
457 : : }
458 : :
459 : 15110 : double current_grip_factor = 1.0;
460 [ + - ]: 15110 : double confidence = calculate_slope_confidence(dAlpha_dt);
461 : :
462 : : // 1. Calculate Grip Loss from G-Slope (Lateral Saturation)
463 [ + - ]: 15110 : double loss_percent_g = inverse_lerp((double)m_slope_min_threshold, (double)m_slope_max_threshold, m_slope_current);
464 : :
465 : : // 2. Calculate Grip Loss from Torque-Slope (Pneumatic Trail Drop)
466 : 15110 : double loss_percent_torque = 0.0;
467 [ + + + + ]: 15110 : volatile bool use_torque_fusion = (m_slope_use_torque && data != nullptr);
468 [ + + ]: 15110 : if (use_torque_fusion) {
469 [ + + ]: 14609 : if (m_slope_torque_current < 0.0) {
470 : 43 : loss_percent_torque = std::abs(m_slope_torque_current) * (double)m_slope_torque_sensitivity;
471 : 43 : loss_percent_torque = (std::max)(0.0, (std::min)(1.0, loss_percent_torque));
472 : : }
473 : : }
474 : :
475 : : // 3. Fusion Logic (Max of both estimators)
476 : 15110 : double loss_percent = (std::max)(loss_percent_g, loss_percent_torque);
477 : :
478 : : // Scale loss by confidence and apply floor (0.2)
479 : : // 0% loss (loss_percent=0) -> 1.0 factor
480 : : // 100% loss (loss_percent=1) -> 0.2 factor
481 : 15110 : current_grip_factor = 1.0 - (loss_percent * 0.8 * confidence);
482 : :
483 : : // Apply Floor (Safety)
484 : 15110 : current_grip_factor = (std::max)(0.2, (std::min)(1.0, current_grip_factor));
485 : :
486 : : // 5. Smoothing (v0.7.0)
487 : 15110 : double alpha = dt / ((double)m_slope_smoothing_tau + dt);
488 : 15110 : alpha = (std::max)(0.001, (std::min)(1.0, alpha));
489 : 15110 : m_slope_smoothed_output += alpha * (current_grip_factor - m_slope_smoothed_output);
490 : :
491 : 15110 : return m_slope_smoothed_output;
492 : : }
493 : :
494 : : // Helper: Calculate confidence factor for slope detection
495 : : // Extracted to avoid code duplication between slope detection and logging
496 : 15153 : double FFBEngine::calculate_slope_confidence(double dAlpha_dt) {
497 [ - + ]: 15153 : if (!m_slope_confidence_enabled) return 1.0;
498 : :
499 : : // v0.7.21 FIX: Use smoothstep confidence ramp [m_slope_alpha_threshold, m_slope_confidence_max_rate] rad/s
500 : : // to reject singularity artifacts near zero.
501 : 15153 : return smoothstep((double)m_slope_alpha_threshold, (double)m_slope_confidence_max_rate, std::abs(dAlpha_dt));
502 : : }
503 : :
504 : : // Helper: Calculate Slip Ratio from wheel (v0.6.36 - Extracted from lambdas)
505 : : // Unified slip ratio calculation for lockup and spin detection.
506 : : // Returns the ratio of longitudinal slip: (PatchVel - GroundVel) / GroundVel
507 : 11258 : double FFBEngine::calculate_wheel_slip_ratio(const TelemWheelV01& w) {
508 : 11258 : double v_long = std::abs(w.mLongitudinalGroundVel);
509 [ + + ]: 11258 : if (v_long < MIN_SLIP_ANGLE_VELOCITY) v_long = MIN_SLIP_ANGLE_VELOCITY;
510 : 11258 : return w.mLongitudinalPatchVel / v_long;
511 : : }
|
