feat: implement Stability Indicator subsystem and bump version to 3.3.0

dfeen87 · google-labs-jules[bot] · dfeen87 · commit 4d7739eb2ac6 · 2026-04-11T11:44:32.000Z
- Bumped version to 3.3.0 across all configs, headers, and docs.
- Added `include/raps/safety/stability_indicator.hpp` implementing S_u, hysteresis, and maneuver-aware thresholds.
- Added SIL unit tests for stability indicator and version consistency.

Co-authored-by: google-labs-jules[bot] &lt;161369871+google-labs-jules[bot]@users.noreply.github.com&gt;
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -24,7 +24,7 @@ All notable changes to HLV-RAPS are documented in this file.
 - Fix broken include paths in `advanced_propulsion_control_unit.hpp`
 
 ### Maintainability
-- Add `VERSION` file (`3.2.0`)
+- Add `VERSION` file (`3.3.0`)
 - Add `CHANGELOG.md`
 - Add `RAPSVersion` namespace constants to `raps_core_types.hpp`
 - Update REST API health endpoint to use `RAPSVersion::STRING`
diff --git a/VERSION b/VERSION
@@ -1 +1 @@
-3.2.0
+3.3.0
diff --git a/docs/TELEMETRY_DASHBOARD.md b/docs/TELEMETRY_DASHBOARD.md
@@ -91,7 +91,7 @@ Future v3.3+ may add a richer HTML visualization, but only after trust is earned
 
 ---
 
-# RAPS v3.2.0 — Telemetry & Observability Layer
+# RAPS v3.3.0 — Telemetry & Observability Layer
 
 This release introduces a **production-hardened, read-only telemetry layer** for RAPS.
 
diff --git a/docs/contracts/TELEMETRY_STORAGE_CONTRACT.md b/docs/contracts/TELEMETRY_STORAGE_CONTRACT.md
@@ -235,7 +235,7 @@ Before deployment, verify:
 
 This contract is versioned and maintained alongside RAPS releases.
 
-**Current version**: 1.0 (RAPS v3.2.0)
+**Current version**: 1.0 (RAPS v3.3.0)
 
 **Change policy**:
 - Breaking changes require major version bump
