ImuMinimal.cpp

Go to the documentation of this file.

/**
 * @file ImuMinimal.cpp
 * @brief IMU interface and data handling implementation for VOXL OpenVINS
 * @author Joao Leonardo Silva Cotta (@zauberflote1)
 * @date 2025
 * @version 1.0
 *
 * This file implements the IMU interface and callback functions for handling
 * inertial measurement unit data in the VOXL OpenVINS system. It provides
 * the connection to the IMU service and data processing capabilities.
 *
 * The implementation provides:
 * - IMU data reception and validation
 * - Frame transformation for different IMU orientations
 * - Batch processing for efficient VIO integration
 * - Camera-IMU synchronization
 * - VIO state management and publishing
 * - Thread-safe operations with mutex protection
 */
 
#include "ImuMinimal.h"
#include <cstdio>
#include <state/State.h>
 
 
/** @brief Mutex for IMU data access synchronization */
std::mutex imu_lock_mutex;
 
/**
 * @brief Handler for incoming IMU data
 *
 * This callback processes incoming IMU data, updates the system state,
 * and triggers appropriate processing based on motion state. It serves
 * as the primary entry point for all IMU data processing in the system.
 *
 * The function performs the following operations:
 * - Validates and parses IMU data packets
 * - Updates frame transformation based on gravity direction
 * - Converts IMU data to OpenVINS format
 * - Performs batch processing to reduce mutex operations
 * - Feeds IMU data to the VIO manager
 * - Synchronizes camera data with IMU timestamps
 * - Triggers VIO processing and data publishing
 *
 * The function uses batch processing to minimize mutex contention
 * at high IMU frequencies and ensures proper temporal synchronization
 * between IMU and camera data.
 *
 * @param ch Channel number (unused)
 * @param data Pointer to IMU data buffer
 * @param bytes Size of data buffer in bytes
 * @param context Context pointer (unused)
 */
void _imu_data_handler_cb(int ch, char *data, int bytes, void *context)
{
    int64_t t_start = _apps_time_monotonic_ns();
    is_imu_connected.store(true, std::memory_order_release);
 
    // ---- validate data ----
    int n_packets = 0;
    imu_data_t *arr = pipe_validate_imu_data_t(data, bytes, &n_packets);
 
    active_callbacks.fetch_add(1, std::memory_order_acquire);
    // acquire: pairs with the release store that clears is_resetting at the end of a reset, so
    // the first post-reset callback also observes the bumped reset_num_counter -- a relaxed load
    // could pair the NEW VioManager with the publisher's OLD quality/hysteresis statics for one
    // packet (no happens-before edge from the reset thread otherwise)
    if (is_resetting.load(std::memory_order_acquire))
    {
        active_callbacks.fetch_sub(1, std::memory_order_release);
        return;
    }
    if (en_debug)
    {
        std::cout << "number of IMU packets: " << n_packets << std::endl;
    }
    if (!arr || n_packets <= 0)
    {
 
        vio_error_codes |= ERROR_CODE_IMU_MISSING;
        active_callbacks.fetch_sub(1, std::memory_order_release);
        return;
    }
 
    // ---- build batch ----
    std::vector<ov_core::ImuData> imu_batch;
    imu_batch.reserve(n_packets);
 
    for (int i = 0; i < n_packets; ++i)
    {
        frame_transform.update(arr[i]);
 
        ov_core::ImuData sample;
        sample.timestamp = arr[i].timestamp_ns * 1e-9;
        sample.wm = Eigen::Vector3d(arr[i].gyro_rad[0], arr[i].gyro_rad[1], arr[i].gyro_rad[2]);
        sample.am = Eigen::Vector3d(arr[i].accl_ms2[0], arr[i].accl_ms2[1], arr[i].accl_ms2[2]);
 
        if (last_imu_timestamp_ns != 0 &&
            (sample.timestamp * 1e9 <= last_imu_timestamp_ns))
        {
            int64_t time_diff = last_imu_timestamp_ns - (sample.timestamp * 1e9);
            if (time_diff > 1000000) // 1 ms
            {
                // No print here: this runs per-sample at IMU rate; HealthCheck reports the
                // BAD_TIMESTAMP code edge-triggered instead
                vio_error_codes |= ERROR_CODE_BAD_TIMESTAMP;
            }
        }
        imu_batch.push_back(std::move(sample));
    }
 
    if (imu_batch.empty())
    {
        vio_error_codes |= ERROR_CODE_DROPPED_IMU;
        if (active_callbacks.fetch_sub(1, std::memory_order_release) == 1)
        {
            std::lock_guard<std::mutex> lk(reset_mtx);
            reset_cv.notify_one();
        }
        return;
    }
 
    // ---- feed IMU (this also releases + processes every camera frame whose global ordering the
    // batch has decided: the lock-free per-camera ingest lives inside ov_msckf, and per-frame
    // cl_mem release + publishing run through the VoxlVioIngest callback on this same thread) ----
    int imu_batch_size = (imu_model == IMU_MODEL_BMI270) ? 800 : 330;
    if (frame_transform.is_initialized) vio_manager->feed_measurement_batch_imu(imu_batch, imu_batch_size);
    last_imu_timestamp_ns = imu_batch.back().timestamp * 1e9;
 
    // ---- finish ----
    int64_t t_end = _apps_time_monotonic_ns();
    double dt_ms = (double)(t_end - t_start) / 1e6;
    if (dt_ms > 32.0 && en_debug)
    {
        printf("WARNING: IMU callback took too long: %8.3f ms\n", dt_ms);
    }
 
    if (active_callbacks.fetch_sub(1, std::memory_order_release) == 1)
    {
        std::lock_guard<std::mutex> lk(reset_mtx);
        reset_cv.notify_one();
    }
}
 
