/*
 * imu_accel.c
 *
 *  Created on: May 15, 2025
 *      Author: 911BL
 */
#include "app/imu_accel.h"
#include "drivers/imu_lsm6ds.h"


void imu_accel_init(void) {
//     Enable accelerometer in high-performance mode
    imu_lsm6ds_write_byte(CTRL1_XL, CTRL1_XL_HIGH_PERFORMANCE);
}

static int16_t imu_read_axis(imu_register_t low_reg, imu_register_t high_reg) {
    uint8_t low  = imu_lsm6ds_read_byte(low_reg);
    uint8_t high = imu_lsm6ds_read_byte(high_reg);
    return (int16_t)((high << 8) | low);
}

void imu_accel_read_all(int16_t* x, int16_t* y, int16_t* z) {
    *x = imu_read_axis(OUTX_L_XL, OUTX_H_XL);
    *y = imu_read_axis(OUTY_L_XL, OUTY_H_XL);
    *z = imu_read_axis(OUTZ_L_XL, OUTZ_H_XL);
}

