Merge branch 'master' into 20260402_Quickflash-ESO

Author: nerdCopter

OVERVIEW.md

@@ -25,6 +25,9 @@ cargo build --release
 ```
 
 ### Usage
+
+> **Notice:** Default output is **core plots only**. Use `--extended` for all plots.
+
 ```shell
 Usage: ./BlackBox_CSV_Render <input1> [<input2> ...] [OPTIONS]
 
@@ -36,27 +39,29 @@ Usage: ./BlackBox_CSV_Render <input1> [<input2> ...] [OPTIONS]
 
 === PLOT TYPE SELECTION ===
 
-  Note: Plot flags are combinable. Without flags, all plots generated.
-
-  --step: Generate only step response plots.
-  --motor: Generate only motor spectrum plots.
-  --setpoint: Generate only setpoint-related plots.
-  --pid: Generate only P, I, D activity plot.
-  --bode: Bode plot analysis (requires chirp/sweep system-id test flight, not normal logs).
+  --core           [default] Step Response, Gyro Spectrums, D-term Spectrums,
+                   Setpoint vs Gyro, Gyro vs Unfiltered, Motor Spectrums.
+  --extended       All plots except Bode — adds PIDsum/Error, PID Activity,
+                   Setpoint Derivative, Gyro PSD, D-term PSD, and heatmaps.
+  --step           Step response only.
+  --bode           Bode only (requires chirp/sweep system-id test flight).
 
 === ANALYSIS OPTIONS ===
 
-  --butterworth: Show Butterworth PT1 cutoffs on gyro/D-term spectrum plots.
-  --dps <value>: Deg/s threshold for detailed step response plots (positive number).
+  --butterworth    Show Butterworth PT1 cutoffs on gyro/D-term spectrum plots.
+  --dps <value>    Deg/s threshold for detailed step response plots (positive number).
+  --estimate-optimal-p  [EXPERIMENTAL] Optimal P estimation from throttle-punch
+                        dynamics. Requires .headers.csv; skips if absent.
 
 === GENERAL ===
 
-  --debug: Show detailed metadata during processing.
-  -h, --help: Show this help message and exit.
-  -V, --version: Show version information.
+  --debug          Show detailed metadata during processing.
+  -h, --help       Show this help message and exit.
+  -V, --version    Show version information.
 ```
 
 Arguments can be in any order. Wildcards (e.g., *.csv) are shell-expanded and work with mixed file/directory patterns.
+
 ### Example execution commands
 ```shell
 ./target/release/BlackBox_CSV_Render path/to/BTFL_Log.csv
@@ -71,35 +76,41 @@ Arguments can be in any order. Wildcards (e.g., *.csv) are shell-expanded and wo
 ./target/release/BlackBox_CSV_Render path/to/ -R --step --output-dir ./step-only
 ```
 ```shell
-./target/release/BlackBox_CSV_Render path/to/ --setpoint --output-dir ./setpoint-only
+./target/release/BlackBox_CSV_Render path/to/ --extended --output-dir ./all-plots
 ```
 ```shell
