diff --git a/32blit-pico/sensor/lsm6ds3tr_c.cpp b/32blit-pico/sensor/lsm6ds3tr_c.cpp index 1b7ff85dc..85dd542b0 100644 --- a/32blit-pico/sensor/lsm6ds3tr_c.cpp +++ b/32blit-pico/sensor/lsm6ds3tr_c.cpp @@ -3,6 +3,7 @@ #include "hardware/i2c.h" +#include "math/constants.hpp" #include "engine/api_private.hpp" #ifndef LSM6DS3_ADDR @@ -12,12 +13,21 @@ static bool lsm6ds3_present = true; static blit::SensorDataVec3 accel_data = {nullptr, blit::SensorType::ACCELEROMETER, {}}; +static blit::SensorDataVec3 gyro_data = {nullptr, blit::SensorType::GYROSCOPE, {}}; enum LSM6DS3Reg { CTRL1_XL = 0x10, + CTRL2_G, STATUS_REG = 0x1E, + OUTX_L_G = 0x22, + OUTX_H_G, + OUTY_L_G, + OUTY_H_G, + OUTZ_L_G, + OUTZ_H_G, + OUTX_L_XL = 0x28, OUTX_H_XL, OUTY_L_XL, @@ -27,6 +37,7 @@ enum LSM6DS3Reg { }; void init_sensor() { + // init accelerometer uint8_t data[2]; data[0] = CTRL1_XL; data[1] = 4 << 4; // 104Hz normal mode @@ -35,7 +46,12 @@ void init_sensor() { return; } + // init gyroscope + data[0] = CTRL2_G; + i2c_write_blocking(i2c0, LSM6DS3_ADDR, data, 2, false); + blit::insert_api_sensor_data(&accel_data); + blit::insert_api_sensor_data(&gyro_data); } void update_sensor(uint32_t time) { @@ -59,4 +75,16 @@ void update_sensor(uint32_t time) { accel_data.data = new_tilt; } + + if(status & 2) { // GDA (new gyro data) + reg = OUTX_L_G; + i2c_write_blocking(i2c0, LSM6DS3_ADDR, ®, 1, true); + i2c_read_blocking(i2c0, LSM6DS3_ADDR, (uint8_t *)&data, 6, false); + + blit::Vec3 new_gyro(-data[1], -data[0], data[2]); + // apply scale and convert to rad/s + new_gyro *= (245.0f/*dps*/ / 32768) * (blit::pi / 180.0f); + + gyro_data.data = new_gyro; + } }