Merge pull request #23 from dfeen87/feature/wnn-dsm-integration-2662924972226960742

Integrate WNN telemetry into DSM and implement immediate safety rollback

Author: dfeen87

include/core/raps_definitions.hpp

@@ -106,6 +106,11 @@ struct ITLEntry {
         AileeStatus status;
     };
 
+    struct WnnAlertPayload {
+        double curvature_proxy;
+        double oscillatory_prefactor;
+    };
+
     // --- Union Payload Container ---
 
     union PayloadData {
@@ -123,6 +128,7 @@ struct ITLEntry {
         AileeSafetyStatusPayload ailee_safety_status;
         AileeGraceResultPayload ailee_grace_result;
         AileeConsensusResultPayload ailee_consensus_result;
+        WnnAlertPayload wnn_alert;
     };
 
     // --- Entry Type ---
@@ -144,7 +150,8 @@ struct ITLEntry {
         SUPERVISOR_EXCEPTION,
         AILEE_SAFETY_STATUS,
         AILEE_GRACE_RESULT,
-        AILEE_CONSENSUS_RESULT
+        AILEE_CONSENSUS_RESULT,
+        WNN_ALERT
     };
 
     // --- ITL Entry Header ---

include/itl/itl_manager.hpp

@@ -3,8 +3,8 @@
 #include <cstddef>
 #include <cstdint>
 
-#include "RAPSDefinitions.hpp"
-#include "PlatformHAL.hpp"
+#include "core/raps_definitions.hpp"
+#include "platform/platform_hal.hpp"
 
 // Immutable Telemetry Ledger (ITL) Manager
 // Owns queueing, durability, flash IO, and Merkle batching lifecycle.
@@ -35,4 +35,23 @@ class ITLManager {
 
     // Background processing (low-priority task)
     void flush_pending();
+
+    // Log WNN rollback event
+    void log_wnn_rollback_event(double curvature, double prefactor);
 };
+
+inline void ITLManager::log_wnn_rollback_event(double curvature, double prefactor) {
+    ITLEntry wnn_entry{};
+    wnn_entry.type = ITLEntry::Type::WNN_ALERT;
+    wnn_entry.timestamp_ms = PlatformHAL::now_ms();
+    wnn_entry.payload.wnn_alert.curvature_proxy = curvature;
+    wnn_entry.payload.wnn_alert.oscillatory_prefactor = prefactor;
+    commit(wnn_entry);
+
+    ITLEntry rollback_entry{};
+    rollback_entry.type = ITLEntry::Type::ROLLBACK_COMMIT;
+    rollback_entry.timestamp_ms = PlatformHAL::now_ms();
+    // Payload for rollback commit (CommandExecutionPayload)
+    // we just commit the entry to mark the rollback execution triggered by WNN
+    commit(rollback_entry);
+}

include/raps/safety/deterministic_safety_monitor.hpp

@@ -4,6 +4,9 @@
 #include <iostream>
 #include <limits>
 
+#include "raps/rollback_execution.hpp"
+#include "itl/itl_manager.hpp"
+
 // =====================================================
 // Deterministic Safety Monitor (DSM)
 // =====================================================
@@ -25,8 +28,17 @@ constexpr double MAX_TCC_COUPLING_J = 1.0e+04;
 // Failsafe parameters
 constexpr double MIN_RESONANCE_AMPLITUDE_CUTOFF = 0.10;
 
+// WNN Constraints
+constexpr double WNN_MAX_CURVATURE_PROXY = 5.0e-11;
+constexpr double WNN_MIN_OSCILLATORY_PREFACTOR = 0.85;
+
 } // namespace DSM_Config
 
+struct WnnTelemetry {
+    double curvature_proxy;
+    double oscillatory_prefactor;
+};
+
 // =====================================================
 // DSM Sensor Inputs (Independent Channels)
 // =====================================================
