VoxlVars.h

Go to the documentation of this file.

/**
 * @file VoxlVars.h
 * @brief Global variable declarations and constants for VOXL OpenVINS server
 * @author Joao Leonardo Silva Cotta (@zauberflote1)
 * @date 2025
 * @version 1.0
 *
 * This header file declares all global variables, constants, and configuration
 * parameters used throughout the VOXL OpenVINS server. It provides centralized
 * management of system state, configuration, and operational parameters.
 *
 * The file contains:
 * - Pipe channel definitions and constants
 * - Global state variables for VIO operation
 * - Configuration parameters for auto-reset and error handling
 * - Sensor-specific variables and synchronization primitives
 * - Image processing and camera management variables
 */
 
#ifndef VOXL_VARS_H
#define VOXL_VARS_H
 
#pragma once
 
// Standard includes
#include <memory>
#include <atomic>
#include <string>
#include <vector>
#include <mutex>
#include <cstdint>
#include <condition_variable>
 
// Third-party includes
#include <core/VioManager.h>
#include <modal_flow/StereoMatcher.hpp>
#include <core/VioManagerOptions.h>
#include <modal_pipe.h>
#include <modal_json.h>
#include <voxl_common_config.h>
 
// Local includes
#include "VoxlCommon.h"
// FOR NOW WE FORCE TO SUBSAMPLE IT ALWAYS TO CHECK THE EFFECTS OF IT
#define GYRO_VAR_THRESHOLD 0.09f     ///< Gyro variance threshold for motion detection
#define ACC_VAR_THRESHOLD 0.09f      ///< Accelerometer variance threshold for motion detection
 
// ============================================================================
// DATA STRUCTURES
// ============================================================================
namespace voxl
{
    /**
     * @struct img_ringbuf_packet
     * @brief Structure to hold image data packet for the ring buffer
     *
     * This structure encapsulates all the data needed for a single image frame,
     * including camera ID, metadata, and the actual pixel data.
     */
    typedef struct img_ringbuf_packet
    {
        int camid;                            ///< Camera identifier
        camera_image_metadata_t metadata;     ///< Image metadata (timestamp, format, etc.)
        uint8_t image_pixels[MAX_IMAGE_SIZE]; ///< Raw image pixel data
    } img_ringbuf_packet;
 
    enum class ImageType
    {
        CV_MAT,
        CL_MEM
    };
 
    /**
     * @struct FrameTransform
     * @brief Structure for handling IMU frame transformations
     *
     * This structure handles the transformation of IMU data to accommodate
     * different IMU mounting orientations. It automatically detects the gravity
     * axis and direction to compute the appropriate correction matrix.
     */
    struct FrameTransform
    {
        // WE NEED TO FIND THE AXIS WHERE GRAVITY IS MOST PREDOMINANT AND ITS DIRECTION
        // THEN WE CAN COMPUTE THE CORRECTION MATRIX
        enum class Axis
        {
            X,
            Y,
            Z
        };
        enum class Direction
        {
            POSITIVE,
            NEGATIVE
        };
 
        enum class JerkOption
        {
            ACCEL_ONLY,
            GYRO_ONLY,
            ACCEL_AND_GYRO,
            NONE
        };
 
        // ASSUME IMU IS MOUNTED ALIGNED WITH THE BODY FRAME
        Axis gravity_axis{Axis::Z};
        Direction gravity_direction{Direction::NEGATIVE};
        bool is_initialized{false};
        // JERK DETECTION
        Eigen::Matrix3d correction_matrix{Eigen::Matrix3d::Identity()};
        Eigen::Vector3d avg_acc_1t0{Eigen::Vector3d::Zero()};
        Eigen::Vector3d avg_gyro_1t0{Eigen::Vector3d::Zero()};
        Eigen::Vector3d avg_acc_2t1{Eigen::Vector3d::Zero()};
        Eigen::Vector3d avg_gyro_2t1{Eigen::Vector3d::Zero()};
        float expected_total_samples{1024.0f};
        float current_total_samples{0.0f};
        std::deque<Eigen::Vector3d> acc1t0_samples;
        std::deque<Eigen::Vector3d> gyro1t0_samples;
        std::deque<Eigen::Vector3d> acc2t1_samples;
        std::deque<Eigen::Vector3d> gyro2t1_samples;
        JerkOption jerk_opt{JerkOption::ACCEL_ONLY}; // FOR NOW WE FORCE TO USE ACCELEROMETER ONLY TO CHECK THE EFFECTS OF IT
        // PROVE THE ASSUMPTION
        void update(const imu_data_t &data);
 