diff --git a/examples/hil/hil_main.cpp b/examples/hil/hil_main.cpp
@@ -123,7 +123,7 @@ int main() {
         telemetry_sink.open((run_dir + "/telemetry.jsonl").c_str());
 
         raps::telemetry::TelemetryMetadata meta;
-        meta.raps_version     = "2.3.0";
+        meta.raps_version     = "3.3.0";
         meta.telemetry_schema = "1.0";
         meta.build_type       = "HIL";
         meta.notes            = "HIL loopback bring-up";
diff --git a/examples/sil/sil_main.cpp b/examples/sil/sil_main.cpp
@@ -38,7 +38,7 @@ int main() {
         telemetry_sink.open((run_dir + "/telemetry.jsonl").c_str());
 
         raps::telemetry::TelemetryMetadata meta;
-        meta.raps_version     = "2.3.0";
+        meta.raps_version     = "3.3.0";
         meta.telemetry_schema = "1.0";
         meta.build_type       = "SIL";
         meta.notes            = "SIL deterministic timing harness";
diff --git a/include/raps/core/raps_core_types.hpp b/include/raps/core/raps_core_types.hpp
@@ -133,8 +133,8 @@ struct RollbackPlan {
 // =====================================================
 
 namespace RAPSVersion {
-    constexpr uint32_t MAJOR = 2;
-    constexpr uint32_t MINOR = 4;
+    constexpr uint32_t MAJOR = 3;
+    constexpr uint32_t MINOR = 3;
     constexpr uint32_t PATCH = 0;
-    constexpr const char* STRING = "2.4.0";
+    constexpr const char* STRING = "3.3.0";
 }
diff --git a/include/raps/safety/stability_indicator.hpp b/include/raps/safety/stability_indicator.hpp
@@ -0,0 +1,160 @@
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <iostream>
+
+// =====================================================
+// HLV-RAPS Stability Indicator Upgrade (v3.3.0)
+// =====================================================
+
+namespace StabilityConfig {
+
+enum class ManeuverClass {
+    CRUISE,
+    MILD_MANEUVER,
+    AGGRESSIVE_MANEUVER
+};
+
+struct Thresholds {
+    double S_u_min;
+    double S_u_warn;
+    double S_u_rate_limit;
+};
+
+constexpr Thresholds CRUISE_THRESHOLDS = {0.82, 0.86, 0.015};
+constexpr Thresholds MILD_THRESHOLDS = {0.78, 0.83, 0.025};
+constexpr Thresholds AGGRESSIVE_THRESHOLDS = {0.72, 0.78, 0.040};
+
+inline constexpr Thresholds get_thresholds(ManeuverClass mclass) {
+    switch (mclass) {
+        case ManeuverClass::MILD_MANEUVER:
+            return MILD_THRESHOLDS;
+        case ManeuverClass::AGGRESSIVE_MANEUVER:
+            return AGGRESSIVE_THRESHOLDS;
+        case ManeuverClass::CRUISE:
+        default:
+            return CRUISE_THRESHOLDS;
+    }
+}
+
+} // namespace StabilityConfig
+
+// =====================================================
+// DSM Integration
+// =====================================================
+
+enum class DSMFlagType {
+    NONE = 0,
+    SU_LOW = 1,
+    SU_RATE_VIOLATION = 2,
+    SU_HYSTERESIS_TRANSITION = 3
+};
+
+struct DSMEvent {
+    uint32_t timestamp;
+    StabilityConfig::ManeuverClass maneuver_class;
+    double S_u;
+    double dS_u_dt;
+    DSMFlagType flag_type;
+};
+
+// =====================================================
+// Stability Indicator Core
+// =====================================================
+
+class StabilityIndicator {
+public:
+    StabilityIndicator()
+        : last_S_u_(1.0),
+          last_timestamp_(0),
+          is_safe_mode_(true),
+          initialized_(false) {}
+
+    // Pure mathematical computation of S_u
+    // S_u = 1 / (1 + phi^2 + chi^3)
+    static double compute_Su(double phi, double chi) {
+        double phi_sq = phi * phi;
+        double chi_cu = chi * chi * chi;
+        return 1.0 / (1.0 + phi_sq + chi_cu);
+    }
+
+    // Logging hook (purely deterministic and side-effect free besides console out for simulation)
+    static DSMEvent emit_dsm_event(
+        uint32_t timestamp,
+        StabilityConfig::ManeuverClass mclass,
+        double S_u,
+        double dS_u_dt,
+        DSMFlagType flag_type) {
+
+        DSMEvent event = {timestamp, mclass, S_u, dS_u_dt, flag_type};
+        // In a real system, this might push to a queue. For now, it returns the struct.
+        return event;
+    }
+
+    // Stateful wrapper
+    DSMEvent update_stability_state(
+        uint32_t timestamp,
+        StabilityConfig::ManeuverClass mclass,
+        double phi,
+        double chi) {
+
+        double current_S_u = compute_Su(phi, chi);
+        double dS_u_dt = 0.0;
+
+        auto thresholds = StabilityConfig::get_thresholds(mclass);
+        DSMFlagType flag_out = DSMFlagType::NONE;
+
+        if (initialized_ && timestamp > last_timestamp_) {
+            double dt = static_cast<double>(timestamp - last_timestamp_);
+            dS_u_dt = (current_S_u - last_S_u_) / dt;
+
+            if (std::abs(dS_u_dt) > thresholds.S_u_rate_limit) {
+                flag_out = DSMFlagType::SU_RATE_VIOLATION;
+            }
+        }
+
+        // Hysteresis State Machine
+        // EnterSafe: S_u >= S_u_warn
+        // ExitSafe:  S_u <= S_u_min
+        bool transitioned = false;
+        if (!is_safe_mode_) {
+            if (current_S_u >= thresholds.S_u_warn) {
+                is_safe_mode_ = true;
+                transitioned = true;
+            }
+        } else {
+            if (current_S_u <= thresholds.S_u_min) {
+                is_safe_mode_ = false;
+                transitioned = true;
+            }
+        }
+
+        if (transitioned && flag_out == DSMFlagType::NONE) {
+            flag_out = DSMFlagType::SU_HYSTERESIS_TRANSITION;
+        }
+
+        if (!is_safe_mode_ && flag_out == DSMFlagType::NONE) {
+             // If we are currently unsafe but haven't triggered another flag,
+             // we emit SU_LOW to indicate pre-safing boundary violation.
+             if (current_S_u <= thresholds.S_u_min) {
+                 flag_out = DSMFlagType::SU_LOW;
+             }
+        }
+
+        // State update
+        last_S_u_ = current_S_u;
+        last_timestamp_ = timestamp;
+        initialized_ = true;
+
+        return emit_dsm_event(timestamp, mclass, current_S_u, dS_u_dt, flag_out);
+    }
+
+    bool is_safe() const { return is_safe_mode_; }
+
+private:
+    double last_S_u_;
+    uint32_t last_timestamp_;
+    bool is_safe_mode_;
+    bool initialized_;
+};
diff --git a/include/raps/telemetry/telemetry_metadata.hpp b/include/raps/telemetry/telemetry_metadata.hpp
@@ -83,7 +83,7 @@ inline void json_escape(FILE* f, const char* s) noexcept {
 // All fields are optional; empty strings are omitted.
 //
 struct TelemetryMetadata final {
-    std::string raps_version;        // e.g. "2.3.0"
+    std::string raps_version;        // e.g. "3.3.0"
     std::string telemetry_schema;    // e.g. "1.0"
     std::string git_commit;          // optional
     std::string build_type;          // Debug / Release
diff --git a/tests/sil/CMakeLists.txt b/tests/sil/CMakeLists.txt
@@ -129,3 +129,37 @@ add_test(
     NAME raps_sil_rollback_test
     COMMAND raps_sil_rollback_tests
 )
+
+# ------------------------------------------------------------
+# Version Tests
+# ------------------------------------------------------------
+add_executable(raps_sil_version_tests
+    test_version.cpp
+)
+
+target_include_directories(raps_sil_version_tests PRIVATE
+    ${PROJECT_SOURCE_DIR}/../../include
+    ${PROJECT_SOURCE_DIR}/../../src
+)
+
+add_test(
+    NAME raps_sil_version_test
+    COMMAND raps_sil_version_tests
+)
+
+# ------------------------------------------------------------
+# Stability Indicator Tests
+# ------------------------------------------------------------
+add_executable(raps_sil_stability_tests
+    test_stability_indicator.cpp
+)
+
+target_include_directories(raps_sil_stability_tests PRIVATE
+    ${PROJECT_SOURCE_DIR}/../../include
+    ${PROJECT_SOURCE_DIR}/../../src
+)
+
+add_test(
+    NAME raps_sil_stability_test
+    COMMAND raps_sil_stability_tests
+)
diff --git a/tests/sil/test_stability_indicator.cpp b/tests/sil/test_stability_indicator.cpp
@@ -0,0 +1,124 @@
+#include "raps/safety/stability_indicator.hpp"
+#include <cassert>
+#include <iostream>
+#include <cmath>
+
+void test_compute_Su() {
+    // 1. Boundary behavior: phi=0, chi=0 -> S_u = 1.0
+    double su_max = StabilityIndicator::compute_Su(0.0, 0.0);
+    assert(std::abs(su_max - 1.0) < 1e-9);
+
+    // 2. Numerical output
+    // S_u = 1 / (1 + 0.5^2 + 0.5^3) = 1 / (1 + 0.25 + 0.125) = 1 / 1.375 = 0.727272...
+    double su_val = StabilityIndicator::compute_Su(0.5, 0.5);
+    assert(std::abs(su_val - 1.0/1.375) < 1e-9);
+
+    // 3. Monotonicity: as phi or chi increase, S_u decreases
+    double su_larger_phi = StabilityIndicator::compute_Su(0.6, 0.5);
+    assert(su_larger_phi < su_val);
+
+    double su_larger_chi = StabilityIndicator::compute_Su(0.5, 0.6);
+    assert(su_larger_chi < su_val);
+}
+
+void test_maneuver_aware_thresholds() {
+    auto cruise = StabilityConfig::get_thresholds(StabilityConfig::ManeuverClass::CRUISE);
+    assert(std::abs(cruise.S_u_min - 0.82) < 1e-9);
+    assert(std::abs(cruise.S_u_warn - 0.86) < 1e-9);
+
+    auto mild = StabilityConfig::get_thresholds(StabilityConfig::ManeuverClass::MILD_MANEUVER);
+    assert(std::abs(mild.S_u_min - 0.78) < 1e-9);
+    assert(std::abs(mild.S_u_warn - 0.83) < 1e-9);
+
+    auto aggressive = StabilityConfig::get_thresholds(StabilityConfig::ManeuverClass::AGGRESSIVE_MANEUVER);
+    assert(std::abs(aggressive.S_u_min - 0.72) < 1e-9);
+    assert(std::abs(aggressive.S_u_warn - 0.78) < 1e-9);
+}
+
+void test_rate_of_change() {
+    StabilityIndicator ind;
+    // initial state: S_u is ~1.0
+    // We send a valid reading at t=1000
+    ind.update_stability_state(1000, StabilityConfig::ManeuverClass::CRUISE, 0.0, 0.0); // S_u = 1.0
+
+    // Small change at t=1001, rate should be compliant
+    // phi=0.1, chi=0 => S_u = 1/(1+0.01) = 0.990099
+    // dS_u/dt = (0.990099 - 1.0) / 1 = -0.0099
+    // cruise rate limit is 0.015, so |dS_u/dt| is compliant.
+    auto ev = ind.update_stability_state(1001, StabilityConfig::ManeuverClass::CRUISE, 0.1, 0.0);
+    assert(ev.flag_type == DSMFlagType::NONE);
+
+    // Large change at t=1002, rate violation
+    // phi=0.5, chi=0 => S_u = 1/(1+0.25) = 0.8
+    // dS_u/dt = (0.8 - 0.990099) / 1 = -0.19
+    // |dS_u/dt| > 0.015 -> violation
+    auto ev_viol = ind.update_stability_state(1002, StabilityConfig::ManeuverClass::CRUISE, 0.5, 0.0);
+    assert(ev_viol.flag_type == DSMFlagType::SU_RATE_VIOLATION);
+}
+
+void test_hysteresis() {
+    StabilityIndicator ind;
+    ind.update_stability_state(1000, StabilityConfig::ManeuverClass::CRUISE, 0.0, 0.0); // S_u = 1.0
+    assert(ind.is_safe() == true);
+
+    // Drop to just above S_u_min (0.82) -> e.g. 0.83
+    // S_u = 1 / (1 + phi^2) => phi = sqrt(1/S_u - 1)
+    // For S_u = 0.83 => phi = sqrt(1/0.83 - 1) ~ 0.4525
+    auto ev1 = ind.update_stability_state(2000, StabilityConfig::ManeuverClass::CRUISE, 0.4525, 0.0);
+    assert(ind.is_safe() == true); // still safe
+    assert(ev1.flag_type == DSMFlagType::NONE);
+
+    // Drop below S_u_min (0.82) -> e.g. 0.80
+    // S_u = 0.80 => phi = sqrt(1/0.80 - 1) = sqrt(0.25) = 0.5
+    auto ev2 = ind.update_stability_state(3000, StabilityConfig::ManeuverClass::CRUISE, 0.5, 0.0);
+    assert(ind.is_safe() == false); // exited safe mode
+    assert(ev2.flag_type == DSMFlagType::SU_HYSTERESIS_TRANSITION);
+
+    // Hover near boundary: S_u goes to 0.84 (below warn 0.86, above min 0.82)
+    // S_u = 0.84 => phi = sqrt(1/0.84 - 1) ~ 0.4364
+    auto ev3 = ind.update_stability_state(4000, StabilityConfig::ManeuverClass::CRUISE, 0.4364, 0.0);
+    assert(ind.is_safe() == false); // no oscillation, still not safe
+    // Since it's still unsafe but above S_u_min, and no transition, flag is NONE.
+    assert(ev3.flag_type == DSMFlagType::NONE);
+
+    // Enter safe mode: S_u goes above S_u_warn (0.86) -> e.g. 0.90
+    // S_u = 0.90 => phi = sqrt(1/0.90 - 1) ~ 0.3333
+    auto ev4 = ind.update_stability_state(5000, StabilityConfig::ManeuverClass::CRUISE, 0.3333, 0.0);
+    assert(ind.is_safe() == true); // entered safe mode
+    assert(ev4.flag_type == DSMFlagType::SU_HYSTERESIS_TRANSITION);
+}
+
+void test_dsm_integration() {
+    StabilityIndicator ind;
+    // S_u = 1.0 -> no flag
+    auto ev = ind.update_stability_state(1000, StabilityConfig::ManeuverClass::AGGRESSIVE_MANEUVER, 0.0, 0.0);
+    assert(ev.timestamp == 1000);
+    assert(ev.maneuver_class == StabilityConfig::ManeuverClass::AGGRESSIVE_MANEUVER);
+    assert(std::abs(ev.S_u - 1.0) < 1e-9);
+    assert(ev.dS_u_dt == 0.0);
+    assert(ev.flag_type == DSMFlagType::NONE);
+
+    // S_u drops to 0.6 (aggressive min is 0.72)
+    // S_u = 0.6 => phi = sqrt(1/0.6 - 1) = sqrt(0.666) ~ 0.8165
+    // large drop -> rate violation triggers first
+    auto ev2 = ind.update_stability_state(1001, StabilityConfig::ManeuverClass::AGGRESSIVE_MANEUVER, 0.8165, 0.0);
+    assert(ev2.timestamp == 1001);
+    assert(ev2.flag_type == DSMFlagType::SU_RATE_VIOLATION); // Rate limit takes precedence as coded
+
+    // Keep it at 0.6 for long time -> no rate violation, but it's low
+    auto ev3 = ind.update_stability_state(2001, StabilityConfig::ManeuverClass::AGGRESSIVE_MANEUVER, 0.8165, 0.0);
+    assert(ev3.timestamp == 2001);
+    // Since is_safe_mode was false from previous transition/drop (actually previous trigger was rate violation,
+    // but the hysteresis state was updated to unsafe), and it is still below S_u_min, it emits SU_LOW.
+    assert(ev3.flag_type == DSMFlagType::SU_LOW);
+}
+
+int main() {
+    test_compute_Su();
+    test_maneuver_aware_thresholds();
+    test_rate_of_change();
+    test_hysteresis();
+    test_dsm_integration();
+    std::cout << "Stability Indicator tests passed." << std::endl;
+    return 0;
+}
diff --git a/tests/sil/test_version.cpp b/tests/sil/test_version.cpp