@@ -55,6 +67,14 @@ class DeterministicSafetyMonitor {
 
     int evaluateSafety(const DsmSensorInputs& inputs);
 
+    bool pollWnnAndEnforce(
+        const WnnTelemetry& wnn_telem,
+        ITLManager& itl_manager,
+        const RollbackPlan* rollback_store,
+        uint32_t rollback_count,
+        PhysicsState& active_state_pointer
+    );
+
 private:
     double last_estimated_Rmax_;
     bool safing_sequence_active_;
@@ -164,3 +184,26 @@ DeterministicSafetyMonitor::evaluateSafety(
 
     return ACTION_NONE;
 }
+
+inline bool
+DeterministicSafetyMonitor::pollWnnAndEnforce(
+    const WnnTelemetry& wnn_telem,
+    ITLManager& itl_manager,
+    const RollbackPlan* rollback_store,
+    uint32_t rollback_count,
+    PhysicsState& active_state_pointer
+) {
+    if (wnn_telem.curvature_proxy > DSM_Config::WNN_MAX_CURVATURE_PROXY ||
+        wnn_telem.oscillatory_prefactor < DSM_Config::WNN_MIN_OSCILLATORY_PREFACTOR) {
+
+        // Breach detected! Log to ITL and execute immediate rollback
+        itl_manager.log_wnn_rollback_event(wnn_telem.curvature_proxy, wnn_telem.oscillatory_prefactor);
+
+        return trigger_wnn_immediate_rollback(
+            rollback_store,
+            rollback_count,
+            active_state_pointer
+        );
+    }
+    return false; // No breach
+}

include/raps/telemetry/telemetry_ring_buffer.hpp

@@ -43,6 +43,18 @@ class TelemetryRingBuffer final {
     return true;
   }
 
+  // Peek the latest written item without popping.
+  bool try_peek_latest(T& out) const noexcept {
+    const uint64_t w = _write_idx.load(std::memory_order_acquire);
+    const uint64_t r = _read_idx.load(std::memory_order_relaxed);
+
+    if (r == w) return false;
+
+    // The most recent valid write is at w - 1
+    out = _data[(w - 1) & (CapacityPow2 - 1)];
+    return true;
+  }
+
   // Pop one item if available.
   bool try_pop(T& out) noexcept {
     const uint64_t r = _read_idx.load(std::memory_order_relaxed);

src/itl/itl_payload_sizing.hpp

@@ -49,6 +49,9 @@ inline size_t itl_effective_payload_len(
         case ITLEntry::Type::AILEE_CONSENSUS_RESULT:
             return sizeof(ITLEntry::AileeConsensusResultPayload);
 
+        case ITLEntry::Type::WNN_ALERT:
+            return sizeof(ITLEntry::WnnAlertPayload);
+
         default:
             return 0;
     }

src/itl/itl_state_snapshot.hpp

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "itl/itl_manager.hpp"
+
 inline void commit_state_snapshot(
     ITLManager& itl_manager,
     const PhysicsState& current_state) {

src/raps/rollback_execution.hpp

@@ -6,6 +6,7 @@
 
 #include "raps/core/raps_core_types.hpp"
 #include "platform/platform_hal.hpp"
+#include "safety/rollback_store.hpp"
 
 // Executes a rollback plan via the actuator interface.
 // Returns true if execution succeeded.
@@ -45,3 +46,30 @@ inline bool execute_rollback_plan(
         RAPSConfig::WATCHDOG_MS / 4
     );
 }
+
+// Triggers an immediate rollback due to WNN constraints breach
+inline bool trigger_wnn_immediate_rollback(
+    const RollbackPlan* rollback_store,
+    uint32_t rollback_count,
+    PhysicsState& active_state_pointer
+) {
+    if (rollback_count == 0) {
+        return false;
+    }
+
+    const RollbackPlan& latest_plan = rollback_store[rollback_count - 1];
+
+    std::string tx_id;
+    if (!execute_rollback_plan(latest_plan, tx_id)) {
+        return false;
+    }
+
+    // Peek the latest snapshot without destructive reading
+    PhysicsState last_valid_snapshot;
+    if (StateSnapshotBuffer.try_peek_latest(last_valid_snapshot)) {
+        // Point the active state pointer to the last valid state
+        active_state_pointer = last_valid_snapshot;
+    }
+
+    return true;
+}

src/safety/rollback_store.hpp

@@ -3,6 +3,16 @@
 #include <optional>
 #include <cstring>
 
+#include "raps/telemetry/telemetry_ring_buffer.hpp"
+#include "itl/itl_state_snapshot.hpp"
+
+// Continuous, statically allocated snapshot buffer
+inline raps::telemetry::TelemetryRingBuffer<PhysicsState, 64> StateSnapshotBuffer;
+
+inline void store_state_snapshot_tick(const PhysicsState& state) {
+    StateSnapshotBuffer.try_push(state);
+}
+
 inline void store_rollback_plan(
     RollbackPlan* rollback_store,
     uint32_t& rollback_count,