Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

drivers/mpu9x50: clean up code #19657

Merged
merged 1 commit into from
May 24, 2023
Merged

drivers/mpu9x50: clean up code #19657

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
145 changes: 63 additions & 82 deletions drivers/mpu9x50/mpu9x50.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -236,28 +236,19 @@ int mpu9x50_set_compass_power(mpu9x50_t *dev, mpu9x50_pwr_t pwr_conf)
return 0;
}

static inline uint32_t gyro_fsr_enum_to_value(mpu9x50_gyro_ranges_t fsr)
{
return 250U << fsr;
}

int mpu9x50_read_gyro(const mpu9x50_t *dev, mpu9x50_results_t *output)
{
uint8_t data[6];
int16_t temp;
float fsr;

switch (dev->conf.gyro_fsr) {
case MPU9X50_GYRO_FSR_250DPS:
fsr = 250.0;
break;
case MPU9X50_GYRO_FSR_500DPS:
fsr = 500.0;
break;
case MPU9X50_GYRO_FSR_1000DPS:
fsr = 1000.0;
break;
case MPU9X50_GYRO_FSR_2000DPS:
fsr = 2000.0;
break;
default:
return -2;
}

assert((unsigned)dev->conf.gyro_fsr <= (unsigned)MPU9X50_GYRO_FSR_2000DPS);

uint32_t fsr = gyro_fsr_enum_to_value(dev->conf.gyro_fsr);

/* Acquire exclusive access */
i2c_acquire(DEV_I2C);
Expand All @@ -267,38 +258,29 @@ int mpu9x50_read_gyro(const mpu9x50_t *dev, mpu9x50_results_t *output)
i2c_release(DEV_I2C);

/* Normalize data according to configured full scale range */
temp = (data[0] << 8) | data[1];
temp = byteorder_lebuftohs(&data[0]);
output->x_axis = (temp * fsr) / MAX_VALUE;
temp = (data[2] << 8) | data[3];
temp = byteorder_lebuftohs(&data[2]);
output->y_axis = (temp * fsr) / MAX_VALUE;
temp = (data[4] << 8) | data[5];
temp = byteorder_lebuftohs(&data[4]);
output->z_axis = (temp * fsr) / MAX_VALUE;

return 0;
}

static inline uint32_t accel_fsr_enum_to_value(mpu9x50_accel_ranges_t fsr)
{
return 2000U << fsr;
}

int mpu9x50_read_accel(const mpu9x50_t *dev, mpu9x50_results_t *output)
{
uint8_t data[6];
int16_t temp;
float fsr;

switch (dev->conf.accel_fsr) {
case MPU9X50_ACCEL_FSR_2G:
fsr = 2000.0;
break;
case MPU9X50_ACCEL_FSR_4G:
fsr = 4000.0;
break;
case MPU9X50_ACCEL_FSR_8G:
fsr = 8000.0;
break;
case MPU9X50_ACCEL_FSR_16G:
fsr = 16000.0;
break;
default:
return -2;
}

assert((unsigned)dev->conf.accel_fsr <= (unsigned)MPU9X50_ACCEL_FSR_16G);

uint32_t fsr = accel_fsr_enum_to_value(dev->conf.accel_fsr);

/* Acquire exclusive access */
i2c_acquire(DEV_I2C);
Expand All @@ -308,16 +290,25 @@ int mpu9x50_read_accel(const mpu9x50_t *dev, mpu9x50_results_t *output)
i2c_release(DEV_I2C);

/* Normalize data according to configured full scale range */
temp = (data[0] << 8) | data[1];
temp = byteorder_lebuftohs(&data[0]);
output->x_axis = (temp * fsr) / MAX_VALUE;
temp = (data[2] << 8) | data[3];
temp = byteorder_lebuftohs(&data[2]);
output->y_axis = (temp * fsr) / MAX_VALUE;
temp = (data[4] << 8) | data[5];
temp = byteorder_lebuftohs(&data[4]);
output->z_axis = (temp * fsr) / MAX_VALUE;

return 0;
}

static int16_t convert_magnometer(int16_t raw, uint8_t adjust)
{
int32_t tmp = raw;
/* Compute sensitivity adjustment */
tmp += tmp * ((int32_t)adjust - 128) / 256;
/* return normalized data */
return (tmp * 3) / 10;
}

