VoxlPublisher.cpp

Go to the documentation of this file.

/**
 * @file VoxlPublisher.cpp
 * @brief Publisher implementation for VOXL OpenVINS
 * @author Joao Leonardo Silva Cotta (@zauberflote1)
 * @date 2025
 * @version 1.0
 *
 * This file implements the publisher for the VOXL OpenVINS server.
 */
 
#include "VoxlHK.h"
#include <atomic>
#include <iterator>
using namespace voxl;
 
#define STR_MATCH(s, lit) (strncmp((s), (lit), strlen(lit)) == 0)
 
// ============================================================================
// PUBLISHER CLASS IMPLEMENTATION
// ============================================================================
 
/**
 * @brief Constructor for the Publisher class
 *
 * Initializes the publisher by zeroing out the VIO data packet structure.
 * This ensures that all fields start with known values.
 */
Publisher::Publisher()
{
    // Initialize the publisher
    memset(&vio_packet, 0, sizeof(vio_data_t));
}
 
/**
 * @brief Destructor for the Publisher class
 *
 * Performs cleanup by calling the stop method to ensure proper
 * resource deallocation.
 */
Publisher::~Publisher()
{
    // Stop the publisher
    stop();
}
 
/**
 * @brief Start the publisher
 *
 * Initializes the publisher and prepares it for data transmission.
 * Sets the first_packet flag to true to handle the initial angular
 * velocity calculation.
 */
void Publisher::start()
{
    // Initialize publisher if needed
    first_packet = true;
 
    // Set VIO state to initializing
    vio_state = VIO_STATE_INITIALIZING;
 
    // Start the health check system
    HealthCheck::getInstance().start();
}
 
/**
 * @brief Stop the publisher
 *
 * Stops the publisher and cleans up resources. Currently a placeholder
 * for future cleanup operations.
 */
void Publisher::stop()
{
    // Stop the health check system
    HealthCheck::getInstance().stop();
 
    // Clean up resources if needed
}
 
/**
 * @brief Control-pipe callback for VIO commands
 *
 * This function is invoked every time a message
 * is received on the VIO **control pipe**.
 *
 * @param ch      Channel id supplied by the pipe framework.
 * @param string  Pointer to the received buffer.
 * @param bytes   Number of valid bytes in @p string.
 * @param context User context pointer supplied during registration.
 *
 * @note Matching is performed with the `STR_MATCH()` macro, which compares the
 *       prefix of @p string against the command literal.
 */
void Publisher::ov_vio_control_pipe_cb(__attribute__((unused)) int ch,
                                       char *string,
                                       int bytes,
                                       __attribute__((unused)) void *context)
{
    if (STR_MATCH(string, RESET_VIO_HARD))
    {
        if (reset_requested.load() || is_resetting.load())
        {
            fprintf(stderr, "[WARNING] Already resetting VIO, ignoring hard reset command.\n");
            return;
        }
 
        reset_requested.store(true);
        fprintf(stderr, "[ERROR] Client requested hard reset\n");
        return;
    }
    else if (STR_MATCH(string, RESET_VIO_SOFT))
    {
        if (reset_requested.load() || soft_reset_requested.load() || is_resetting.load())
        {
            fprintf(stderr, "[WARNING] Already resetting VIO, ignoring soft reset command.\n");
            return;
        }
        soft_reset_requested.store(true);
        fprintf(stderr, "[INFO] Client requested soft reset (front-end preserving)\n");
        return;
    }
    else
    {
        // Unrecognized command, log an error or handle appropriately
        printf("Unrecognized control command: %.*s\n", bytes, string);
    }
 
    return;
}
 
/**
 * @brief Publish VIO data to external systems
 *
 * This method formats and publishes the current VIO state and tracking
 * information to external systems through the configured pipe interfaces.
 *
 * The function performs the following operations:
 * - Formats VIO data packet with current state information
 * - Performs coordinate frame transformations (OpenVINS to FRD)
 * - Calculates angular velocity from quaternion differences
 * - Extracts and formats covariance matrices
 * - Handles camera-to-IMU extrinsic parameters
 * - Publishes both simple and extended VIO data packets
 *
 * The coordinate frame transformation involves:
 * - Converting from OpenVINS coordinate frame to Front-Right-Down (FRD)
 * - Handling initialization state with NED rotation zeroing
 * - Applying proper quaternion and rotation matrix transformations
 *
 * @param state Current VIO state containing pose, velocity, and covariance
 */
