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 <iostream>
19 : : #include <mutex>
20 : :
21 : : extern std::recursive_mutex g_engine_mutex;
22 : : #include <cmath>
23 : :
24 : : using namespace ffb_math;
25 : :
26 : : // Helper: Learn static front load reference (v0.7.46)
27 : 332 : void FFBEngine::update_static_load_reference(double current_load, double speed, double dt) {
28 [ + - ]: 332 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
29 [ + + ]: 332 : if (m_static_load_latched) return; // Do not update if latched
30 : :
31 [ + + + + ]: 328 : if (speed > 2.0 && speed < 15.0) {
32 [ + + ]: 205 : if (m_static_front_load < 100.0) {
33 : 3 : m_static_front_load = current_load;
34 : : } else {
35 : 202 : m_static_front_load += (dt / 5.0) * (current_load - m_static_front_load);
36 : : }
37 [ + + + - ]: 123 : } else if (speed >= 15.0 && m_static_front_load > 1000.0) {
38 : : // Latch the value once we exceed 15 m/s (aero begins to take over)
39 : 6 : m_static_load_latched = true;
40 : :
41 : : // Save to config map (v0.7.70)
42 [ + - ]: 6 : std::string vName = m_vehicle_name;
43 [ + - + + : 6 : if (vName != "Unknown" && vName != "") {
+ - + + +
+ ]
44 [ + - ]: 1 : Config::SetSavedStaticLoad(vName, m_static_front_load);
45 : 1 : Config::m_needs_save = true; // Flag main thread to write to disk
46 [ + - + - : 1 : std::cout << "[FFB] Latched and saved static load for " << vName << ": " << m_static_front_load << "N" << std::endl;
+ - + - +
- + - ]
47 : : }
48 : 6 : }
49 : :
50 : : // Safety fallback: Ensure we have a sane baseline if learning failed
51 [ + + ]: 328 : if (m_static_front_load < 1000.0) {
52 : 2 : m_static_front_load = m_auto_peak_load * 0.5;
53 : : }
54 [ + + ]: 332 : }
55 : :
56 : : // Initialize the load reference based on vehicle class and name seeding
57 : 52 : void FFBEngine::InitializeLoadReference(const char* className, const char* vehicleName) {
58 [ + - ]: 52 : std::lock_guard<std::recursive_mutex> lock(g_engine_mutex);
59 : :
60 : : // v0.7.109: Perform a full normalization reset on car change
61 : : // This ensures that session-learned peaks from a previous car don't pollute the new session.
62 [ + - ]: 52 : ResetNormalization();
63 : :
64 [ + - ]: 52 : ParsedVehicleClass vclass = ParseVehicleClass(className, vehicleName);
65 : :
66 : : // Stage 3 Reset: Ensure peak load starts at class baseline
67 [ + - ]: 52 : m_auto_peak_load = GetDefaultLoadForClass(vclass);
68 : :
69 [ + + + - ]: 52 : std::string vName = vehicleName ? vehicleName : "Unknown";
70 : :
71 : : // Check if we already have a saved static load for this specific car (v0.7.70)
72 : 52 : double saved_load = 0.0;
73 [ + - + + ]: 52 : if (Config::GetSavedStaticLoad(vName, saved_load)) {
74 : 2 : m_static_front_load = saved_load;
75 : 2 : m_static_load_latched = true; // Skip the 2-15 m/s learning phase
76 [ + - + - : 2 : std::cout << "[FFB] Loaded persistent static load for " << vName << ": " << m_static_front_load << "N" << std::endl;
+ - + - +
- + - ]
77 : : } else {
78 : : // Reset static load reference for new car class
79 : 50 : m_static_front_load = m_auto_peak_load * 0.5;
80 : 50 : m_static_load_latched = false;
81 [ + - + - : 50 : std::cout << "[FFB] No saved load for " << vName << ". Learning required." << std::endl;
+ - + - ]
82 : : }
83 : :
84 : 52 : m_smoothed_tactile_mult = 1.0;
85 : :
86 : : std::cout << "[FFB] Vehicle Identification -> Detected Class: " << VehicleClassToString(vclass)
87 [ + - + - : 52 : << " | Seed Load: " << m_auto_peak_load << "N"
+ - + - +
- ]
88 : : << " (Raw -> Class: " << (className ? className : "Unknown")
89 [ + - + - : 52 : << ", Name: " << vName << ")" << std::endl;
+ + + - +
- + - + -
+ - ]
90 : 52 : }
91 : :
92 : : // Helper: Calculate Raw Slip Angle for a pair of wheels (v0.4.9 Refactor)
93 : : // Returns the average slip angle of two wheels using atan2(lateral_vel, longitudinal_vel)
94 : : // v0.4.19: Removed abs() from lateral velocity to preserve sign for debug visualization
95 : 10469 : double FFBEngine::calculate_raw_slip_angle_pair(const TelemWheelV01& w1, const TelemWheelV01& w2) {
96 : 10469 : double v_long_1 = std::abs(w1.mLongitudinalGroundVel);
97 : 10469 : double v_long_2 = std::abs(w2.mLongitudinalGroundVel);
98 [ + + ]: 10469 : if (v_long_1 < MIN_SLIP_ANGLE_VELOCITY) v_long_1 = MIN_SLIP_ANGLE_VELOCITY;
99 [ + + ]: 10469 : if (v_long_2 < MIN_SLIP_ANGLE_VELOCITY) v_long_2 = MIN_SLIP_ANGLE_VELOCITY;
100 : 10469 : double raw_angle_1 = std::atan2(w1.mLateralPatchVel, v_long_1);
101 : 10469 : double raw_angle_2 = std::atan2(w2.mLateralPatchVel, v_long_2);
102 : 10469 : return (raw_angle_1 + raw_angle_2) / 2.0;
103 : : }
104 : :
105 : 49407 : double FFBEngine::calculate_slip_angle(const TelemWheelV01& w, double& prev_state, double dt) {
106 : 49407 : double v_long = std::abs(w.mLongitudinalGroundVel);
107 [ + + ]: 49407 : if (v_long < MIN_SLIP_ANGLE_VELOCITY) v_long = MIN_SLIP_ANGLE_VELOCITY;
108 : :
109 : : // v0.4.19: PRESERVE SIGN - Do NOT use abs() on lateral velocity
110 : : // Positive lateral vel (+X = left) → Positive slip angle
111 : : // Negative lateral vel (-X = right) → Negative slip angle
112 : : // This sign is critical for directional counter-steering
113 : 49407 : double raw_angle = std::atan2(w.mLateralPatchVel, v_long); // SIGN PRESERVED
114 : :
115 : : // LPF: Time Corrected Alpha (v0.4.37)
116 : : // Target: Alpha 0.1 at 400Hz (dt = 0.0025)
117 : : // Formula: alpha = dt / (tau + dt) -> 0.1 = 0.0025 / (tau + 0.0025) -> tau approx 0.0225s
118 : : // v0.4.40: Using configurable m_slip_angle_smoothing
119 : 49407 : double tau = (double)m_slip_angle_smoothing;
120 [ + + ]: 49407 : if (tau < 0.0001) tau = 0.0001; // Safety clamp
121 : :
122 : 49407 : double alpha = dt / (tau + dt);
123 : :
124 : : // Safety clamp
125 : 49407 : alpha = (std::min)(1.0, (std::max)(0.001, alpha));
126 : 49407 : prev_state = prev_state + alpha * (raw_angle - prev_state);
127 : 49407 : return prev_state;
128 : : }
129 : :
130 : : // Helper: Calculate Grip with Fallback (v0.4.6 Hardening)
131 : 24703 : GripResult FFBEngine::calculate_grip(const TelemWheelV01& w1,
132 : : const TelemWheelV01& w2,
133 : : double avg_load,
134 : : bool& warned_flag,
135 : : double& prev_slip1,
136 : : double& prev_slip2,
137 : : double car_speed,
138 : : double dt,
139 : : const char* vehicleName,
140 : : const TelemInfoV01* data,
141 : : bool is_front) {
142 : : GripResult result;
143 : 24703 : double total_load = w1.mTireLoad + w2.mTireLoad;
144 [ + + ]: 24703 : if (total_load > 1.0) {
145 : 18744 : result.original = (w1.mGripFract * w1.mTireLoad + w2.mGripFract * w2.mTireLoad) / total_load;
146 : : } else {
147 : : // Fallback for zero load (e.g. jump/missing data)
148 : 5959 : result.original = (w1.mGripFract + w2.mGripFract) / 2.0;
149 : : }
150 : :
151 : 24703 : result.value = result.original;
152 : 24703 : result.approximated = false;
153 : 24703 : result.slip_angle = 0.0;
154 : :
155 : : // ==================================================================================
156 : : // CRITICAL LOGIC FIX (v0.4.14) - DO NOT MOVE INSIDE CONDITIONAL BLOCK
157 : : // ==================================================================================
158 : : // We MUST calculate slip angle every single frame, regardless of whether the
159 : : // grip fallback is triggered or not.
160 : : //
161 : : // Reason 1 (Physics State): The Low Pass Filter (LPF) inside calculate_slip_angle
162 : : // relies on continuous execution. If we skip frames (because telemetry
163 : : // is good), the 'prev_slip' state becomes stale. When telemetry eventually
164 : : // fails, the LPF will smooth against ancient history, causing a math spike.
165 : : //
166 : : // Reason 2 (Dependency): The 'Rear Aligning Torque' effect (calculated later)
167 : : // reads 'result.slip_angle'. If we only calculate this when grip is
168 : : // missing, the Rear Torque effect will toggle ON/OFF randomly based on
169 : : // telemetry health, causing violent kicks and "reverse FFB" sensations.
170 : : // ==================================================================================
171 : 24703 : double slip1 = calculate_slip_angle(w1, prev_slip1, dt);
172 : 24703 : double slip2 = calculate_slip_angle(w2, prev_slip2, dt);
173 : 24703 : result.slip_angle = (slip1 + slip2) / 2.0;
174 : :
175 : : // Fallback condition: Grip is essentially zero BUT car has significant load
176 [ + + + + ]: 24703 : if (result.value < 0.0001 && avg_load > 100.0) {
177 : 20880 : result.approximated = true;
178 : :
179 [ + + ]: 20880 : if (car_speed < 5.0) {
180 : : // Note: We still keep the calculated slip_angle in result.slip_angle
181 : : // for visualization/rear torque, even if we force grip to 1.0 here.
182 : 566 : result.value = 1.0;
183 : : } else {
184 [ + + + + : 20314 : if (m_slope_detection_enabled && is_front && data) {
+ - ]
185 : : // Dynamic grip estimation via derivative monitoring
186 : 2042 : result.value = calculate_slope_grip(
187 : 2042 : data->mLocalAccel.x / 9.81,
188 : : result.slip_angle,
189 : : dt,
190 : : data
191 : : );
192 : : } else {
193 : : // v0.4.38: Combined Friction Circle (Advanced Reconstruction)
194 : :
195 : : // 1. Lateral Component (Alpha)
196 : : // USE CONFIGURABLE THRESHOLD (v0.5.7)
197 : 18272 : double lat_metric = std::abs(result.slip_angle) / (double)m_optimal_slip_angle;
198 : :
199 : : // 2. Longitudinal Component (Kappa)
200 : : // Calculate manual slip for both wheels and average the magnitude
201 : 18272 : double ratio1 = calculate_manual_slip_ratio(w1, car_speed);
202 : 18272 : double ratio2 = calculate_manual_slip_ratio(w2, car_speed);
203 : 18272 : double avg_ratio = (std::abs(ratio1) + std::abs(ratio2)) / 2.0;
204 : :
205 : : // USE CONFIGURABLE THRESHOLD (v0.5.7)
206 : 18272 : double long_metric = avg_ratio / (double)m_optimal_slip_ratio;
207 : :
208 : : // 3. Combined Vector (Friction Circle)
209 : 18272 : double combined_slip = std::sqrt((lat_metric * lat_metric) + (long_metric * long_metric));
210 : :
211 : : // 4. Map to Grip Fraction
212 : :
213 [ + + ]: 18272 : if (combined_slip > 1.0) {
214 : 7212 : double excess = combined_slip - 1.0;
215 : 7212 : result.value = 1.0 / (1.0 + excess * 2.0);
216 : : } else {
217 : 11060 : result.value = 1.0;
218 : : }
219 : : }
220 : : }
221 : :
222 : :
223 : : // Safety Clamp (v0.4.6): Never drop below 0.2 in approximation
224 : 20880 : result.value = (std::max)(0.2, result.value);
225 : :
226 [ + + ]: 20880 : if (!warned_flag) {
227 : 282 : std::cout << "Warning: Data for mGripFract from the game seems to be missing for this car (" << vehicleName << "). (Likely Encrypted/DLC Content). A fallback estimation will be used." << std::endl;
228 : 282 : warned_flag = true;
229 : : }
230 : : }
231 : :
232 : : // Apply Adaptive Smoothing (v0.7.47)
233 [ + + ]: 24703 : double& state = is_front ? m_front_grip_smoothed_state : m_rear_grip_smoothed_state;
234 : 49406 : result.value = apply_adaptive_smoothing(result.value, state, dt,
235 : 24703 : (double)m_grip_smoothing_steady,
236 : 24703 : (double)m_grip_smoothing_fast,
237 : 24703 : (double)m_grip_smoothing_sensitivity);
238 : :
239 : 24703 : result.value = (std::max)(0.0, (std::min)(1.0, result.value));
240 : 24703 : return result;
241 : : }
242 : :
243 : : // Helper: Approximate Load (v0.4.5)
244 : 259 : double FFBEngine::approximate_load(const TelemWheelV01& w) {
245 : : // Base: Suspension Force + Est. Unsprung Mass (300N)
246 : : // Note: mSuspForce captures weight transfer and aero
247 : 259 : return w.mSuspForce + 300.0;
248 : : }
249 : :
250 : : // Helper: Approximate Rear Load (v0.4.10)
251 : 24701 : double FFBEngine::approximate_rear_load(const TelemWheelV01& w) {
252 : : // Base: Suspension Force + Est. Unsprung Mass (300N)
253 : : // This captures weight transfer (braking/accel) and aero downforce implicitly via suspension compression
254 : 24701 : return w.mSuspForce + 300.0;
255 : : }
256 : :
257 : : // Helper: Calculate Kinematic Load (v0.4.39)
258 : : // Estimates tire load from chassis physics when telemetry (mSuspForce) is missing.
259 : : // This is critical for encrypted DLC content where suspension sensors are blocked.
260 : 2364 : double FFBEngine::calculate_kinematic_load(const TelemInfoV01* data, int wheel_index) {
261 : : // 1. Static Weight Distribution
262 : 2364 : bool is_rear = (wheel_index >= 2);
263 [ + + ]: 2364 : double bias = is_rear ? m_approx_weight_bias : (1.0 - m_approx_weight_bias);
264 : 2364 : double static_weight = (m_approx_mass_kg * 9.81 * bias) / 2.0;
265 : :
266 : : // 2. Aerodynamic Load (Velocity Squared)
267 : 2364 : double speed = std::abs(data->mLocalVel.z);
268 : 2364 : double aero_load = m_approx_aero_coeff * (speed * speed);
269 : 2364 : double wheel_aero = aero_load / 4.0;
270 : :
271 : : // 3. Longitudinal Weight Transfer (Braking/Acceleration)
272 : : // COORDINATE SYSTEM VERIFIED (v0.4.39):
273 : : // - LMU: +Z axis points REARWARD (out the back of the car)
274 : : // - Braking: Chassis decelerates → Inertial force pushes rearward → +Z acceleration
275 : : // - Result: Front wheels GAIN load, Rear wheels LOSE load
276 : : // - Source: docs/dev_docs/references/Reference - coordinate_system_reference.md
277 : : //
278 : : // Formula: (Accel / g) * WEIGHT_TRANSFER_SCALE
279 : : // We use SMOOTHED acceleration to simulate chassis pitch inertia (~35ms lag)
280 : 2364 : double long_transfer = (m_accel_z_smoothed / 9.81) * WEIGHT_TRANSFER_SCALE;
281 [ + + ]: 2364 : if (is_rear) long_transfer *= -1.0; // Subtract from Rear during Braking
282 : :
283 : : // 4. Lateral Weight Transfer (Cornering)
284 : : // COORDINATE SYSTEM VERIFIED (v0.4.39):
285 : : // - LMU: +X axis points LEFT (out the left side of the car)
286 : : // - Right Turn: Centrifugal force pushes LEFT → +X acceleration
287 : : // - Result: LEFT wheels (outside) GAIN load, RIGHT wheels (inside) LOSE load
288 : : // - Source: docs/dev_docs/references/Reference - coordinate_system_reference.md
289 : : //
290 : : // Formula: (Accel / g) * WEIGHT_TRANSFER_SCALE * Roll_Stiffness
291 : : // We use SMOOTHED acceleration to simulate chassis roll inertia (~35ms lag)
292 : 2364 : double lat_transfer = (m_accel_x_smoothed / 9.81) * WEIGHT_TRANSFER_SCALE * m_approx_roll_stiffness;
293 [ + + + + ]: 2364 : bool is_left = (wheel_index == 0 || wheel_index == 2);
294 [ + + ]: 2364 : if (!is_left) lat_transfer *= -1.0; // Subtract from Right wheels
295 : :
296 : : // Sum and Clamp
297 : 2364 : double total_load = static_weight + wheel_aero + long_transfer + lat_transfer;
298 : 2364 : return (std::max)(0.0, total_load);
299 : : }
300 : :
301 : : // Helper: Calculate Manual Slip Ratio (v0.4.6)
302 : 36549 : double FFBEngine::calculate_manual_slip_ratio(const TelemWheelV01& w, double car_speed_ms) {
303 : : // Safety Trap: Force 0 slip at very low speeds (v0.4.6)
304 [ + + ]: 36549 : if (std::abs(car_speed_ms) < 2.0) return 0.0;
305 : :
306 : : // Radius in meters (stored as cm unsigned char)
307 : : // Explicit cast to double before division (v0.4.6)
308 : 36546 : double radius_m = (double)w.mStaticUndeflectedRadius / 100.0;
309 [ + + ]: 36546 : if (radius_m < 0.1) radius_m = 0.33; // Fallback if 0 or invalid
310 : :
311 : 36546 : double wheel_vel = w.mRotation * radius_m;
312 : :
313 : : // Avoid div-by-zero at standstill
314 : 36546 : double denom = std::abs(car_speed_ms);
315 [ - + ]: 36546 : if (denom < 1.0) denom = 1.0;
316 : :
317 : : // Ratio = (V_wheel - V_car) / V_car
318 : : // Lockup: V_wheel < V_car -> Ratio < 0
319 : : // Spin: V_wheel > V_car -> Ratio > 0
320 : 36546 : return (wheel_vel - car_speed_ms) / denom;
321 : : }
322 : :
323 : : // Helper: Calculate Grip Factor from Slope - v0.7.40 REWRITE
324 : : // Robust projected slope estimation with hold-and-decay logic and torque-based anticipation.
325 : 2547 : double FFBEngine::calculate_slope_grip(double lateral_g, double slip_angle, double dt, const TelemInfoV01* data) {
326 : : // 1. Signal Hygiene (Slew Limiter & Pre-Smoothing)
327 : :
328 : : // Initialize slew limiter and smoothing state on first sample to avoid ramp-up transients
329 [ + + ]: 2547 : if (m_slope_buffer_count == 0) {
330 : 37 : m_slope_lat_g_prev = std::abs(lateral_g);
331 : 37 : m_slope_lat_g_smoothed = std::abs(lateral_g);
332 : 37 : m_slope_slip_smoothed = std::abs(slip_angle);
333 [ + + ]: 37 : if (data) {
334 : 26 : m_slope_torque_smoothed = std::abs(data->mSteeringShaftTorque);
335 : 26 : m_slope_steer_smoothed = std::abs(data->mUnfilteredSteering);
336 : : }
337 : : }
338 : :
339 [ + - ]: 2547 : double lat_g_slew = apply_slew_limiter(std::abs(lateral_g), m_slope_lat_g_prev, (double)m_slope_g_slew_limit, dt);
340 : 2547 : m_debug_lat_g_slew = lat_g_slew;
341 : :
342 : 2547 : double alpha_smooth = dt / (0.01 + dt);
343 : :
344 [ + + ]: 2547 : if (m_slope_buffer_count > 0) {
345 : 2510 : m_slope_lat_g_smoothed += alpha_smooth * (lat_g_slew - m_slope_lat_g_smoothed);
346 : 2510 : m_slope_slip_smoothed += alpha_smooth * (std::abs(slip_angle) - m_slope_slip_smoothed);
347 [ + + ]: 2510 : if (data) {
348 : 2023 : m_slope_torque_smoothed += alpha_smooth * (std::abs(data->mSteeringShaftTorque) - m_slope_torque_smoothed);
349 : 2023 : m_slope_steer_smoothed += alpha_smooth * (std::abs(data->mUnfilteredSteering) - m_slope_steer_smoothed);
350 : : }
351 : : }
352 : :
353 : : // 2. Update Buffers with smoothed values
354 : 2547 : m_slope_lat_g_buffer[m_slope_buffer_index] = m_slope_lat_g_smoothed;
355 : 2547 : m_slope_slip_buffer[m_slope_buffer_index] = m_slope_slip_smoothed;
356 [ + + ]: 2547 : if (data) {
357 : 2049 : m_slope_torque_buffer[m_slope_buffer_index] = m_slope_torque_smoothed;
358 : 2049 : m_slope_steer_buffer[m_slope_buffer_index] = m_slope_steer_smoothed;
359 : : }
360 : :
361 : 2547 : m_slope_buffer_index = (m_slope_buffer_index + 1) % SLOPE_BUFFER_MAX;
362 [ + + ]: 2547 : if (m_slope_buffer_count < SLOPE_BUFFER_MAX) m_slope_buffer_count++;
363 : :
364 : : // 3. Calculate G-based Derivatives (Savitzky-Golay)
365 : 2547 : double dG_dt = calculate_sg_derivative(m_slope_lat_g_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
366 : 2547 : double dAlpha_dt = calculate_sg_derivative(m_slope_slip_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
367 : :
368 : 2547 : m_slope_dG_dt = dG_dt;
369 : 2547 : m_slope_dAlpha_dt = dAlpha_dt;
370 : :
371 : : // 4. Projected Slope Logic (G-based)
372 [ + + ]: 2547 : if (std::abs(dAlpha_dt) > (double)m_slope_alpha_threshold) {
373 : 546 : m_slope_hold_timer = SLOPE_HOLD_TIME;
374 : 546 : m_debug_slope_num = dG_dt * dAlpha_dt;
375 : 546 : m_debug_slope_den = (dAlpha_dt * dAlpha_dt) + 0.000001;
376 : 546 : m_debug_slope_raw = m_debug_slope_num / m_debug_slope_den;
377 : 546 : m_slope_current = std::clamp(m_debug_slope_raw, -20.0, 20.0);
378 : : } else {
379 : 2001 : m_slope_hold_timer -= dt;
380 [ + + ]: 2001 : if (m_slope_hold_timer <= 0.0) {
381 : 1846 : m_slope_hold_timer = 0.0;
382 : 1846 : m_slope_current += (double)m_slope_decay_rate * dt * (0.0 - m_slope_current);
383 : : }
384 : : }
385 : :
386 : : // 5. Calculate Torque-based Slope (Pneumatic Trail Anticipation)
387 [ + + + + ]: 2547 : volatile bool can_calc_torque = (m_slope_use_torque && data != nullptr);
388 [ + + ]: 2547 : if (can_calc_torque) {
389 : 2046 : double dTorque_dt = calculate_sg_derivative(m_slope_torque_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
390 : 2046 : double dSteer_dt = calculate_sg_derivative(m_slope_steer_buffer, m_slope_buffer_count, m_slope_sg_window, dt, m_slope_buffer_index);
391 : :
392 [ + + ]: 2046 : if (std::abs(dSteer_dt) > (double)m_slope_alpha_threshold) { // Unified threshold for steering movement
393 : 141 : m_debug_slope_torque_num = dTorque_dt * dSteer_dt;
394 : 141 : m_debug_slope_torque_den = (dSteer_dt * dSteer_dt) + 0.000001;
395 : 141 : m_slope_torque_current = std::clamp(m_debug_slope_torque_num / m_debug_slope_torque_den, -50.0, 50.0);
396 : : } else {
397 : 1905 : m_slope_torque_current += (double)m_slope_decay_rate * dt * (0.0 - m_slope_torque_current);
398 : : }
399 : : } else {
400 : 501 : m_slope_torque_current = 20.0; // Positive value means no grip loss detected
401 : : }
402 : :
403 : 2547 : double current_grip_factor = 1.0;
404 [ + - ]: 2547 : double confidence = calculate_slope_confidence(dAlpha_dt);
405 : :
406 : : // 1. Calculate Grip Loss from G-Slope (Lateral Saturation)
407 [ + - ]: 2547 : double loss_percent_g = inverse_lerp((double)m_slope_min_threshold, (double)m_slope_max_threshold, m_slope_current);
408 : :
409 : : // 2. Calculate Grip Loss from Torque-Slope (Pneumatic Trail Drop)
410 : 2547 : double loss_percent_torque = 0.0;
411 [ + + + + ]: 2547 : volatile bool use_torque_fusion = (m_slope_use_torque && data != nullptr);
412 [ + + ]: 2547 : if (use_torque_fusion) {
413 [ + + ]: 2046 : if (m_slope_torque_current < 0.0) {
414 : 38 : loss_percent_torque = std::abs(m_slope_torque_current) * (double)m_slope_torque_sensitivity;
415 : 38 : loss_percent_torque = (std::max)(0.0, (std::min)(1.0, loss_percent_torque));
416 : : }
417 : : }
418 : :
419 : : // 3. Fusion Logic (Max of both estimators)
420 : 2547 : double loss_percent = (std::max)(loss_percent_g, loss_percent_torque);
421 : :
422 : : // Scale loss by confidence and apply floor (0.2)
423 : : // 0% loss (loss_percent=0) -> 1.0 factor
424 : : // 100% loss (loss_percent=1) -> 0.2 factor
425 : 2547 : current_grip_factor = 1.0 - (loss_percent * 0.8 * confidence);
426 : :
427 : : // Apply Floor (Safety)
428 : 2547 : current_grip_factor = (std::max)(0.2, (std::min)(1.0, current_grip_factor));
429 : :
430 : : // 5. Smoothing (v0.7.0)
431 : 2547 : double alpha = dt / ((double)m_slope_smoothing_tau + dt);
432 : 2547 : alpha = (std::max)(0.001, (std::min)(1.0, alpha));
433 : 2547 : m_slope_smoothed_output += alpha * (current_grip_factor - m_slope_smoothed_output);
434 : :
435 : 2547 : return m_slope_smoothed_output;
436 : : }
437 : :
438 : : // Helper: Calculate confidence factor for slope detection
439 : : // Extracted to avoid code duplication between slope detection and logging
440 : 2825 : double FFBEngine::calculate_slope_confidence(double dAlpha_dt) {
441 [ - + ]: 2825 : if (!m_slope_confidence_enabled) return 1.0;
442 : :
443 : : // v0.7.21 FIX: Use smoothstep confidence ramp [m_slope_alpha_threshold, m_slope_confidence_max_rate] rad/s
444 : : // to reject singularity artifacts near zero.
445 : 2825 : return smoothstep((double)m_slope_alpha_threshold, (double)m_slope_confidence_max_rate, std::abs(dAlpha_dt));
446 : : }
447 : :
448 : : // Helper: Calculate Slip Ratio from wheel (v0.6.36 - Extracted from lambdas)
449 : : // Unified slip ratio calculation for lockup and spin detection.
450 : : // Returns the ratio of longitudinal slip: (PatchVel - GroundVel) / GroundVel
451 : 10188 : double FFBEngine::calculate_wheel_slip_ratio(const TelemWheelV01& w) {
452 : 10188 : double v_long = std::abs(w.mLongitudinalGroundVel);
453 [ + + ]: 10188 : if (v_long < MIN_SLIP_ANGLE_VELOCITY) v_long = MIN_SLIP_ANGLE_VELOCITY;
454 : 10188 : return w.mLongitudinalPatchVel / v_long;
455 : : }
|