int mpu9x50_read_compass(const mpu9x50_t *dev, mpu9x50_results_t *output)
{
uint8_t data[6];
Expand All @@ -329,22 +320,12 @@ int mpu9x50_read_compass(const mpu9x50_t *dev, mpu9x50_results_t *output)
/* Release the bus */
i2c_release(DEV_I2C);

output->x_axis = (data[1] << 8) | data[0];
output->y_axis = (data[3] << 8) | data[2];
output->z_axis = (data[5] << 8) | data[4];

/* Compute sensitivity adjustment */
output->x_axis = (int16_t) (((float)output->x_axis) *
((((dev->conf.compass_x_adj - 128) * 0.5) / 128.0) + 1));
output->y_axis = (int16_t) (((float)output->y_axis) *
((((dev->conf.compass_y_adj - 128) * 0.5) / 128.0) + 1));
output->z_axis = (int16_t) (((float)output->z_axis) *
((((dev->conf.compass_z_adj - 128) * 0.5) / 128.0) + 1));

/* Normalize data according to full-scale range */
output->x_axis = output->x_axis * 0.3;
output->y_axis = output->y_axis * 0.3;
output->z_axis = output->z_axis * 0.3;
output->x_axis = convert_magnometer(byteorder_lebuftohs(&data[0]),
dev->conf.compass_x_adj);
output->y_axis = convert_magnometer(byteorder_lebuftohs(&data[2]),
dev->conf.compass_y_adj);
output->z_axis = convert_magnometer(byteorder_lebuftohs(&data[4]),
dev->conf.compass_z_adj);

return 0;
}
Expand Down Expand Up @@ -374,18 +355,18 @@ int mpu9x50_set_gyro_fsr(mpu9x50_t *dev, mpu9x50_gyro_ranges_t fsr)
}

switch (fsr) {
case MPU9X50_GYRO_FSR_250DPS:
case MPU9X50_GYRO_FSR_500DPS:
case MPU9X50_GYRO_FSR_1000DPS:
case MPU9X50_GYRO_FSR_2000DPS:
i2c_acquire(DEV_I2C);
i2c_write_reg(DEV_I2C, DEV_ADDR,
MPU9X50_GYRO_CFG_REG, (fsr << 3), 0);
i2c_release(DEV_I2C);
dev->conf.gyro_fsr = fsr;
break;
default:
return -2;
case MPU9X50_GYRO_FSR_250DPS:
case MPU9X50_GYRO_FSR_500DPS:
case MPU9X50_GYRO_FSR_1000DPS:
case MPU9X50_GYRO_FSR_2000DPS:
i2c_acquire(DEV_I2C);
i2c_write_reg(DEV_I2C, DEV_ADDR,
MPU9X50_GYRO_CFG_REG, (fsr << 3), 0);
i2c_release(DEV_I2C);
dev->conf.gyro_fsr = fsr;
break;
default:
return -2;
}

return 0;
Expand All @@ -398,18 +379,18 @@ int mpu9x50_set_accel_fsr(mpu9x50_t *dev, mpu9x50_accel_ranges_t fsr)
}

switch (fsr) {
case MPU9X50_ACCEL_FSR_2G:
case MPU9X50_ACCEL_FSR_4G:
case MPU9X50_ACCEL_FSR_8G:
case MPU9X50_ACCEL_FSR_16G:
i2c_acquire(DEV_I2C);
i2c_write_reg(DEV_I2C, DEV_ADDR,
MPU9X50_ACCEL_CFG_REG, (fsr << 3), 0);
i2c_release(DEV_I2C);
dev->conf.accel_fsr = fsr;
break;
default:
return -2;
case MPU9X50_ACCEL_FSR_2G:
case MPU9X50_ACCEL_FSR_4G:
case MPU9X50_ACCEL_FSR_8G:
case MPU9X50_ACCEL_FSR_16G:
i2c_acquire(DEV_I2C);
i2c_write_reg(DEV_I2C, DEV_ADDR,
MPU9X50_ACCEL_CFG_REG, (fsr << 3), 0);
i2c_release(DEV_I2C);
dev->conf.accel_fsr = fsr;
break;
default:
return -2;
}

return 0;
Expand Down