void Publisher::publish(std::shared_ptr<ov_msckf::State> state,
                        const std::map<double, std::vector<std::shared_ptr<ov_core::Feature>>> &used_features_map)
{
    // Handle FAILED state - publish failure packet and ensure reset is triggered
    static bool wait_for_steady_init = true;
    vio_packet.quality = -1;
 
    if (vio_state.load() == VIO_STATE_FAILED && !is_resetting.load())
    {
        wait_for_steady_init = true;
 
        // CRITICAL: Ensure reset is requested when in FAILED state
        if (!reset_requested.load(std::memory_order_acquire))
        {
            reset_requested.store(true, std::memory_order_release);
            fprintf(stderr, "[PUBLISH] VIO in FAILED state - REQUESTING RESET\n");
        }
 
        // Publish FAILED packet so external systems know status
        memset(&vio_packet, 0, sizeof(vio_data_t));
        vio_packet.magic_number = VIO_MAGIC_NUMBER;
        vio_packet.timestamp_ns = state->_timestamp * 1e9;
        vio_packet.quality = -1;
        vio_packet.state = VIO_STATE_FAILED;
        vio_packet.error_code = vio_error_codes.load();
 
        pipe_server_write(SIMPLE_CH, (char *)&vio_packet, sizeof(vio_data_t));
 
        if (en_debug)
        {
            static int64_t last_failed_msg = 0;
            int64_t now = _apps_time_monotonic_ns();
            if (now - last_failed_msg > 1000000000) // Print once per second
            {
                printf("[PUBLISH] Published FAILED packet, reset_requested=%d\n",
                       reset_requested.load());
                last_failed_msg = now;
            }
        }
        return;
    }
 
    vio_packet.magic_number = VIO_MAGIC_NUMBER;
    vio_packet.timestamp_ns = state->_timestamp * 1e9;
 
    // NOW LET'S DEAL WITH THE ACTUAL STATE
    // RECALL: WE WANT TO EXPRESS THE GLOBAL FRAME IN THE IMU FRAME NOT THE IMU FRAME IN THE GLOBAL FRAME
    // QUICK CLARIFICATION: IF WE WANTED PURELY IMU FRAME ESTIMATES, THEN P = 0, HENCE, WE REALLY WANT THE ABOVE-MENTIONED!
 
    // LET'S GRAB THE QUATERNIONS FROM THE STATE: IN LATEX: {I}q_{G}
    // USING FEJ -- LESS NOISY BUT "DELAYED" --> NOT A PROBLEM DUE TO IMU RATE
    Eigen::Matrix<double, 4, 1> q_I_G = state->_imu->quat_fej();
 
    // NOW DEAL WITH VELOCITY AND POSITION FROM THE STATE: IN LATEX: {G}p_{I} AND {G}v_{I}
    // GLOBAL VELOCITY IN IMU FRAME FOLLOWS: v_I = {I}q_{G} \otimes v_G \otimes {G}q_{I}
    Eigen::Matrix3d R_I_G = ov_core::quat_2_Rot(q_I_G);
    auto RPY = ov_core::rot2rpy(R_I_G);
    // AT THIS POINT, BETTER TO ROTATE IT USING THE CORRECTION MATRIX...
    // EXECUTE THE FORBIDDEN TECHNIQUE:
    // CHECK IF THE IMU IS MOUNTED IN THE CORRECT WAY, I.E.,
    // GRAVITY IS POINTING DOWN Z
    // IF NOT, EXECUTE THE FORBIDDEN TECHNIQUE:
    float grav_vec[3];
    if (frame_transform.is_initialized)
    {
        if (frame_transform.gravity_axis == FrameTransform::Axis::Z && frame_transform.gravity_direction == FrameTransform::Direction::POSITIVE)
        {
            // FORBIDDEN TECHNIQUE (STINGER CASE)
            RPY(0) = -RPY(0);
            RPY(1) = M_PI - RPY(1);
            RPY(2) = -M_PI + RPY(2);
            R_I_G = ov_core::rot_x(RPY(0)) * ov_core::rot_y(RPY(1)) * ov_core::rot_z(RPY(2));
            // GRAVITY VECTOR
            grav_vec[0] = 0;
            grav_vec[1] = 0;
            grav_vec[2] = static_cast<float>(-9.81); // CHECK THIS VALUE OR CALCULATE IT BY MEASURING THE GRAVITY VECTOR
        }
        else
        {
            // CLASSIC CASE: STARLING2, STARLING MAX, D8V4, D8V5
            RPY(0) = -RPY(0);
            RPY(1) = -M_PI + RPY(1);
            RPY(2) = M_PI - RPY(2);
            R_I_G = ov_core::rot_x(RPY(0)) * ov_core::rot_y(RPY(1)) * ov_core::rot_z(RPY(2));
            // GRAVITY VECTOR
            grav_vec[0] = 0;
            grav_vec[1] = 0;
            grav_vec[2] = static_cast<float>(9.81); // CHECK THIS VALUE OR CALCULATE IT BY MEASURING THE GRAVITY VECTOR
        }
    }
    else
    {
        // Handle the case where frame_transform is not initialized
        printf("Frame transform is not initialized. Not publishing packet.\n");
        return;
    }
 
    memcpy(vio_packet.gravity_vector, grav_vec, sizeof(float) * 3);
 
    // NOW CONVERT IT TO FRD FRAME
    auto ov2frd = R_OV_FRD(); // TODO: PASS AN ARG FOR HINTING THE RIGHT BOARD ORIENTATION ETC
    // GLOBAL VELOCITY IN IMU AXIS:
    Eigen::Matrix<double, 3, 1> v_I_G = ov2frd * state->_imu->vel();
    // GLOBAL POSITION IN IMU AXIS:
    Eigen::Matrix<double, 3, 1> p_I_G = ov2frd * (state->_imu->pos());
    // Re-origin the published trajectory at each init. Only YAW and position are
    // unobservable, so only those are zeroed; gravity-anchored roll/pitch are
    // preserved. ned_rot_zero is re-captured on the rising edge of
    // vio_manager->initialized() so it stays valid across in-flight resets
    // (doHardReset rebuilds vio_manager but keeps this Publisher alive; soft
    // reset re-inits in place). Incorporates the origin-heading fix from
    // origin/tilt_init (a6f8aa6).
    bool now_init = vio_manager && vio_manager->initialized();
    if (now_init)
    {
        if (!prev_initialized) // fresh init OR first frame after any reset
        {
            double yaw = ov_core::rot2rpy(R_I_G)(2); // body heading (psi) in the world frame at init
 
            // ned_rot_zero is the world_uncorrected->world_corrected rotation
            // (Wu->Wc): rot_z(-psi) cancels the init heading. In ZYX,
            // rot_z(-psi) * R = rot_y(pitch) * rot_x(roll) has EXACTLY zero yaw at
            // any tilt, so the published heading is 0 at init for any attitude.
            ned_rot_zero = ov_core::rot_z(-yaw);
        }
 
        // Apply Wu->Wc as ONE consistent left-multiply to orientation AND the world
        // vectors (velocity, position) -- a world-frame gauge (yaw) rotation:
        //   orientation: R_{Wu->Wc} * R_{B->Wu} = R_{B->Wc}
        //   vectors:     R_{Wu->Wc} * v_{Wu}     = v_{Wc}
        // Right-multiplying orientation (R_{B->Wu} * R_{Wc->Wu}) does NOT chain
        // (B != Wu): it re-references the BODY, desyncing attitude from velocity
        // -> lateral drift. That was the pre-fix bug (plus it zeroed full R, i.e.
        // roll/pitch too -- catastrophic at a tilted init).
        R_I_G = ned_rot_zero * R_I_G;
        v_I_G = ned_rot_zero * v_I_G;
        p_I_G = ned_rot_zero * p_I_G;
 
        // Set VIO state to OK when system is initialized
        if (vio_state.load() == VIO_STATE_INITIALIZING)
        {
            vio_state = VIO_STATE_OK;
        }
    }
    prev_initialized = now_init;
    // FILL IN THE VIO PACKET - Fix casting issues
    for (int i = 0; i < 3; i++)
    {
        vio_packet.T_imu_wrt_vio[i] = static_cast<float>(p_I_G(i));
        vio_packet.vel_imu_wrt_vio[i] = static_cast<float>(v_I_G(i));
    }
    alt_z.store(static_cast<float>(p_I_G(2)), std::memory_order_release);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            vio_packet.R_imu_to_vio[i][j] = static_cast<float>(R_I_G(i, j));
        }
    }
    q_I_G = ov_core::rot_2_quat(R_I_G);
 
    static int64_t prev_timestamp_ns = 0;
 
    // NOW LET'S HANDLE THE ANGULAR VELOCITY
    if (first_packet)
    {
 
        for (int i = 0; i < 3; i++)
        {
            vio_packet.imu_angular_vel[i] = 0.0f;
        }
        past_q_I_G = q_I_G;
        prev_timestamp_ns = vio_packet.timestamp_ns;
    }
    else
    {
        double dt = (vio_packet.timestamp_ns - prev_timestamp_ns); // in nanoseconds
 
        Eigen::Matrix<double, 3, 1> ang_vel_imu = dirtyOmega(past_q_I_G, q_I_G, dt);
 
        for (int i = 0; i < 3; i++)
        {
            vio_packet.imu_angular_vel[i] = static_cast<float>(ang_vel_imu(i));
        }
 
        past_q_I_G = q_I_G;
        prev_timestamp_ns = vio_packet.timestamp_ns;
    }
 
    // NOW HANDLE THE COVARIANCE, HAS TO BE DONE THIS WAY FOR MAVLINK
    std::vector<std::shared_ptr<ov_type::Type>> statevars;
    statevars.push_back(state->_imu->p());
    statevars.push_back(state->_imu->q());
    statevars.push_back(state->_imu->v());
    Eigen::Matrix<double, 9, 9> covariance_posori =
        ov_msckf::StateHelper::get_marginal_covariance(state,
                                                       statevars);
 
    // Fill covariances (upper triangular format)
    vio_packet.pose_covariance[0] = static_cast<float>(covariance_posori(0, 0));
    vio_packet.pose_covariance[6] = static_cast<float>(covariance_posori(1, 1));
    vio_packet.pose_covariance[11] = static_cast<float>(covariance_posori(2, 2));
    vio_packet.pose_covariance[15] = static_cast<float>(covariance_posori(3, 3));
    vio_packet.pose_covariance[18] = static_cast<float>(covariance_posori(4, 4));
    vio_packet.pose_covariance[20] = static_cast<float>(covariance_posori(5, 5));
    vio_packet.velocity_covariance[0] = static_cast<float>(covariance_posori(6, 6));
    vio_packet.velocity_covariance[6] = static_cast<float>(covariance_posori(7, 7));
    vio_packet.velocity_covariance[11] = static_cast<float>(covariance_posori(8, 8));
 
    // NOW LET'S HANDLE THE EXTRINSICS CAMERA TO IMU
    Eigen::Matrix3d cam_out = ov_core::quat_2_Rot(state->_calib_IMUtoCAM[0]->quat()).transpose();
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            vio_packet.R_cam_to_imu[i][j] = static_cast<float>(cam_out(i, j));
        }
    }
 
    Eigen::Vector3d t_cam_wrt_imu = -(ov_core::quat_2_Rot(state->_calib_IMUtoCAM[0]->quat()).transpose() * state->_calib_IMUtoCAM[0]->pos());
    for (int i = 0; i < 3; i++)
    {
        vio_packet.T_cam_wrt_imu[i] = static_cast<float>(t_cam_wrt_imu(i));
    }
 
    // ERROR CODE - Update atomic variable and copy to packet
    // Check for covariance issues (negative diagonal elements)
    if (covariance_posori(3, 3) < 0.0 || covariance_posori(4, 4) < 0.0 || covariance_posori(5, 5) < 0.0)
    {
        fprintf(stderr, "ERROR: covariance diagonal went negative\n");
        vio_error_codes |= ERROR_CODE_COVARIANCE;
    }
 
    // Check for timestamp issues (packets from the past)
    static int64_t last_sent_timestamp_ns = 0;
    if (vio_packet.timestamp_ns < last_sent_timestamp_ns)
    {
        if (first_packet)
        {
            // During first packet, just update the timestamp without error
            first_packet = false;
        }
        else
        {
            // Only flag error if timestamp is significantly in the past (more than 1ms)
            int64_t time_diff = last_sent_timestamp_ns - vio_packet.timestamp_ns;
            if (time_diff > 1000000)
            { // 1ms in nanoseconds
                fprintf(stderr, "WARNING: skipping pose data from the past %ld %ld (diff: %ld ns)\n",
                        vio_packet.timestamp_ns, last_sent_timestamp_ns, time_diff);
                vio_error_codes |= ERROR_CODE_BAD_TIMESTAMP;
            }
        }
    }
    last_sent_timestamp_ns = vio_packet.timestamp_ns;
 
    // Check for velocity uncertainty issues
    double V_uncertainty = 0.0;
    V_uncertainty += covariance_posori(6, 6) * covariance_posori(6, 6);
    V_uncertainty += covariance_posori(7, 7) * covariance_posori(7, 7);
    V_uncertainty += covariance_posori(8, 8) * covariance_posori(8, 8);
    V_uncertainty = sqrt(V_uncertainty);
 
 
    // Check for excessive velocity
    double current_velocity = state->_imu->vel().norm();
    if (current_velocity > auto_reset_max_velocity)
    {
        fprintf(stderr, "ERROR: exceeded maximum velocity %f vs %f\n",
                current_velocity, auto_reset_max_velocity);
        vio_error_codes |= ERROR_CODE_VEL_WINDOW_CERT;
    }
 
    std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> SLAM_FEATS = state->_features_SLAM;
 
    // NUMBER OF FEATURE POINTS
    // FOR NOW, WE ONLY CONSIDER SLAM FEATURES, AS THESE ARE THE ONLY ONES IN THE STATE
    vio_packet.n_feature_points = static_cast<uint16_t>(SLAM_FEATS.size());
 
    // Check for insufficient features
    static int64_t last_good_feat_ts = 0;
    static int64_t last_good_qual_ts = 0;
    static bool wait_for_features = true;
    static uint32_t last_reset_count = reset_num_counter.load();
    static int64_t last_good_state_ns = 0;
 
    // C++17: Use const for reset counter check
    const uint32_t current_reset_count = reset_num_counter.load(std::memory_order_acquire);
    if (last_reset_count != current_reset_count)
    {
        // FIX: Reset tracking variables but DON'T return early
        // Continuing allows first packet after reset to be published
        last_good_state_ns = 0;
        wait_for_steady_init = true;
        last_reset_count = current_reset_count;
        wait_for_features = true;
        last_good_feat_ts = vio_packet.timestamp_ns;
        last_good_qual_ts = vio_packet.timestamp_ns;
 
        if (en_debug)
        {
            printf("[QUALITY] VIO reset detected (reset_count=%u), feature/quality tracking reset\n",
                   current_reset_count);
        }
        // CRITICAL FIX: Removed early return - allow publishing to continue
    }
 
    if (wait_for_features)
    {
        vio_error_codes.fetch_and(~ERROR_CODE_NO_FEATURES, std::memory_order_relaxed);
        if (vio_packet.n_feature_points > auto_reset_min_features)
        {
            last_good_feat_ts = vio_packet.timestamp_ns;
            wait_for_features = false;
        }
    }
    else
    {
        if (vio_packet.n_feature_points > auto_reset_min_features)
        {
            last_good_feat_ts = vio_packet.timestamp_ns;
        }
 
        double ts = (vio_packet.timestamp_ns - last_good_feat_ts) * 1e-9;
        if (ts > auto_reset_min_feature_timeout_s)
        {
            fprintf(stderr, "ERROR: insufficient features for too long! cur: %d, min_req: %d\n",
                    vio_packet.n_feature_points, auto_reset_min_features);
            vio_error_codes |= ERROR_CODE_NO_FEATURES;
            wait_for_features = true;
        }
    }
 
    // Check for fast yaw changes (spinning in place)
    static int64_t start_spin_time = 0;
    static bool spinning_detected = false;
 
    double yawrate = vio_packet.imu_angular_vel[2];
    bool spinning_in_place = (fabs(yawrate) > fast_yaw_thresh &&
                              fabs(vio_packet.vel_imu_wrt_vio[0]) <= 1.0 &&
                              fabs(vio_packet.vel_imu_wrt_vio[1]) <= 1.0);
 
    if (!spinning_in_place)
    {
        start_spin_time = vio_packet.timestamp_ns;
        spinning_detected = false;
    }
    else if (!spinning_detected)
    {
        double spin_duration = (vio_packet.timestamp_ns - start_spin_time) * 1e-9;
        if (spin_duration > fast_yaw_timeout_s)
        {
            fprintf(stderr, "ERROR: exceeded spin rate over time threshold %f!\n", fast_yaw_timeout_s);
            vio_error_codes |= ERROR_CODE_IMU_OOB;
            spinning_detected = true;
        }
    }
 
    // Copy error codes from atomic variable to packet
    vio_packet.error_code = vio_error_codes.load();
 
    // QUALITY CALCULATION
    // Calculate uncertainty metrics
    double T_uncertainty = 0.0;
    T_uncertainty += covariance_posori(0, 0) * covariance_posori(0, 0);
    T_uncertainty += covariance_posori(1, 1) * covariance_posori(1, 1);
    T_uncertainty += covariance_posori(2, 2) * covariance_posori(2, 2);
    T_uncertainty = sqrt(T_uncertainty);
 
    double R_uncertainty = 0.0;
    R_uncertainty += covariance_posori(3, 3) * covariance_posori(3, 3);
    R_uncertainty += covariance_posori(4, 4) * covariance_posori(4, 4);
    R_uncertainty += covariance_posori(5, 5) * covariance_posori(5, 5);
    R_uncertainty = sqrt(R_uncertainty);
 
    // Helper lambda for CEP-based quality calculation (DRY principle)
    constexpr double max_allowable_cep = 0.15; // 15cm CEP threshold
    auto calculate_cep_quality = [max_allowable_cep](double t_uncertainty) -> double
    {
        return std::max(0.0, 100.0 * (1.0 - t_uncertainty / max_allowable_cep));
    };
 
    // Get used features map from VioManager if not provided and NOT RESETTING!!
    double calculated_quality;
    bool is_during_initialization = ((!vio_manager || !vio_manager->initialized()) && (!is_resetting.load(std::memory_order_acquire)));
    const bool throttling_active =
        (vio_manager && vio_manager->initialized() &&
         vio_state.load(std::memory_order_acquire) == VIO_STATE_OK &&
         (!non_static.load(std::memory_order_acquire) || !has_acc_jerk.load(std::memory_order_acquire)));
 
    if (!is_during_initialization && !throttling_active)
    {
        // Calculate quality using the new feature-based method
        calculated_quality = calcQuality(used_features_map, SLAM_FEATS, state);
    }
    else
    {
        // WE NEED THIS FOR ARMING IN POSITION
        // FIX: During initialization or when throttling frames, use CEP-based quality.
        // Throttling reduces feature updates; CEP avoids falsely penalizing quality.
        // Use already-calculated uncertainties to avoid redundant operations
 
        calculated_quality = calculate_cep_quality(T_uncertainty);
 
        if (en_debug && is_during_initialization)
        {
            printf("[QUALITY_INIT] VIO initializing - CEP quality: %.1f (features=%d, T_unc=%.4f m)\n",
                   calculated_quality, vio_packet.n_feature_points, T_uncertainty);
        }
        else if (en_debug && throttling_active)
        {
            printf("[QUALITY_THROTTLE] Using CEP quality during throttling: %.1f (T_unc=%.4f m, static=%d, acc_no_jerk=%d)\n",
                   calculated_quality, T_uncertainty,
                   !non_static.load(std::memory_order_relaxed),
                   !has_acc_jerk.load(std::memory_order_relaxed));
        }
    }
 
    // Ensure calculated quality is within bounds
    calculated_quality = std::max(0.0, std::min(100.0, calculated_quality));
 
    // Check for quality issues using ACTUAL calculated quality (not hysteresis-filtered)
    // This check monitors if quality has been consistently bad for too long. It only runs while
    // initialized, so the timer must be armed at the INITIALIZED RISING EDGE: last_good_qual_ts
    // was last refreshed before/at the reset, and measuring the whole multi-second re-init window
    // against the sub-second threshold fired the error on the first initialized packet -- a
    // freshly re-initialized filter never got a chance (auto-reset loop).
    static bool qual_check_armed = false;
    if (vio_manager && vio_manager->initialized())
    {
        if (!qual_check_armed)
        {
            last_good_qual_ts = vio_packet.timestamp_ns;
            qual_check_armed = true;
        }
        double ts_threshold = auto_reset_max_v_cov_timeout_s;
        if (calculated_quality >= 1)
        {
            last_good_qual_ts = vio_packet.timestamp_ns;
        }
 
        double qual_ts = (vio_packet.timestamp_ns - last_good_qual_ts) * 1e-9;
        if (qual_ts > ts_threshold)
        {
            fprintf(stderr, "ERROR: actual quality was bad for too long!\n");
            vio_error_codes |= ERROR_CODE_NOT_STATIONARY;
        }
    }
    else
    {
        qual_check_armed = false;
    }
 
    // ========================================================================
    // QUALITY HYSTERESIS SYSTEM - State Machine Approach
    // ========================================================================
    // States:
    // 1. INITIAL: Report quality as-is (no filtering) on first run after reset
    // 2. BAD: Quality degraded - apply strict hysteresis to recover
    // 3. GOOD: Quality stable - apply hysteresis to prevent false alarms
    //
    // Transitions:
    // - INITIAL → BAD: When quality drops below 20 for 10 consecutive samples
    // - BAD → GOOD: When quality above 40 for 60 consecutive samples
    // - GOOD → BAD: When quality below 20 for 45 consecutive samples
    // - Any state → INITIAL: On VIO reset or FAILED state
    // ========================================================================
 
    enum QualityState
    {
        INITIAL,
        BAD,
        GOOD
    };
    static QualityState quality_state = INITIAL;
    static int consecutive_above_40 = 0;
    static int consecutive_below_20 = 0;
    static uint32_t last_quality_reset_count = reset_num_counter.load();
 
    // Reset to INITIAL state on VIO reset OR when entering FAILED state
    bool should_reset_quality_state = (last_quality_reset_count != current_reset_count) ||
                                      (vio_state.load() == VIO_STATE_FAILED);
 
    if (should_reset_quality_state)
    {
        quality_state = INITIAL;
        consecutive_above_40 = 0;
        consecutive_below_20 = 0;
        last_quality_reset_count = current_reset_count;
        // Quality-metric lifecycle matches the hysteresis machine: reset_num_counter bumps for
        // BOTH hard and soft resets, so smoothed values from the previous episode can never
        // pre-charge this one (sensor time is continuous across resets)
        qual_ema = -1.0;
        qual_ema_t = 0.0;
        qual_src_key = 0.0;
        qual_src_size = 0;
        qual_src_map_size = 0;
 
        if (en_debug)
        {
            printf("[QUALITY] RESET to INITIAL state - will report quality as-is (reset_count=%u)\n",
                   current_reset_count);
        }
    }
 
    // State machine transitions based on current quality.
    // Gated on initialized(): during (re)initialization calculated_quality is CEP of a freshly
    // seeded covariance (~1e-3 sigma => CEP ~100), which would latch GOOD before the filter has
    // earned it -- the counters must only evolve on real feature-backed samples.
    if (!should_reset_quality_state && vio_manager && vio_manager->initialized())
    {
        switch (quality_state)
        {
        case INITIAL:
            // Report quality as-is, but monitor for degradation
            if (calculated_quality <= quality_low_thresh_initial)
            {
                consecutive_below_20++;
                if (consecutive_below_20 >= quality_initial_to_bad_count)
                {
                    quality_state = BAD;
                    consecutive_below_20 = 0;
                    consecutive_above_40 = 0;
                    if (en_debug)
                        printf("[QUALITY] Transition: INITIAL → BAD (quality degraded)\n");
                }
            }
            else
            {
                consecutive_below_20 = 0;
            }
            if (calculated_quality > quality_high_thresh)
            {
                consecutive_above_40++;
                if (consecutive_above_40 >= quality_initial_to_good_count)
                {
                    quality_state = GOOD;
                    consecutive_above_40 = 0;
                    consecutive_below_20 = 0;
                    std::cout << "[QUALITY] Transition: INITIAL → GOOD (" << quality_initial_to_good_count << " samples > " << quality_high_thresh << ")" << std::endl;
                }
            }
            else
            {
                consecutive_above_40 = 0;
            }
            break;
 
        case BAD:
            // Quality is bad - require strong evidence to recover
            if (calculated_quality > quality_high_thresh)
            {
                consecutive_above_40++;
                if (consecutive_above_40 >= quality_bad_to_good_count)
                {
                    quality_state = GOOD;
                    consecutive_above_40 = 0;
                    consecutive_below_20 = 0;
                    std::cout << "[QUALITY] Transition: BAD → GOOD (" << quality_bad_to_good_count << " samples > " << quality_high_thresh << ")" << std::endl;
                }
            }
            else
            {
                consecutive_above_40 = 0;
            }
            break;
 
        case GOOD:
            // Quality is good - require strong evidence of degradation
            if (calculated_quality <= quality_low_thresh_good)
            {
                consecutive_below_20++;
                if (consecutive_below_20 >= quality_good_to_bad_count)
                {
                    quality_state = BAD;
                    consecutive_below_20 = 0;
                    consecutive_above_40 = 0;
                    std::cout << "[QUALITY] Transition: GOOD → BAD (" << quality_good_to_bad_count << " samples ≤ " << quality_low_thresh_good << ")" << std::endl;
                }
            }
            else
            {
                consecutive_below_20 = 0;
            }
            break;
        }
    }
 
    // Determine reported quality based on state
    int reported_quality;
    switch (quality_state)
    {
    case INITIAL:
        // Report CEP quality in INITIAL state ONLY if ZUPT update occurred
        // Otherwise report 0 until system proves itself
        {
            if (vio_manager && vio_manager->get_did_zupt_update())
            {
                const double cep_quality = calculate_cep_quality(T_uncertainty);
                reported_quality = static_cast<int>(cep_quality);
 
                if (en_debug)
                {
                    printf("[QUALITY_INIT] INITIAL state - CEP quality: %.1f (T_unc=%.4f m) [ZUPT done]\n",
                           cep_quality, T_uncertainty);
                }
            }
            else
            {
                reported_quality = 0;
 
                if (en_debug)
                {
                    printf("[QUALITY_INIT] INITIAL state - Quality: 0 [waiting for ZUPT]\n");
                }
            }
        }
        break;
    case BAD:
        // Report 0 until quality recovers
        reported_quality = 0;
        break;
    case GOOD:
        // Report calculated quality (feature-based or CEP-based depending on VIO state)
        reported_quality = static_cast<int>(calculated_quality);
        break;
    }
 
    // Ensure reported quality is within bounds
    reported_quality = std::max(0, std::min(100, reported_quality));
 
    // Debug logging
    if (en_debug && (quality_state != GOOD || reported_quality < 10))
    {
        const char *state_str = (quality_state == INITIAL) ? "INITIAL" : (quality_state == BAD) ? "BAD"
                                                                                                : "GOOD";
        printf("[QUALITY] State: %s, Reported: %d, Calculated: %.1f, Consec(>40): %d, Consec(≤20): %d\n",
               state_str, reported_quality, calculated_quality,
               consecutive_above_40, consecutive_below_20);
    }
 
    // For error detection and auto-reset, use the ACTUAL calculated quality, not the hysteresis-filtered one
    // This prevents false auto-resets during the initial warm-up period
    int quality_for_error_detection = static_cast<int32_t>(calculated_quality);
    if (quality_for_error_detection > 100)
        quality_for_error_detection = 100;
    if (quality_for_error_detection < 0)
        quality_for_error_detection = 0;
 
    // Determine if we're in the grace period after initialization/reset
    bool in_grace_period = false;
    if (vio_manager && vio_manager->initialized())
    {
        const int64_t now_ns = _apps_time_monotonic_ns();
 
        // If we just entered OK (or after a reset), start the grace window.
        if ((last_good_state_ns == 0 || vio_packet.n_feature_points < auto_reset_min_features) && wait_for_steady_init)
        {
            last_good_state_ns = now_ns;
        }
 
        // Duration since OK started
        const int64_t dt_ok_ns = now_ns - last_good_state_ns;
        const int64_t grace_ns = (int64_t)(ok_state_grace_timeout_s * 1e9);
 
        in_grace_period = (dt_ok_ns < grace_ns && wait_for_steady_init);
    }
 
    // ========================================================================
    // STATE DETERMINATION
    // ========================================================================
    // Priority order:
    // 1. INITIALIZING - if VIO manager not initialized yet or resetting
    // 2. FAILED - if auto-reset conditions met (only when fully initialized)
    // 3. OK - normal operation
    // ========================================================================
 
    // FIRST: Check if we're still initializing (vio_manager not ready OR currently resetting)
    // Note: Once vio_manager is initialized, we report OK state externally, even during grace period
    if (!vio_manager || !vio_manager->initialized())
    {
        // System is INITIALIZING - VIO manager not ready yet
 
        vio_state.store(VIO_STATE_INITIALIZING, std::memory_order_release);
        if (vio_manager->get_did_zupt_update())
        {
            vio_packet.state = VIO_STATE_OK;
        }
        else
        {
            vio_packet.state = VIO_STATE_INITIALIZING;
        }
        // During initialization, use actual CEP quality for arming checks
        // This allows position-based arming to work correctly -- but never claim more than the
        // grace-period cap: a freshly (re)built covariance makes CEP read ~100 before the filter
        // has earned it (a stale-ZUPT soft reset would otherwise publish near-perfect quality
        // throughout re-init)
        vio_packet.quality = std::min(reported_quality, 15);
 
        if (en_debug)
        {
            printf("[STATE] VIO_STATE_INITIALIZING - vio_manager: %s, initialized: %s, quality: %d\n",
                   vio_manager ? "exists" : "null",
                   (vio_manager && vio_manager->initialized()) ? "yes" : "no",
                   vio_packet.quality);
        }
    }
    // Handle resetting state separately - report as INITIALIZING but with different internal handling
    else if (is_resetting.load(std::memory_order_acquire))
    {
        // System is resetting - report as INITIALIZING to external systems
        vio_packet.state = VIO_STATE_INITIALIZING;
        vio_state.store(VIO_STATE_INITIALIZING, std::memory_order_release);
        vio_packet.quality = 0; // Quality is invalid during reset
 
        if (en_debug)
        {
            printf("[STATE] VIO_STATE_INITIALIZING (resetting) - quality: %d\n", vio_packet.quality);
        }
    }
    // SECOND: Check for auto-reset conditions (only if initialized and not in grace period)
    else if (!in_grace_period &&
             should_auto_reset(state, quality_for_error_detection, vio_packet.n_feature_points,
                               yawrate, current_velocity,
                               vio_packet.vel_imu_wrt_vio[0], vio_packet.vel_imu_wrt_vio[1]))
    {
        // Auto-reset conditions met - system has FAILED
        fprintf(stderr, "WARNING: Auto-reset conditions detected! Quality: %d, Features: %d, V_uncertainty: %f\n",
                quality_for_error_detection, vio_packet.n_feature_points, V_uncertainty);
 
        vio_packet.quality = -1;
        vio_packet.state = VIO_STATE_FAILED;
        vio_state.store(VIO_STATE_FAILED, std::memory_order_release);
 
        // CRITICAL: Request the actual reset
        if (!reset_requested.load(std::memory_order_acquire))
        {
            reset_requested.store(true, std::memory_order_release);
            fprintf(stderr, "[AUTO_RESET] REQUESTING RESET due to auto-reset conditions\n");
        }
    }
    // THIRD: System is OK - normal operation
    else
    {
        vio_packet.state = VIO_STATE_OK;
        vio_state.store(VIO_STATE_OK, std::memory_order_release);
 
        if (in_grace_period)
        {
            // During grace period, respect hysteresis while being conservative
            if (reported_quality == 0)
            {
                vio_packet.quality = 0; // Respect hysteresis: system needs to prove itself
            }
            else
            {
                // Quality has passed hysteresis, but still in grace period - cap conservatively
                vio_packet.quality = std::min(reported_quality, 15);
            }
 
            if (en_debug)
            {
                printf("[GRACE_PERIOD] In grace period: reported_quality=%d, vio_packet.quality=%d\n",
                       reported_quality, vio_packet.quality);
            }
        }
        else
        {
            wait_for_steady_init = false;
            // Set the reported quality with hysteresis applied
            vio_packet.quality = reported_quality;
        }
    }
 
    // FRAME
    vio_packet.frame = 0; // Set appropriate frame value
 
    // A reset can begin while this (pre-reset) publish is still in flight -- do not let one last
    // stale-good packet race the teardown
    if (is_resetting.load(std::memory_order_acquire))
    {
        vio_packet.quality = 0;
        vio_packet.state = VIO_STATE_INITIALIZING;
    }
 
    // publish the packet
    pipe_server_write(SIMPLE_CH, (char *)&vio_packet, sizeof(vio_data_t));
 
    if (pipe_server_get_num_clients(EXTENDED_CH) > 0)
    {
        ext_vio_data_t ext_vio_packet;
        ext_vio_packet.v = vio_packet;
        ext_vio_packet.n_total_features = 0;
 
        double current_timestamp = state->_timestamp;
        auto timestamp_iter = used_features_map.find(current_timestamp);
        // Async/ZUPT ticks may have no entry at the exact state time (the state advanced without
        // a feature update) -- fall back to the newest entry so the extended feature list does
        // not flicker empty. The per-measurement search below must then use the ENTRY's key:
        // epoch-snapped measurements are stamped with the entry time, not the state time.
        if (timestamp_iter == used_features_map.end() && !used_features_map.empty())
        {
            timestamp_iter = std::prev(used_features_map.end());
            current_timestamp = timestamp_iter->first;
        }
 
        if (timestamp_iter != used_features_map.end())
        {
            const auto &features = timestamp_iter->second;
 
            // Group features by camera ID
            std::map<int, std::vector<std::shared_ptr<ov_core::Feature>>> features_by_camera;
            for (const auto &feature : features)
            {
                // Get camera ID from feature measurements
                for (const auto &cam_meas : feature->timestamps)
                {
                    int cam_id = static_cast<int>(cam_meas.first);
                    features_by_camera[cam_id].push_back(feature);
                }
            }
 
            ov_type::LandmarkRepresentation::Representation msckf_ref = state->_options.feat_rep_msckf;
 
            // Process each camera's features
            for (const auto &camera_pair : features_by_camera)
            {
                int cam_id = camera_pair.first;
                const auto &cam_features = camera_pair.second;
 
               for (const auto &feature : cam_features)
                {
                    // Check if this feature has measurements for this camera at current timestamp
                    if (feature->timestamps.find(cam_id) == feature->timestamps.end() ||
                        feature->uvs.find(cam_id) == feature->uvs.end())
                    {
                        continue;
                    }
 
                    // Find the measurement index for the current timestamp
                    const auto &timestamps = feature->timestamps.at(cam_id);
                    auto time_iter = std::find(timestamps.begin(), timestamps.end(), current_timestamp);
                    if (time_iter == timestamps.end())
                    {
                        continue;
                    }
 
                    size_t meas_idx = std::distance(timestamps.begin(), time_iter);
                    const auto &uv_meas = feature->uvs.at(cam_id)[meas_idx];
 
                    // check if feature id found in SLAM set
                    if (SLAM_FEATS.find(feature->featid) != SLAM_FEATS.end())
                    {
                        if (ext_vio_packet.n_total_features >= VIO_MAX_REPORTED_FEATURES)
                            break;
 
                        int i_global = ext_vio_packet.n_total_features++;
 
                        ext_vio_packet.features[i_global].id = feature->featid;
                        ext_vio_packet.features[i_global].cam_id = cam_id;
 
                        // Extract pixel location from UV measurement
                        ext_vio_packet.features[i_global].pix_loc[0] = uv_meas(0);
                        ext_vio_packet.features[i_global].pix_loc[1] = uv_meas(1);
 
                        Eigen::Vector3d p_FinG;
                        if (SLAM_FEATS[feature->featid]->_feat_representation == ov_type::LandmarkRepresentation::Representation::GLOBAL_3D)
                        {
                            p_FinG = SLAM_FEATS[feature->featid]->get_xyz(true); // GRAB FEJ VALUE --> AVOID SNAP BACK EFFECT IN VIZ
                            p_FinG = ov2frd * p_FinG;
                        }
                        else if (SLAM_FEATS[feature->featid]->_feat_representation == ov_type::LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH)
                        {
                            try
                            {
                                auto &slam_feat = SLAM_FEATS[feature->featid];
 
                                Eigen::Vector3d p_FinA = slam_feat->get_xyz(false);
 
                                // Anchor IMU clone
                                auto imu_clone = state->_clones_IMU.at(slam_feat->_anchor_clone_timestamp);
                                auto calib = state->_calib_IMUtoCAM.at(slam_feat->_anchor_cam_id);
 
                                Eigen::Matrix3d R_GtoI = imu_clone->Rot();
                                Eigen::Vector3d p_IinG = imu_clone->pos();
 
                                Eigen::Matrix3d R_ItoC = calib->Rot();
                                Eigen::Vector3d p_IinC = calib->pos();
 
                                // Camera pose at anchor
                                Eigen::Matrix3d R_GtoC = R_ItoC * R_GtoI;
                                Eigen::Vector3d p_CinG = p_IinG - R_GtoC.transpose() * p_IinC;
 
                                // Feature in OpenVINS global
                                Eigen::Vector3d p_FinG_ov = R_GtoC.transpose() * p_FinA + p_CinG;
 
                                // Convert to FRD
                                p_FinG = ov2frd * p_FinG_ov;
                            }
                            catch (const std::exception &e)
                            {
                                std::cerr << e.what() << '\n';
                            }
                        }
                        else
                        {
                            printf("[WARNING] SLAM feat representation: %d not recognized, 3D point locations are likely invalid\n", SLAM_FEATS[feature->featid]->_feat_representation);
                        }
 
                        ext_vio_packet.features[i_global].tsf[0] = p_FinG[0];
                        ext_vio_packet.features[i_global].tsf[1] = p_FinG[1];
                        ext_vio_packet.features[i_global].tsf[2] = p_FinG[2];
 
                        ext_vio_packet.features[i_global].point_quality = HIGH;
                    }
                    // if not found in SLAM, then it is an MSCKF feature
                    else
                    {
                        if (ext_vio_packet.n_total_features >= VIO_MAX_REPORTED_FEATURES)
                            break;
 
                        int i_global = ext_vio_packet.n_total_features++;
 
                        ext_vio_packet.features[i_global].id = feature->featid;
                        ext_vio_packet.features[i_global].cam_id = cam_id;
 
                        // Extract pixel location from UV measurement
                        ext_vio_packet.features[i_global].pix_loc[0] = uv_meas(0);
                        ext_vio_packet.features[i_global].pix_loc[1] = uv_meas(1);
 
                        Eigen::Vector3d p_FinA = feature->p_FinA;
                        Eigen::Vector3d p_FinG = feature->p_FinG;
 
                        if (msckf_ref == ov_type::LandmarkRepresentation::Representation::GLOBAL_3D)
                        {
                            p_FinG = ov2frd * p_FinG;
                        }
                        else if (msckf_ref == ov_type::LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH)
                        {
                            try
                            {
                                // Anchor IMU clone
                                auto imu_clone = state->_clones_IMU.at(feature->anchor_clone_timestamp);
                                auto calib = state->_calib_IMUtoCAM.at(feature->anchor_cam_id);
 
                                Eigen::Matrix3d R_GtoI = imu_clone->Rot();
                                Eigen::Vector3d p_IinG = imu_clone->pos();
 
                                Eigen::Matrix3d R_ItoC = calib->Rot();
                                Eigen::Vector3d p_IinC = calib->pos();
 
                                // Camera pose at anchor
                                Eigen::Matrix3d R_GtoC = R_ItoC * R_GtoI;
                                Eigen::Vector3d p_CinG = p_IinG - R_GtoC.transpose() * p_IinC;
 
                                // Feature in OpenVINS global
                                Eigen::Vector3d p_FinG_ov = R_GtoC.transpose() * p_FinA + p_CinG;
 
                                // Convert to FRD
                                p_FinG = ov2frd * p_FinG_ov;
                            }
                            catch (const std::exception &e)
                            {
                                std::cerr << e.what() << '\n';
                            }
                        }
                        else
                        {
                            printf("[WARNING] MSCKF feat representation: %s not recognized, 3D point locations are likely invalid\n", ov_type::LandmarkRepresentation::as_string(msckf_ref).c_str());
                        }
                        ext_vio_packet.features[i_global].tsf[0] = p_FinG[0];
                        ext_vio_packet.features[i_global].tsf[1] = p_FinG[1];
                        ext_vio_packet.features[i_global].tsf[2] = p_FinG[2];
 
                        ext_vio_packet.features[i_global].point_quality = MEDIUM;
                    }
                }
            }
        }
 
        pipe_server_write(EXTENDED_CH, (char *)&ext_vio_packet, sizeof(ext_vio_data_t));
    }
}
 
