pim/mgos_i2c_mock
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Rename 'sensor' to 'imu' consistently

Browse Source
This commit is contained in:
Pim van Pelt
2018-04-19 17:52:06 +02:00
parent 88e287770c
commit acec109ae8
2 changed files with 80 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
include/mgos_mpu9250.h
View File

@ -54,7 +54,7 @@ enum mgos_mpu9250_magnetometer_speed {
/*
* Initialize a MPU9250 on the I2C bus `i2c` at address specified in `i2caddr`
* parameter (default MPU9250 is on address 0x68). The sensor will be polled for
* parameter (default MPU9250 is on address 0x68). The imu will be polled for
* validity, upon success a new `struct mgos_mpu9250` is allocated and
* returned. If the device could not be found, NULL is returned.
*/
@ -66,21 +66,21 @@ struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr);
* successful destruction, its associated memory will be freed and the pointer
* set to NULL.
*/
void mgos_mpu9250_destroy(struct mgos_mpu9250 **sensor);
void mgos_mpu9250_destroy(struct mgos_mpu9250 **imu);
bool mgos_mpu9250_set_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_accelerometer_range range);
bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_accelerometer_range *range);
bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *sensor, float *x, float *y, float *z);
bool mgos_mpu9250_set_accelerometer_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_accelerometer_range range);
bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_accelerometer_range *range);
bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *imu, float *x, float *y, float *z);
bool mgos_mpu9250_set_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_gyroscope_range range);
bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_gyroscope_range *range);
bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *sensor, float *x, float *y, float *z);
bool mgos_mpu9250_set_gyroscope_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_gyroscope_range range);
bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_gyroscope_range *range);
bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *imu, float *x, float *y, float *z);
bool mgos_mpu9250_set_magnetometer_scale(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_scale scale);
bool mgos_mpu9250_get_magnetometer_scale(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_scale *scale);
bool mgos_mpu9250_set_magnetometer_speed(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_speed speed);
bool mgos_mpu9250_get_magnetometer_speed(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_speed *speed);
bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *sensor, float *x, float *y, float *z);
bool mgos_mpu9250_set_magnetometer_scale(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_scale scale);
bool mgos_mpu9250_get_magnetometer_scale(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_scale *scale);
bool mgos_mpu9250_set_magnetometer_speed(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_speed speed);
bool mgos_mpu9250_get_magnetometer_speed(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_speed *speed);
bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *imu, float *x, float *y, float *z);
/*
* Initialization function for MGOS -- currently a noop.

134
src/mgos_mpu9250.c
View File

@ -22,42 +22,42 @@
//
// Private functions follow
static bool mgos_mpu9250_ak8963_init(struct mgos_mpu9250 *sensor, uint8_t i2caddr) {
static bool mgos_mpu9250_ak8963_init(struct mgos_mpu9250 *imu, uint8_t i2caddr) {
int device_id;
if (!sensor) {
if (!imu) {
return false;
}
sensor->i2caddr_ak8963 = i2caddr;
imu->i2caddr_ak8963 = i2caddr;
device_id = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_WHO_AM_I);
device_id = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_WHO_AM_I);
if (device_id != MGOS_MPU9250_DEVID_AK8963) {
return false;
}
LOG(LL_INFO, ("Detected AK8963 at I2C 0x%02x", i2caddr));
mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
mgos_usleep(10000);
mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x0F);
mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x0F);
mgos_usleep(10000);
uint8_t data[3];
if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_ASAX, 3, data)) {
if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_ASAX, 3, data)) {
LOG(LL_ERROR, ("Could not read magnetometer adjustment registers"));
return false;
}
sensor->mag_adj[0] = (float)(data[0] - 128) / 256. + 1.;
sensor->mag_adj[1] = (float)(data[1] - 128) / 256. + 1.;
sensor->mag_adj[2] = (float)(data[2] - 128) / 256. + 1.;
LOG(LL_DEBUG, ("magnetometer adjustment %.2f %.2f %.2f", sensor->mag_adj[0], sensor->mag_adj[1], sensor->mag_adj[2]));
imu->mag_adj[0] = (float)(data[0] - 128) / 256. + 1.;
imu->mag_adj[1] = (float)(data[1] - 128) / 256. + 1.;
imu->mag_adj[2] = (float)(data[2] - 128) / 256. + 1.;
LOG(LL_DEBUG, ("magnetometer adjustment %.2f %.2f %.2f", imu->mag_adj[0], imu->mag_adj[1], imu->mag_adj[2]));
mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
mgos_usleep(10000);
// Set magnetometer data resolution and sample ODR
mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x16);
mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x16);
mgos_usleep(10000);
return true;
@ -67,19 +67,19 @@ static bool mgos_mpu9250_ak8963_init(struct mgos_mpu9250 *sensor, uint8_t i2cadd
// Public functions follow
struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr) {
struct mgos_mpu9250 *sensor;
struct mgos_mpu9250 *imu;
int device_id;
if (!i2c) {
return NULL;
}
sensor = calloc(1, sizeof(struct mgos_mpu9250));
if (!sensor) {
imu = calloc(1, sizeof(struct mgos_mpu9250));
if (!imu) {
return NULL;
}
sensor->i2caddr = i2caddr;
sensor->i2c = i2c;
imu->i2caddr = i2caddr;
imu->i2c = i2c;
device_id = mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_WHO_AM_I);
switch (device_id) {
@ -93,7 +93,7 @@ struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr)
default:
LOG(LL_ERROR, ("Failed to detect MPU9250 at I2C 0x%02x (device_id=0x%02x)", i2caddr, device_id));
free(sensor);
free(imu);
return NULL;
}
@ -101,53 +101,53 @@ struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr)
mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_1, 0x80);
mgos_usleep(100000);
// Enable sensors
// Enable imus
mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_2, 0x00);
// Magnetometer enable
mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_INT_PIN_CFG, 0x02);
// TODO(pim): is the mag always on 0x0C ?
if (false == (sensor->mag_enabled = mgos_mpu9250_ak8963_init(sensor, MGOS_AK8963_DEFAULT_I2CADDR))) {
if (false == (imu->mag_enabled = mgos_mpu9250_ak8963_init(imu, MGOS_AK8963_DEFAULT_I2CADDR))) {
LOG(LL_ERROR, ("Could not detect/initialize AK8963 magnetometer, disabling"));
}
return sensor;
return imu;
}
void mgos_mpu9250_destroy(struct mgos_mpu9250 **sensor) {
if (!*sensor) {
void mgos_mpu9250_destroy(struct mgos_mpu9250 **imu) {
if (!*imu) {
return;
}
free(*sensor);
*sensor = NULL;
free(*imu);
*imu = NULL;
return;
}
bool mgos_mpu9250_set_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_accelerometer_range range) {
bool mgos_mpu9250_set_accelerometer_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_accelerometer_range range) {
int val;
if (!sensor) {
if (!imu) {
return false;
}
if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) {
if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) {
return false;
}
val &= 0xE7; // 11100111
val |= range << 3;
return mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG, val);
return mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG, val);
}
bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_accelerometer_range *range) {
bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_accelerometer_range *range) {
int val;
if (!sensor) {
if (!imu) {
return false;
}
if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) {
if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) {
return false;
}
val &= 0x18; // 00011000
@ -156,19 +156,19 @@ bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos
return true;
}
bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *sensor, float *x, float *y, float *z) {
bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *imu, float *x, float *y, float *z) {
uint8_t data[6];
int16_t ax, ay, az;
enum mgos_mpu9250_accelerometer_range acc_range;
uint16_t divider;
if (!sensor) {
if (!imu) {
return false;
}
if (!mgos_mpu9250_get_accelerometer_range(sensor, &acc_range)) {
if (!mgos_mpu9250_get_accelerometer_range(imu, &acc_range)) {
return false;
}
if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_ACCEL_XOUT_H, 6, data)) {
if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_XOUT_H, 6, data)) {
return false;
}
ax = (data[0] << 8) | (data[1]);
@ -194,30 +194,30 @@ bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *sensor, float *x, float
return true;
}
bool mgos_mpu9250_set_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_gyroscope_range range) {
bool mgos_mpu9250_set_gyroscope_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_gyroscope_range range) {
int val;
if (!sensor) {
if (!imu) {
return false;
}
if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) {
if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) {
return false;
}
val &= 0xE7; // 11100111
val |= range << 3;
return mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG, val);
return mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG, val);
}
bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_gyroscope_range *range) {
bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_gyroscope_range *range) {
int val;
if (!sensor) {
if (!imu) {
return false;
}
if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) {
if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) {
return false;
}
val &= 0x18; // 00011000
@ -226,19 +226,19 @@ bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu
return true;
}
bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *sensor, float *x, float *y, float *z) {
bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *imu, float *x, float *y, float *z) {
uint8_t data[6];
int16_t gx, gy, gz;
enum mgos_mpu9250_gyroscope_range gyr_range;
float divider;
if (!sensor) {
if (!imu) {
return false;
}
if (!mgos_mpu9250_get_gyroscope_range(sensor, &gyr_range)) {
if (!mgos_mpu9250_get_gyroscope_range(imu, &gyr_range)) {
return false;
}
if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_GYRO_XOUT_H, 6, data)) {
if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_XOUT_H, 6, data)) {
return false;
}
gx = (data[0] << 8) | (data[1]);
@ -263,66 +263,66 @@ bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *sensor, float *x, float *y,
return true;
}
bool mgos_mpu9250_set_magnetometer_scale(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_scale scale) {
bool mgos_mpu9250_set_magnetometer_scale(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_scale scale) {
int val;
if (!sensor || !sensor->mag_enabled) {
if (!imu || !imu->mag_enabled) {
return false;
}
if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL)) < 0) {
if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL)) < 0) {
return false;
}
val &= 0x06;
mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
mgos_usleep(10000);
mgos_i2c_write_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, (scale << 4) | val);
mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, (scale << 4) | val);
mgos_usleep(10000);
return true;
}
bool mgos_mpu9250_get_magnetometer_scale(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_scale *scale) {
bool mgos_mpu9250_get_magnetometer_scale(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_scale *scale) {
int val;
if (!sensor || !sensor->mag_enabled) {
if (!imu || !imu->mag_enabled) {
return false;
}
if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL)) < 0) {
if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL)) < 0) {
return false;
}
*scale = (val >> 4) & 0x01;
return true;
}
bool mgos_mpu9250_set_magnetometer_speed(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_speed speed) {
if (!sensor || !sensor->mag_enabled) {
bool mgos_mpu9250_set_magnetometer_speed(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_speed speed) {
if (!imu || !imu->mag_enabled) {
return false;
}
return false;
}
bool mgos_mpu9250_get_magnetometer_speed(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_speed *speed) {
if (!sensor || !sensor->mag_enabled) {
bool mgos_mpu9250_get_magnetometer_speed(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_speed *speed) {
if (!imu || !imu->mag_enabled) {
return false;
}
return false;
}
bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *sensor, float *x, float *y, float *z) {
bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *imu, float *x, float *y, float *z) {
uint8_t data[7];
int16_t mx, my, mz;
enum mgos_mpu9250_magnetometer_scale mag_scale;
float divider;
if (!sensor || !sensor->mag_enabled) {
if (!imu || !imu->mag_enabled) {
return false;
}
if (!mgos_mpu9250_get_magnetometer_scale(sensor, &mag_scale)) {
if (!mgos_mpu9250_get_magnetometer_scale(imu, &mag_scale)) {
return false;
}
if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_XOUT_L, 7, data)) {
if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_XOUT_L, 7, data)) {
return false;
}
if (data[6] & 0x08) {
@ -341,9 +341,9 @@ bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *sensor, float *x, float
default: return false;
}
*x = (float)mx * 4912.0 * sensor->mag_adj[0] / divider;
*y = (float)my * 4912.0 * sensor->mag_adj[1] / divider;
*z = (float)mz * 4912.0 * sensor->mag_adj[2] / divider;
*x = (float)mx * 4912.0 * imu->mag_adj[0] / divider;
*y = (float)my * 4912.0 * imu->mag_adj[1] / divider;
*z = (float)mz * 4912.0 * imu->mag_adj[2] / divider;
return true;
}