-./target/release/BlackBox_CSV_Render path/to/ --step --setpoint --motor --output-dir ./all-selective
+./target/release/BlackBox_CSV_Render path/to/BTFL_Log.csv --step --estimate-optimal-p
 ```
 
 ### Output
 
 #### PNG Files Generated
+
+**Core (default):**
 - `*_Step_Response_stacked_plot_*.png` — Step response analysis with P:D recommendations
-- `*_PIDsum_PIDerror_Setpoint_stacked.png` — PIDsum, PID error, and setpoint traces
 - `*_SetpointVsGyro_stacked.png` — Setpoint vs. filtered gyro comparison
 - `*_GyroVsUnfilt_stacked.png` — Filtered vs. unfiltered gyro comparison with delay estimates
 - `*_Gyro_Spectrums_comparative.png` — Frequency-domain gyro amplitude spectrums
-- `*_Gyro_PSD_comparative.png` — Gyro power spectral density (dB scale)
 - `*_D_Term_Spectrums_comparative.png` — Frequency-domain D-term amplitude spectrums
+- `*_Motor_Spectrums_stacked.png` — Motor output frequency analysis (supports any motor count; colors wrap every 8 motors)
+
+**Extended (`--extended` adds these to the core set):**
+- `*_PIDsum_PIDerror_Setpoint_stacked.png` — PIDsum, PID error, and setpoint traces
+- `*_PID_Activity_stacked.png` — P, I, D term activity over time
+- `*_SetpointDerivative_stacked.png` — Setpoint rate-of-change / feed-forward proxy
+- `*_Gyro_PSD_comparative.png` — Gyro power spectral density (dB scale)
 - `*_D_Term_PSD_comparative.png` — D-term power spectral density (dB scale)
 - `*_D_Term_Heatmap_comparative.png` — D-term throttle/frequency heatmap
 - `*_Gyro_PSD_Spectrogram_comparative.png` — Gyro spectrogram (PSD vs. time)
 - `*_Throttle_Freq_Heatmap_comparative.png` — Throttle/frequency heatmap analysis
-- `*_Motor_Spectrums_stacked.png` — Motor output frequency analysis (supports any motor count; colors wrap every 8 motors)
-- `*_PID_Activity_stacked.png` — P, I, D term activity over time (stacked plot showing all three PID components)
 
 #### Console Output:
 - Current P:D ratio and peak analysis with response assessment
 - Conservative and Moderate tuning recommendations (with D/D-Min/D-Max values)
 - Warning indicators for severe overshoot or unreasonable ratios
 - Gyro filtering delay estimates (filtered vs. unfiltered, with confidence)
 - Filter configuration parsing and spectrum peak detection summaries
+- Optimal P estimation (`--estimate-optimal-p`): Td timing, target deviation, noise level, consistency, P/D recommendations and skip-reason warnings
 - Use `--debug` flag for additional metadata: header information, flight data key mapping, sample header values, and debug mode identification
 
 #### Code and Output Overview

README.md

@@ -13,25 +13,29 @@
 /// Panics if index is greater than 2
 #[allow(dead_code)]
 pub fn axis_name(index: usize) -> &'static str {
-    match index {
-        0 => "Roll",
-        1 => "Pitch",
-        2 => "Yaw",
-        _ => panic!(
+    AXIS_NAMES.get(index).copied().unwrap_or_else(|| {
+        panic!(
             "Invalid axis index: {}. Expected 0 (Roll), 1 (Pitch), or 2 (Yaw)",
             index
-        ),
-    }
+        )
+    })
 }
 
 /// Number of axes (Roll, Pitch, Yaw)
 pub const AXIS_COUNT: usize = 3;
 
+/// Number of primary control axes (Roll, Pitch only - excludes Yaw)
+pub const ROLL_PITCH_AXIS_COUNT: usize = 2;
+
 /// Get all axis names as a static array
 pub const AXIS_NAMES: [&str; AXIS_COUNT] = ["Roll", "Pitch", "Yaw"];
 
-// Compile-time check to prevent drift between AXIS_COUNT and AXIS_NAMES.len()
+// Compile-time assertions to prevent invariant drift
 const _: [(); AXIS_COUNT] = [(); AXIS_NAMES.len()];
+const _: () = assert!(
+    ROLL_PITCH_AXIS_COUNT > 0 && ROLL_PITCH_AXIS_COUNT < AXIS_COUNT,
+    "ROLL_PITCH_AXIS_COUNT must be > 0 and < AXIS_COUNT"
+);
 
 #[cfg(test)]
 mod tests {

src/axis_names.rs

@@ -49,6 +49,12 @@ pub const DEFAULT_SETPOINT_THRESHOLD: f64 = 500.0; // Default setpoint threshold
 
 // Constants for filtering data based on movement and flight phase.
 pub const MOVEMENT_THRESHOLD_DEG_S: f64 = 20.0; // Minimum setpoint/gyro magnitude (from PTB/PlasmaTree)
+/// Mean window-max setpoint (deg/s) thresholds for Setpoint Authority classification.
+/// LOW  < 100 dps : hover/slow-cruise — Moderate/Aggressive recommendations suppressed.
+/// MODERATE 100–250 dps : normal flight inputs.
+/// HIGH > 250 dps : aggressive/race inputs.
+pub const LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S: f32 = 100.0;
+pub const HIGH_AUTHORITY_SETPOINT_THRESHOLD_DEG_S: f32 = 250.0;
 pub const EXCLUDE_START_S: f64 = 3.0; // Exclude seconds from the start of the log
 pub const EXCLUDE_END_S: f64 = 3.0; // Exclude seconds from the end of the log
 
@@ -189,6 +195,14 @@ pub const COLOR_STEP_RESPONSE_LOW_SP: &RGBColor = &LIGHTBLUE;
 pub const COLOR_STEP_RESPONSE_HIGH_SP: &RGBColor = &ORANGE;
 pub const COLOR_STEP_RESPONSE_COMBINED: &RGBColor = &RED;
 
+// Optimal P Estimation Legend Colors
+pub const COLOR_OPTIMAL_P_DIVIDER: RGBColor = RGBColor(40, 40, 40);
+pub const COLOR_OPTIMAL_P_HEADER: RGBColor = RGBColor(0, 100, 200);
+pub const COLOR_OPTIMAL_P_TEXT: RGBColor = RGBColor(80, 80, 80);
+pub const COLOR_OPTIMAL_P_WARNING: RGBColor = RGBColor(200, 100, 0);
+pub const COLOR_OPTIMAL_P_RECOMMENDATION: RGBColor = RGBColor(0, 150, 0);
+pub const COLOR_OPTIMAL_P_SKIP: RGBColor = RGBColor(120, 60, 60);
+
 // Stroke widths for lines
 pub const LINE_WIDTH_PLOT: u32 = 1; // Width for plot lines
 pub const LINE_WIDTH_LEGEND: u32 = 2; // Width for legend lines
@@ -221,26 +235,26 @@ pub const PD_RATIO_MIN_CHANGE_THRESHOLD: f64 = 0.05; // Minimum P:D ratio change
 // Note: Works for all aircraft sizes including 10"+ where D > P (P:D < 1.0)
 pub const PD_RATIO_CONSERVATIVE_MULTIPLIER: f64 = 0.85; // Conservative: reduce P:D by 15% (≈+17.6% D)
 pub const PD_RATIO_MODERATE_MULTIPLIER: f64 = 0.75; // Moderate: reduce P:D by 25% (≈+33.3% D)
+pub const PD_RATIO_AGGRESSIVE_MULTIPLIER: f64 = 0.65; // Aggressive: reduce P:D by 35% (≈+53.8% D)
 
-// Peak range adjustment multipliers for different overshoot levels
-// These create a graduated response based on step response quality
-pub const PEAK_ACCEPTABLE_MULTIPLIER: f64 = 0.95; // Acceptable (1.05-1.10): Small adjustment, +≈5.3% D
-pub const PEAK_MINOR_MULTIPLIER: f64 = 0.92; // Minor overshoot (1.11-1.15): Moderate adjustment, +≈8.7% D
-pub const PEAK_MODERATE_MULTIPLIER: f64 = 0.88; // Moderate overshoot (1.16-1.20): Larger adjustment, +≈13.6% D
-
-// Peak range thresholds for step response quality assessment
-pub const PEAK_OPTIMAL_MIN: f64 = 0.95; // Optimal response: 0.95-1.04 (0-5% overshoot/undershoot)
+// Step response quality zone thresholds
+pub const PEAK_UNDERSHOOT_MAX: f64 = 1.00; // < this = undershoot: Recommendation (conservative) D decrease
+pub const PEAK_OPTIMAL_MIN: f64 = 1.02; // near-optimal ends / sweet spot start; 1.00–1.02 = near optimal (none), 1.02–1.08 = optimal (none)
 #[allow(dead_code)]
-pub const PEAK_OPTIMAL_MAX: f64 = 1.04;
-pub const PEAK_ACCEPTABLE_MIN: f64 = 1.05; // Acceptable: 1.05-1.10 (5-10% overshoot)
-pub const PEAK_ACCEPTABLE_MAX: f64 = 1.10;
-#[allow(dead_code)]
-pub const PEAK_MINOR_MIN: f64 = 1.11; // Minor overshoot: 1.11-1.15 (11-15% overshoot)
-pub const PEAK_MINOR_MAX: f64 = 1.15;
-pub const PEAK_MODERATE_MIN: f64 = 1.16; // Moderate overshoot: 1.16-1.20 (16-20% overshoot)
-#[allow(dead_code)]
-pub const PEAK_MODERATE_MAX: f64 = 1.20;
-pub const PEAK_SIGNIFICANT_MIN: f64 = 1.20; // Significant overshoot: >1.20 (>20% overshoot)
+pub const PEAK_OPTIMAL_MAX: f64 = 1.08; // sweet spot end
+pub const PEAK_ACCEPTABLE_MIN: f64 = 1.08; // 1.08–1.12 = acceptable: Recommendation (conservative) D increase
+pub const PEAK_ACCEPTABLE_MAX: f64 = 1.12; // above this = overshoot territory
+pub const PEAK_SIGNIFICANT_MIN: f64 = 1.20; // > this = significant overshoot: conservative + moderate + aggressive
+
+// Target peak value for undershoot correction: centre of the optimal sweet spot
+pub const PEAK_OPTIMAL_TARGET: f64 = (PEAK_OPTIMAL_MIN + PEAK_OPTIMAL_MAX) / 2.0;
+
+// Peak range adjustment multipliers for different overshoot/undershoot levels
+// These create a graduated response based on step response quality
+pub const PEAK_ACCEPTABLE_MULTIPLIER: f64 = 0.98; // Acceptable (PEAK_ACCEPTABLE_MIN–PEAK_ACCEPTABLE_MAX): +≈2% D
+pub const PEAK_OVERSHOOT_MULTIPLIER: f64 = 0.92; // Overshoot (PEAK_ACCEPTABLE_MAX–PEAK_SIGNIFICANT_MIN): +≈8.7% D
+pub const PEAK_VALUE_MIN_CLAMP: f64 = 0.1; // Minimum peak value clamp to prevent divide-by-zero in undershoot multiplier
+pub const D_STEP_OPTIONAL: u32 = 1; // D decrement for optional near-optimal fine-tune hint
 
 // Sanity check limits for P:D ratio recommendations
 // Note: MIN_REASONABLE_PD_RATIO of 0.3 accommodates large aircraft where D > P
@@ -282,4 +296,100 @@ pub const COLOR_ESO_MEAS: &RGBColor = &LIGHTBLUE; // Measured gyro rate
 pub const COLOR_ESO_HAT: &RGBColor = &ORANGE; // ESO estimated rate (omega_hat)
 pub const COLOR_ESO_FHAT: &RGBColor = &GREEN; // ESO disturbance estimate (f_hat, scaled)
 
-// src/constants.rs
+// High-frequency noise analysis for P headroom estimation
+// D-term energy above this frequency threshold indicates noise constraints
+pub const DTERM_HF_CUTOFF_HZ: f64 = 200.0; // Frequency above which high-frequency noise is measured
+pub const DTERM_HF_ENERGY_THRESHOLD: f64 = 0.15; // 15% of total D-term energy (high noise level)
+pub const DTERM_HF_ENERGY_MODERATE: f64 = 0.10; // 10% of total D-term energy (moderate noise level)
+
+// Response consistency quality control
+// Ensures Td measurements are reliable across multiple step responses
+pub const TD_CONSISTENCY_MIN_THRESHOLD: f64 = 0.70; // 70% of responses within tolerance (was 85%)
+pub const TD_COEFFICIENT_OF_VARIATION_MAX: f64 = 0.40; // 40% CV (std/mean) is acceptable for noisy flight logs (was 20%)
+
+// P headroom estimation multipliers
+// Conservative approach for users who want safe incremental improvements
+pub const P_HEADROOM_CONSERVATIVE_MULTIPLIER: f64 = 1.05; // +5% from current P
+                                                          // Moderate approach for experienced pilots
+pub const P_HEADROOM_MODERATE_MULTIPLIER: f64 = 1.10; // +10% from current P
+                                                      // Aggressive approach for optimization (use with caution)
+#[allow(dead_code)]
+pub const P_HEADROOM_AGGRESSIVE_MULTIPLIER: f64 = 1.15; // +15% from current P (reserved for future use)
+
+// P reduction multipliers (when Td is too fast or noise is too high)
+pub const P_REDUCTION_MODERATE_MULTIPLIER: f64 = 0.95; // -5% from current P
+#[allow(dead_code)]
+pub const P_REDUCTION_AGGRESSIVE_MULTIPLIER: f64 = 0.90; // -10% from current P
+
+// Td statistics computation constants
+pub const MIN_TD_MS: f64 = 0.1; // Minimum valid Td (time to 50%) in milliseconds (domain-appropriate threshold)
+pub const TD_MEAN_EPSILON: f64 = 1e-12; // Threshold for near-zero mean values (avoid division by zero)
+pub const TD_SAMPLES_MIN_FOR_STDDEV: usize = 2; // Minimum samples needed for std dev calculation
+
+// Td deviation thresholds (percentage deviation from target)
+// Deviation thresholds for classifying Td behavior
+// Note: The thresholds are intentionally asymmetric — there is no separate
+// 'moderately faster' threshold. Faster-than-target deviations are treated
+// more strictly because they often indicate potential oscillation or unsafe
+// aggressive tuning. Therefore any significant speed-up beyond
+// TD_DEVIATION_SIGNIFICANTLY_FASTER_THRESHOLD is flagged immediately. Slower
+// deviations are given two thresholds (moderate and significant) to allow
+// finer-grained handling when Td is lagging behind the target.
+pub const TD_DEVIATION_SIGNIFICANTLY_SLOWER_THRESHOLD: f64 = 30.0; // > 30% slower
+pub const TD_DEVIATION_MODERATELY_SLOWER_THRESHOLD: f64 = 15.0; // > 15% slower
+pub const TD_DEVIATION_SIGNIFICANTLY_FASTER_THRESHOLD: f64 = -15.0; // < -15% faster
+
+// Optimal P estimation data collection thresholds
+pub const OPTIMAL_P_MIN_DTERM_SAMPLES: usize = 100; // Minimum D-term samples for noise analysis
+pub const OPTIMAL_P_SECONDS_TO_MS_MULTIPLIER: f64 = 1000.0; // Convert seconds to milliseconds
+
+// Torque-Inertia Profiler constants
+// Used by torque_inertia_profiler.rs to derive aircraft-specific Td targets from
+// throttle-punch events in flight logs, replacing the empirical frame-class table.
+
+/// Maximum throttle command value in firmware units (setpoint[3] range: 0–1000).
+/// Divide raw delta by this to normalise to 0.0–1.0.
+pub const THROTTLE_COMMAND_SCALE: f64 = 1000.0;
+
+/// Minimum time delta (seconds) for angular-acceleration computation in the torque profiler.
+/// Guards against division by near-zero or identical consecutive timestamps.
+pub const TORQUE_PROFILER_MIN_DT_S: f64 = 1e-9;
+
+/// Minimum throttle increase (in 0–1000 units) to qualify as a throttle punch.
+pub const THROTTLE_PUNCH_MIN_DELTA: f64 = 200.0;
+
+/// Time window (ms) within which the throttle increase must occur to count as a punch.
+pub const THROTTLE_PUNCH_WINDOW_MS: f64 = 50.0;
+
+/// Window (ms) after punch onset in which to measure peak angular acceleration.
+pub const THROTTLE_RESPONSE_WINDOW_MS: f64 = 150.0;
+
+/// Minimum number of throttle-punch events required for a reliable Td target.
+/// Analysis is skipped (with a console warning) when this threshold is not met.
+pub const TORQUE_PROFILER_MIN_EVENTS: usize = 5;
+
+/// Minimum normalised command delta (0–1) for a punch event to be valid.
+pub const TORQUE_PROFILER_MIN_CMD_DELTA_NORMALIZED: f64 = 0.10;
+
+/// Time (ms) to skip at the start of the gyro response window after a throttle punch.
+/// Converted to samples at runtime using the actual log sample rate, so it is
+/// correct for all loop rates (1 kHz, 3.2 kHz BMI270, 4 kHz, 8 kHz, etc.).
+/// 5 ms covers typical DSHOT ESC/motor lag; increase for slow 50 Hz PWM ESCs.
+pub const TORQUE_PROFILER_SETTLE_MS: f64 = 5.0;
+
+/// Numerator constant for Td calculation: K = π × 1000 / 2
+/// Td_ms = K / sqrt((P / P_SCALE) × torque_inertia_ratio)
+pub const TORQUE_PROFILER_TD_CALC_K: f64 = 1_570.796_326_794_896_6;
+
+/// Betaflight/EmuFlight P gain scaling factor.
+/// Converts raw firmware P gain (e.g. 45) to an effective physical gain.
+/// This constant may require empirical calibration; starting value: 100.0.
+pub const TORQUE_PROFILER_P_SCALE: f64 = 100.0;
+
+/// Real-world achievability factor for physics-derived Td targets.
+/// Bridges the gap between theoretical torque capacity and actual flight performance
+/// (accounts for ESC lag, motor startup, prop-wash efficiency, etc).
+/// Higher values = more relaxed targets.
+/// Empirically calibrated on a 5" 6S freestyle build (HELIO H7); may need
+/// adjustment for significantly heavier or lighter aircraft classes.
+pub const TORQUE_PROFILER_ACHIEVABILITY_FACTOR: f64 = 2.50;

src/constants.rs

@@ -8,13 +8,57 @@ use crate::types::StepResponseResult;
 
 use crate::constants::{
     APPLY_INDIVIDUAL_RESPONSE_Y_CORRECTION, ENABLE_NORMALIZED_STEADY_STATE_MEAN_CHECK,
-    FRAME_LENGTH_S, INITIAL_GYRO_SMOOTHING_WINDOW, MOVEMENT_THRESHOLD_DEG_S,
+    FRAME_LENGTH_S, HIGH_AUTHORITY_SETPOINT_THRESHOLD_DEG_S, INITIAL_GYRO_SMOOTHING_WINDOW,
+    LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S, MOVEMENT_THRESHOLD_DEG_S,
     NORMALIZED_STEADY_STATE_MAX_VAL, NORMALIZED_STEADY_STATE_MEAN_MAX,
     NORMALIZED_STEADY_STATE_MEAN_MIN, NORMALIZED_STEADY_STATE_MIN_VAL, RESPONSE_LENGTH_S,
     STEADY_STATE_END_S, STEADY_STATE_START_S, SUPERPOSITION_FACTOR, TUKEY_ALPHA,
     Y_CORRECTION_MIN_UNNORMALIZED_MEAN_ABS,
 };
 
+/// Setpoint authority classification derived from the mean of per-window max setpoints.
+/// Describes how aggressively the pilot flew during the step-response windows.
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum SetpointAuthority {
+    Low,      // mean < LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S
+    Moderate, // LOW..HIGH
+    High,     // >= HIGH_AUTHORITY_SETPOINT_THRESHOLD_DEG_S
+}
+
+impl SetpointAuthority {
+    pub fn name(&self) -> &'static str {
+        match self {
+            Self::Low => "LOW",
+            Self::Moderate => "MODERATE",
+            Self::High => "HIGH",
+        }
+    }
+
+    pub fn is_low(&self) -> bool {
+        matches!(self, Self::Low)
+    }
+}
+
+/// Compute SetpointAuthority and mean window-max setpoint from QC-passed window data.
+/// Returns None if the slice is empty.
+pub fn compute_setpoint_authority(
+    valid_window_max_setpoints: &[f32],
+) -> Option<(SetpointAuthority, f32)> {
+    if valid_window_max_setpoints.is_empty() {
+        return None;
+    }
+    let mean =
+        valid_window_max_setpoints.iter().sum::<f32>() / valid_window_max_setpoints.len() as f32;
+    let level = if mean < LOW_AUTHORITY_SETPOINT_THRESHOLD_DEG_S {
+        SetpointAuthority::Low
+    } else if mean < HIGH_AUTHORITY_SETPOINT_THRESHOLD_DEG_S {
+        SetpointAuthority::Moderate
+    } else {
+        SetpointAuthority::High
+    };
+    Some((level, mean))
+}
+
 use crate::data_analysis::fft_utils;
 
 // tukeywin, winstacker_contiguous, wiener_deconvolution_window, cumulative_sum,

src/data_analysis/calc_step_response.rs

@@ -6,7 +6,9 @@ pub mod derivative;
 pub mod fft_utils;
 pub mod filter_delay;
 pub mod filter_response;
+pub mod optimal_p_estimation;
 pub mod spectral_analysis;
+pub mod torque_inertia_profiler;
 pub mod transfer_function_estimation;
 
 // src/data_analysis/mod.rs