/**
 * @brief Check if auto-reset should be triggered
 *
 * This function evaluates the current VIO state and various error conditions
 * to determine if an automatic reset should be triggered. It implements the
 * same logic as the legacy code but in a more modular way.
 *
 * @param state Current VIO state
 * @param quality Current quality value
 * @param n_features Number of tracked features
 * @return true if auto-reset should be triggered, false otherwise
 */
bool Publisher::should_auto_reset(std::shared_ptr<ov_msckf::State> state,
                                  int quality,
                                  int n_features,
                                  double yawrate,
                                  double current_velocity,
                                  double vel_x,
                                  double vel_y)
{
    // Only check for auto-reset if enabled and system is stable
    if (!en_auto_reset)
    {
        return false;
    }
 
    // FIX: Reset static variables when reset_num_counter changes
    // This prevents stale state from previous runs affecting current run
    static int64_t last_good_qual_ts = 0;
    static int64_t last_good_feat_ts = 0;
    static bool wait_for_features = true;
    static uint32_t last_reset_check_count = 0;
 
    const uint32_t current_reset_count = reset_num_counter.load(std::memory_order_acquire);
    if (last_reset_check_count != current_reset_count)
    {
        // Reset occurred - clear all static tracking variables
        const int64_t current_ts_ns = state->_timestamp * 1e9;
        last_good_qual_ts = current_ts_ns;
        last_good_feat_ts = current_ts_ns;
        wait_for_features = true;
        last_reset_check_count = current_reset_count;
 
        if (en_debug)
        {
            printf("[AUTO_RESET] Static variables reset for new VIO cycle (reset_count=%u)\n",
                   current_reset_count);
        }
    }
 
    // CRITICAL FIX: Remove instant quality check - conflicts with hysteresis
    // Quality hysteresis reports 0 during transition, but actual quality may be good
    // This was causing resets when quality improves!
    // Only check for SUSTAINED bad quality, not instant quality
    bool stable_quality_bad = false;
 
    // Check for stable quality issues (quality bad for extended period)
    if (quality >= 1)
    {
        last_good_qual_ts = state->_timestamp * 1e9;
    }
    const double qual_elapsed_s = (state->_timestamp * 1e9 - last_good_qual_ts) * 1e-9;
    if (qual_elapsed_s > auto_reset_max_v_cov_timeout_s)
    {
        stable_quality_bad = true;
    }
 
    // Check feature conditions
    bool stable_features_bad = false;
 
    if (wait_for_features)
    {
        if (n_features > auto_reset_min_features)
        {
            last_good_feat_ts = state->_timestamp * 1e9;
            wait_for_features = false;
        }
    }
    else
    {
        if (n_features > auto_reset_min_features)
        {
            last_good_feat_ts = state->_timestamp * 1e9;
        }
 
        double ts = (state->_timestamp * 1e9 - last_good_feat_ts) * 1e-9;
        if (ts > auto_reset_min_feature_timeout_s)
        {
            stable_features_bad = true;
            wait_for_features = true;
        }
    }
 
    // Check velocity conditions using passed values
    bool too_fast = current_velocity > auto_reset_max_velocity;
 
    // Check for excessive spinning using passed yawrate
    // FIX: Reset spin tracking on VIO reset
    static int64_t start_spin_time = 0;
    static bool spinning_detected = false;
    static uint32_t last_spin_reset_count = 0;
 
    if (last_spin_reset_count != current_reset_count)
    {
        start_spin_time = state->_timestamp * 1e9;
        spinning_detected = false;
        last_spin_reset_count = current_reset_count;
    }
 
    bool too_much_spinning = false;
 
    // Use the passed yawrate value and actual velocity components
    const bool spinning_in_place = (fabs(yawrate) > fast_yaw_thresh &&
                                    fabs(vel_x) <= 1.0 &&
                                    fabs(vel_y) <= 1.0);
 
    if (!spinning_in_place)
    {
        start_spin_time = state->_timestamp * 1e9;
        spinning_detected = false;
    }
    else if (!spinning_detected)
    {
        const double spin_duration = (state->_timestamp * 1e9 - start_spin_time) * 1e-9;
        if (spin_duration > fast_yaw_timeout_s)
        {
            too_much_spinning = true;
            spinning_detected = true;
        }
    }
 
    // Return true if any condition is met
    // CRITICAL FIX: Removed quality_bad - was causing resets during hysteresis transitions
    return stable_quality_bad || stable_features_bad || too_fast || too_much_spinning;
}
 
