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

Add MPU9250

Browse Source
This commit is contained in:
Pim van Pelt
2018-04-18 22:54:06 +02:00
parent e4bb9e39cc
commit 1e1c3087cd
4 changed files with 553 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
include/mgos_mpu9250.h Normal file
View File

@ -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

43
src/main.c
View File

@ -21,6 +21,7 @@
#include "mgos_htu21df.h"
#include "mgos_mcp9808.h"
#include "mgos_ccs811.h"
#include "mgos_mpu9250.h"
#include <fcntl.h>
#include <sys/ioctl.h>
@ -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;
}

354
src/mgos_mpu9250.c Normal file
View File

@ -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

71
src/mgos_mpu9250_internal.h Normal file
View File

@ -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 <math.h>
#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