TransportReader.hpp

Go to the documentation of this file.

#pragma once
 
#include "transport_core/shared_transport.hpp"
 
#include <raccoon/Channels.h>
#include <raccoon/Options.h>
#include <raccoon/vector3f_t.hpp>
#include <raccoon/scalar_i8_t.hpp>
#include <raccoon/scalar_i32_t.hpp>
#include <raccoon/scalar_f_t.hpp>
#include <raccoon/quaternion_t.hpp>
#include <raccoon/string_t.hpp>
#include <string>
#include <unordered_map>
#include <mutex>
#include <atomic>
#include <functional>
#include <condition_variable>
 
namespace platform::wombat::core {
    class TransportReader {
    public:
        explicit TransportReader();
        ~TransportReader();
 
        static TransportReader& instance()
        {
            static TransportReader impl;
            return impl;
        }
 
        raccoon::scalar_i8_t readServoMode(int port);
        raccoon::scalar_f_t readServoValue(int port);

        raccoon::vector3f_t readGyro();
        raccoon::vector3f_t readAccel();
        raccoon::vector3f_t readLinearAccel();
        raccoon::vector3f_t readAccelVelocity();
        void resetAccelVelocity();
        raccoon::vector3f_t readMag();
        raccoon::scalar_f_t readHeading();
        raccoon::quaternion_t readQuaternion();
        raccoon::scalar_i32_t readBemf(int idx);
        int32_t readMotorPosition(int port);
        bool readMotorDone(int port);
 
        raccoon::scalar_i32_t readAnalog(int port);
        raccoon::scalar_i32_t readDigital(int port);
 
        raccoon::scalar_f_t readTemp();

        struct OdometrySnapshot {
            float pos_x{0.0f};
            float pos_y{0.0f};
            float heading{0.0f};
            float vx{0.0f};
            float vy{0.0f};
            float wz{0.0f};
        };
        OdometrySnapshot readOdometry();

        void resetOdometry();

        struct CalibOdometrySnapshot {
            float pos_x{0.0f};   
            float pos_y{0.0f};   
            float heading{0.0f}; 
            bool connected{false};
        };
        CalibOdometrySnapshot readCalibOdometry();

        bool isCalibBoardConnected();

        bool waitForOdometryReset(int timeout_ms = 500);

        bool waitForImuReady(int timeout_ms = 1000);

        bool waitForBemfData(int timeout_ms = 500);
 
        // Deprecated: callback mechanism removed, use getIntegratedVelocity via IIMU instead.
        void setLinearAccelCallback(std::function<void(float, float, float)> /*callback*/) {}
 
    private:
        libstp::transport_core::SharedTransport& transport_;
 
        // Cached copies of the most recent transport data.
        std::mutex cache_mutex_;
        std::unordered_map<int, int8_t> servo_mode_cache_;
        std::unordered_map<int, float> servo_value_cache_;
        std::unordered_map<int, int32_t> bemf_cache_;
        std::unordered_map<int, int32_t> motor_position_cache_;
        std::unordered_map<int, int32_t> motor_done_cache_;
        std::unordered_map<int, int32_t> analog_cache_;
        std::unordered_map<int, int32_t> digital_cache_;
 
        raccoon::vector3f_t gyro_cache_{};
        raccoon::vector3f_t accel_cache_{};
        raccoon::vector3f_t linear_accel_cache_{};
        raccoon::vector3f_t accel_velocity_cache_{};
        raccoon::vector3f_t accel_velocity_offset_{};
        raccoon::vector3f_t mag_cache_{};
        raccoon::scalar_f_t heading_cache_{};
        raccoon::quaternion_t quaternion_cache_{};
        raccoon::scalar_f_t temp_cache_{};
 
        // STM32 odometry cache
        OdometrySnapshot odom_cache_{};
        std::condition_variable odom_cv_;
 
        // External calibration-board odometry cache (raw board units: cm / deg).
        float calib_pos_x_cm_{0.0f};
        float calib_pos_y_cm_{0.0f};
        float calib_heading_deg_{0.0f};
        // Board attached per status/board channel.
        std::atomic<bool> calib_board_attached_{false};
        // At least one odometry sample has arrived since startup.
        std::atomic<bool> calib_odom_received_{false};
 
        // Track whether real IMU heading data has been received.
        std::atomic<bool> imu_heading_received_{false};
 
        // Track whether real BEMF telemetry has been received from the STM32.
        std::atomic<bool> bemf_received_{false};
 
    };
 
    enum class MotorDir : uint8_t { Off = 0b00, CCW = 0b01, CW = 0b10, ServoLike = 0b11 };
 
    enum class ServoMode : uint8_t { FullyDisabled = 0, Disabled = 1, Enabled = 2 };
}