// ---------------------------------------------------------------------------------------------
// Quality metric internals (async-aware, per-camera freshness ledger)
//
// Async dual-cam updates are per-camera: the reference camera opens a new epoch entry in
// used_features_map and the other camera's features append to the SAME entry up to one bind
// horizon later (or land in their own entry on an epoch fallback). Scoring only the entry at
// exactly state->_timestamp therefore sees HALF the rig on ref-camera ticks, and nothing at all
// on ZUPT/featureless ticks (flat 10) -- which starved the downstream hysteresis (needs >40 for
// 50 CONSECUTIVE samples) and pinned published async quality at 0.
//
// Fix: pool every entry inside a short union window (dedup by featid) and score each camera over
// the POOLED set with the unchanged synced-path scorer, sum-then-clamp as sync always fused. One
// update's used-feature list is a sparse, noisy sample of tracking health (MSCKF features only
// appear when their tracks end), so scoring single entries makes the value jumpy; the union
// reproduces the full per-camera track set the synced batches effectively scored, and a
// time-constant EMA removes what noise remains. Everything here runs exclusively on the VIO
// thread (the ingest camera-processed callback) -- single writer, no locks by construction.
// ---------------------------------------------------------------------------------------------
 
static constexpr int QUAL_DEDUP_CAP = 256;           // max unique features pooled per window
static constexpr double QUAL_UNION_WINDOW_S = 0.120; // pool entries this far back (~3-4 epochs
                                                     // at 30 Hz -- also the horizon over which a
                                                     // dead camera's evidence ages out)