/**
 * @brief Callback for IMU disconnect events
 *
 * This function is called when the IMU service disconnects. It handles
 * the cleanup and state management required when IMU data becomes unavailable.
 *
 * The function uses thread-safe mutex locking to ensure proper
 * state management during disconnection events.
 *
 * @param ch Channel number (unused)
 * @param context Context pointer (unused)
 */
void _imu_disconnect_cb(__attribute__((unused)) int ch,
                        __attribute__((unused)) void *context)
// THREAD SAFE DISCONNECT CALLBACK
{
    std::lock_guard<std::mutex> lg(imu_lock_mutex);
    if (!is_imu_connected.load())
        return;
    pipe_client_flush(IMU_CH);
    // Small delay to ensure pending operations complete
    usleep(50000);
    is_imu_connected.store(false, std::memory_order_release);
    return;
}
 
/**
 * @brief Creates IMU pipe client and associated callbacks
 *
 * This function sets up the disconnect and data handler callbacks,
 * and opens the client pipe connection to the IMU service. It initializes
 * the complete IMU data pipeline for the VIO system.
 *
 * The function performs the following operations:
 * - Sets up disconnect callback for graceful handling of service disconnection
 * - Sets up data handler callback for processing incoming IMU measurements
 * - Configures thread priority for high-frequency IMU processing
 * - Opens the client pipe connection to the IMU service
 * - Configures the pipe for optimal data flow
 * - Sets the IMU connection status flag
 *
 * @return 0 on success, -1 on failure
 */
int connect_imu_service(void)
{
    // MPA REGULAR CLIENT SETUP
 
    pipe_client_set_disconnect_cb(IMU_CH, _imu_disconnect_cb, NULL);
    pipe_client_set_simple_helper_cb(IMU_CH, _imu_data_handler_cb, NULL);
    pipe_client_set_helper_thread_priority(IMU_CH, THREAD_PRIORITY_RT_HIGH);
 
    int flags = CLIENT_FLAG_EN_SIMPLE_HELPER;
 
    if (pipe_client_open(IMU_CH, imu_name, PROCESS_NAME, flags,
                         IMU_RECOMMENDED_READ_BUF_SIZE * 2) != 0)
    {
        fprintf(stderr, "failed to open imu client pipe\n");
        vio_error_codes |= ERROR_CODE_IMU_MISSING;
        return -1;
    }
    is_imu_connected = true;
 
    return 0;
}