diff --git a/include/mgos_mpu9250.h b/include/mgos_mpu9250.h new file mode 100644 index 0000000..024408a --- /dev/null +++ b/include/mgos_mpu9250.h @@ -0,0 +1,92 @@ +/* + * Copyright 2018 Google Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include "mgos.h" +#include "mgos_i2c.h" + +#define MGOS_MPU9250_READ_DELAY (2) + +#ifdef __cplusplus +extern "C" { + #endif + +struct mgos_mpu9250; + +enum mgos_mpu9250_accelerometer_range { + RANGE_16G = 3, + RANGE_8G = 2, + RANGE_4G = 1, + RANGE_2G = 0 +}; + +enum mgos_mpu9250_gyroscope_range { + RANGE_GYRO_2000 = 3, + RANGE_GYRO_1000 = 2, + RANGE_GYRO_500 = 1, + RANGE_GYRO_250 = 0 +}; + +enum mgos_mpu9250_magnetometer_scale { + SCALE_14_BITS = 0, + SCALE_16_BITS = 1 +}; + +enum mgos_mpu9250_magnetometer_speed { + MAG_8_HZ = 0, + MAG_100_HZ = 1 +}; + + +/* + * 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 + * validity, upon success a new `struct mgos_mpu9250` is allocated and + * returned. If the device could not be found, NULL is returned. + */ +struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr); + +/* + * Destroy the data structure associated with a MPU9250 device. The reference + * to the pointer of the `struct mgos_mpu9250` has to be provided, and upon + * successful destruction, its associated memory will be freed and the pointer + * set to NULL. + */ +void mgos_mpu9250_destroy(struct mgos_mpu9250 **sensor); + +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_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_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); + +/* + * Initialization function for MGOS -- currently a noop. + */ +bool mgos_mpu9250_i2c_init(void); + + #ifdef __cplusplus +} +#endif diff --git a/src/main.c b/src/main.c index 33445a3..8519030 100644 --- a/src/main.c +++ b/src/main.c @@ -21,6 +21,7 @@ #include "mgos_htu21df.h" #include "mgos_mcp9808.h" #include "mgos_ccs811.h" +#include "mgos_mpu9250.h" #include #include @@ -131,6 +132,27 @@ bool do_mcp9808(struct mgos_mcp9808 *sensor) { return true; } +bool do_mpu9250(struct mgos_mpu9250 *sensor) { + float ax, ay, az; + float gx, gy, gz; + float mx, my, mz; + + if (!sensor) { + return false; + } + + if (mgos_mpu9250_get_accelerometer(sensor, &ax, &ay, &az)) { + LOG(LL_INFO, ("Accel X=%.2f Y=%.2f Z=%.2f", ax, ay, az)); + } + if (mgos_mpu9250_get_gyroscope(sensor, &gx, &gy, &gz)) { + LOG(LL_INFO, ("Gyro X=%.2f Y=%.2f Z=%.2f", gx, gy, gz)); + } + if (mgos_mpu9250_get_magnetometer(sensor, &mx, &my, &mz)) { + LOG(LL_INFO, ("Mag X=%.2f Y=%.2f Z=%.2f", mx, my, mz)); + } + return true; +} + int main() { struct mgos_i2c * i2c; struct mgos_si7021 * si7021; @@ -138,6 +160,7 @@ int main() { struct mgos_htu21df *htu21df; struct mgos_mcp9808 *mcp9808; struct mgos_ccs811 * ccs811; + struct mgos_mpu9250 *mpu9250; if (!mgos_i2c_open(I2CBUSNR)) { LOG(LL_ERROR, ("Cannot open I2C bus %u", I2CBUSNR)); @@ -153,19 +176,23 @@ int main() { /* * if (!(sht31 = mgos_sht31_create(i2c, 0x44))) * LOG(LL_ERROR, ("Cannot create SHT31 device")); - * * if (!(si7021 = mgos_si7021_create(i2c, 0x40))) * LOG(LL_ERROR, ("Cannot create SI7021 device")); - * * if (!(htu21df = mgos_htu21df_create(i2c, 0x40))) * LOG(LL_ERROR, ("Cannot create HTU21DF device")); - * * if (!(mcp9808 = mgos_mcp9808_create(i2c, 0x18))) * LOG(LL_ERROR, ("Cannot create MCP9808 device")); + * if (!(ccs811 = mgos_ccs811_create(i2c, 0x5A))) + * LOG(LL_ERROR, ("Cannot create CCS811 device")); */ - if (!(ccs811 = mgos_ccs811_create(i2c, 0x5A))) { - LOG(LL_ERROR, ("Cannot create CCS811 device")); + if (!(mpu9250 = mgos_mpu9250_create(i2c, 0x68))) { + LOG(LL_ERROR, ("Cannot create MPU9250 device")); + } else { + mgos_mpu9250_set_accelerometer_range(mpu9250, RANGE_2G); + mgos_mpu9250_set_gyroscope_range(mpu9250, RANGE_GYRO_250); + mgos_mpu9250_set_magnetometer_scale(mpu9250, SCALE_14_BITS); + mgos_mpu9250_set_magnetometer_speed(mpu9250, MAG_100_HZ); } for (;;) { @@ -174,9 +201,10 @@ int main() { * do_si7021(si7021); * do_htu21df(htu21df); * do_mcp9808(mcp9808); + * do_ccs811(ccs811); */ - do_ccs811(ccs811); - sleep(5); + do_mpu9250(mpu9250); + sleep(1); } mgos_sht31_destroy(&sht31); @@ -184,6 +212,7 @@ int main() { mgos_htu21df_destroy(&htu21df); mgos_mcp9808_destroy(&mcp9808); mgos_ccs811_destroy(&ccs811); + mgos_mpu9250_destroy(&mpu9250); return 0; } diff --git a/src/mgos_mpu9250.c b/src/mgos_mpu9250.c new file mode 100644 index 0000000..c53e7c5 --- /dev/null +++ b/src/mgos_mpu9250.c @@ -0,0 +1,354 @@ +/* + * Copyright 2018 Google Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "mgos.h" +#include "mgos_mpu9250_internal.h" +#include "mgos_i2c.h" + +// Datasheet: +// + +// Private functions follow +static bool mgos_mpu9250_ak8963_init(struct mgos_mpu9250 *sensor, uint8_t i2caddr) { + int device_id; + + if (!sensor) { + return false; + } + + sensor->i2caddr_ak8963 = i2caddr; + + device_id = mgos_i2c_read_reg_b(sensor->i2c, sensor->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_usleep(10000); + + mgos_i2c_write_reg_b(sensor->i2c, sensor->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)) { + 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])); + + mgos_i2c_write_reg_b(sensor->i2c, sensor->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_usleep(10000); + + return true; +} + +// Private functions end + +// Public functions follow +struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr) { + struct mgos_mpu9250 *sensor; + int device_id; + + if (!i2c) { + return NULL; + } + + sensor = calloc(1, sizeof(struct mgos_mpu9250)); + if (!sensor) { + return NULL; + } + sensor->i2caddr = i2caddr; + sensor->i2c = i2c; + + device_id = mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_WHO_AM_I); + switch (device_id) { + case MGOS_MPU9250_DEVID_9250: + LOG(LL_INFO, ("Detected MPU9250 at I2C 0x%02x", i2caddr)); + break; + + case MGOS_MPU9250_DEVID_9255: + LOG(LL_INFO, ("Detected MPU9255 at I2C 0x%02x", i2caddr)); + break; + + default: + LOG(LL_ERROR, ("Failed to detect MPU9250 at I2C 0x%02x (device_id=0x%02x)", i2caddr, device_id)); + free(sensor); + return NULL; + } + + // Reset + mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_1, 0x80); + mgos_usleep(100000); + + // Enable sensors + 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))) { + LOG(LL_ERROR, ("Could not detect/initialize AK8963 magnetometer, disabling")); + } + + return sensor; +} + +void mgos_mpu9250_destroy(struct mgos_mpu9250 **sensor) { + if (!*sensor) { + return; + } + free(*sensor); + *sensor = NULL; + return; +} + +bool mgos_mpu9250_set_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_accelerometer_range range) { + int val; + + if (!sensor) { + return false; + } + + if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->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); +} + +bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_accelerometer_range *range) { + int val; + + if (!sensor) { + return false; + } + + if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) { + return false; + } + val &= 0x18; // 00011000 + val >>= 3; + *range = val; + return true; +} + +bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *sensor, 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) { + return false; + } + if (!mgos_mpu9250_get_accelerometer_range(sensor, &acc_range)) { + return false; + } + if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_ACCEL_XOUT_H, 6, data)) { + return false; + } + ax = (data[0] << 8) | (data[1]); + ay = (data[2] << 8) | (data[3]); + az = (data[4] << 8) | (data[5]); +// LOG(LL_DEBUG, ("ax=%d ay=%d az=%d", ax, ay, az)); + + switch (acc_range) { + case RANGE_16G: divider = 2048; break; + + case RANGE_8G: divider = 4096; break; + + case RANGE_4G: divider = 8192; break; + + case RANGE_2G: divider = 16384; break; + + default: return false; + } + *x = (float)ax / divider; + *y = (float)ay / divider; + *z = (float)az / divider; + + return true; +} + +bool mgos_mpu9250_set_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_gyroscope_range range) { + int val; + + if (!sensor) { + return false; + } + + if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->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); +} + +bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_gyroscope_range *range) { + int val; + + if (!sensor) { + return false; + } + + if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) { + return false; + } + val &= 0x18; // 00011000 + val >>= 3; + *range = val; + return true; +} + +bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *sensor, 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) { + return false; + } + if (!mgos_mpu9250_get_gyroscope_range(sensor, &gyr_range)) { + return false; + } + if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr, MGOS_MPU9250_REG_GYRO_XOUT_H, 6, data)) { + return false; + } + gx = (data[0] << 8) | (data[1]); + gy = (data[2] << 8) | (data[3]); + gz = (data[4] << 8) | (data[5]); +// LOG(LL_DEBUG, ("gx=%d gy=%d gz=%d", gx, gy, gz)); + + switch (gyr_range) { + case RANGE_GYRO_2000: divider = 16.4; break; + + case RANGE_GYRO_1000: divider = 32.8; break; + + case RANGE_GYRO_500: divider = 65.5; break; + + case RANGE_GYRO_250: divider = 131.0; break; + + default: return false; + } + *x = (float)gx / divider; + *y = (float)gy / divider; + *z = (float)gz / divider; + return true; +} + +bool mgos_mpu9250_set_magnetometer_scale(struct mgos_mpu9250 *sensor, enum mgos_mpu9250_magnetometer_scale scale) { + int val; + + if (!sensor || !sensor->mag_enabled) { + return false; + } + + if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->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_usleep(10000); + + mgos_i2c_write_reg_b(sensor->i2c, sensor->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) { + int val; + + if (!sensor || !sensor->mag_enabled) { + return false; + } + if ((val = mgos_i2c_read_reg_b(sensor->i2c, sensor->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) { + 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) { + return false; + } + return false; +} + +bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *sensor, 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) { + return false; + } + if (!mgos_mpu9250_get_magnetometer_scale(sensor, &mag_scale)) { + return false; + } + if (!mgos_i2c_read_reg_n(sensor->i2c, sensor->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_XOUT_L, 7, data)) { + return false; + } + if (data[6] & 0x08) { + return false; + } + + mx = (data[1] << 8) | (data[0]); + my = (data[3] << 8) | (data[2]); + mz = (data[5] << 8) | (data[4]); +// LOG(LL_DEBUG, ("mx=%d my=%d mz=%d", mx, my, mz)); + + switch (mag_scale) { + case SCALE_14_BITS: divider = 8190.0; break; + + case SCALE_16_BITS: divider = 32760.0; break; + + 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; + return true; +} + +bool mgos_mpu9250_i2c_init(void) { + return true; +} + +// Public functions end diff --git a/src/mgos_mpu9250_internal.h b/src/mgos_mpu9250_internal.h new file mode 100644 index 0000000..c53c64d --- /dev/null +++ b/src/mgos_mpu9250_internal.h @@ -0,0 +1,71 @@ +/* + * Copyright 2018 Google Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include "mgos.h" +#include "mgos_i2c.h" +#include "mgos_mpu9250.h" +#include + +#define MGOS_MPU9250_DEFAULT_I2CADDR (0x68) +#define MGOS_AK8963_DEFAULT_I2CADDR (0x0C) + +#define MGOS_MPU9250_DEVID_9250 (0x71) +#define MGOS_MPU9250_DEVID_9255 (0x73) +#define MGOS_MPU9250_DEVID_AK8963 (0x48) + +// MPU9250 -- Accelerometer and Gyro registers +#define MGOS_MPU9250_REG_SMPLRT_DIV (0x19) +#define MGOS_MPU9250_REG_CONFIG (0x1A) +#define MGOS_MPU9250_REG_GYRO_CONFIG (0x1B) +#define MGOS_MPU9250_REG_ACCEL_CONFIG (0x1C) +#define MGOS_MPU9250_REG_ACCEL_CONFIG2 (0x1D) +#define MGOS_MPU9250_REG_INT_PIN_CFG (0x37) +#define MGOS_MPU9250_REG_INT_ENABLE (0x38) +#define MGOS_MPU9250_REG_INT_STATUS (0x3A) +#define MGOS_MPU9250_REG_ACCEL_XOUT_H (0x3B) +#define MGOS_MPU9250_REG_TEMP_OUT_H (0x41) +#define MGOS_MPU9250_REG_GYRO_XOUT_H (0x43) +#define MGOS_MPU9250_REG_PWR_MGMT_1 (0x6B) +#define MGOS_MPU9250_REG_PWR_MGMT_2 (0x6C) +#define MGOS_MPU9250_REG_WHO_AM_I (0x75) + +// AK8963 Companion -- Magnetometer Registers +#define MGOS_MPU9250_REG_AK8963_WHO_AM_I (0x00) +#define MGOS_MPU9250_REG_AK8963_ST1 (0x02) +#define MGOS_MPU9250_REG_AK8963_XOUT_L (0x03) +#define MGOS_MPU9250_REG_AK8963_CNTL (0x0A) +#define MGOS_MPU9250_REG_AK8963_ASAX (0x10) +#define MGOS_MPU9250_REG_AK8963_ASAY (0x11) +#define MGOS_MPU9250_REG_AK8963_ASAZ (0x12) + +#ifdef __cplusplus +extern "C" { + #endif + +struct mgos_mpu9250 { + struct mgos_i2c *i2c; + uint8_t i2caddr; + uint8_t i2caddr_ak8963; + + bool mag_enabled; + float mag_adj[3]; +}; + + #ifdef __cplusplus +} +#endif