static constexpr double QUAL_EMA_TAU_S = 0.25;       // published-value smoothing time constant:
                                                     // one weak update moves the value ~2 pts; a
                                                     // real collapse reaches the BAD band in ~0.5 s
 
/// Append an entry's features into a fixed dedup array (by featid), returning the new count --
/// epoch entries accumulate per-camera update lists and the union spans several entries, so the
/// same feature object shows up repeatedly. O(n^2) pointer walk, n <= QUAL_DEDUP_CAP; zero
/// allocation.
static int collect_unique_features(const std::vector<std::shared_ptr<ov_core::Feature>> &entry_feats,
                                   const ov_core::Feature **uniq, int n, int cap)
{
    for (const auto &f : entry_feats)
    {
        if (f == nullptr)
            continue;
        bool dup = false;
        for (int i = 0; i < n; i++)
        {
            if (uniq[i]->featid == f->featid)
            {
                dup = true;
                break;
            }
        }
        if (!dup)
        {
            if (n >= cap)
                break;
            uniq[n++] = f.get();
        }
    }
    return n;
}
 
/**
 * @brief Score one camera's used-feature entry on a 5x5 coverage grid
 *
 * Scoring is UNCHANGED from the original synced-path metric: SLAM features weighted by
 * marginal-covariance largest eigenvalue and tracker quality field, MSCKF features by quality
 * field and track length, combined 50/50 with grid coverage. Features without measurements on
 * cam_id are skipped, so the whole (multi-camera, already deduplicated) union can be passed
 * directly.
 *
 * Called only from calcQuality() on the VIO thread.
 */