        void detectJerk(const imu_data_t &data);
 
        void resetJerkDetection();
 
        Eigen::Vector3d transform(const Eigen::Vector3d &v) const
        {
            return correction_matrix * v;
        }
    };
}
 
// ============================================================================
// SERVER CHANNEL DEFINITIONS
// ============================================================================
 
/**
 * @brief MPA server channel for extended VIO data output
 *
 * Channel used for publishing comprehensive VIO data including
 * full state information, covariance matrices, and detailed tracking data.
 */
#define EXTENDED_CH 0
 
/**
 * @brief MPA server channel for simple VIO data output
 *
 * Channel used for publishing simplified VIO data with essential
 * pose and velocity information for basic applications.
 */
#define SIMPLE_CH 1
 
/**
 * @brief MPA server channel for status information
 *
 * Channel used for publishing system status, health information,
 * and operational state data.
 */
#define OVSTATUS_CH 3
 
// ============================================================================
// CLIENT CHANNEL DEFINITIONS
// ============================================================================
 
/**
 * @brief MPA client channel for IMU data input
 *
 * Channel used for receiving inertial measurement unit data
 * from the IMU service.
 */
#define IMU_CH 0
 
// ============================================================================
// PIPE NAMES AND LOCATIONS
// ============================================================================
 
/**
 * @brief Control commands available for VIO system
 *
 * Comma-separated list of control commands that can be sent
 * to the VIO system for operational control.
 */
#define OV_VIO_CONTROL_COMMANDS (RESET_VIO_SOFT "," RESET_VIO_HARD)
 
/**
 * @brief Name for extended VIO data pipe
 *
 * Pipe name for publishing comprehensive VIO data including
 * full state information and covariance matrices.
 */
#define OV_VIO_EXTENDED_NAME "ov_extended"
 
/**
 * @brief Location for extended VIO data pipe
 *
 * Full path location for the extended VIO data pipe.
 */
#define OV_VIO_EXTENDED_LOCATION MODAL_PIPE_DEFAULT_BASE_DIR OV_VIO_EXTENDED_NAME "/"
 
/**
 * @brief Name for simple VIO data pipe
 *
 * Pipe name for publishing simplified VIO data with essential
 * pose and velocity information.
 */
#define OV_VIO_SIMPLE_NAME "ov"
 
/**
 * @brief Location for simple VIO data pipe
 *
 * Full path location for the simple VIO data pipe.
 */
#define OV_VIO_SIMPLE_LOCATION MODAL_PIPE_DEFAULT_BASE_DIR OV_VIO_SIMPLE_NAME "/"
 
/**
 * @brief Name for status information pipe
 *
 * Pipe name for publishing system status and health information.
 */
#define OV_STATUS_NAME "ov_status"
 
/**
 * @brief Location for status information pipe
 *
 * Full path location for the status information pipe.
 */
#define OV_STATUS_LOCATION MODAL_PIPE_DEFAULT_BASE_DIR OV_STATUS_NAME "/"
 
// ============================================================================
// SYSTEM CONSTANTS
// ============================================================================
 
/**
 * @brief Process name for the VOXL OpenVINS server
 *
 * Used for process identification, logging, and system integration.
 */
#define PROCESS_NAME "voxl-open-vins-server"
 
/**
 * @brief Maximum number of cameras supported by VOXL2
 *
 * Hardware limitation for the number of cameras that can be
 * simultaneously processed by the VOXL2 platform.
 */
#define MAX_CAM_CNT 6
 
/**
 * @brief Standard character buffer size
 *
 * Default buffer size for string operations and name storage.
 */
#define CHAR_BUF_SIZE 128
 
// ============================================================================
// CORE VIO MANAGER
// ============================================================================
 
