BNO085

-- Инициализация RVC на UART2
bno085.init_rvc(2, 17, 16)

-- С указанием скорости
bno085.init_rvc(2, 17, 16, 115200)

-- Инициализация SHTP на UART2
bno085.init_shtp(2, 17, 16)

-- С указанием скорости 1 МБод
bno085.init_shtp(2, 17, 16, 1000000)

-- Подключение I2C устройства
i2c_dev = i2c.attach(i2c.I2C0, i2c.MASTER, 400000)

-- Инициализация BNO085 через I2C
bno085.init_i2c(i2c_dev)

-- С указанием альтернативного адреса (SA0 = 1)
bno085.init_i2c(i2c_dev, bno085.I2C_ADDR_ALT)  -- 0x4B

-- Подключение SPI устройства
spi_dev = spi.attach(spi.SPI3, spi.MASTER, cs_pin, 3000000, 8, 0)

-- Инициализация BNO085 через SPI
bno085.init_spi(spi_dev)

-- Новый универсальный API
bno085.enable({accel=100, gyro=200, rotation=50, mag=10})

-- Включить только акселерометр и гироскоп
bno085.enable({accel=100, gyro=100})

-- Все доступные сенсоры
bno085.enable({
    accel = 100,           -- Акселерометр 100 Гц
    gyro = 200,            -- Гироскоп 200 Гц
    mag = 10,              -- Магнитометр 10 Гц
    linear_accel = 100,    -- Линейное ускорение
    gravity = 50,          -- Гравитация
    rotation = 100,        -- Вектор вращения
    game_rotation = 100,   -- Игровой вектор вращения
    geomag_rotation = 50,  -- Геомагнитный вектор
    step_counter = 1,      -- Счётчик шагов
    stability = 10,        -- Стабильность
    activity = 1           -- Активность
})

-- Старый API (для обратной совместимости)
bno085.enable(100, 200)  -- accel=100Hz, gyro=200Hz

bno085.init_rvc(2, 17, 16)

local data = bno085.read()
if data then
    print(string.format("Yaw: %.2f, Pitch: %.2f, Roll: %.2f",
        data.yaw, data.pitch, data.roll))
    print(string.format("Accel: %.3f, %.3f, %.3f",
        data.ax, data.ay, data.az))
end

bno085.init_shtp(2, 17, 16)
bno085.enable({accel=100, gyro=100, rotation=50})

local data = bno085.read()
if data then
    -- Акселерометр
    if data.ax then
        print(string.format("Accel: %.3f, %.3f, %.3f m/s²",
            data.ax, data.ay, data.az))
    end

    -- Гироскоп
    if data.gx then
        print(string.format("Gyro: %.4f, %.4f, %.4f rad/s",
            data.gx, data.gy, data.gz))
    end

    -- Кватернион вращения
    if data.qi then
        print(string.format("Rotation: i=%.4f j=%.4f k=%.4f r=%.4f",
            data.qi, data.qj, data.qk, data.qr))
    end
end

