BNO080.hpp

#ifndef BNO080_HPP
#define BNO080_HPP

#include <cstdint>
#include <vector>
#include <string>

// BNO080 Constants
#define BNO080_DEFAULT_ADDRESS 0x4A
#define BNO080_I2C_BUS 1

// Channels
#define CHANNEL_COMMAND 0
#define CHANNEL_EXECUTABLE 1
#define CHANNEL_CONTROL 2
#define CHANNEL_REPORTS 3
#define CHANNEL_WAKE_REPORTS 4
#define CHANNEL_GYRO 5

// Report IDs
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05

struct IMUData {
    uint64_t timestamp_ns;
    float accel[3];
    float gyro[3];
    float mag[3];
    bool has_accel;
    bool has_gyro;
    bool has_mag;
};

class BNO080 {
public:
    BNO080();
    ~BNO080();

    bool begin(int i2c_bus = BNO080_I2C_BUS, int address = BNO080_DEFAULT_ADDRESS);
    
    // Enable a specific sensor report with a given interval in microseconds
    void enableReport(uint8_t reportId, uint32_t interval_us);
    
    // Check for new data and parse it. Returns true if a complete packet was parsed.
    bool update();
    
    // Get the latest data
    void getLatestData(IMUData& data);
    
    // Clear data flags (for manual sync)
    void clearFlags();

private:
    int i2c_fd;
    uint8_t sequence_number[6]; // Track sequence numbers per channel
    
    IMUData current_data;

    // SHTP Protocol Handling
    bool readPacket(uint8_t* buffer, size_t max_len, size_t* len, uint8_t* channel);
    bool sendPacket(uint8_t channel, uint8_t* data, size_t len);
    
    void parseInputReport(const uint8_t* data, size_t len);
    void parseCommandResponse(const uint8_t* data, size_t len);

    // Helpers
    float qToFloat(int16_t fixedPointValue, uint8_t qPoint);
};

#endif // BNO080_HPP