/** @brief VIO manager options
 *
 * Options for configuring the VIO manager instance.
 * This includes settings for state initialization, estimator options,
 * and other operational parameters.
 */
extern ov_msckf::VioManagerOptions vio_manager_options;
 
/**
 * @brief Global VIO manager instance
 *
 * Main VIO manager that handles the complete visual-inertial
 * odometry pipeline. This is the central component that processes
 * camera and IMU data to estimate pose and velocity.
 */
extern std::unique_ptr<ov_msckf::VioManager> vio_manager;
 
// ============================================================================
// ATOMIC STATE VARIABLES
// ============================================================================
 
/**
 * @brief Main server running state
 *
 * Volatile flag indicating whether the main server loop is running.
 * Used for graceful shutdown and state management.
 */
extern volatile int main_running;
 
/**
 * @brief Current VIO system state
 *
 * Atomic variable representing the current operational state
 * of the VIO system (initializing, running, error, etc.).
 */
extern std::atomic<uint8_t> vio_state;
 
/**
 * @brief VIO error codes
 *
 * Atomic variable containing error codes and flags for
 * various VIO system errors and warnings.
 * Uses uint32_t to accommodate all error codes (up to bit 21).
 */
extern std::atomic<uint32_t> vio_error_codes;
 
/** @brief Should reset floag
 *
 * Flag indicating that system should reset
 */
extern std::atomic<bool> reset_requested;
 
/** Flag indicating that a front-end-preserving SOFT reset was requested */
extern std::atomic<bool> soft_reset_requested;
 
/**
 * @brief VIO reset state flag
 *
 * Atomic flag indicating whether the VIO system is currently
 * undergoing a reset operation.
 */
extern std::atomic<bool> is_resetting;
 
/**
 * @brief Counter which increments on resets
 *
 * The counter is used to track the number of resets that have occurred.
 * This can be useful for debugging, logging, and ensuring that
 * reset operations are being performed as expected.
 */
extern std::atomic<uint32_t> reset_num_counter;
 
/**
 * @brief Counter which increments only on SOFT (front-end-preserving) resets.
 *
 * Lets field logs distinguish soft resets from hard ones (hard count =
 * reset_num_counter - soft_reset_num_counter) without a packet-ABI change.
 */
extern std::atomic<uint32_t> soft_reset_num_counter;
 
/**
 * @brief Number of callbacks inside the system
 *
 * Atomic counter for tracking the number of in-flight
 * callbacks or operations currently being processed that need
 * to be accounted for during reset.
 */
extern std::atomic<uint32_t> active_callbacks;
 
/**
 * @brief Mutex for reset
 *
 * Synchronisation mutex used *only* for the reset hand-off
 */
extern std::mutex reset_mtx;
 
/**
 * @brief Reset conditional variable
 *
 * Condition variable used to synchronize reset operations,
 * the reset thread will wait on active_callbacks to reach zero before proceeding.
 * */
extern std::condition_variable reset_cv;
 
/**
 * @brief Camera connection state
 *
 * Atomic flag indicating whether camera services are connected
 * and providing data.
 */
extern std::atomic<bool> is_cam_connected;
 
/**
 * @brief IMU connection state
 *
 * Atomic flag indicating whether the IMU service is connected
 * and providing data.
 */
extern std::atomic<bool> is_imu_connected;
 
// ============================================================================
// AUTO-RESET CONFIGURATION VARIABLES
// ============================================================================
 
/**
 * @brief Enable automatic VIO reset
 *
 * Flag to enable automatic reset of the VIO system when
 * certain error conditions are detected.
 */
extern int en_auto_reset;
 
/**
 * @brief Maximum velocity threshold for auto-reset
 *
 * Velocity threshold above which the system will trigger
 * an automatic reset if other conditions are met.
 */
extern float auto_reset_max_velocity;
 
/**
 * @brief Timeout for velocity covariance auto-reset
 *
 * Duration in seconds that the velocity covariance must exceed
 * the threshold before triggering an auto-reset.
 */
extern float auto_reset_max_v_cov_timeout_s;
 