-- Чтение всех доступных пакетов
local packets = bno085.read_all()
print("Получено пакетов: " .. #packets)

for i, data in ipairs(packets) do
    if data.ax then
        print(string.format("[%d] Accel: %.3f, %.3f, %.3f",
            i, data.ax, data.ay, data.az))
    end
end

-- Чтение максимум 10 пакетов
local packets = bno085.read_all(10)

local stats = bno085.stats()

print("Статистика BNO085:")
print("  Получено: " .. stats.received)
print("  Валидных: " .. stats.valid)
print("  Ошибочных: " .. stats.invalid)
print("  Пропущено: " .. stats.sequence_gaps)
print("  Escape ошибок: " .. stats.escape_errors)
if stats.error_rate then
    print(string.format("  Уровень ошибок: %.2f%%", stats.error_rate))
end

-- Сброс сенсора
bno085.reset()

-- После сброса нужно заново включить сенсоры
bno085.enable({accel=100, gyro=100})

-- Очистка буфера перед началом чтения
bno085.flush()

-- Закрытие драйвера
bno085.close()

local mode = bno085.mode()
if mode then
    print("Текущий режим: " .. mode)
else
    print("Драйвер не инициализирован")
end

local iface = bno085.interface()
if iface then
    print("Текущий интерфейс: " .. iface)
else
    print("Драйвер не инициализирован")
end

-- Инициализация
bno085.init_rvc(2, 17, 16)

-- Цикл чтения
while true do
    local data = bno085.read()
    if data then
        print(string.format("Yaw=%.1f Pitch=%.1f Roll=%.1f",
            data.yaw, data.pitch, data.roll))
    end
    tmr.delayms(10)
end

-- Инициализация SHTP
bno085.init_shtp(2, 17, 16, 3000000)

-- Включение сенсоров
bno085.enable({
    accel = 200,
    gyro = 200,
    rotation = 100
})

-- Чтение с мониторингом статистики
local last_stats = 0
while true do
    local packets = bno085.read_all(50)

    for _, data in ipairs(packets) do
        if data.qi then
            -- Обработка кватерниона вращения
            local yaw = math.atan2(2*(data.qr*data.qk + data.qi*data.qj),
                                   1 - 2*(data.qj*data.qj + data.qk*data.qk))
            print(string.format("Yaw: %.2f rad", yaw))
        end
    end

    -- Периодический вывод статистики
    local now = os.time()
    if now - last_stats >= 5 then
        local stats = bno085.stats()
        print(string.format("Stats: valid=%d, errors=%d (%.2f%%)",
            stats.valid, stats.invalid, stats.error_rate or 0))
        last_stats = now
    end

    tmr.delayms(5)
end

bno085.init_shtp(2, 17, 16)
bno085.enable({activity=1, step_counter=1})

local activity_names = {
    [0] = "unknown",
    [1] = "in_vehicle",
    [2] = "on_bicycle",
    [3] = "on_foot",
    [4] = "still",
    [5] = "tilting",
    [6] = "walking",
    [7] = "running",
    [8] = "on_stairs"
}

while true do
    local data = bno085.read()
    if data then
        if data.activity then
            print("Активность: " .. (activity_names[data.activity] or "?"))
        end
        if data.steps then
            print("Шаги: " .. data.steps)
        end
    end
    tmr.delayms(100)
end

-- Подключение I2C (шина 0, 400 кГц)
local i2c_dev = i2c.attach(i2c.I2C0, i2c.MASTER, 400000)

-- Инициализация BNO085 через I2C
bno085.init_i2c(i2c_dev)

-- Включение сенсоров
bno085.enable({accel=100, gyro=100, rotation=50})

-- Проверка интерфейса
print("Интерфейс: " .. bno085.interface())  -- "i2c"

-- Цикл чтения
while true do
    local data = bno085.read()
    if data then
        if data.ax then
            print(string.format("Accel: %.3f, %.3f, %.3f",
                data.ax, data.ay, data.az))
        end
    end
    tmr.delayms(10)
end

-- Подключение SPI (шина 3, CS на GPIO 5, режим 0, 3 МГц)
local spi_dev = spi.attach(spi.SPI3, spi.MASTER, pio.GPIO5, 3000000, 8, 0)

-- Инициализация BNO085 через SPI
bno085.init_spi(spi_dev)

-- Включение сенсоров
bno085.enable({accel=200, gyro=200})

-- Проверка интерфейса
print("Интерфейс: " .. bno085.interface())  -- "spi"

-- Цикл чтения
while true do
    local packets = bno085.read_all(20)
    for _, data in ipairs(packets) do
        if data.gx then
            print(string.format("Gyro: %.4f, %.4f, %.4f rad/s",
                data.gx, data.gy, data.gz))
        end
    end
    tmr.delayms(5)
end