static float score_camera_features(int cam_id,
                                   const ov_core::Feature **feats,
                                   int n_feats,
                                   std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> &slam_features,
                                   const std::shared_ptr<ov_msckf::State> &state,
                                   int &slam_count,
                                   int &msckf_count)
{
    // Constants for quality calculation
    constexpr int GRID_SIZE = 5;                                                                // 5x5 grid
    constexpr int TARGET_FEATURES_PER_CAM = 50;                                                 // 50 features per camera
    constexpr int TARGET_FEATURES_PER_GRID = TARGET_FEATURES_PER_CAM / (GRID_SIZE * GRID_SIZE); // 2 features per grid
    constexpr int CLONES = 11;                                                                  // Number of clones for MSCKF weighting
    constexpr double MAX_GRID_SCORE = 100.0;                                                    // Maximum score when 50% of grids are filled
    constexpr double GRID_FILL_THRESHOLD = 0.5;                                                 // 50% of grids should be filled for max score
    const int en_qual_debug = 0;
 
    // Fixed-size grids on the stack -- no per-call allocation
    int grid[GRID_SIZE][GRID_SIZE] = {};
    double grid_quality[GRID_SIZE][GRID_SIZE] = {};
 
    {
        int cam_slam_count = 0;
        int cam_msckf_count = 0;
 
        // Process features for this camera (already deduplicated by the caller)
        for (int fi = 0; fi < n_feats; fi++)
        {
            const ov_core::Feature *feature = feats[fi];
            // Get feature position in image for this camera
            if (feature->timestamps.find(cam_id) != feature->timestamps.end() &&
                !feature->uvs_norm.empty() &&
                feature->uvs_norm.find(cam_id) != feature->uvs_norm.end() &&
                !feature->uvs_norm.at(cam_id).empty())
            {
 
                const auto &uv_norm = feature->uvs_norm.at(cam_id).back();
                if (en_qual_debug)
                    printf("[QUALITY_DEBUG] Feature %zu - Camera %d - UV Norm: (%.3f, %.3f)\n",
                           feature->featid, cam_id, uv_norm(0), uv_norm(1));
 
                // Convert normalized coordinates to grid coordinates
                // Assuming normalized coordinates are in [-1, 1] range
                int grid_x = static_cast<int>((uv_norm(0) + 1.0) * 0.5 * GRID_SIZE);
                int grid_y = static_cast<int>((uv_norm(1) + 1.0) * 0.5 * GRID_SIZE);
 
                // Clamp to valid grid range
                grid_x = std::max(0, std::min(GRID_SIZE - 1, grid_x));
                grid_y = std::max(0, std::min(GRID_SIZE - 1, grid_y));
 
                // Calculate feature quality based on whether it's SLAM or MSCKF
                double feature_quality = 0.0;
 
                // Check if this is a SLAM feature
                if (slam_features.find(feature->featid) != slam_features.end())
                {
                    // This is an active SLAM feature
                    cam_slam_count++;
                    auto slam_feat = slam_features[feature->featid];
 
                    if (en_qual_debug)
                        printf("[QUALITY_DEBUG] Feature %zu (SLAM) - Grid[%d][%d] - Quality field: %.3f, uniq_cam: %d, anchor_cam: %d\n",
                               feature->featid, grid_x, grid_y, slam_feat->_quality, slam_feat->_unique_camera_id, slam_feat->_anchor_cam_id);
                    // ATTENTION: COULD SOLVE THIS IN A BATCH MANNER, BUT FOR NOW DO THE FOR LOOP DIRECTLY
                    //  Get covariance for this SLAM feature
                    Eigen::MatrixXd slam_cov;
                    if (slam_feat->id() >= 0)
                    {
                        // Get the covariance block for this feature
                        std::vector<std::shared_ptr<ov_type::Type>> slam_feat_vec = {slam_feat};
                        slam_cov = ov_msckf::StateHelper::get_marginal_covariance(state, slam_feat_vec);
                    }
                    else
                    {
                        // Feature not in state yet, use default covariance
                        slam_cov = Eigen::MatrixXd::Identity(3, 3) * 1.4 * 1.4; // Default multiply by slam sigma pixel^2 --> UNITS TBD
                    }
 
                    // Calculate largest eigenvalue of covariance
                    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(slam_cov);
                    double max_eigenvalue = eigensolver.eigenvalues().maxCoeff();
 
                    // Quality based on covariance (smaller is better)
                    // double cov_quality = std::max(0.0, 1.0 - max_eigenvalue);
                    const double sigma_ref = 1.4; // Reference sigma value for normalization
                    double cov_quality = 1.0 / (1.0 + max_eigenvalue / sigma_ref);
 
                    // Combine with feature quality field (if available)
                    double feat_quality_field = (slam_feat->_quality >= 0) ? slam_feat->_quality : 0.0;
 
                    // Weight SLAM features more heavily
                    feature_quality = 0.5 * cov_quality + 0.5 * feat_quality_field;
                    // feature->quality = feature_quality; // Store in feature
 
                    if (en_qual_debug)
                        printf("[QUALITY_DEBUG]   SLAM Feature %zu - Max eigenvalue: %.6f, Cov quality: %.3f, Final quality: %.3f\n",
                               feature->featid, max_eigenvalue, cov_quality, feature_quality);
                }
                else
                {
                    // This is an MSCKF feature
                    cam_msckf_count++;
 
                    // Count number of measurements for this feature
                    int num_measurements = 0;
                    for (const auto &cam_meas : feature->timestamps)
                    {
                        num_measurements += static_cast<int>(cam_meas.second.size());
                    }
 
                    // Weight down if measurements/clones < 1
                    double measurement_weight = 1.0;
                    if (num_measurements < CLONES)
                    {
                        measurement_weight = static_cast<double>(num_measurements) / CLONES;
                    }
 
                    // Use feature quality field (if available)
                    double feat_quality_field = (feature->quality >= 0) ? feature->quality : 0.0;
 
                    // MSCKF features weighted less than SLAM features
                    feature_quality = 0.4 * feat_quality_field * measurement_weight;
 
                    if (en_qual_debug)
                        printf("[QUALITY_DEBUG] Feature %zu (MSCKF) - Grid[%d][%d] - Quality field: %.3f, Measurements: %d, Weight: %.3f, Final quality: %.3f\n",
                               feature->featid, grid_x, grid_y, feat_quality_field, num_measurements, measurement_weight, feature_quality);
                }
 
                // Add feature to grid
                grid[grid_y][grid_x]++;
                grid_quality[grid_y][grid_x] += feature_quality;
            }
        }
 
        slam_count = cam_slam_count;
        msckf_count = cam_msckf_count;
 
        if (en_qual_debug)
            printf("[QUALITY_DEBUG] Camera %d summary - SLAM: %d, MSCKF: %d\n", cam_id, cam_slam_count, cam_msckf_count);
 
        // Calculate grid distribution score for this camera
        int grids_with_features = 0;
        double camera_grid_score = 0.0;
 
        if (en_qual_debug)
            printf("[QUALITY_DEBUG] Camera %d grid analysis:\n", cam_id);
        for (int i = 0; i < GRID_SIZE; i++)
        {
            for (int j = 0; j < GRID_SIZE; j++)
            {
                if (grid[i][j] > 0)
                {
                    grids_with_features++;
 
                    // Calculate quality for this grid cell
                    double grid_cell_quality = 0.0;
                    if (grid[i][j] >= TARGET_FEATURES_PER_GRID)
                    {
                        // Grid has enough features, use average quality
                        grid_cell_quality = grid_quality[i][j] / grid[i][j];
                    }
                    else
                    {
                        // Grid has insufficient features, penalize
                        double fill_ratio = std::min(1.0, static_cast<double>(grid[i][j]) / TARGET_FEATURES_PER_GRID);
                        grid_cell_quality = (grid_quality[i][j] / std::max(1, grid[i][j])) * std::sqrt(fill_ratio);
                    }
                    camera_grid_score += grid_cell_quality;
 
                    if (en_qual_debug)
                        printf("[QUALITY_DEBUG]   Grid[%d][%d]: %d features, avg quality: %.3f\n",
                               i, j, grid[i][j], grid_quality[i][j] / grid[i][j]);
                }
            }
        }
 
        // Calculate grid distribution score
        double grid_fill_ratio = static_cast<double>(grids_with_features) / (GRID_SIZE * GRID_SIZE);
        double grid_distribution_score = 0.0;
 
        if (grid_fill_ratio >= GRID_FILL_THRESHOLD)
        {
            // 50% or more grids filled, give maximum score
            grid_distribution_score = MAX_GRID_SCORE;
        }
        else
        {
            // Linear interpolation for partial fill
            grid_distribution_score = (grid_fill_ratio / GRID_FILL_THRESHOLD) * MAX_GRID_SCORE;
        }
 
        // Combine grid distribution with feature quality
        double camera_score = 0.5 * grid_distribution_score + 0.5 * camera_grid_score;
 
        if (en_qual_debug)
            printf("[QUALITY_DEBUG] Camera %d scores - Grid fill ratio: %.3f, Grid distribution: %.3f, Grid quality: %.3f, Final camera score: %.3f\n",
                   cam_id, grid_fill_ratio, grid_distribution_score, camera_grid_score, camera_score);
 
        return static_cast<float>(camera_score);
    }
}
 