/**
 * @brief Minimum number of features for auto-reset
 *
 * Minimum number of tracked features required to avoid
 * triggering an automatic reset.
 */
extern int auto_reset_min_features;
 
/**
 * @brief Timeout for minimum features auto-reset
 *
 * Duration in seconds that the feature count must be below
 * the minimum before triggering an auto-reset.
 */
extern float auto_reset_min_feature_timeout_s;
 
/**
 * @brief Grace period timeout after entering OK state
 *
 * Duration in seconds after entering the OK state during which
 * quality is held low to avoid bad initialization.
 */
extern float ok_state_grace_timeout_s;
 
/**
 * @brief Fast yaw threshold
 *
 * Threshold for detecting rapid yaw changes that
 * may indicate tracking issues.
 */
extern float fast_yaw_thresh;
 
/**
 * @brief Fast yaw timeout
 *
 * Duration in seconds for fast yaw change detection
 * before triggering corrective actions.
 */
extern float fast_yaw_timeout_s;
 
// ============================================================================
// QUALITY HYSTERESIS THRESHOLD CONFIGURATION
// ============================================================================
 
/**
 * @brief Quality low threshold for INITIAL state
 *
 * Quality threshold below which degradation is detected in INITIAL state.
 * When quality drops to or below this value, consecutive sample counter increments.
 */
extern int quality_low_thresh_initial;
 
/**
 * @brief Quality low threshold for GOOD state
 *
 * Quality threshold below which degradation is detected in GOOD state.
 * When quality drops to or below this value, consecutive sample counter increments.
 */
extern int quality_low_thresh_good;
 
/**
 * @brief Quality high threshold for recovery
 *
 * Quality threshold above which recovery is detected.
 * Used for transitions from INITIAL→GOOD and BAD→GOOD states.
 */
extern int quality_high_thresh;
 
/**
 * @brief Consecutive samples for INITIAL→BAD transition
 *
 * Number of consecutive samples with quality below low threshold
 * required to transition from INITIAL to BAD state.
 */
extern int quality_initial_to_bad_count;
 
/**
 * @brief Consecutive samples for INITIAL→GOOD transition
 *
 * Number of consecutive samples with quality above high threshold
 * required to transition from INITIAL to GOOD state.
 */
extern int quality_initial_to_good_count;
 
/**
 * @brief Consecutive samples for BAD→GOOD transition
 *
 * Number of consecutive samples with quality above high threshold
 * required to transition from BAD to GOOD state.
 */
extern int quality_bad_to_good_count;
 
/**
 * @brief Consecutive samples for GOOD→BAD transition
 *
 * Number of consecutive samples with quality below low threshold
 * required to transition from GOOD to BAD state.
 */
extern int quality_good_to_bad_count;
 
/**
 * @brief Using stereo camera configuration
 *
 * Flag to indicate whether the system is configured
 * to use stereo cameras for depth estimation.
 */
 
extern int using_stereo;
 
/**
 * @brief Base folder for yaml configuration files
 *
 * Base folder for storing YAML configuration files
 * for the VINS.
 */
extern char folder_base[CHAR_BUF_SIZE];
/**
 * @brief Enable debug output
 *
 * Flag to enable detailed debug output and logging
 * for development and troubleshooting.
 */
extern int en_debug;
 
/**
 * @brief Enable verbose output
 *
 * Flag to enable verbose logging and detailed status information
 * for monitoring and debugging purposes.
 */
extern int en_verbose;
 
/**
 * @brief Configuration only mode
 *
 * Flag to enable configuration-only mode where the server
 * only loads and validates configuration without starting
 * the main processing loop.
 */
extern int config_only;
 
/**
 * @brief Enable IMU body measurements
 *
 * Flag to enable the use of IMU body measurements in
 * the VIO system for enhanced state estimation.
 */
extern bool en_imu_body;
 
// ============================================================================
// SENSOR CONFIGURATION VARIABLES
// ============================================================================
 
/**
 * @brief IMU service name
 *
 * Name of the IMU service to connect to for
 * inertial measurement data.
 */
extern char imu_name[64];
 
/**
 * @brief Enumeration of supported IMU models
 */
