132 lines
4.3 KiB
C
132 lines
4.3 KiB
C
#include "l3g4200d.h"
|
|
|
|
#include <stddef.h>
|
|
|
|
#define L3G4200D_REG_WHO_AM_I 0x0F
|
|
#define L3G4200D_REG_CTRL1 0x20
|
|
#define L3G4200D_REG_CTRL2 0x21
|
|
#define L3G4200D_REG_CTRL3 0x22
|
|
#define L3G4200D_REG_CTRL4 0x23
|
|
#define L3G4200D_REG_CTRL5 0x24
|
|
#define L3G4200D_REG_OUT_X_L 0x28
|
|
|
|
#define L3G4200D_WHO_AM_I_VALUE 0xD3
|
|
#define L3G4200D_CTRL1_100_HZ 0x1F
|
|
#define L3G4200D_CTRL4_500_DPS 0x90
|
|
#define L3G4200D_AUTO_INCREMENT 0x80
|
|
#define L3G4200D_TIMEOUT_MS 100
|
|
|
|
static esp_err_t read_registers(i2c_master_dev_handle_t device, uint8_t start_register,
|
|
uint8_t *data, size_t length)
|
|
{
|
|
return i2c_master_transmit_receive(device, &start_register, 1, data, length,
|
|
L3G4200D_TIMEOUT_MS);
|
|
}
|
|
|
|
static esp_err_t write_register(i2c_master_dev_handle_t device, uint8_t reg, uint8_t value)
|
|
{
|
|
const uint8_t bytes[] = {reg, value};
|
|
return i2c_master_transmit(device, bytes, sizeof(bytes), L3G4200D_TIMEOUT_MS);
|
|
}
|
|
|
|
static esp_err_t verify_register(i2c_master_dev_handle_t device, uint8_t reg, uint8_t expected)
|
|
{
|
|
uint8_t actual = 0;
|
|
esp_err_t err = read_registers(device, reg, &actual, 1);
|
|
if (err != ESP_OK) {
|
|
return err;
|
|
}
|
|
return actual == expected ? ESP_OK : ESP_ERR_INVALID_RESPONSE;
|
|
}
|
|
|
|
esp_err_t l3g4200d_init(l3g4200d_t *sensor, i2c_master_bus_handle_t bus, uint32_t bus_speed_hz)
|
|
{
|
|
if (sensor == NULL || bus == NULL) {
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
*sensor = (l3g4200d_t){0};
|
|
static const uint8_t candidate_addresses[] = {0x69, 0x68};
|
|
|
|
for (size_t i = 0; i < sizeof(candidate_addresses); ++i) {
|
|
const i2c_device_config_t config = {
|
|
.dev_addr_length = I2C_ADDR_BIT_LEN_7,
|
|
.device_address = candidate_addresses[i],
|
|
.scl_speed_hz = bus_speed_hz,
|
|
};
|
|
|
|
i2c_master_dev_handle_t device = NULL;
|
|
esp_err_t err = i2c_master_bus_add_device(bus, &config, &device);
|
|
if (err != ESP_OK) {
|
|
return err;
|
|
}
|
|
|
|
uint8_t device_id = 0;
|
|
err = read_registers(device, L3G4200D_REG_WHO_AM_I, &device_id, 1);
|
|
if (err == ESP_OK && device_id == L3G4200D_WHO_AM_I_VALUE) {
|
|
sensor->device = device;
|
|
sensor->address = candidate_addresses[i];
|
|
break;
|
|
}
|
|
|
|
i2c_master_bus_rm_device(device);
|
|
}
|
|
|
|
if (sensor->device == NULL) {
|
|
return ESP_ERR_NOT_FOUND;
|
|
}
|
|
|
|
// Configure control registers before enabling normal mode in CTRL1.
|
|
esp_err_t err = write_register(sensor->device, L3G4200D_REG_CTRL1, 0x00);
|
|
if (err == ESP_OK) {
|
|
err = write_register(sensor->device, L3G4200D_REG_CTRL2, 0x00);
|
|
}
|
|
if (err == ESP_OK) {
|
|
err = write_register(sensor->device, L3G4200D_REG_CTRL3, 0x00);
|
|
}
|
|
if (err == ESP_OK) {
|
|
// Block data updates while a six-byte sample is being read; select +/-500 dps.
|
|
err = write_register(sensor->device, L3G4200D_REG_CTRL4, L3G4200D_CTRL4_500_DPS);
|
|
}
|
|
if (err == ESP_OK) {
|
|
err = write_register(sensor->device, L3G4200D_REG_CTRL5, 0x00);
|
|
}
|
|
if (err == ESP_OK) {
|
|
// 100 Hz ODR, 25 Hz bandwidth, normal mode, all axes enabled.
|
|
err = write_register(sensor->device, L3G4200D_REG_CTRL1, L3G4200D_CTRL1_100_HZ);
|
|
}
|
|
if (err == ESP_OK) {
|
|
err = verify_register(sensor->device, L3G4200D_REG_CTRL1, L3G4200D_CTRL1_100_HZ);
|
|
}
|
|
if (err == ESP_OK) {
|
|
err = verify_register(sensor->device, L3G4200D_REG_CTRL4, L3G4200D_CTRL4_500_DPS);
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
esp_err_t l3g4200d_read_raw(const l3g4200d_t *sensor, l3g4200d_sample_t *sample)
|
|
{
|
|
if (sensor == NULL || sensor->device == NULL || sample == NULL) {
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
uint8_t data[6] = {0};
|
|
uint8_t start_register = L3G4200D_REG_OUT_X_L | L3G4200D_AUTO_INCREMENT;
|
|
esp_err_t err = read_registers(sensor->device, start_register, data, sizeof(data));
|
|
if (err != ESP_OK) {
|
|
return err;
|
|
}
|
|
|
|
sample->x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8));
|
|
sample->y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8));
|
|
sample->z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8));
|
|
return ESP_OK;
|
|
}
|
|
|
|
uint8_t l3g4200d_address(const l3g4200d_t *sensor)
|
|
{
|
|
return sensor != NULL ? sensor->address : 0;
|
|
}
|
|
|