Branch data Line data Source code
1 : : #ifndef FFBENGINE_H
2 : : #define FFBENGINE_H
3 : :
4 : : #include <cmath>
5 : : #include <algorithm>
6 : : #include <vector>
7 : : #include <mutex>
8 : : #include <atomic>
9 : : #include <iostream>
10 : : #include <chrono>
11 : : #include <array>
12 : : #include <cstring>
13 : : #include "io/lmu_sm_interface/InternalsPluginWrapper.h"
14 : : #include "AsyncLogger.h"
15 : : #include "MathUtils.h"
16 : : #include "PerfStats.h"
17 : : #include "VehicleUtils.h"
18 : :
19 : : #ifdef _WIN32
20 : : #define NOINLINE __declspec(noinline)
21 : : #else
22 : : #define NOINLINE __attribute__((noinline))
23 : : #endif
24 : :
25 : : // Bring common math into scope
26 : : using namespace ffb_math;
27 : : // Default FFB calculation timestep. Used by FFBCalculationContext (defined before
28 : : // FFBEngine, so cannot reference FFBEngine::DEFAULT_CALC_DT directly).
29 : : // Note: FFBEngine also has a private member of the same name; this file-scope
30 : : // constant does NOT trigger GCC's -Wchanges-meaning because it is only looked up
31 : : // inside FFBCalculationContext, not inside FFBEngine's own class body.
32 : : static constexpr double DEFAULT_CALC_DT = 0.0025; // 400 Hz (1/400 s)
33 : :
34 : :
35 : : // ChannelStats moved to PerfStats.h
36 : :
37 : : enum class LoadTransform {
38 : : LINEAR = 0,
39 : : CUBIC = 1,
40 : : QUADRATIC = 2,
41 : : HERMITE = 3
42 : : };
43 : :
44 : : // 1. Define the Snapshot Struct (Unified FFB + Telemetry)
45 : : #include "FFBSnapshot.h"
46 : :
47 : : // Refactored Managers
48 : : #include "FFBSafetyMonitor.h"
49 : : #include "FFBMetadataManager.h"
50 : : #include "FFBDebugBuffer.h"
51 : :
52 : : // BiquadNotch moved to MathUtils.h
53 : :
54 : : // Helper Result Struct for calculate_axle_grip
55 : : struct GripResult {
56 : : double value; // Final grip value
57 : : bool approximated; // Was approximation used?
58 : : double original; // Original telemetry value
59 : : double slip_angle; // Calculated slip angle (if approximated)
60 : : };
61 : :
62 : : struct Preset;
63 : :
64 : : namespace FFBEngineTests { class FFBEngineTestAccess; }
65 : :
66 : : struct FFBCalculationContext {
67 : : double dt = DEFAULT_CALC_DT;
68 : : double car_speed = 0.0; // Absolute m/s
69 : : double car_speed_long = 0.0; // Longitudinal m/s (Raw)
70 : : double speed_gate = 1.0;
71 : : double texture_load_factor = 1.0;
72 : : double brake_load_factor = 1.0;
73 : : double avg_front_load = 0.0;
74 : : double avg_front_grip = 0.0;
75 : :
76 : : // Diagnostics
77 : : bool frame_warn_load = false;
78 : : bool frame_warn_grip = false;
79 : : bool frame_warn_rear_grip = false;
80 : : bool frame_warn_dt = false;
81 : :
82 : : // Intermediate results
83 : : double grip_factor = 1.0; // 1.0 = full grip, 0.0 = no grip
84 : : double sop_base_force = 0.0;
85 : : double sop_unboosted_force = 0.0; // For snapshot compatibility
86 : : double lat_load_force = 0.0; // New v0.7.154 (Issue #282)
87 : : double rear_torque = 0.0;
88 : : double yaw_force = 0.0;
89 : : double scrub_drag_force = 0.0;
90 : : double gyro_force = 0.0;
91 : : double avg_rear_grip = 0.0;
92 : : double calc_rear_lat_force = 0.0;
93 : : double avg_rear_load = 0.0;
94 : : double long_load_force = 0.0; // New #301
95 : :
96 : : // Effect outputs
97 : : double road_noise = 0.0;
98 : : double slide_noise = 0.0;
99 : : double lockup_rumble = 0.0;
100 : : double spin_rumble = 0.0;
101 : : double bottoming_crunch = 0.0;
102 : : double abs_pulse_force = 0.0;
103 : : double soft_lock_force = 0.0;
104 : : double gain_reduction_factor = 1.0;
105 : : };
106 : :
107 : : // FFB Engine Class
108 : : class FFBEngine {
109 : : public:
110 : : using ParsedVehicleClass = ::ParsedVehicleClass;
111 : :
112 : : // Buffer size constants (declared first so they can be used as array bounds below)
113 : : static constexpr int STR_BUF_64 = 64;
114 : :
115 : : // Settings (GUI Sliders)
116 : : bool m_dynamic_normalization_enabled = false; // Issue #207: Structural force normalization toggle
117 : : bool m_auto_load_normalization_enabled = false; // Stage 3: Vibration Normalization (Load-based)
118 : : float m_vibration_gain = 1.0f; // Issue #206: Global vibration scaling
119 : : float m_gain;
120 : : float m_understeer_effect;
121 : : float m_understeer_gamma = 1.0f; // NEW: Gamma curve for understeer
122 : : float m_sop_effect;
123 : : float m_lat_load_effect = 0.0f; // New v0.7.121 (Issue #213 add, not replace)
124 : : LoadTransform m_lat_load_transform = LoadTransform::LINEAR; // New v0.7.154 (Issue #282)
125 : : float m_long_load_effect = 0.0f; // Renamed from dynamic_weight_gain (#301)
126 : : LoadTransform m_long_load_transform = LoadTransform::LINEAR; // New #301
127 : : float m_min_force;
128 : :
129 : : // Smoothing Settings (v0.7.47)
130 : : float m_long_load_smoothing; // Renamed from dynamic_weight_smoothing (#301)
131 : : float m_grip_smoothing_steady;
132 : : float m_grip_smoothing_fast;
133 : : float m_grip_smoothing_sensitivity;
134 : :
135 : : // Configurable Smoothing & Caps (v0.3.9)
136 : : float m_sop_smoothing_factor;
137 : : float m_texture_load_cap = 1.5f;
138 : : float m_brake_load_cap = 1.5f;
139 : : float m_sop_scale;
140 : :
141 : : // v0.4.4 Features
142 : : float m_wheelbase_max_nm;
143 : : float m_target_rim_nm;
144 : : bool m_invert_force = true;
145 : :
146 : : // Base Force Debugging (v0.4.13)
147 : : float m_steering_shaft_gain;
148 : : float m_ingame_ffb_gain = 1.0f; // New v0.7.71 (Issue #160)
149 : : int m_torque_source = 0;
150 : : bool m_torque_passthrough = false;
151 : :
152 : : // New Effects (v0.2)
153 : : float m_oversteer_boost;
154 : : float m_rear_align_effect;
155 : : float m_kerb_strike_rejection = 0.0f; // NEW: Kerb strike rejection slider
156 : : float m_sop_yaw_gain;
157 : : float m_gyro_gain;
158 : : float m_gyro_smoothing;
159 : : float m_yaw_accel_smoothing;
160 : : float m_chassis_inertia_smoothing;
161 : :
162 : : bool m_lockup_enabled;
163 : : float m_lockup_gain;
164 : : // NEW Lockup Tuning (v0.5.11)
165 : : float m_lockup_start_pct = 5.0f;
166 : : float m_lockup_full_pct = 15.0f;
167 : : float m_lockup_rear_boost = 1.5f;
168 : : float m_lockup_gamma = 2.0f;
169 : : float m_lockup_prediction_sens = 50.0f;
170 : : float m_lockup_bump_reject = 1.0f;
171 : :
172 : : bool m_abs_pulse_enabled = true;
173 : : float m_abs_gain = 1.0f;
174 : :
175 : : bool m_spin_enabled;
176 : : float m_spin_gain;
177 : :
178 : : // Texture toggles
179 : : bool m_slide_texture_enabled;
180 : : float m_slide_texture_gain;
181 : : float m_slide_freq_scale;
182 : :
183 : : bool m_road_texture_enabled;
184 : : float m_road_texture_gain;
185 : :
186 : : // Bottoming Effect (v0.3.2)
187 : : bool m_bottoming_enabled = true;
188 : : float m_bottoming_gain = 1.0f;
189 : :
190 : : // Soft Lock (Issue #117)
191 : : bool m_soft_lock_enabled = true;
192 : : float m_soft_lock_stiffness = 20.0f;
193 : : float m_soft_lock_damping = 0.5f;
194 : :
195 : : float m_slip_angle_smoothing;
196 : :
197 : : // NEW: Grip Estimation Settings (v0.5.7)
198 : : float m_optimal_slip_angle;
199 : : float m_optimal_slip_ratio;
200 : :
201 : : // NEW: Steering Shaft Smoothing (v0.5.7)
202 : : float m_steering_shaft_smoothing;
203 : :
204 : : // v0.4.41: Signal Filtering Settings
205 : : bool m_flatspot_suppression = false;
206 : : float m_notch_q = 2.0f;
207 : : float m_flatspot_strength = 1.0f;
208 : :
209 : : // Static Notch Filter (v0.4.43)
210 : : bool m_static_notch_enabled = false;
211 : : float m_static_notch_freq = 11.0f;
212 : : float m_static_notch_width = 2.0f;
213 : : float m_yaw_kick_threshold = 0.2f;
214 : :
215 : : // Unloaded Yaw Kick (Braking/Lift-off) - v0.7.164 (Issue #322)
216 : : float m_unloaded_yaw_gain = 0.0f;
217 : : float m_unloaded_yaw_threshold = 0.2f;
218 : : float m_unloaded_yaw_sens = 1.0f;
219 : : float m_unloaded_yaw_gamma = 0.5f;
220 : : float m_unloaded_yaw_punch = 0.05f;
221 : :
222 : : // Power Yaw Kick (Acceleration) - v0.7.164 (Issue #322)
223 : : float m_power_yaw_gain = 0.0f;
224 : : float m_power_yaw_threshold = 0.2f;
225 : : float m_power_slip_threshold = 0.10f;
226 : : float m_power_yaw_gamma = 0.5f;
227 : : float m_power_yaw_punch = 0.05f;
228 : :
229 : : // v0.6.23: User-Adjustable Speed Gate
230 : : float m_speed_gate_lower = 1.0f;
231 : : float m_speed_gate_upper = 5.0f;
232 : :
233 : : // REST API Fallback (Issue #221)
234 : : bool m_rest_api_enabled = false;
235 : : int m_rest_api_port = 6397;
236 : :
237 : : // v0.6.23: Additional Advanced Physics (Reserved for future use)
238 : : float m_road_fallback_scale = 0.05f;
239 : : bool m_understeer_affects_sop = false;
240 : :
241 : : // ===== SLOPE DETECTION (v0.7.0 -> v0.7.3 stability fixes) =====
242 : : bool m_slope_detection_enabled = false;
243 : : int m_slope_sg_window = 15;
244 : : float m_slope_sensitivity = 0.5f;
245 : :
246 : : float m_slope_smoothing_tau = 0.04f;
247 : :
248 : : // NEW v0.7.3: Stability fixes
249 : : float m_slope_alpha_threshold = 0.02f;
250 : : float m_slope_decay_rate = 5.0f;
251 : : bool m_slope_confidence_enabled = true;
252 : : float m_slope_confidence_max_rate = 0.10f;
253 : :
254 : : // NEW v0.7.11: Min/Max Threshold System
255 : : float m_slope_min_threshold = -0.3f;
256 : : float m_slope_max_threshold = -2.0f;
257 : :
258 : : // NEW v0.7.40: Advanced Features
259 : : float m_slope_g_slew_limit = 50.0f;
260 : : bool m_slope_use_torque = true;
261 : : float m_slope_torque_sensitivity = 0.5f;
262 : :
263 : : // Signal Diagnostics
264 : : double m_debug_freq = 0.0;
265 : : double m_theoretical_freq = 0.0;
266 : :
267 : : // Rate Monitoring (Issue #129)
268 : : double m_ffb_rate = 0.0;
269 : : double m_telemetry_rate = 0.0;
270 : : double m_hw_rate = 0.0;
271 : : double m_torque_rate = 0.0;
272 : : double m_gen_torque_rate = 0.0;
273 : : double m_physics_rate = 0.0; // New v0.7.117 (Issue #217)
274 : :
275 : : // Warning States (Console logging)
276 : : bool m_warned_load = false;
277 : : bool m_warned_grip = false;
278 : : bool m_warned_rear_grip = false;
279 : : bool m_warned_dt = false;
280 : : bool m_warned_lat_force_front = false;
281 : : bool m_warned_lat_force_rear = false;
282 : : bool m_warned_susp_force = false;
283 : : bool m_warned_susp_deflection = false;
284 : : bool m_warned_vert_deflection = false;
285 : :
286 : : // Diagnostics (v0.4.5 Fix)
287 : : struct GripDiagnostics {
288 : : bool front_approximated = false;
289 : : bool rear_approximated = false;
290 : : double front_original = 0.0;
291 : : double rear_original = 0.0;
292 : : double front_slip_angle = 0.0;
293 : : double rear_slip_angle = 0.0;
294 : : } m_grip_diag;
295 : :
296 : : // Hysteresis for missing load
297 : : int m_missing_load_frames = 0;
298 : : int m_missing_lat_force_front_frames = 0;
299 : : int m_missing_lat_force_rear_frames = 0;
300 : : int m_missing_susp_force_frames = 0;
301 : : int m_missing_susp_deflection_frames = 0;
302 : : int m_missing_vert_deflection_frames = 0;
303 : :
304 : : // Internal state
305 : : TelemInfoV01 m_working_info; // Persistent storage for upsampled telemetry
306 : : double m_last_telemetry_time = -1.0;
307 : :
308 : : ffb_math::LinearExtrapolator m_upsample_lat_patch_vel[4];
309 : : ffb_math::LinearExtrapolator m_upsample_long_patch_vel[4];
310 : : ffb_math::LinearExtrapolator m_upsample_vert_deflection[4];
311 : : ffb_math::LinearExtrapolator m_upsample_susp_force[4];
312 : : ffb_math::LinearExtrapolator m_upsample_brake_pressure[4];
313 : : ffb_math::LinearExtrapolator m_upsample_rotation[4];
314 : : ffb_math::LinearExtrapolator m_upsample_steering;
315 : : ffb_math::LinearExtrapolator m_upsample_throttle;
316 : : ffb_math::LinearExtrapolator m_upsample_brake;
317 : : ffb_math::LinearExtrapolator m_upsample_local_accel_x;
318 : : ffb_math::LinearExtrapolator m_upsample_local_accel_y;
319 : : ffb_math::LinearExtrapolator m_upsample_local_accel_z;
320 : : ffb_math::LinearExtrapolator m_upsample_local_rot_accel_y;
321 : : ffb_math::LinearExtrapolator m_upsample_local_rot_y;
322 : : ffb_math::HoltWintersFilter m_upsample_shaft_torque;
323 : :
324 : : double m_prev_vert_deflection[4] = {0.0, 0.0, 0.0, 0.0};
325 : : double m_prev_vert_accel = 0.0;
326 : : double m_prev_slip_angle[4] = {0.0, 0.0, 0.0, 0.0};
327 : : double m_prev_load[4] = {0.0, 0.0, 0.0, 0.0}; // NEW: Smoothed load state
328 : : double m_prev_rotation[4] = {0.0, 0.0, 0.0, 0.0};
329 : : double m_prev_brake_pressure[4] = {0.0, 0.0, 0.0, 0.0};
330 : :
331 : : // Gyro State (v0.4.17)
332 : : double m_prev_steering_angle = 0.0;
333 : : double m_steering_velocity_smoothed = 0.0;
334 : :
335 : : // Yaw Acceleration Smoothing State (v0.4.18)
336 : : double m_yaw_accel_smoothed = 0.0;
337 : : double m_prev_yaw_rate = 0.0; // New v0.7.144 (Solution 2)
338 : : bool m_yaw_rate_seeded = false; // New v0.7.144 (Solution 2)
339 : : double m_prev_yaw_rate_log = 0.0;
340 : :
341 : : // REPLACE m_prev_derived_yaw_accel WITH THESE:
342 : : double m_fast_yaw_accel_smoothed = 0.0;
343 : : double m_prev_fast_yaw_accel = 0.0;
344 : : bool m_yaw_accel_seeded = false; // New v0.7.164 (Issue #322)
345 : :
346 : : // NEW: Vulnerability gate smoothing
347 : : double m_unloaded_vulnerability_smoothed = 0.0;
348 : : double m_power_vulnerability_smoothed = 0.0;
349 : :
350 : : // Derived Acceleration State (Issue #278)
351 : : TelemVect3 m_prev_local_vel = {};
352 : : bool m_local_vel_seeded = false;
353 : : double m_derived_accel_y_100hz = 0.0;
354 : : double m_derived_accel_z_100hz = 0.0;
355 : : bool m_yaw_rate_log_seeded = false;
356 : :
357 : : // Internal state for Steering Shaft Smoothing (v0.5.7)
358 : : double m_steering_shaft_torque_smoothed = 0.0;
359 : :
360 : : // Kinematic Smoothing State (v0.4.38)
361 : : double m_accel_x_smoothed = 0.0;
362 : : double m_accel_z_smoothed = 0.0;
363 : :
364 : :
365 : : // Phase Accumulators for Dynamic Oscillators
366 : : double m_kerb_timer = 0.0; // NEW: Kerb strike attenuation timer
367 : : double m_lockup_phase = 0.0;
368 : : double m_spin_phase = 0.0;
369 : : double m_slide_phase = 0.0;
370 : : double m_abs_phase = 0.0;
371 : : double m_bottoming_phase = 0.0;
372 : :
373 : : // Phase Accumulators for Dynamic Oscillators (v0.6.20)
374 : : float m_abs_freq_hz = 20.0f;
375 : : float m_lockup_freq_scale = 1.0f;
376 : : float m_spin_freq_scale = 1.0f;
377 : :
378 : : // Internal state for Bottoming (Method B)
379 : : double m_prev_susp_force[4] = {0.0, 0.0, 0.0, 0.0};
380 : :
381 : : // Seeding state (Issue #379)
382 : : bool m_derivatives_seeded = false;
383 : :
384 : : // New Settings (v0.4.5)
385 : : int m_bottoming_method = 0;
386 : : float m_scrub_drag_gain;
387 : :
388 : : // Smoothing State
389 : : double m_sop_lat_g_smoothed = 0.0;
390 : : double m_sop_load_smoothed = 0.0; // New v0.7.121
391 : :
392 : : // Filter Instances (v0.4.41)
393 : : BiquadNotch m_notch_filter;
394 : : BiquadNotch m_static_notch_filter;
395 : :
396 : : // Slope Detection Buffers (Circular) - v0.7.0
397 : : static constexpr int SLOPE_BUFFER_MAX = 41;
398 : : std::array<double, SLOPE_BUFFER_MAX> m_slope_lat_g_buffer = {};
399 : : std::array<double, SLOPE_BUFFER_MAX> m_slope_slip_buffer = {};
400 : : int m_slope_buffer_index = 0;
401 : : int m_slope_buffer_count = 0;
402 : :
403 : : // Slope Detection State (Public for diagnostics) - v0.7.0
404 : : double m_slope_current = 0.0;
405 : : double m_slope_grip_factor = 1.0;
406 : : double m_slope_smoothed_output = 1.0;
407 : :
408 : : // NEW v0.7.38: Input Smoothing State
409 : : double m_slope_lat_g_smoothed = 0.0;
410 : : double m_slope_slip_smoothed = 0.0;
411 : :
412 : : // NEW v0.7.38: Steady State Logic
413 : : double m_slope_hold_timer = 0.0;
414 : : static constexpr double SLOPE_HOLD_TIME = 0.25;
415 : :
416 : : // NEW v0.7.38: Debug members for Logger
417 : : double m_debug_slope_raw = 0.0;
418 : : double m_debug_slope_num = 0.0;
419 : : double m_debug_slope_den = 0.0;
420 : :
421 : : // NEW v0.7.40: Advanced Slope Detection State
422 : : double m_slope_lat_g_prev = 0.0;
423 : : std::array<double, SLOPE_BUFFER_MAX> m_slope_torque_buffer = {};
424 : : std::array<double, SLOPE_BUFFER_MAX> m_slope_steer_buffer = {};
425 : : double m_slope_torque_smoothed = 0.0;
426 : : double m_slope_steer_smoothed = 0.0;
427 : : double m_slope_torque_current = 0.0;
428 : :
429 : : // NEW v0.7.40: More Debug members
430 : : double m_debug_slope_torque_num = 0.0;
431 : : double m_debug_slope_torque_den = 0.0;
432 : : double m_debug_lat_g_slew = 0.0;
433 : :
434 : : // Dynamic Weight State (v0.7.46)
435 : : double m_static_front_load = 0.0;
436 : : double m_static_rear_load = 0.0; // New v0.7.164 (Issue #322)
437 : : bool m_static_load_latched = false;
438 : : double m_smoothed_vibration_mult = 1.0;
439 : : double m_long_load_smoothed = 1.0; // Renamed from dynamic_weight_smoothed (#301)
440 : : double m_front_grip_smoothed_state = 1.0;
441 : : double m_rear_grip_smoothed_state = 1.0;
442 : :
443 : : // Metadata Manager (Option A Extracted)
444 : : FFBMetadataManager m_metadata;
445 : :
446 : : // Logging intermediate values (exposed for AsyncLogger)
447 : : double m_slope_dG_dt = 0.0;
448 : : double m_slope_dAlpha_dt = 0.0;
449 : :
450 : : // Frequency Estimator State (v0.4.41)
451 : : double m_last_crossing_time = 0.0;
452 : : double m_torque_ac_smoothed = 0.0;
453 : : double m_prev_ac_torque = 0.0;
454 : :
455 : : // Safety Monitor (Option A Extracted)
456 : : FFBSafetyMonitor m_safety;
457 : :
458 : : // Telemetry Stats
459 : : ChannelStats s_torque;
460 : : ChannelStats s_front_load;
461 : : ChannelStats s_front_grip;
462 : : ChannelStats s_lat_g;
463 : : std::chrono::steady_clock::time_point last_log_time;
464 : :
465 : : // Thread-Safe Buffer (Producer-Consumer)
466 : : FFBDebugBuffer m_debug_buffer{100}; // DEBUG_BUFFER_CAP
467 : :
468 : : friend class FFBEngineTests::FFBEngineTestAccess;
469 : : friend struct Preset;
470 : :
471 : : FFBEngine();
472 : :
473 : 141 : std::vector<FFBSnapshot> GetDebugBatch() { return m_debug_buffer.GetBatch(); }
474 : :
475 : : // UI Reference & Physics Multipliers (v0.4.50)
476 : : static constexpr float BASE_NM_SOP_LATERAL = 1.0f;
477 : : static constexpr float BASE_NM_REAR_ALIGN = 3.0f;
478 : : static constexpr float BASE_NM_YAW_KICK = 5.0f;
479 : : static constexpr float BASE_NM_GYRO_DAMPING = 1.0f;
480 : : static constexpr float BASE_NM_SLIDE_TEXTURE = 1.5f;
481 : : static constexpr float BASE_NM_ROAD_TEXTURE = 2.5f;
482 : : static constexpr float BASE_NM_LOCKUP_VIBRATION = 4.0f;
483 : : static constexpr float BASE_NM_SPIN_VIBRATION = 2.5f;
484 : : static constexpr float BASE_NM_SCRUB_DRAG = 5.0f;
485 : : static constexpr float BASE_NM_BOTTOMING = 1.0f;
486 : : static constexpr float BASE_NM_SOFT_LOCK = 50.0f;
487 : :
488 : : private:
489 : : static constexpr double MIN_SLIP_ANGLE_VELOCITY = 0.5; // m/s
490 : : static constexpr double REAR_TIRE_STIFFNESS_COEFFICIENT = 15.0;
491 : : static constexpr double MAX_REAR_LATERAL_FORCE = 6000.0; // N
492 : : static constexpr double REAR_ALIGN_TORQUE_COEFFICIENT = 0.001; // Nm per N
493 : : static constexpr double KERB_LOAD_CAP_MULT = 1.5;
494 : : static constexpr double KERB_DETECTION_THRESHOLD_M_S = 0.8;
495 : : static constexpr double KERB_HOLD_TIME_S = 0.1;
496 : : static constexpr double DEFAULT_STEERING_RANGE_RAD = 9.4247;
497 : : static constexpr double GYRO_SPEED_SCALE = 10.0;
498 : : static constexpr double WEIGHT_TRANSFER_SCALE = 2000.0; // N per G
499 : : static constexpr double MIN_VALID_SUSP_FORCE = 10.0; // N
500 : : static constexpr double LOCKUP_FREQ_MULTIPLIER_REAR = 0.3;
501 : : static constexpr double LOCKUP_AMPLITUDE_BOOST_REAR = 1.5;
502 : : static constexpr double AXLE_DIFF_HYSTERESIS = 0.01;
503 : : static constexpr double ABS_PEDAL_THRESHOLD = 0.5;
504 : : static constexpr double ABS_PRESSURE_RATE_THRESHOLD = 2.0;
505 : : static constexpr double PREDICTION_BRAKE_THRESHOLD = 0.02;
506 : : static constexpr double PREDICTION_LOAD_THRESHOLD = 50.0;
507 : :
508 : : double m_auto_peak_front_load = 4500.0; // DEFAULT_AUTO_PEAK_LOAD
509 : :
510 : : static constexpr double HPF_TIME_CONSTANT_S = 0.1;
511 : : static constexpr double ZERO_CROSSING_EPSILON = 0.05;
512 : : static constexpr double MIN_FREQ_PERIOD = 0.005;
513 : : static constexpr double MAX_FREQ_PERIOD = 1.0;
514 : : static constexpr double RADIUS_FALLBACK_DEFAULT_M = 0.33;
515 : : static constexpr double RADIUS_FALLBACK_MIN_M = 0.1;
516 : : static constexpr double UNIT_CM_TO_M = 100.0;
517 : : static constexpr double GRAVITY_MS2 = 9.81;
518 : : static constexpr double EPSILON_DIV = 1e-9;
519 : : static constexpr double TORQUE_SPIKE_RATIO = 3.0;
520 : : static constexpr double TORQUE_SPIKE_MIN_NM = 15.0;
521 : : static constexpr int MISSING_TELEMETRY_WARN_THRESHOLD = 50;
522 : : static constexpr double PHASE_WRAP_LIMIT = 50.0;
523 : : static constexpr double LAT_G_CLEAN_LIMIT = 8.0;
524 : : static constexpr double TORQUE_SLEW_CLEAN_LIMIT = 1000.0;
525 : : static constexpr double TORQUE_ROLL_AVG_TAU = 1.0;
526 : :
527 : : static constexpr float PEAK_TORQUE_DECAY = 0.005f;
528 : : static constexpr float PEAK_TORQUE_FLOOR = 1.0f;
529 : : static constexpr float PEAK_TORQUE_CEILING = 100.0f;
530 : : static constexpr float GAIN_SMOOTHING_TAU = 0.25f;
531 : : static constexpr float STRUCT_MULT_SMOOTHING_TAU = 0.25f;
532 : : static constexpr float IDLE_SPEED_MIN_M_S = 3.0f;
533 : : static constexpr float IDLE_BLEND_FACTOR = 0.1f;
534 : : static constexpr float SESSION_PEAK_DECAY_RATE = 0.005f;
535 : :
536 : : static constexpr double DT_EPSILON = 0.000001;
537 : : static constexpr double DEFAULT_DT = 0.0025;
538 : : static constexpr double SPEED_EPSILON = 1.0;
539 : : static constexpr double ACCEL_EPSILON = 1.0;
540 : : static constexpr double G_FORCE_THRESHOLD = 3.0;
541 : : static constexpr double SPEED_HIGH_THRESHOLD = 10.0;
542 : : static constexpr double LOAD_SAFETY_FLOOR = 3000.0;
543 : : static constexpr double LOAD_DECAY_RATE = 100.0;
544 : : static constexpr double COMPRESSION_KNEE_THRESHOLD = 1.5;
545 : : static constexpr double COMPRESSION_KNEE_WIDTH = 0.5;
546 : : static constexpr double COMPRESSION_RATIO = 4.0;
547 : : static constexpr double VIBRATION_EMA_TAU = 0.1;
548 : : static constexpr double USER_CAP_MAX = 10.0;
549 : : static constexpr double CLIPPING_THRESHOLD = 0.99;
550 : : static constexpr int STR_MAX_64 = 63;
551 : : static constexpr int STR_MAX_256 = 255;
552 : : static constexpr int MISSING_LOAD_WARN_THRESHOLD = 20;
553 : : static constexpr double LONG_LOAD_MIN = 0.0; // Relaxed #301
554 : : static constexpr double LONG_LOAD_MAX = 10.0; // Increased #301
555 : : static constexpr double MIN_TAU_S = 0.0001;
556 : : static constexpr double ALPHA_MIN = 0.001;
557 : : static constexpr double ALPHA_MAX = 1.0;
558 : : static constexpr double DUAL_DIVISOR = 2.0;
559 : : static constexpr double HALF_PERIOD_MULT = 0.5;
560 : : static constexpr double MIN_NOTCH_WIDTH_HZ = 0.1;
561 : : static constexpr int VEHICLE_NAME_CHECK_IDX = 10;
562 : : static constexpr int DEBUG_BUFFER_CAP = 100;
563 : : static constexpr double OVERSTEER_BOOST_MULT = 2.0;
564 : : static constexpr double MIN_YAW_KICK_SPEED_MS = 5.0;
565 : : static constexpr double MIN_SLIP_WINDOW = 0.01;
566 : : static constexpr double SAWTOOTH_SCALE = 2.0;
567 : : static constexpr double SAWTOOTH_OFFSET = 1.0;
568 : : static constexpr double FFB_EPSILON = 0.0001;
569 : : static constexpr double SOFT_LOCK_MUTE_THRESHOLD_NM = 0.1;
570 : : static constexpr double SOP_SMOOTHING_MAX_TAU = 0.1;
571 : :
572 : : // Default Values
573 : : static constexpr float DEFAULT_SAFETY_SLEW_FULL_SCALE_TIME_S = 1.0f; // matches Issue #316 expectations
574 : : static constexpr float DEFAULT_SAFETY_WINDOW_DURATION = 0.0f;
575 : : static constexpr float DEFAULT_SAFETY_GAIN_REDUCTION = 0.3f;
576 : : static constexpr float DEFAULT_SAFETY_SMOOTHING_TAU = 0.2f;
577 : : static constexpr float DEFAULT_SPIKE_DETECTION_THRESHOLD = 500.0f;
578 : : static constexpr float DEFAULT_IMMEDIATE_SPIKE_THRESHOLD = 1500.0f;
579 : :
580 : : static constexpr bool DEFAULT_STUTTER_SAFETY_ENABLED = false;
581 : : static constexpr float DEFAULT_STUTTER_THRESHOLD = 1.5f;
582 : :
583 : : static constexpr double MIN_LFM_ALPHA = 0.001;
584 : : static constexpr double G_LIMIT_5G = 5.0;
585 : : static constexpr double SMOOTHNESS_LIMIT_0999 = 0.999;
586 : : static constexpr double BOTTOMING_RH_THRESHOLD_M = 0.002;
587 : : static constexpr double BOTTOMING_IMPULSE_THRESHOLD_N_S = 100000.0;
588 : : static constexpr double BOTTOMING_IMPULSE_RANGE_N_S = 200000.0;
589 : : static constexpr double BOTTOMING_LOAD_MULT = 2.5;
590 : : static constexpr double BOTTOMING_INTENSITY_SCALE = 0.05;
591 : : static constexpr double BOTTOMING_FREQ_HZ = 50.0;
592 : : static constexpr double SPIN_THROTTLE_THRESHOLD = 0.05;
593 : : static constexpr double SPIN_SLIP_THRESHOLD = 0.2;
594 : : static constexpr double SPIN_SEVERITY_RANGE = 0.5;
595 : : static constexpr double SPIN_TORQUE_DROP_FACTOR = 0.6;
596 : : static constexpr double SPIN_BASE_FREQ = 10.0;
597 : : static constexpr double SPIN_FREQ_SLIP_MULT = 2.5;
598 : : static constexpr double SPIN_MAX_FREQ = 80.0;
599 : : static constexpr double LOCKUP_BASE_FREQ = 10.0;
600 : : static constexpr double LOCKUP_FREQ_SPEED_MULT = 1.5;
601 : : static constexpr double SLIDE_VEL_THRESHOLD = 1.5;
602 : : static constexpr double SLIDE_BASE_FREQ = 10.0;
603 : : static constexpr double SLIDE_FREQ_VEL_MULT = 5.0;
604 : : static constexpr double SLIDE_MAX_FREQ = 250.0;
605 : : static constexpr double LOCKUP_ACCEL_MARGIN = 2.0;
606 : : static constexpr double LOW_PRESSURE_LOCKUP_THRESHOLD = 0.1;
607 : : static constexpr double LOW_PRESSURE_LOCKUP_FIX = 0.5;
608 : : static constexpr double ABS_PULSE_MAGNITUDE_SCALER = 2.0;
609 : : static constexpr double PERCENT_TO_DECIMAL = 100.0;
610 : : static constexpr double SCRUB_VEL_THRESHOLD = 0.001;
611 : : static constexpr double SCRUB_FADE_RANGE = 0.5;
612 : :
613 : : static constexpr double DEFLECTION_DELTA_LIMIT = 0.01;
614 : : static constexpr double DEFLECTION_ACTIVE_THRESHOLD = 1e-6;
615 : : static constexpr double DEFLECTION_NEAR_ZERO_M = 1e-6; // Near-zero threshold for susp/vert deflection checks (m)
616 : : static constexpr double MIN_VALID_LAT_FORCE_N = 1.0; // Minimum lateral force to consider data present (N)
617 : : static constexpr double ROAD_TEXTURE_SPEED_THRESHOLD = 5.0;
618 : : static constexpr double DEFLECTION_NM_SCALE = 50.0;
619 : : static constexpr double ACCEL_ROAD_TEXTURE_SCALE = 0.05;
620 : : static constexpr double DEBUG_FREQ_SMOOTHING = 0.9;
621 : : static constexpr double GAIN_REDUCTION_MAX = 50.0;
622 : : double m_session_peak_torque = 25.0; // DEFAULT_SESSION_PEAK_TORQUE
623 : : double m_smoothed_structural_mult = 1.0 / 25.0;
624 : : double m_rolling_average_torque = 0.0;
625 : : double m_last_raw_torque = 0.0;
626 : : bool m_was_allowed = true; // Track transition for filter reset
627 : :
628 : : // Diagnostic Log Cooldowns (v0.7.x)
629 : : double m_last_core_nan_log_time = -999.0;
630 : : double m_last_aux_nan_log_time = -999.0;
631 : : double m_last_math_nan_log_time = -999.0;
632 : :
633 : : void update_static_load_reference(double current_front_load, double current_rear_load, double speed, double dt);
634 : : void InitializeLoadReference(const char* className, const char* vehicleName);
635 : :
636 : : public:
637 : : double calculate_raw_slip_angle_pair(const TelemWheelV01& w1, const TelemWheelV01& w2);
638 : : double calculate_slip_angle(const TelemWheelV01& w, double& prev_state, double dt);
639 : :
640 : : GripResult calculate_axle_grip(const TelemWheelV01& w1,
641 : : const TelemWheelV01& w2,
642 : : double avg_axle_load,
643 : : bool& warned_flag,
644 : : double& prev_slip1,
645 : : double& prev_slip2,
646 : : double& prev_load1, // NEW: State for load smoothing
647 : : double& prev_load2, // NEW: State for load smoothing
648 : : double car_speed,
649 : : double dt,
650 : : const char* vehicleName,
651 : : const TelemInfoV01* data,
652 : : bool is_front);
653 : :
654 : : double approximate_load(const TelemWheelV01& w);
655 : : double approximate_rear_load(const TelemWheelV01& w);
656 : : double calculate_manual_slip_ratio(const TelemWheelV01& w, double car_speed_ms);
657 : : NOINLINE double calculate_slope_grip(double lateral_g, double slip_angle, double dt, const TelemInfoV01* data = nullptr);
658 : : double calculate_slope_confidence(double dAlpha_dt);
659 : : double calculate_wheel_slip_ratio(const TelemWheelV01& w);
660 : :
661 : : double calculate_force(const TelemInfoV01* data, const char* vehicleClass = nullptr, const char* vehicleName = nullptr, float genFFBTorque = 0.0f, bool allowed = true, double override_dt = -1.0, signed char mControl = 0);
662 : :
663 : : void UpdateMetadata(const struct SharedMemoryObjectOut& data);
664 : : double apply_signal_conditioning(double raw_torque, const TelemInfoV01* data, FFBCalculationContext& ctx);
665 : : void ResetNormalization();
666 : :
667 : : private:
668 : : void calculate_sop_lateral(const TelemInfoV01* data, FFBCalculationContext& ctx);
669 : : NOINLINE void calculate_gyro_damping(const TelemInfoV01* data, FFBCalculationContext& ctx);
670 : : NOINLINE void calculate_abs_pulse(const TelemInfoV01* data, FFBCalculationContext& ctx);
671 : : void calculate_lockup_vibration(const TelemInfoV01* data, FFBCalculationContext& ctx);
672 : : void calculate_wheel_spin(const TelemInfoV01* data, FFBCalculationContext& ctx);
673 : : void calculate_slide_texture(const TelemInfoV01* data, FFBCalculationContext& ctx);
674 : : void calculate_road_texture(const TelemInfoV01* data, FFBCalculationContext& ctx);
675 : : void calculate_suspension_bottoming(const TelemInfoV01* data, FFBCalculationContext& ctx);
676 : : void calculate_soft_lock(const TelemInfoV01* data, FFBCalculationContext& ctx);
677 : : };
678 : :
679 : : #endif // FFBENGINE_H
|