enum imu_model_t {
    IMU_MODEL_UNKNOWN = 0,
    IMU_MODEL_ICM42688,
    IMU_MODEL_BMI270,
};
 
/**
 * @brief Active IMU model detected from pipe info JSON
 */
extern imu_model_t imu_model;
 
/**
 * @brief Camera information vector
 *
 * Vector containing configuration and calibration information
 * for all cameras in the system.
 */
extern std::vector<cam_info> cam_info_vec;
 
// ============================================================================
// IMU-SPECIFIC VARIABLES
// ============================================================================
 
/**
 * @brief Last IMU timestamp in nanoseconds
 *
 * Timestamp of the most recent IMU measurement.
 * Used for synchronization and timing validation.
 */
extern volatile int64_t last_imu_timestamp_ns;
 
/**
 * @brief Mutex for IMU data access synchronization
 *
 * Mutex used to synchronize access to IMU data
 * between multiple threads.
 */
extern std::mutex imu_lock_mutex;
 
/**
 * @brief Server-side IMU frame-transform bring-up gravity gate (degrees)
 *
 * If >= 0, the server holds the sensor feed to the estimator until the measured gravity tilt is within
 * this angle (opt-in "must be level" boot gate). Default -1 => any attitude (feed immediately), which is
 * required for the ceres-free S2 dynamic initializer to (re)init/reset at any attitude. Keep in sync with
 * (and generally >=) the estimator's init_gravity_max_angle.
 */
extern float imu_init_max_gravity_angle_deg;
 
/**
 * @brief Global frame transform instance
 *
 * Global instance of the frame transform structure used for
 * handling IMU frame transformations across the system.
 */
extern voxl::FrameTransform frame_transform;
 
/**
 * @brief Flag indicating if accelerometer jerk is detected
 */
extern std::atomic<bool> has_acc_jerk;
 
/**
 * @brief Non-static flag for jerk detection
 */
extern std::atomic<bool> non_static;
 
// ============================================================================
// CAMERA-SPECIFIC VARIABLES
// ============================================================================
 
/**
 * @brief Last camera timestamp
 *
 * Timestamp of the most recent camera measurement.
 * Used for synchronization and timing validation.
 */
extern volatile int64_t last_cam_time;
 
/**
 * @brief Number of cameras currently in use
 *
 * Count of cameras that are currently active
 * and providing data to the VIO system.
 */
extern int cameras_used;
 
/**
 * @brief Altitude z
 *
 * Atomic variable for the altitude z
 */
extern std::atomic<float> alt_z;
 
/** @brief Takeoff altitude threshold */
extern float takeoff_alt_threshold;
 
/** @brief Occlude stereo left */
extern bool occlude_stereo_left;
 
/** @brief Occlude stereo right */
extern bool occlude_stereo_right;
 
// ============================================================================
// PIPE CHANNEL VARIABLES
// ============================================================================
 
/**
 * @brief Camera pipe channel array
 *
 * Array containing the pipe channel numbers for each camera.
 * Used for managing camera data connections.
 */
extern int camera_pipe_channels[MAX_CAM_CNT];
 
// ============================================================================
// IMAGE PROCESSING VARIABLES
// ============================================================================
 
// ============================================================================
// SETUP EXPERIMENTAL VARIABLES
// ============================================================================
 
extern bool sync_config; ///< Flag to indicate if configuration synchronization is enabled
 
// ---- stereo-matcher config (read by VoxlConfigure, applied in VoxlSpinner) ----
// IMPORTANT: VoxlSpinner does `vio_manager_options = VioManagerOptions()` after
// VoxlConfigure runs, which wipes any direct stash into vio_manager_options.
// So we keep the packed calib here in VoxlVars (survives the reset) and copy
// it into vio_manager_options after the reset+YAML load -- see VoxlSpinner.cpp.
extern float stereo_z_min;                        ///< near depth bound for epipolar search (m)
extern float stereo_z_max;                        ///< far depth bound for epipolar search (m)
extern modal_flow::StereoCalib g_stereo_calib;    ///< composed by VoxlConfigure
extern bool                    g_stereo_calib_valid;
 
#endif // VOXL_VARS_H