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