double Publisher::calcQuality(const std::map<double, std::vector<std::shared_ptr<ov_core::Feature>>> &used_features_map,
                              std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> &slam_features,
                              std::shared_ptr<ov_msckf::State> state)
{
    // Metric-level floor: quality alone must never drive the sustained quality<1 auto-reset
    // timers (should_auto_reset / ERROR_CODE_NOT_STATIONARY refresh on quality >= 1). Collapse
    // is reported through the hysteresis machine instead (10 <= BAD threshold -> reported 0),
    // and the reset/FAILED paths force 0/-1 through their own branches in publish().
    constexpr double QUAL_FLOOR = 10.0;
    const int en_qual_debug = 0;
 
    if (used_features_map.empty())
        return QUAL_FLOOR;
 
    // ZUPT ticks advance state->_timestamp without inserting a map entry: anchor the evaluation
    // clock at the newest feature-update time on those ticks so a parked vehicle holds its last
    // smoothed value instead of decaying it (and then drifting the hysteresis machine).
    double t_now = state->_timestamp;
    const double newest_key = used_features_map.rbegin()->first;
    bool anchored = false;
    if (t_now > newest_key && vio_manager && vio_manager->get_did_zupt_update())
    {
        t_now = newest_key;
        anchored = true;
    }
 
    // Rescore only when the union can have changed: entries are append-only, so the newest key,
    // its vector size, and the map size are a sufficient fingerprint. Time passing WITHOUT new
    // entries (mid-flight starvation) must also re-evaluate so the union thins honestly -- but
    // not while ZUPT-anchored.
    const size_t newest_size = used_features_map.rbegin()->second.size();
    const bool dirty = (newest_key != qual_src_key) || (newest_size != qual_src_size) ||
                       (used_features_map.size() != qual_src_map_size) ||
                       (!anchored && t_now > qual_ema_t);
 
    if (dirty)
    {
        qual_src_key = newest_key;
        qual_src_size = newest_size;
        qual_src_map_size = used_features_map.size();
 
        // Pool every entry inside the union window, deduplicated by featid. One update's
        // used-feature list is a sparse, noisy sample of the rig's tracking health (MSCKF
        // features only enter it when their tracks END), and scoring single entries is what
        // made the published value jumpy. The pooled set is each camera's full recent track
        // set -- the same thing the synced batches effectively scored -- so healthy flight
        // saturates the sum at 100 exactly like the synced path did.
        const ov_core::Feature *uniq[QUAL_DEDUP_CAP];
        int n_uniq = 0;
        const double t_min = t_now - QUAL_UNION_WINDOW_S;
        for (auto it = used_features_map.rbegin(); it != used_features_map.rend() && it->first >= t_min; ++it)
        {
            n_uniq = collect_unique_features(it->second, uniq, n_uniq, QUAL_DEDUP_CAP);
        }
 
        // Which cameras have recent evidence?
        bool cam_present[QUAL_MAX_CAMS] = {};
        for (int fi = 0; fi < n_uniq; fi++)
        {
            for (const auto &cam_meas : uniq[fi]->timestamps)
            {
                const int cam_id = static_cast<int>(cam_meas.first);
                if (cam_id < 0 || cam_id >= QUAL_MAX_CAMS)
                    continue;
                auto uv_it = uniq[fi]->uvs_norm.find(cam_meas.first);
                if (uv_it != uniq[fi]->uvs_norm.end() && !uv_it->second.empty())
                    cam_present[cam_id] = true;
            }
        }
 
        // Per-camera scores over the union, sum-then-clamp -- exactly the synced fusion
        double fused = 0.0;
        for (int c = 0; c < QUAL_MAX_CAMS; c++)
        {
            if (!cam_present[c])
                continue;
            int n_slam = 0, n_msckf = 0;
            const double cam_score = score_camera_features(c, uniq, n_uniq, slam_features, state, n_slam, n_msckf);
            fused += cam_score;
 
            if (en_qual_debug)
                printf("[QUALITY_DEBUG] cam %d: %.1f (SLAM %d, MSCKF %d, union %d)\n",
                       c, cam_score, n_slam, n_msckf, n_uniq);
        }
        fused = std::max(0.0, std::min(100.0, fused));
 
        // Time-constant smoothing of the published value: a single weak update moves it by only
        // a couple of points (the synced metric hid this noise inside its 2x clamp headroom); a
        // real collapse still reaches the hysteresis BAD band in ~2*tau, well under the
        // 45-consecutive-sample requirement the machine itself imposes on top.
        if (qual_ema < 0.0)
        {
            qual_ema = fused;
        }
        else
        {
            const double dt = std::max(0.0, t_now - qual_ema_t);
            const double alpha = std::min(1.0, dt / QUAL_EMA_TAU_S);
            qual_ema += alpha * (fused - qual_ema);
        }
        qual_ema_t = t_now;
 
        if (en_qual_debug)
            printf("[QUALITY_DEBUG] fused %.1f -> ema %.1f (union %d feats, window %.0f ms)\n",
                   fused, qual_ema, n_uniq, QUAL_UNION_WINDOW_S * 1e3);
    }
 
    if (qual_ema < 0.0)
        return QUAL_FLOOR;
 
    return std::max(QUAL_FLOOR, std::min(100.0, qual_ema));
}
 
void Publisher::publishBlank()
{
    // Zero out the VIO packet
    memset(&vio_packet, 0, sizeof(vio_data_t));
    vio_packet.magic_number = VIO_MAGIC_NUMBER;
    vio_packet.timestamp_ns = _apps_time_monotonic_ns();
 
    // Set error codes for missing sensors
    uint32_t packet_error = 0;
    if (!is_imu_connected.load())
        packet_error |= ERROR_CODE_IMU_MISSING;
    if (!is_cam_connected.load())
        packet_error |= ERROR_CODE_CAM_MISSING;
    vio_packet.error_code = packet_error;
 
    // Indicate failed state
    vio_packet.quality = -1;
    vio_packet.state = VIO_STATE_FAILED;
 
    // Publish blank packet on simple channel
    pipe_server_write(SIMPLE_CH, (char *)&vio_packet, sizeof(vio_data_t));
}