├── .gitignore ├── docs ├── ADXL345.pdf ├── AK8963.pdf ├── AK8975.pdf ├── ITG3205.pdf ├── L3GD20.pdf ├── LSM303D.pdf ├── MAG3110.pdf ├── lsm6dsl.pdf ├── HMC5883L.pdf ├── ICM20948.pdf ├── MMA8451Q.pdf ├── MPU-60x0.pdf ├── MPU-9250.pdf ├── MPU-9255.pdf └── chrome-client.png ├── mos.yml ├── src ├── mgos_imu_mpu6886.h ├── mgos_imu_ak8963.h ├── mgos_imu_adxl345.h ├── mgos_imu_ak8975.h ├── mgos_imu_mag3110.h ├── mgos_imu_mpu6886.c ├── mgos_imu_hmc5883l.h ├── mgos_imu_itg3205.h ├── mgos_imu.c ├── mgos_imu_mma8451.c ├── mgos_imu_adxl345.c ├── mgos_imu_mag3110.c ├── mgos_imu_l3gd20.h ├── mgos_imu_hmc5883l.c ├── mgos_imu_itg3205.c ├── mgos_imu_ak8963.c ├── mgos_imu_l3gd20.c ├── mgos_imu_bmm150.h ├── mgos_imu_ak8975.c ├── mgos_imu_mma8451.h ├── mgos_imu_mpu925x.h ├── madgwick.h ├── mgos_imu_bmm150.c ├── mgos_imu_icm20948.h ├── mgos_imu_lsm303d.h ├── mgos_imu_mpu60x0.h ├── mgos_imu_internal.h ├── mgos_imu_lsm303d.c ├── mgos_imu_lsm9ds1.h ├── mgos_imu_mpu60x0.c ├── mgos_imu_mpu925x.c ├── mgos_imu_lsm9ds1.c ├── mgos_imu_magnetometer.c ├── mgos_imu_lsm6dsl.h ├── mgos_imu_accelerometer.c ├── mgos_imu_gyroscope.c └── madgwick.c ├── CONTRIBUTING.md ├── third-party └── bosch │ ├── include │ └── bmm150.h │ └── src │ └── bmm150.c ├── include └── mgos_imu.h ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | deps/ 2 | build/ 3 | -------------------------------------------------------------------------------- /docs/ADXL345.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/ADXL345.pdf -------------------------------------------------------------------------------- /docs/AK8963.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/AK8963.pdf -------------------------------------------------------------------------------- /docs/AK8975.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/AK8975.pdf -------------------------------------------------------------------------------- /docs/ITG3205.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/ITG3205.pdf -------------------------------------------------------------------------------- /docs/L3GD20.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/L3GD20.pdf -------------------------------------------------------------------------------- /docs/LSM303D.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/LSM303D.pdf -------------------------------------------------------------------------------- /docs/MAG3110.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/MAG3110.pdf -------------------------------------------------------------------------------- /docs/lsm6dsl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/lsm6dsl.pdf -------------------------------------------------------------------------------- /docs/HMC5883L.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/HMC5883L.pdf -------------------------------------------------------------------------------- /docs/ICM20948.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/ICM20948.pdf -------------------------------------------------------------------------------- /docs/MMA8451Q.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/MMA8451Q.pdf -------------------------------------------------------------------------------- /docs/MPU-60x0.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/MPU-60x0.pdf -------------------------------------------------------------------------------- /docs/MPU-9250.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/MPU-9250.pdf -------------------------------------------------------------------------------- /docs/MPU-9255.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/MPU-9255.pdf -------------------------------------------------------------------------------- /docs/chrome-client.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mongoose-os-libs/imu/HEAD/docs/chrome-client.png -------------------------------------------------------------------------------- /mos.yml: -------------------------------------------------------------------------------- 1 | author: Pim van Pelt 2 | description: Library that implements various IMU devices 3 | type: lib 4 | version: 1.0 5 | 6 | tags: 7 | - c 8 | - hw 9 | - docs:drivers:IMU 10 | 11 | # List of files / directories with C sources. No slashes at the end of dir names. 12 | sources: 13 | - src 14 | - third-party/bosch/src 15 | 16 | includes: 17 | - include 18 | - third-party/bosch/include 19 | 20 | config_schema: 21 | 22 | libs: 23 | - location: https://github.com/mongoose-os-libs/i2c 24 | 25 | # Used by the mos tool to catch mos binaries incompatible with this file format 26 | manifest_version: 2017-05-18 27 | -------------------------------------------------------------------------------- /src/mgos_imu_mpu6886.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Sergio R. Caprile and Cika Electronica S.R.L. 3 | * All rights reserved 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the ""License""); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an ""AS IS"" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "mgos.h" 21 | #include "mgos_imu_internal.h" 22 | 23 | #define MGOS_MPU6886_DEFAULT_I2CADDR (0x68) 24 | #define MGOS_MPU6886_DEVID (0x19) 25 | 26 | #define MGOS_MPU6886_GYRO_OFFSET (0x13) 27 | #define MGOS_MPU6886_ACCEL_CONFIG2 (0x1D) 28 | #define MGOS_MPU6886_ACCEL_INTEL_CTRL (0x69) 29 | 30 | 31 | bool mgos_imu_mpu6886_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 32 | 33 | bool mgos_imu_mpu6886_gyro_detect(struct mgos_imu_gyro *dev, 34 | void *imu_user_data); 35 | -------------------------------------------------------------------------------- /src/mgos_imu_ak8963.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_AK8963_DEFAULT_I2CADDR (0x0C) 23 | #define MGOS_AK8963_DEVID (0x48) 24 | 25 | // AK8963 Magnetometer Registers 26 | #define MGOS_AK8963_REG_WHO_AM_I (0x00) 27 | #define MGOS_AK8963_REG_ST1 (0x02) 28 | #define MGOS_AK8963_REG_XOUT_L (0x03) 29 | #define MGOS_AK8963_REG_CNTL (0x0A) 30 | #define MGOS_AK8963_REG_ASAX (0x10) 31 | #define MGOS_AK8963_REG_ASAY (0x11) 32 | #define MGOS_AK8963_REG_ASAZ (0x12) 33 | 34 | bool mgos_imu_ak8963_detect(struct mgos_imu_mag *dev, void *imu_user_data); 35 | bool mgos_imu_ak8963_create(struct mgos_imu_mag *dev, void *imu_user_data); 36 | bool mgos_imu_ak8963_read(struct mgos_imu_mag *dev, void *imu_user_data); 37 | -------------------------------------------------------------------------------- /src/mgos_imu_adxl345.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_ADXL345_DEFAULT_I2CADDR (0x53) 23 | #define MGOS_ADXL345_DEVID (0xE5) 24 | 25 | // ADXL345 -- Registers 26 | #define MGOS_ADXL345_REG_WHO_AM_I (0x00) 27 | #define MGOS_ADXL345_REG_BW_RATE (0x2C) 28 | #define MGOS_ADXL345_REG_POWER_CTL (0x2D) 29 | #define MGOS_ADXL345_REG_INT_ENABLE (0x2E) 30 | #define MGOS_ADXL345_REG_DATA_FORMAT (0x31) 31 | #define MGOS_ADXL345_REG_DATA_OUT (0x32) 32 | #define MGOS_ADXL345_REG_FIFO_CTL (0x38) 33 | 34 | bool mgos_imu_adxl345_detect(struct mgos_imu_acc *dev, void *imu_user_data); 35 | bool mgos_imu_adxl345_create(struct mgos_imu_acc *dev, void *imu_user_data); 36 | bool mgos_imu_adxl345_read(struct mgos_imu_acc *dev, void *imu_user_data); 37 | -------------------------------------------------------------------------------- /src/mgos_imu_ak8975.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_AK8975_DEFAULT_I2CADDR (0x0C) 23 | #define MGOS_AK8975_DEVID (0x48) 24 | 25 | // AK8975 Magnetometer Registers 26 | #define MGOS_AK8975_REG_WHO_AM_I (0x00) 27 | #define MGOS_AK8975_REG_INFO (0x01) 28 | #define MGOS_AK8975_REG_ST1 (0x02) 29 | #define MGOS_AK8975_REG_XOUT_L (0x03) 30 | #define MGOS_AK8975_REG_ST2 (0x09) 31 | #define MGOS_AK8975_REG_CNTL (0x0A) 32 | #define MGOS_AK8975_REG_ASAX (0x10) 33 | #define MGOS_AK8975_REG_ASAY (0x11) 34 | #define MGOS_AK8975_REG_ASAZ (0x12) 35 | 36 | bool mgos_imu_ak8975_detect(struct mgos_imu_mag *dev, void *imu_user_data); 37 | bool mgos_imu_ak8975_create(struct mgos_imu_mag *dev, void *imu_user_data); 38 | bool mgos_imu_ak8975_read(struct mgos_imu_mag *dev, void *imu_user_data); 39 | -------------------------------------------------------------------------------- /src/mgos_imu_mag3110.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_MAG3110_DEFAULT_I2CADDR (0x0E) 23 | #define MGOS_MAG3110_DEVID (0xC4) 24 | 25 | // MAG3110 Magnetometer Registers 26 | #define MGOS_MAG3110_REG_OUT_X_MSB (0x01) 27 | #define MGOS_MAG3110_REG_OUT_X_LSB (0x02) 28 | #define MGOS_MAG3110_REG_OUT_Y_MSB (0x03) 29 | #define MGOS_MAG3110_REG_OUT_Y_LSB (0x04) 30 | #define MGOS_MAG3110_REG_OUT_Z_MSB (0x05) 31 | #define MGOS_MAG3110_REG_OUT_Z_LSB (0x06) 32 | #define MGOS_MAG3110_REG_WHO_AM_I (0x07) 33 | #define MGOS_MAG3110_REG_SYSMOD (0x08) 34 | #define MGOS_MAG3110_REG_DIE_TEMP (0x0F) 35 | #define MGOS_MAG3110_REG_CTRL_REG1 (0x10) 36 | #define MGOS_MAG3110_REG_CTRL_REG2 (0x11) 37 | 38 | bool mgos_imu_mag3110_detect(struct mgos_imu_mag *dev, void *imu_user_data); 39 | bool mgos_imu_mag3110_create(struct mgos_imu_mag *dev, void *imu_user_data); 40 | bool mgos_imu_mag3110_read(struct mgos_imu_mag *dev, void *imu_user_data); 41 | -------------------------------------------------------------------------------- /src/mgos_imu_mpu6886.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Sergio R. Caprile and Cika Electronica S.R.L. 3 | * All rights reserved 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the ""License""); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an ""AS IS"" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "mgos_imu_mpu6886.h" 19 | #include "mgos.h" 20 | #include "mgos_i2c.h" 21 | #include "mgos_imu_mpu60x0.h" 22 | 23 | static bool mgos_imu_mpu6886_detect(struct mgos_i2c *i2c, uint8_t i2caddr) { 24 | if (!i2c) { 25 | return false; 26 | } 27 | 28 | if (MGOS_MPU6886_DEVID != 29 | mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_MPU60X0_REG_WHO_AM_I)) { 30 | return false; 31 | } 32 | LOG(LL_WARN, ("This driver is experimental and work in progress. Proceed " 33 | "with caution!")); 34 | return true; 35 | } 36 | 37 | bool mgos_imu_mpu6886_acc_detect(struct mgos_imu_acc *dev, 38 | void *imu_user_data) { 39 | return mgos_imu_mpu6886_detect(dev->i2c, dev->i2caddr); 40 | 41 | (void)imu_user_data; 42 | } 43 | 44 | bool mgos_imu_mpu6886_gyro_detect(struct mgos_imu_gyro *dev, 45 | void *imu_user_data) { 46 | return mgos_imu_mpu6886_detect(dev->i2c, dev->i2caddr); 47 | 48 | (void)imu_user_data; 49 | } 50 | -------------------------------------------------------------------------------- /src/mgos_imu_hmc5883l.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_HMC5883L_DEFAULT_I2CADDR (0x0E) 23 | #define MGOS_HMC5883L_DEVID (0xC4) 24 | 25 | // HMC5883L Magnetometer Registers 26 | #define MGOS_HMC5883L_REG_CONF_A (0x00) 27 | #define MGOS_HMC5883L_REG_CONF_B (0x01) 28 | #define MGOS_HMC5883L_REG_MODE (0x02) 29 | #define MGOS_HMC5883L_REG_OUT_X_MSB (0x03) 30 | #define MGOS_HMC5883L_REG_OUT_X_LSB (0x04) 31 | #define MGOS_HMC5883L_REG_OUT_Y_MSB (0x05) 32 | #define MGOS_HMC5883L_REG_OUT_Y_LSB (0x06) 33 | #define MGOS_HMC5883L_REG_OUT_Z_MSB (0x07) 34 | #define MGOS_HMC5883L_REG_OUT_Z_LSB (0x08) 35 | #define MGOS_HMC5883L_REG_STATUS (0x09) 36 | #define MGOS_HMC5883L_REG_ID_A (0x0a) 37 | #define MGOS_HMC5883L_REG_ID_B (0x0b) 38 | #define MGOS_HMC5883L_REG_ID_C (0x0c) 39 | 40 | bool mgos_imu_hmc5883l_detect(struct mgos_imu_mag *dev, void *imu_user_data); 41 | bool mgos_imu_hmc5883l_create(struct mgos_imu_mag *dev, void *imu_user_data); 42 | bool mgos_imu_hmc5883l_read(struct mgos_imu_mag *dev, void *imu_user_data); 43 | -------------------------------------------------------------------------------- /src/mgos_imu_itg3205.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_ITG3205_DEFAULT_I2CADDR (0x68) 23 | #define MGOS_ITG3205_DEVID (0x68) 24 | 25 | // ITG3205 Registers 26 | #define MGOS_ITG3205_REG_WHO_AM_I (0x00) 27 | #define MGOS_ITG3205_REG_SMPLRT_DIV (0x15) 28 | #define MGOS_ITG3205_REG_DLPF_FS (0x16) 29 | #define MGOS_ITG3205_REG_INT_CFG (0x17) 30 | #define MGOS_ITG3205_REG_INT_STATUS (0x1A) 31 | #define MGOS_ITG3205_REG_TEMP_OUT_H (0x1B) 32 | #define MGOS_ITG3205_REG_TEMP_OUT_L (0x1C) 33 | #define MGOS_ITG3205_REG_GYRO_XOUT_H (0x1D) 34 | #define MGOS_ITG3205_REG_GYRO_XOUT_L (0x1E) 35 | #define MGOS_ITG3205_REG_GYRO_YOUT_H (0x1F) 36 | #define MGOS_ITG3205_REG_GYRO_YOUT_L (0x20) 37 | #define MGOS_ITG3205_REG_GYRO_ZOUT_H (0x21) 38 | #define MGOS_ITG3205_REG_GYRO_ZOUT_L (0x22) 39 | #define MGOS_ITG3205_REG_PWR_MGM (0x3E) 40 | 41 | bool mgos_imu_itg3205_detect(struct mgos_imu_gyro *dev, void *imu_user_data); 42 | bool mgos_imu_itg3205_create(struct mgos_imu_gyro *dev, void *imu_user_data); 43 | bool mgos_imu_itg3205_read(struct mgos_imu_gyro *dev, void *imu_user_data); 44 | -------------------------------------------------------------------------------- /src/mgos_imu.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_imu_internal.h" 19 | #include 20 | 21 | // Private functions follow 22 | // Private functions end 23 | 24 | // Public functions follow 25 | struct mgos_imu *mgos_imu_create(void) { 26 | struct mgos_imu *imu; 27 | 28 | imu = calloc(1, sizeof(struct mgos_imu)); 29 | if (!imu) { 30 | return NULL; 31 | } 32 | memset(imu, 0, sizeof(struct mgos_imu)); 33 | return imu; 34 | } 35 | 36 | void mgos_imu_destroy(struct mgos_imu **imu) { 37 | if (!*imu) { 38 | return; 39 | } 40 | mgos_imu_gyroscope_destroy(*imu); 41 | mgos_imu_accelerometer_destroy(*imu); 42 | mgos_imu_magnetometer_destroy(*imu); 43 | 44 | if ((*imu)->user_data) { 45 | free((*imu)->user_data); 46 | } 47 | 48 | free(*imu); 49 | *imu = NULL; 50 | return; 51 | } 52 | 53 | bool mgos_imu_accelerometer_present(struct mgos_imu *imu) { 54 | if (!imu) { 55 | return false; 56 | } 57 | return imu->acc != NULL; 58 | } 59 | 60 | bool mgos_imu_gyroscope_present(struct mgos_imu *imu) { 61 | if (!imu) { 62 | return false; 63 | } 64 | return imu->gyro != NULL; 65 | } 66 | 67 | bool mgos_imu_magnetometer_present(struct mgos_imu *imu) { 68 | if (!imu) { 69 | return false; 70 | } 71 | return imu->mag != NULL; 72 | } 73 | 74 | bool mgos_imu_init(void) { 75 | return true; 76 | } 77 | 78 | // Public functions end 79 | -------------------------------------------------------------------------------- /src/mgos_imu_mma8451.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_mma8451.h" 20 | 21 | bool mgos_imu_mma8451_detect(struct mgos_imu_acc *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_MMA8451_REG_WHO_AM_I); 29 | if (device_id == MGOS_MMA8451_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | 34 | (void)imu_user_data; 35 | } 36 | 37 | bool mgos_imu_mma8451_create(struct mgos_imu_acc *dev, void *imu_user_data) { 38 | if (!dev) { 39 | return false; 40 | } 41 | 42 | // XYZ_DATA_CFG: 000; HPF=0; 00; FS=10 (8G) 43 | // CTRL_REG1: ASLP_RATE=00; DR=011 (100Hz ODR); LNOISE=0; F_READ=0; ACTIVE=1 44 | // CTRL_REG2: ST=0; RST=0; 0; SMODS=10 (hires); SLPE=0; MODS=10 (hires) 45 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MMA8451_REG_XYZ_DATA_CFG, 0x02); 46 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MMA8451_REG_CTRL_REG1, 0x19); 47 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MMA8451_REG_CTRL_REG2, 0x12); 48 | 49 | dev->scale = (8.f / 32767.5); 50 | return true; 51 | 52 | (void)imu_user_data; 53 | } 54 | 55 | bool mgos_imu_mma8451_read(struct mgos_imu_acc *dev, void *imu_user_data) { 56 | uint8_t data[6]; 57 | 58 | if (!dev) { 59 | return false; 60 | } 61 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_MMA8451_REG_OUT_X_MSB, 6, data)) { 62 | return false; 63 | } 64 | dev->ax = (data[1]) | (data[0] << 8); 65 | dev->ay = (data[3]) | (data[2] << 8); 66 | dev->az = (data[5]) | (data[4] << 8); 67 | 68 | return true; 69 | 70 | (void)imu_user_data; 71 | } 72 | -------------------------------------------------------------------------------- /src/mgos_imu_adxl345.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_adxl345.h" 20 | 21 | bool mgos_imu_adxl345_detect(struct mgos_imu_acc *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_ADXL345_REG_WHO_AM_I); 29 | if (device_id == MGOS_ADXL345_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | 34 | (void)imu_user_data; 35 | } 36 | 37 | bool mgos_imu_adxl345_create(struct mgos_imu_acc *dev, void *imu_user_data) { 38 | if (!dev) { 39 | return false; 40 | } 41 | 42 | // POWER_CTL: 00; link=0; auto_sleep=0; measure=1; sleep=0; wakeup=00 (8Hz) 43 | // DATA_FORMAT: SELF_TEST=0; SPI=0; INT_INVERT=0; 0; FULL_RES=1; Justify=0; Range=10 (8G) 44 | // BW_RATE: 000; LOW_POWER=0; Rate=1010 (100Hz ODR, 50Hz filter) 45 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ADXL345_REG_POWER_CTL, 0x08); 46 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ADXL345_REG_DATA_FORMAT, 0x0A); 47 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ADXL345_REG_BW_RATE, 0x0B); 48 | 49 | // FULL_RES: 3.9mg/LSB always 50 | dev->scale = 0.0039; 51 | return true; 52 | 53 | (void)imu_user_data; 54 | } 55 | 56 | bool mgos_imu_adxl345_read(struct mgos_imu_acc *dev, void *imu_user_data) { 57 | uint8_t data[6]; 58 | 59 | if (!dev) { 60 | return false; 61 | } 62 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_ADXL345_REG_DATA_OUT, 6, data)) { 63 | return false; 64 | } 65 | dev->ax = (data[0]) | (data[1] << 8); 66 | dev->ay = (data[2]) | (data[3] << 8); 67 | dev->az = (data[4]) | (data[5] << 8); 68 | 69 | return true; 70 | 71 | (void)imu_user_data; 72 | } 73 | -------------------------------------------------------------------------------- /src/mgos_imu_mag3110.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_mag3110.h" 20 | 21 | bool mgos_imu_mag3110_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_MAG3110_REG_WHO_AM_I); 29 | if (device_id == MGOS_MAG3110_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | 34 | (void)imu_user_data; 35 | } 36 | 37 | bool mgos_imu_mag3110_create(struct mgos_imu_mag *dev, void *imu_user_data) { 38 | if (!dev) { 39 | return false; 40 | } 41 | 42 | // Standby 43 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MAG3110_REG_CTRL_REG1, 0x00); 44 | mgos_usleep(10000); 45 | 46 | // Enable automatic magnetic sensor resets by setting bit AUTO_MRST_EN 47 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MAG3110_REG_CTRL_REG2, 0x80); 48 | mgos_usleep(20000); 49 | 50 | // Put MAG3110 in active mode 10 Hz ODR with 128x oversampling, noise 0.25 uT RMS 51 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MAG3110_REG_CTRL_REG1, 0x19); 52 | mgos_usleep(20000); 53 | 54 | dev->scale = 0.001; 55 | dev->bias[0] = 1.0; 56 | dev->bias[1] = 1.0; 57 | dev->bias[2] = 1.0; 58 | 59 | return true; 60 | 61 | (void)imu_user_data; 62 | } 63 | 64 | bool mgos_imu_mag3110_read(struct mgos_imu_mag *dev, void *imu_user_data) { 65 | uint8_t data[6]; 66 | 67 | if (!dev) { 68 | return false; 69 | } 70 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_MAG3110_REG_OUT_X_MSB, 6, data)) { 71 | return false; 72 | } 73 | 74 | dev->mx = (data[1] << 8) | (data[0]); 75 | dev->my = (data[3] << 8) | (data[2]); 76 | dev->mz = (data[5] << 8) | (data[4]); 77 | return true; 78 | 79 | (void)imu_user_data; 80 | } 81 | -------------------------------------------------------------------------------- /src/mgos_imu_l3gd20.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_L3GD20_DEFAULT_I2CADDR (0x68) 23 | #define MGOS_L3GD20_DEVID (0xD4) 24 | #define MGOS_L3GD20H_DEVID (0xD7) 25 | 26 | // L3GD20 Registers 27 | #define MGOS_L3GD20_REG_WHO_AM_I (0x0F) 28 | #define MGOS_L3GD20_REG_CTRL_REG1 (0x20) 29 | #define MGOS_L3GD20_REG_CTRL_REG2 (0x21) 30 | #define MGOS_L3GD20_REG_CTRL_REG3 (0x22) 31 | #define MGOS_L3GD20_REG_CTRL_REG4 (0x23) 32 | #define MGOS_L3GD20_REG_CTRL_REG5 (0x24) 33 | #define MGOS_L3GD20_REG_DATACAPTURE (0x25) 34 | #define MGOS_L3GD20_REG_OUT_TEMP (0x26) 35 | #define MGOS_L3GD20_REG_STATUS_REG (0x27) 36 | #define MGOS_L3GD20_REG_OUT_X_L (0x28) 37 | #define MGOS_L3GD20_REG_OUT_X_H (0x29) 38 | #define MGOS_L3GD20_REG_OUT_Y_L (0x2A) 39 | #define MGOS_L3GD20_REG_OUT_Y_H (0x2B) 40 | #define MGOS_L3GD20_REG_OUT_Z_L (0x2C) 41 | #define MGOS_L3GD20_REG_OUT_Z_H (0x2D) 42 | #define MGOS_L3GD20_REG_FIFO_CTRL_REG (0x2E) 43 | #define MGOS_L3GD20_REG_FIFO_SRC_REG (0x2F) 44 | #define MGOS_L3GD20_REG_INT1_CFG (0x30) 45 | #define MGOS_L3GD20_REG_INT1_SRC (0x31) 46 | #define MGOS_L3GD20_REG_INT1_THS_XH (0x32) 47 | #define MGOS_L3GD20_REG_INT1_THS_XL (0x33) 48 | #define MGOS_L3GD20_REG_INT1_THS_YH (0x34) 49 | #define MGOS_L3GD20_REG_INT1_THS_YL (0x35) 50 | #define MGOS_L3GD20_REG_INT1_THS_ZH (0x36) 51 | #define MGOS_L3GD20_REG_INT1_THS_ZL (0x37) 52 | #define MGOS_L3GD20_REG_INT1_DURATION (0x38) 53 | 54 | bool mgos_imu_l3gd20_detect(struct mgos_imu_gyro *dev, void *imu_user_data); 55 | bool mgos_imu_l3gd20_create(struct mgos_imu_gyro *dev, void *imu_user_data); 56 | bool mgos_imu_l3gd20_read(struct mgos_imu_gyro *dev, void *imu_user_data); 57 | -------------------------------------------------------------------------------- /src/mgos_imu_hmc5883l.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_hmc5883l.h" 20 | 21 | bool mgos_imu_hmc5883l_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 22 | int idA, idB, idC; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | idA = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_ID_A); 29 | idB = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_ID_B); 30 | idC = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_ID_C); 31 | if (idA == 'H' && idB == '4' && idC == '3') { 32 | return true; 33 | } 34 | 35 | return false; 36 | 37 | (void)imu_user_data; 38 | } 39 | 40 | bool mgos_imu_hmc5883l_create(struct mgos_imu_mag *dev, void *imu_user_data) { 41 | if (!dev) { 42 | return false; 43 | } 44 | 45 | // CRA: 0; MA=00 (no averaging); DO=100 (15Hz ODR); MS=00 (normal) 46 | // CRB: GN=001 (1.3Ga); 00000 47 | // MODE: HS=0; 00000; MD=00 (continuous) 48 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_CONF_A, 0x10)) { 49 | return false; 50 | } 51 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_CONF_B, 0x20)) { 52 | return false; 53 | } 54 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_MODE, 0x00)) { 55 | return false; 56 | } 57 | mgos_usleep(1000); 58 | 59 | dev->scale = 0.92; 60 | dev->bias[0] = 1.0; 61 | dev->bias[1] = 1.0; 62 | dev->bias[2] = 1.0; 63 | 64 | return true; 65 | 66 | (void)imu_user_data; 67 | } 68 | 69 | bool mgos_imu_hmc5883l_read(struct mgos_imu_mag *dev, void *imu_user_data) { 70 | uint8_t data[6]; 71 | 72 | if (!dev) { 73 | return false; 74 | } 75 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_HMC5883L_REG_OUT_X_MSB, 6, data)) { 76 | return false; 77 | } 78 | 79 | dev->mx = (data[0] << 8) | (data[1]); 80 | dev->my = (data[2] << 8) | (data[3]); 81 | dev->mz = (data[4] << 8) | (data[5]); 82 | return true; 83 | 84 | (void)imu_user_data; 85 | } 86 | -------------------------------------------------------------------------------- /src/mgos_imu_itg3205.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_itg3205.h" 20 | 21 | bool mgos_imu_itg3205_detect(struct mgos_imu_gyro *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_ITG3205_REG_WHO_AM_I); 29 | if (device_id == MGOS_ITG3205_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | 34 | (void)imu_user_data; 35 | } 36 | 37 | bool mgos_imu_itg3205_create(struct mgos_imu_gyro *dev, void *imu_user_data) { 38 | if (!dev) { 39 | return false; 40 | } 41 | 42 | // Reset 43 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ITG3205_REG_PWR_MGM, 0x80)) { 44 | return false; 45 | } 46 | mgos_usleep(2000); 47 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ITG3205_REG_PWR_MGM, 0x00)) { 48 | return false; 49 | } 50 | 51 | // SMPLRT_DIV: 00000111 (7, yielding 125Hz ODR with 8x oversampling) 52 | // DLPF_FS: 000; FS_SEL=11 (2000deg/sec); DLPF_CFG=011 (LPF=42Hz, samplerate=1kHz) 53 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ITG3205_REG_SMPLRT_DIV, 7)) { 54 | return false; 55 | } 56 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_ITG3205_REG_DLPF_FS, 0x1B)) { 57 | return false; 58 | } 59 | 60 | // Datasheet says 2000dps is 14.375 LSB per deg/sec 61 | dev->scale = (1 / 14.375); 62 | 63 | return true; 64 | 65 | (void)imu_user_data; 66 | } 67 | 68 | bool mgos_imu_itg3205_read(struct mgos_imu_gyro *dev, void *imu_user_data) { 69 | uint8_t data[6]; 70 | 71 | if (!dev) { 72 | return false; 73 | } 74 | 75 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_ITG3205_REG_GYRO_XOUT_H, 6, data)) { 76 | return false; 77 | } 78 | dev->gx = (int16_t)((data[0] << 8) | data[1]); 79 | dev->gy = (int16_t)((data[2] << 8) | data[3]); 80 | dev->gz = (int16_t)((data[4] << 8) | data[5]); 81 | 82 | return true; 83 | 84 | (void)imu_user_data; 85 | } 86 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # How to contribute 2 | 3 | We'd love to accept your patches and contributions to this project. There are 4 | a just a few small guidelines you need to follow. 5 | 6 | ## Contributor License Agreement 7 | 8 | Contributions to any Google project must be accompanied by a Contributor 9 | License Agreement. This is not a copyright **assignment**, it simply gives 10 | Google permission to use and redistribute your contributions as part of the 11 | project. 12 | 13 | If you are an individual writing original source code and you're sure you own 14 | the intellectual property, then you'll need to sign an [individual 15 | CLA](https://cla.developers.google.com/about/google-individual). 16 | 17 | If you work for a company that wants to allow you to contribute your work, then 18 | you'll need to sign a [corporate 19 | CLA](https://cla.developers.google.com/about/google-corporate). 20 | 21 | You generally only need to submit a CLA once, so if you've already submitted 22 | one (even if it was for a different project), you probably don't need to do it 23 | again. 24 | 25 | ## Submitting a patch 26 | 27 | 1. It's generally best to start by opening a new issue describing the bug or 28 | feature you're intending to fix. Even if you think it's relatively minor, 29 | it's helpful to know what people are working on. Mention in the initial 30 | issue that you are planning to work on that bug or feature so that it can 31 | be assigned to you. 32 | 33 | 1. Follow the normal process of [forking](https://help.github.com/articles/fork-a-repo) 34 | the project, and setup a new branch to work in. It's important that each 35 | group of changes be done in separate branches in order to ensure that a 36 | pull request only includes the commits related to that bug or feature. 37 | 38 | 1. Any significant changes should almost always be accompanied by tests. The 39 | project already has good test coverage, so look at some of the existing 40 | tests if you're unsure how to go about it. 41 | 42 | 1. All contributions must be licensed Apache 2.0 and all files must have a 43 | copy of the boilerplate licence comment which can be generated by the 44 | [autogen](https://github.com/mbrukman/autogen) tool. 45 | 46 | 1. Do your best to have [well-formed commit messages](http://tbaggery.com/2008/04/19/a-note-about-git-commit-messages.html) 47 | for each change. This provides consistency throughout the project, and 48 | ensures that commit messages are able to be formatted properly by various 49 | git tools. 50 | 51 | 1. Finally, push the commits to your fork and submit a [pull request](https://help.github.com/articles/creating-a-pull-request). 52 | -------------------------------------------------------------------------------- /third-party/bosch/include/bmm150.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. 3 | * 4 | * BSD-3-Clause 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 29 | * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 30 | * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | /* Original header has been conveniently modified to fit code modifications */ 36 | 37 | #include "mgos.h" 38 | #include "mgos_imu_internal.h" 39 | 40 | // internal API to obtain compensated magnetometer data in uT 41 | // trim data structure 42 | struct mgos_imu_bmm150_trim_registers { 43 | int8_t dig_x1; 44 | int8_t dig_y1; 45 | int8_t dig_x2; 46 | int8_t dig_y2; 47 | uint16_t dig_z1; 48 | int16_t dig_z2; 49 | int16_t dig_z3; 50 | int16_t dig_z4; 51 | uint8_t dig_xy1; 52 | int8_t dig_xy2; 53 | uint16_t dig_xyz1; 54 | }; 55 | 56 | bool read_trim_registers(struct mgos_imu_mag *dev); 57 | int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall, 58 | const struct mgos_imu_bmm150_trim_registers *trim_data); 59 | int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall, 60 | const struct mgos_imu_bmm150_trim_registers *trim_data); 61 | int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall, 62 | const struct mgos_imu_bmm150_trim_registers *trim_data); 63 | -------------------------------------------------------------------------------- /src/mgos_imu_ak8963.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_ak8963.h" 20 | 21 | bool mgos_imu_ak8963_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_WHO_AM_I); 29 | if (device_id == MGOS_AK8963_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | 34 | (void)imu_user_data; 35 | } 36 | 37 | bool mgos_imu_ak8963_create(struct mgos_imu_mag *dev, void *imu_user_data) { 38 | if (!dev) { 39 | return false; 40 | } 41 | 42 | // Reset 43 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_CNTL, 0x00); 44 | mgos_usleep(10000); 45 | 46 | // Fuse ROM access mode 47 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_CNTL, 0x0F); 48 | mgos_usleep(10000); 49 | 50 | uint8_t data[3]; 51 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_ASAX, 3, data)) { 52 | LOG(LL_ERROR, ("Could not read magnetometer adjustment registers")); 53 | return false; 54 | } 55 | dev->bias[0] = (float)(data[0] - 128) / 256. + 1.; 56 | dev->bias[1] = (float)(data[1] - 128) / 256. + 1.; 57 | dev->bias[2] = (float)(data[2] - 128) / 256. + 1.; 58 | 59 | LOG(LL_DEBUG, ("Magnetometer adjustment bias %.2f %.2f %.2f", dev->bias[0], dev->bias[1], dev->bias[2])); 60 | 61 | // Reset 62 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_CNTL, 0x00); 63 | mgos_usleep(10000); 64 | 65 | // Set magnetometer config: 16-bit, continuous measurement mode 2 (100Hz) 66 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_CNTL, 0x16); 67 | mgos_usleep(10000); 68 | dev->scale = 4192.0 / 32768.0; 69 | 70 | return true; 71 | 72 | (void)imu_user_data; 73 | } 74 | 75 | bool mgos_imu_ak8963_read(struct mgos_imu_mag *dev, void *imu_user_data) { 76 | uint8_t data[7]; 77 | 78 | if (!dev) { 79 | return false; 80 | } 81 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_AK8963_REG_XOUT_L, 7, data)) { 82 | return false; 83 | } 84 | if (data[6] & 0x08) { 85 | return false; 86 | } 87 | 88 | dev->mx = (data[1] << 8) | (data[0]); 89 | dev->my = (data[3] << 8) | (data[2]); 90 | dev->mz = (data[5] << 8) | (data[4]); 91 | return true; 92 | 93 | (void)imu_user_data; 94 | } 95 | -------------------------------------------------------------------------------- /src/mgos_imu_l3gd20.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_l3gd20.h" 20 | 21 | bool mgos_imu_l3gd20_detect(struct mgos_imu_gyro *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_WHO_AM_I); 29 | switch (device_id) { 30 | case MGOS_L3GD20_DEVID: dev->opts.type = GYRO_L3GD20; return true; 31 | 32 | case MGOS_L3GD20H_DEVID: dev->opts.type = GYRO_L3GD20H; return true; 33 | 34 | default: 35 | break; 36 | } 37 | return false; 38 | 39 | (void)imu_user_data; 40 | } 41 | 42 | bool mgos_imu_l3gd20_create(struct mgos_imu_gyro *dev, void *imu_user_data) { 43 | if (!dev) { 44 | return false; 45 | } 46 | 47 | // Reset 48 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_CTRL_REG1, 0x00)) { 49 | return false; 50 | } 51 | mgos_usleep(5000); 52 | 53 | // Enable sensors: DR=01 (190Hz) BW=10 (50Hz) PD=1 [XYZ]EN=1 54 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_CTRL_REG1, 0x6F)) { 55 | return false; 56 | } 57 | 58 | // Set 2000DPS 59 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_CTRL_REG4, 0x20)) { 60 | return false; 61 | } 62 | 63 | // Datasheet says 2000dps is 0.07 deg per LSB. 64 | dev->scale = (2000.f * 1.1468625f) / 32767.5f; 65 | 66 | return true; 67 | 68 | (void)imu_user_data; 69 | } 70 | 71 | bool mgos_imu_l3gd20_read(struct mgos_imu_gyro *dev, void *imu_user_data) { 72 | uint8_t data[6]; 73 | 74 | if (!dev) { 75 | return false; 76 | } 77 | // Device cannot read multiple registers at once. 78 | data[0] = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_OUT_X_L); 79 | data[1] = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_OUT_X_H); 80 | data[2] = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_OUT_Y_L); 81 | data[3] = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_OUT_Y_H); 82 | data[4] = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_OUT_Z_L); 83 | data[5] = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_L3GD20_REG_OUT_Z_H); 84 | dev->gx = (int16_t)((data[1] << 8) | data[0]); 85 | dev->gy = (int16_t)((data[3] << 8) | data[2]); 86 | dev->gz = (int16_t)((data[5] << 8) | data[4]); 87 | 88 | return true; 89 | 90 | (void)imu_user_data; 91 | } 92 | -------------------------------------------------------------------------------- /src/mgos_imu_bmm150.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Sergio R. Caprile and Cika Electronica S.R.L. 3 | * All rights reserved 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the ""License""); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an ""AS IS"" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "mgos.h" 21 | #include "mgos_imu_internal.h" 22 | 23 | #define MGOS_BMM150_DEFAULT_I2CADDR (0x10) 24 | #define MGOS_BMM150_DEVID (0x32) 25 | 26 | // BMM150 Magnetometer Registers 27 | #define MGOS_BMM150_REG_CHIPID (0x40) 28 | #define MGOS_BMM150_REG_OUT_X_LSB (0x42) 29 | #define MGOS_BMM150_REG_OUT_X_MSB (0x43) 30 | #define MGOS_BMM150_REG_OUT_Y_LSB (0x44) 31 | #define MGOS_BMM150_REG_OUT_Y_MSB (0x45) 32 | #define MGOS_BMM150_REG_OUT_Z_LSB (0x46) 33 | #define MGOS_BMM150_REG_OUT_Z_MSB (0x47) 34 | #define MGOS_BMM150_REG_RHALL_LSB (0x48) 35 | #define MGOS_BMM150_REG_RHALL_MSB (0x49) 36 | #define MGOS_BMM150_REG_INTSTATUS (0x4A) 37 | #define MGOS_BMM150_REG_POWMODE (0x4B) 38 | #define MGOS_BMM150_REG_OPMODE (0x4C) 39 | #define MGOS_BMM150_REG_REPXY (0x51) 40 | #define MGOS_BMM150_REG_REPZ (0x52) 41 | 42 | // BMM150 Magnetometer Trim Extended Registers 43 | #define MGOS_BMM150_DIG_X1 (0x5D) 44 | #define MGOS_BMM150_DIG_Y1 (0x5E) 45 | #define MGOS_BMM150_DIG_Z4_LSB (0x62) 46 | #define MGOS_BMM150_DIG_Z4_MSB (0x63) 47 | #define MGOS_BMM150_DIG_X2 (0x64) 48 | #define MGOS_BMM150_DIG_Y2 (0x65) 49 | #define MGOS_BMM150_DIG_Z2_LSB (0x68) 50 | #define MGOS_BMM150_DIG_Z2_MSB (0x69) 51 | #define MGOS_BMM150_DIG_Z1_LSB (0x6A) 52 | #define MGOS_BMM150_DIG_Z1_MSB (0x6B) 53 | #define MGOS_BMM150_DIG_XYZ1_LSB (0x6C) 54 | #define MGOS_BMM150_DIG_XYZ1_MSB (0x6D) 55 | #define MGOS_BMM150_DIG_Z3_LSB (0x6E) 56 | #define MGOS_BMM150_DIG_Z3_MSB (0x6F) 57 | #define MGOS_BMM150_DIG_XY2 (0x70) 58 | #define MGOS_BMM150_DIG_XY1 (0x71) 59 | 60 | // OVERFLOW DEFINITIONS 61 | #define MGOS_BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP (-4096) 62 | #define MGOS_BMM150_OVERFLOW_ADCVAL_ZAXIS_HALL (-16384) 63 | #define MGOS_BMM150_OVERFLOW_OUTPUT (-32768) 64 | #define MGOS_BMM150_NEGATIVE_SATURATION_Z (-32767) 65 | #define MGOS_BMM150_POSITIVE_SATURATION_Z (32767) 66 | 67 | 68 | bool mgos_imu_bmm150_detect(struct mgos_imu_mag *dev, void *imu_user_data); 69 | bool mgos_imu_bmm150_create(struct mgos_imu_mag *dev, void *imu_user_data); 70 | bool mgos_imu_bmm150_read(struct mgos_imu_mag *dev, void *imu_user_data); 71 | -------------------------------------------------------------------------------- /src/mgos_imu_ak8975.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_ak8975.h" 20 | 21 | bool mgos_imu_ak8975_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 22 | int device_id; 23 | 24 | if (!dev) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_WHO_AM_I); 29 | if (device_id == MGOS_AK8975_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | 34 | (void)imu_user_data; 35 | } 36 | 37 | bool mgos_imu_ak8975_create(struct mgos_imu_mag *dev, void *imu_user_data) { 38 | if (!dev) { 39 | return false; 40 | } 41 | 42 | // Reset 43 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_CNTL, 0x00); 44 | mgos_usleep(10000); 45 | 46 | // Fuse ROM access mode 47 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_CNTL, 0x0F); 48 | mgos_usleep(10000); 49 | 50 | uint8_t data[3]; 51 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_ASAX, 3, data)) { 52 | LOG(LL_ERROR, ("Could not read magnetometer adjustment registers")); 53 | return false; 54 | } 55 | dev->bias[0] = (float)(data[0] - 128) / 256. + 1.; 56 | dev->bias[1] = (float)(data[1] - 128) / 256. + 1.; 57 | dev->bias[2] = (float)(data[2] - 128) / 256. + 1.; 58 | 59 | LOG(LL_DEBUG, ("Magnetometer adjustment bias %.2f %.2f %.2f", dev->bias[0], dev->bias[1], dev->bias[2])); 60 | 61 | // Reset 62 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_CNTL, 0x00); 63 | mgos_usleep(10000); 64 | 65 | dev->scale = 1229.0 / 4096.0; 66 | 67 | return true; 68 | 69 | (void)imu_user_data; 70 | } 71 | 72 | bool mgos_imu_ak8975_read(struct mgos_imu_mag *dev, void *imu_user_data) { 73 | uint8_t data[6]; 74 | int drdy; 75 | 76 | if (!dev) { 77 | return false; 78 | } 79 | 80 | // Set single shot measurement 81 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_CNTL, 0x01); 82 | mgos_usleep(9500); 83 | 84 | // Check Data Ready 85 | drdy = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_ST1); 86 | if (drdy != 0x01) { 87 | return false; 88 | } 89 | 90 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_AK8975_REG_XOUT_L, 6, data)) { 91 | return false; 92 | } 93 | 94 | dev->mx = (data[1] << 8) | (data[0]); 95 | dev->my = (data[3] << 8) | (data[2]); 96 | dev->mz = (data[5] << 8) | (data[4]); 97 | return true; 98 | 99 | (void)imu_user_data; 100 | } 101 | -------------------------------------------------------------------------------- /src/mgos_imu_mma8451.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_MMA8451_DEFAULT_I2CADDR (0x68) 23 | #define MGOS_MMA8451_DEVID (0x1A) 24 | 25 | // MMA8451 -- Registers 26 | #define MGOS_MMA8451_REG_STATUS (0x00) 27 | #define MGOS_MMA8451_REG_OUT_X_MSB (0x01) 28 | #define MGOS_MMA8451_REG_OUT_X_LSB (0x02) 29 | #define MGOS_MMA8451_REG_OUT_Y_MSB (0x03) 30 | #define MGOS_MMA8451_REG_OUT_Y_LSB (0x04) 31 | #define MGOS_MMA8451_REG_OUT_Z_MSB (0x05) 32 | #define MGOS_MMA8451_REG_OUT_Z_LSB (0x06) 33 | #define MGOS_MMA8451_REG_F_SETUP (0x09) 34 | #define MGOS_MMA8451_REG_TRIG_CFG (0x0A) 35 | #define MGOS_MMA8451_REG_SYSMOD (0x0B) 36 | #define MGOS_MMA8451_REG_INT_SOURCE (0x0C) 37 | #define MGOS_MMA8451_REG_WHO_AM_I (0x0D) 38 | #define MGOS_MMA8451_REG_XYZ_DATA_CFG (0x0E) 39 | #define MGOS_MMA8451_REG_HP_FILTER_CUTOFF (0x0F) 40 | #define MGOS_MMA8451_REG_PL_STATUS (0x10) 41 | #define MGOS_MMA8451_REG_PL_CFG (0x11) 42 | #define MGOS_MMA8451_REG_PL_COUNT (0x12) 43 | #define MGOS_MMA8451_REG_PL_BF_ZCOMP (0x13) 44 | #define MGOS_MMA8451_REG_P_L_THS_REG (0x14) 45 | #define MGOS_MMA8451_REG_FF_MT_CFG (0x15) 46 | #define MGOS_MMA8451_REG_FF_MT_SRC (0x16) 47 | #define MGOS_MMA8451_REG_FF_MT_THS (0x17) 48 | #define MGOS_MMA8451_REG_FF_MT_COUNT (0x18) 49 | #define MGOS_MMA8451_REG_TRANSIENT_CFG (0x1D) 50 | #define MGOS_MMA8451_REG_TRANSIENT_SCR (0x1E) 51 | #define MGOS_MMA8451_REG_TRANSIENT_THS (0x1F) 52 | #define MGOS_MMA8451_REG_TRANSIENT_COUNT (0x20) 53 | #define MGOS_MMA8451_REG_PULSE_CFG (0x21) 54 | #define MGOS_MMA8451_REG_PULSE_SRC (0x22) 55 | #define MGOS_MMA8451_REG_PULSE_THSX (0x23) 56 | #define MGOS_MMA8451_REG_PULSE_THSY (0x24) 57 | #define MGOS_MMA8451_REG_PULSE_THSZ (0x25) 58 | #define MGOS_MMA8451_REG_PULSE_TMLT (0x26) 59 | #define MGOS_MMA8451_REG_PULSE_LTCY (0x27) 60 | #define MGOS_MMA8451_REG_PULSE_WIND (0x28) 61 | #define MGOS_MMA8451_REG_ASLP_COUNT (0x29) 62 | #define MGOS_MMA8451_REG_CTRL_REG1 (0x2A) 63 | #define MGOS_MMA8451_REG_CTRL_REG2 (0x2B) 64 | #define MGOS_MMA8451_REG_CTRL_REG3 (0x2C) 65 | #define MGOS_MMA8451_REG_CTRL_REG4 (0x2D) 66 | #define MGOS_MMA8451_REG_CTRL_REG5 (0x2E) 67 | #define MGOS_MMA8451_REG_OFF_X (0x2F) 68 | #define MGOS_MMA8451_REG_OFF_Y (0x30) 69 | #define MGOS_MMA8451_REG_OFF_Z (0x31) 70 | 71 | bool mgos_imu_mma8451_detect(struct mgos_imu_acc *dev, void *imu_user_data); 72 | bool mgos_imu_mma8451_create(struct mgos_imu_acc *dev, void *imu_user_data); 73 | bool mgos_imu_mma8451_read(struct mgos_imu_acc *dev, void *imu_user_data); 74 | -------------------------------------------------------------------------------- /src/mgos_imu_mpu925x.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_MPU9250_DEFAULT_I2CADDR (0x68) 23 | 24 | #define MGOS_MPU9250_DEVID_9250 (0x71) 25 | #define MGOS_MPU9250_DEVID_9255 (0x73) 26 | 27 | // MPU9250 -- Accelerometer and Gyro registers 28 | #define MGOS_MPU9250_REG_SMPLRT_DIV (0x19) 29 | #define MGOS_MPU9250_REG_CONFIG (0x1A) 30 | #define MGOS_MPU9250_REG_GYRO_CONFIG (0x1B) 31 | #define MGOS_MPU9250_REG_ACCEL_CONFIG (0x1C) 32 | #define MGOS_MPU9250_REG_ACCEL_CONFIG2 (0x1D) 33 | #define MGOS_MPU9250_REG_INT_PIN_CFG (0x37) 34 | #define MGOS_MPU9250_REG_INT_ENABLE (0x38) 35 | #define MGOS_MPU9250_REG_INT_STATUS (0x3A) 36 | #define MGOS_MPU9250_REG_ACCEL_XOUT_H (0x3B) 37 | #define MGOS_MPU9250_REG_TEMP_OUT_H (0x41) 38 | #define MGOS_MPU9250_REG_GYRO_XOUT_H (0x43) 39 | #define MGOS_MPU9250_REG_PWR_MGMT_1 (0x6B) 40 | #define MGOS_MPU9250_REG_PWR_MGMT_2 (0x6C) 41 | #define MGOS_MPU9250_REG_WHO_AM_I (0x75) 42 | 43 | #define MGOS_MPU9250_ACCEL_FS_SEL_2G (0x00) 44 | #define MGOS_MPU9250_ACCEL_FS_SEL_4G (0x08) 45 | #define MGOS_MPU9250_ACCEL_FS_SEL_8G (0x10) 46 | #define MGOS_MPU9250_ACCEL_FS_SEL_16G (0x18) 47 | #define MGOS_MPU9250_GYRO_FS_SEL_250DPS (0x00) 48 | #define MGOS_MPU9250_GYRO_FS_SEL_500DPS (0x08) 49 | #define MGOS_MPU9250_GYRO_FS_SEL_1000DPS (0x10) 50 | #define MGOS_MPU9250_GYRO_FS_SEL_2000DPS (0x18) 51 | 52 | #define MGOS_MPU9250_DLPF_184 (0x01) 53 | #define MGOS_MPU9250_DLPF_92 (0x02) 54 | #define MGOS_MPU9250_DLPF_41 (0x03) 55 | #define MGOS_MPU9250_DLPF_20 (0x04) 56 | #define MGOS_MPU9250_DLPF_10 (0x05) 57 | #define MGOS_MPU9250_DLPF_5 (0x06) 58 | 59 | struct mgos_imu_mpu925x_userdata { 60 | bool initialized; 61 | }; 62 | 63 | struct mgos_imu_mpu925x_userdata *mgos_imu_mpu925x_userdata_create(void); 64 | 65 | bool mgos_imu_mpu925x_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 66 | bool mgos_imu_mpu925x_acc_create(struct mgos_imu_acc *dev, void *imu_user_data); 67 | bool mgos_imu_mpu925x_acc_read(struct mgos_imu_acc *dev, void *imu_user_data); 68 | bool mgos_imu_mpu925x_acc_get_scale(struct mgos_imu_acc *dev, void *imu_user_data, float *scale); 69 | bool mgos_imu_mpu925x_acc_set_scale(struct mgos_imu_acc *dev, void *imu_user_data, float scale); 70 | 71 | bool mgos_imu_mpu925x_gyro_detect(struct mgos_imu_gyro *dev, void *imu_user_data); 72 | bool mgos_imu_mpu925x_gyro_create(struct mgos_imu_gyro *dev, void *imu_user_data); 73 | bool mgos_imu_mpu925x_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data); 74 | bool mgos_imu_mpu925x_gyro_get_scale(struct mgos_imu_gyro *dev, void *imu_user_data, float *scale); 75 | bool mgos_imu_mpu925x_gyro_set_scale(struct mgos_imu_gyro *dev, void *imu_user_data, float scale); 76 | -------------------------------------------------------------------------------- /src/madgwick.h: -------------------------------------------------------------------------------- 1 | //============================================================================================= 2 | // madgwick.h 3 | //============================================================================================= 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 7 | // 8 | // From the x-io website "Open-source resources available on this website are 9 | // provided under the GNU General Public Licence unless an alternative licence 10 | // is provided in source." 11 | // 12 | // Date Author Notes 13 | // 29/09/2011 SOH Madgwick Initial release 14 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 15 | // 12/11/2018 Pim van Pelt Rewritten to C for Mongoose OS 16 | // 17 | //============================================================================================= 18 | #pragma once 19 | #include "mgos.h" 20 | #include 21 | 22 | /* Madgwick filter structure. */ 23 | struct mgos_imu_madgwick { 24 | float beta; 25 | float q0; 26 | float q1; 27 | float q2; 28 | float q3; 29 | float freq; 30 | float inv_freq; 31 | uint32_t counter; 32 | }; 33 | 34 | /* Create a new filter and initialize it by resetting the Quaternion and setting 35 | * the update rate to 100Hz and the gain to 0.1 36 | * Returns a pointer to a `struct mgos_imu_madgwick`, or NULL otherwise. 37 | */ 38 | struct mgos_imu_madgwick *mgos_imu_madgwick_create(void); 39 | 40 | /* Clean up and return memory for the filter 41 | */ 42 | bool mgos_imu_madgwick_destroy(struct mgos_imu_madgwick **filter); 43 | 44 | /* Sets the filter update rate and gain (defaults to freq=100Hz and gain=0.1) 45 | * The `mgos_imu_madgwick_update()` function then expects to be called at `freq` 46 | * per second. 47 | */ 48 | bool mgos_imu_madgwick_set_params(struct mgos_imu_madgwick *filter, float frequency, float beta); 49 | 50 | /* Resets the filter Quaternion to an initial state (of {1,0,0,0}). 51 | */ 52 | bool mgos_imu_madgwick_reset(struct mgos_imu_madgwick *filter); 53 | 54 | /* Run an update cycle on the filter. Inputs gx/gy/gz are in any calibrated input (for example, 55 | * m/s/s or G), inputs of ax/ay/az are in Rads/sec, inputs of mx/my/mz are in any calibrated 56 | * input (for example, uTesla or Gauss). The inputs of mx/my/mz can be passed as 0.0, in which 57 | * case the magnetometer fusion will not occur. 58 | * Returns true on success, false on failure. 59 | */ 60 | bool mgos_imu_madgwick_update(struct mgos_imu_madgwick *filter, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 61 | 62 | /* 63 | * Returns AHRS Quaternion, as values between -1.0 and +1.0. 64 | * Each of q0, q1, q2, q3 pointers may be NULL, in which case they will not be 65 | * filled in. 66 | * Returns true on success, false in case of error, in which case the values of 67 | * q0, q1, q2 and q3 are undetermined. 68 | */ 69 | bool mgos_imu_madgwick_get_quaternion(struct mgos_imu_madgwick *filter, float *q0, float *q1, float *q2, float *q3); 70 | 71 | /* 72 | * Returns AHRS angles of roll, pitch and yaw, in Radians between -Pi and +Pi. 73 | * Each of the roll, pitch and yaw pointers may be NULL, in which case they will 74 | * not be filled in. 75 | * Returns true on success, false in case of error, in which case the values of 76 | * roll, pitch and yaw are undetermined. 77 | */ 78 | bool mgos_imu_madgwick_get_angles(struct mgos_imu_madgwick *filter, float *roll, float *pitch, float *yaw); 79 | 80 | /* 81 | * Returns filter counter. Each call to `mgos_imu_madgwick_update()` increments the 82 | * counter by one. 83 | * Returns true on success, false in case of error, in which case the value of 84 | * counter is undetermined. 85 | */ 86 | bool mgos_imu_madgwick_get_counter(struct mgos_imu_madgwick *filter, uint32_t *counter); 87 | -------------------------------------------------------------------------------- /src/mgos_imu_bmm150.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Sergio R. Caprile and Cika Electronica S.R.L. 3 | * All rights reserved 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the ""License""); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an ""AS IS"" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "mgos_imu_bmm150.h" 19 | #include "bmm150.h" 20 | #include "mgos.h" 21 | #include "mgos_i2c.h" 22 | 23 | bool mgos_imu_bmm150_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 24 | int device_id; 25 | 26 | if (!dev) { 27 | return false; 28 | } 29 | 30 | // Exit from Suspend mode 31 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_POWMODE, 0x01); 32 | mgos_usleep(5000); 33 | 34 | // Now read Chip ID 35 | device_id = 36 | mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_CHIPID); 37 | if (device_id == MGOS_BMM150_DEVID) { 38 | return true; 39 | } 40 | return false; 41 | 42 | (void)imu_user_data; 43 | } 44 | 45 | bool mgos_imu_bmm150_create(struct mgos_imu_mag *dev, void *imu_user_data) { 46 | struct mgos_imu_bmm150_trim_registers *imud; 47 | 48 | if (!dev) { 49 | return false; 50 | } 51 | 52 | imud = calloc(1, sizeof(struct mgos_imu_bmm150_trim_registers)); 53 | if (!imud) { 54 | return false; 55 | } 56 | dev->user_data = imud; 57 | 58 | // Exit from Suspend mode 59 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_POWMODE, 0x01); 60 | mgos_usleep(5000); 61 | 62 | // Soft Reset 63 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_POWMODE, 0x83); 64 | mgos_usleep(5000); 65 | 66 | // Regular repetition rate, active mode 10 Hz ODR, noise 0.6 uT RMS 67 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_OPMODE, 0x00); 68 | // datasheet page 31, table 3 in page 13. Bosch API sets as if 1+2REPZ (?) 69 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_REPXY, 70 | 4); // 9; 1+2REPXY 71 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_REPZ, 72 | 14); // 15; 1+REPZ 73 | mgos_usleep(5000); 74 | 75 | dev->scale = 0.3; 76 | dev->bias[0] = 1.0; 77 | dev->bias[1] = 1.0; 78 | dev->bias[2] = 1.0; 79 | 80 | return read_trim_registers(dev); 81 | 82 | (void)imu_user_data; 83 | } 84 | 85 | bool mgos_imu_bmm150_read(struct mgos_imu_mag *dev, void *imu_user_data) { 86 | uint8_t reg_data[8]; 87 | int16_t raw_datax, raw_datay, raw_dataz; 88 | uint16_t raw_data_r; 89 | struct mgos_imu_bmm150_trim_registers *trim_regs = 90 | (struct mgos_imu_bmm150_trim_registers *)dev->user_data; 91 | 92 | if (!dev) { 93 | return false; 94 | } 95 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_BMM150_REG_OUT_X_LSB, 8, 96 | reg_data)) { 97 | return false; 98 | } 99 | 100 | raw_datax = (((int8_t)reg_data[1]) << 5) | 101 | (((uint8_t)(reg_data[0] & 0xF8) >> 3)); // 13-bits LSB first 102 | raw_datay = (((int8_t)reg_data[3]) << 5) | 103 | (((uint8_t)(reg_data[2] & 0xF8) >> 3)); // 13-bits 104 | raw_dataz = (((int8_t)reg_data[5]) << 7) | 105 | (((uint8_t)(reg_data[4] & 0xFE) >> 1)); // 15-bits 106 | raw_data_r = 107 | (reg_data[7] << 6) | (((uint8_t)(reg_data[6] & 0xFE) >> 2)); // 14-bits 108 | 109 | /* Compensated Mag data in int16_t format */ 110 | dev->mx = compensate_x(raw_datax, raw_data_r, trim_regs); 111 | dev->my = compensate_y(raw_datay, raw_data_r, trim_regs); 112 | dev->mz = compensate_z(raw_dataz, raw_data_r, trim_regs); 113 | 114 | return true; 115 | 116 | (void)imu_user_data; 117 | } 118 | -------------------------------------------------------------------------------- /src/mgos_imu_icm20948.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Vasily Kiniv 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_ICM20948_DEFAULT_I2CADDR (0x68) 23 | #define MGOS_ICM20948_DEFAULT_M_I2CADDR (0x0C) 24 | #define MGOS_ICM20948_DEVID (0xEA) 25 | #define MGOS_ICM20948_DEVID_M (0x09) 26 | 27 | // ICM20948 -- Registers (Acc/Gyro) 28 | #define MGOS_ICM20948_REG0_WHO_AM_I (0x00) 29 | #define MGOS_ICM20948_REG0_USER_CTRL (0x03) 30 | #define MGOS_ICM20948_REG0_LP_CONFIG (0x05) 31 | #define MGOS_ICM20948_REG0_PWR_MGMT_1 (0x06) 32 | #define MGOS_ICM20948_REG0_PWR_MGMT_2 (0x07) 33 | #define MGOS_ICM20948_REG0_INT_PIN_CFG (0x0f) 34 | #define MGOS_ICM20948_REG0_ACCEL_XOUT_H (0x2d) 35 | #define MGOS_ICM20948_REG0_GYRO_XOUT_H (0x33) 36 | #define MGOS_ICM20948_REG0_EXT_SLV_SENS_DATA_00 (0x3b) 37 | #define MGOS_ICM20948_REG0_BANK_SEL (0x7f) 38 | #define MGOS_ICM20948_REG2_GYRO_SMPLRT_DIV (0x00) 39 | #define MGOS_ICM20948_REG2_GYRO_CONFIG_1 (0x01) 40 | #define MGOS_ICM20948_REG2_ACCEL_SMPLRT_DIV_1 (0x10) 41 | #define MGOS_ICM20948_REG2_ACCEL_SMPLRT_DIV_2 (0x11) 42 | #define MGOS_ICM20948_REG2_ACCEL_CONFIG (0x14) 43 | #define MGOS_ICM20948_REG3_I2C_MST_CTRL (0x01) 44 | #define MGOS_ICM20948_REG3_I2C_SLV0_ADDR (0x03) 45 | #define MGOS_ICM20948_REG3_I2C_SLV0_REG (0x04) 46 | #define MGOS_ICM20948_REG3_I2C_SLV0_CTRL (0x05) 47 | #define MGOS_ICM20948_REG3_I2C_SLV0_DO (0x06) 48 | 49 | // ICM20948 -- Registers (Mag) 50 | #define MGOS_ICM20948_WHO_AM_I_M (0x01) 51 | #define MGOS_ICM20948_HXL_M (0x11) 52 | #define MGOS_ICM20948_ST2_M (0x18) 53 | #define MGOS_ICM20948_CNTL2_M (0x31) 54 | #define MGOS_ICM20948_CNTL3_M (0x32) 55 | 56 | struct mgos_imu_icm20948_userdata { 57 | bool accgyro_initialized; 58 | int8_t current_bank_no; 59 | }; 60 | 61 | struct mgos_imu_icm20948_userdata *mgos_imu_icm20948_userdata_create(void); 62 | 63 | bool mgos_imu_icm20948_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 64 | bool mgos_imu_icm20948_acc_create(struct mgos_imu_acc *dev, void *imu_user_data); 65 | bool mgos_imu_icm20948_acc_read(struct mgos_imu_acc *dev, void *imu_user_data); 66 | bool mgos_imu_icm20948_acc_get_scale(struct mgos_imu_acc *dev, void *imu_user_data, float *scale); 67 | bool mgos_imu_icm20948_acc_set_scale(struct mgos_imu_acc *dev, void *imu_user_data, float scale); 68 | bool mgos_imu_icm20948_acc_get_odr(struct mgos_imu_acc *dev, void *imu_user_data, float *odr); 69 | bool mgos_imu_icm20948_acc_set_odr(struct mgos_imu_acc *dev, void *imu_user_data, float odr); 70 | 71 | bool mgos_imu_icm20948_gyro_detect(struct mgos_imu_gyro *dev, void *imu_user_data); 72 | bool mgos_imu_icm20948_gyro_create(struct mgos_imu_gyro *dev, void *imu_user_data); 73 | bool mgos_imu_icm20948_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data); 74 | bool mgos_imu_icm20948_gyro_get_scale(struct mgos_imu_gyro *dev, void *imu_user_data, float *scale); 75 | bool mgos_imu_icm20948_gyro_set_scale(struct mgos_imu_gyro *dev, void *imu_user_data, float scale); 76 | bool mgos_imu_icm20948_gyro_get_odr(struct mgos_imu_gyro *dev, void *imu_user_data, float *odr); 77 | bool mgos_imu_icm20948_gyro_set_odr(struct mgos_imu_gyro *dev, void *imu_user_data, float odr); 78 | 79 | bool mgos_imu_icm20948_mag_detect(struct mgos_imu_mag *dev, void *imu_user_data); 80 | bool mgos_imu_icm20948_mag_create(struct mgos_imu_mag *dev, void *imu_user_data); 81 | bool mgos_imu_icm20948_mag_read(struct mgos_imu_mag *dev, void *imu_user_data); 82 | bool mgos_imu_icm20948_mag_get_odr(struct mgos_imu_mag *dev, void *imu_user_data, float *odr); 83 | bool mgos_imu_icm20948_mag_set_odr(struct mgos_imu_mag *dev, void *imu_user_data, float odr); 84 | -------------------------------------------------------------------------------- /src/mgos_imu_lsm303d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_LSM303D_DEFAULT_I2CADDR (0x1d) 23 | #define MGOS_LSM303D_DEVID (0x49) 24 | #define MGOS_LSM303DLM_DEVID (0x3c) 25 | 26 | // LSM303D -- Registers 27 | #define MGOS_LSM303D_REG_TEMP_OUT_L (0x05) 28 | #define MGOS_LSM303D_REG_TEMP_OUT_H (0x06) 29 | #define MGOS_LSM303D_REG_STATUS_M (0x07) 30 | #define MGOS_LSM303D_REG_OUT_X_L_M (0x08) 31 | #define MGOS_LSM303D_REG_OUT_X_H_M (0x09) 32 | #define MGOS_LSM303D_REG_OUT_Y_L_M (0x0A) 33 | #define MGOS_LSM303D_REG_OUT_Y_H_M (0x0B) 34 | #define MGOS_LSM303D_REG_OUT_Z_L_M (0x0C) 35 | #define MGOS_LSM303D_REG_OUT_Z_H_M (0x0D) 36 | #define MGOS_LSM303D_REG_WHO_AM_I (0x0F) 37 | #define MGOS_LSM303D_REG_INT_CTRL_M (0x12) 38 | #define MGOS_LSM303D_REG_INT_SRC_M (0x13) 39 | #define MGOS_LSM303D_REG_INT_THS_L_M (0x14) 40 | #define MGOS_LSM303D_REG_INT_THS_H_M (0x15) 41 | #define MGOS_LSM303D_REG_OFFSET_X_L_M (0x16) 42 | #define MGOS_LSM303D_REG_OFFSET_X_H_M (0x17) 43 | #define MGOS_LSM303D_REG_OFFSET_Y_L_M (0x18) 44 | #define MGOS_LSM303D_REG_OFFSET_Y_H_M (0x19) 45 | #define MGOS_LSM303D_REG_OFFSET_Z_L_M (0x1A) 46 | #define MGOS_LSM303D_REG_OFFSET_Z_H_M (0x1B) 47 | #define MGOS_LSM303D_REG_REFERENCE_X (0x1C) 48 | #define MGOS_LSM303D_REG_REFERENCE_Y (0x1D) 49 | #define MGOS_LSM303D_REG_REFERENCE_Z (0x1E) 50 | #define MGOS_LSM303D_REG_CTRL0 (0x1F) 51 | #define MGOS_LSM303D_REG_CTRL1 (0x20) 52 | #define MGOS_LSM303D_REG_CTRL2 (0x21) 53 | #define MGOS_LSM303D_REG_CTRL3 (0x22) 54 | #define MGOS_LSM303D_REG_CTRL4 (0x23) 55 | #define MGOS_LSM303D_REG_CTRL5 (0x24) 56 | #define MGOS_LSM303D_REG_CTRL6 (0x25) 57 | #define MGOS_LSM303D_REG_CTRL7 (0x26) 58 | #define MGOS_LSM303D_REG_STATUS_A (0x27) 59 | #define MGOS_LSM303D_REG_OUT_X_L_A (0x28) 60 | #define MGOS_LSM303D_REG_OUT_X_H_A (0x29) 61 | #define MGOS_LSM303D_REG_OUT_Y_L_A (0x2A) 62 | #define MGOS_LSM303D_REG_OUT_Y_H_A (0x2B) 63 | #define MGOS_LSM303D_REG_OUT_Z_L_A (0x2C) 64 | #define MGOS_LSM303D_REG_OUT_Z_H_A (0x2D) 65 | #define MGOS_LSM303D_REG_FIFO_CTRL (0x2E) 66 | #define MGOS_LSM303D_REG_FIFO_SRC (0x2F) 67 | #define MGOS_LSM303D_REG_IG_CFG1 (0x30) 68 | #define MGOS_LSM303D_REG_IG_SRC1 (0x31) 69 | #define MGOS_LSM303D_REG_IG_THS1 (0x32) 70 | #define MGOS_LSM303D_REG_IG_DUR1 (0x33) 71 | #define MGOS_LSM303D_REG_IG_CFG2 (0x34) 72 | #define MGOS_LSM303D_REG_IG_SRC2 (0x35) 73 | #define MGOS_LSM303D_REG_IG_THS2 (0x36) 74 | #define MGOS_LSM303D_REG_IG_DUR2 (0x37) 75 | #define MGOS_LSM303D_REG_CLICK_CFG (0x38) 76 | #define MGOS_LSM303D_REG_CLICK_SRC (0x39) 77 | #define MGOS_LSM303D_REG_CLICK_THS (0x3A) 78 | #define MGOS_LSM303D_REG_TIME_LIMIT (0x3B) 79 | #define MGOS_LSM303D_REG_TIME_LATENCY (0x3C) 80 | #define MGOS_LSM303D_REG_TIME_WINDOW (0x3D) 81 | #define MGOS_LSM303D_REG_ACT_THS (0x3E) 82 | #define MGOS_LSM303D_REG_ACT_DUR (0x3F) 83 | 84 | struct mgos_imu_lsm303d_userdata { 85 | bool initialized; 86 | }; 87 | 88 | struct mgos_imu_lsm303d_userdata *mgos_imu_lsm303d_userdata_create(void); 89 | 90 | bool mgos_imu_lsm303d_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 91 | bool mgos_imu_lsm303d_acc_create(struct mgos_imu_acc *dev, void *imu_user_data); 92 | bool mgos_imu_lsm303d_acc_read(struct mgos_imu_acc *dev, void *imu_user_data); 93 | 94 | bool mgos_imu_lsm303d_mag_detect(struct mgos_imu_mag *dev, void *imu_user_data); 95 | bool mgos_imu_lsm303d_mag_create(struct mgos_imu_mag *dev, void *imu_user_data); 96 | bool mgos_imu_lsm303d_mag_read(struct mgos_imu_mag *dev, void *imu_user_data); 97 | -------------------------------------------------------------------------------- /src/mgos_imu_mpu60x0.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | // MPU6000 allows for both I2C and SPI, while MPU6050 has only I2C. 23 | #define MGOS_MPU60X0_DEFAULT_I2CADDR (0x68) 24 | #define MGOS_MPU60X0_DEVID (0x68) 25 | 26 | #define MGOS_MPU60X0_REG_SELF_TEST_X (0x0D) 27 | #define MGOS_MPU60X0_REG_SELF_TEST_Y (0x0E) 28 | #define MGOS_MPU60X0_REG_SELF_TEST_Z (0x0F) 29 | #define MGOS_MPU60X0_REG_SELF_TEST_A (0x10) 30 | #define MGOS_MPU60X0_REG_SMPLRT_DIV (0x19) 31 | #define MGOS_MPU60X0_REG_CONFIG (0x1A) 32 | #define MGOS_MPU60X0_REG_GYRO_CONFIG (0x1B) 33 | #define MGOS_MPU60X0_REG_ACCEL_CONFIG (0x1C) 34 | #define MGOS_MPU60X0_REG_FIFO_EN (0x23) 35 | #define MGOS_MPU60X0_REG_I2C_MST_CTRL (0x24) 36 | #define MGOS_MPU60X0_REG_I2C_SLV0_ADDR (0x25) 37 | #define MGOS_MPU60X0_REG_I2C_SLV0_REG (0x26) 38 | #define MGOS_MPU60X0_REG_I2C_SLV0_CTRL (0x27) 39 | #define MGOS_MPU60X0_REG_I2C_SLV1_ADDR (0x28) 40 | #define MGOS_MPU60X0_REG_I2C_SLV1_REG (0x29) 41 | #define MGOS_MPU60X0_REG_I2C_SLV1_CTRL (0x2A) 42 | #define MGOS_MPU60X0_REG_I2C_SLV2_ADDR (0x2B) 43 | #define MGOS_MPU60X0_REG_I2C_SLV2_REG (0x2C) 44 | #define MGOS_MPU60X0_REG_I2C_SLV2_CTRL (0x2D) 45 | #define MGOS_MPU60X0_REG_I2C_SLV3_ADDR (0x2E) 46 | #define MGOS_MPU60X0_REG_I2C_SLV3_REG (0x2F) 47 | #define MGOS_MPU60X0_REG_I2C_SLV3_CTRL (0x30) 48 | #define MGOS_MPU60X0_REG_I2C_SLV4_ADDR (0x31) 49 | #define MGOS_MPU60X0_REG_I2C_SLV4_REG (0x32) 50 | #define MGOS_MPU60X0_REG_I2C_SLV4_DO (0x33) 51 | #define MGOS_MPU60X0_REG_I2C_SLV4_CTRL (0x34) 52 | #define MGOS_MPU60X0_REG_I2C_SLV4_DI (0x35) 53 | #define MGOS_MPU60X0_REG_I2C_MST_STATUS (0x36) 54 | #define MGOS_MPU60X0_REG_INT_PIN_CFG (0x37) 55 | #define MGOS_MPU60X0_REG_INT_ENABLE (0x38) 56 | #define MGOS_MPU60X0_REG_INT_STATUS (0x3A) 57 | #define MGOS_MPU60X0_REG_ACCEL_XOUT_H (0x3B) 58 | #define MGOS_MPU60X0_REG_ACCEL_XOUT_L (0x3C) 59 | #define MGOS_MPU60X0_REG_ACCEL_YOUT_H (0x3D) 60 | #define MGOS_MPU60X0_REG_ACCEL_YOUT_L (0x3E) 61 | #define MGOS_MPU60X0_REG_ACCEL_ZOUT_H (0x3F) 62 | #define MGOS_MPU60X0_REG_ACCEL_ZOUT_L (0x40) 63 | #define MGOS_MPU60X0_REG_TEMP_OUT_H (0x41) 64 | #define MGOS_MPU60X0_REG_TEMP_OUT_L (0x42) 65 | #define MGOS_MPU60X0_REG_GYRO_XOUT_H (0x43) 66 | #define MGOS_MPU60X0_REG_GYRO_XOUT_L (0x44) 67 | #define MGOS_MPU60X0_REG_GYRO_YOUT_H (0x45) 68 | #define MGOS_MPU60X0_REG_GYRO_YOUT_L (0x46) 69 | #define MGOS_MPU60X0_REG_GYRO_ZOUT_H (0x47) 70 | #define MGOS_MPU60X0_REG_GYRO_ZOUT_L (0x48) 71 | #define MGOS_MPU60X0_REG_I2C_MST_DELAY_CT (0x67) 72 | #define MGOS_MPU60X0_REG_SIGNAL_PATH_RES (0x68) 73 | #define MGOS_MPU60X0_REG_USER_CTRL (0x6A) 74 | #define MGOS_MPU60X0_REG_PWR_MGMT_1 (0x6B) 75 | #define MGOS_MPU60X0_REG_PWR_MGMT_2 (0x6C) 76 | #define MGOS_MPU60X0_REG_FIFO_COUNTH (0x72) 77 | #define MGOS_MPU60X0_REG_FIFO_COUNTL (0x73) 78 | #define MGOS_MPU60X0_REG_FIFO_R_W (0x74) 79 | #define MGOS_MPU60X0_REG_WHO_AM_I (0x75) 80 | 81 | struct mgos_imu_mpu60x0_userdata { 82 | bool initialized; 83 | }; 84 | 85 | struct mgos_imu_mpu60x0_userdata *mgos_imu_mpu60x0_userdata_create(void); 86 | 87 | bool mgos_imu_mpu60x0_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 88 | bool mgos_imu_mpu60x0_acc_create(struct mgos_imu_acc *dev, void *imu_user_data); 89 | bool mgos_imu_mpu60x0_acc_read(struct mgos_imu_acc *dev, void *imu_user_data); 90 | bool mgos_imu_mpu60x0_acc_get_scale(struct mgos_imu_acc *dev, 91 | void *imu_user_data, float *scale); 92 | bool mgos_imu_mpu60x0_acc_set_scale(struct mgos_imu_acc *dev, 93 | void *imu_user_data, float scale); 94 | 95 | bool mgos_imu_mpu60x0_gyro_detect(struct mgos_imu_gyro *dev, 96 | void *imu_user_data); 97 | bool mgos_imu_mpu60x0_gyro_create(struct mgos_imu_gyro *dev, 98 | void *imu_user_data); 99 | bool mgos_imu_mpu60x0_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data); 100 | bool mgos_imu_mpu60x0_gyro_get_scale(struct mgos_imu_gyro *dev, 101 | void *imu_user_data, float *scale); 102 | bool mgos_imu_mpu60x0_gyro_set_scale(struct mgos_imu_gyro *dev, 103 | void *imu_user_data, float scale); 104 | -------------------------------------------------------------------------------- /src/mgos_imu_internal.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu.h" 21 | 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | 26 | struct mgos_imu_mag; 27 | struct mgos_imu_acc; 28 | struct mgos_imu_gyro; 29 | 30 | struct mgos_imu { 31 | struct mgos_imu_mag * mag; 32 | struct mgos_imu_acc * acc; 33 | struct mgos_imu_gyro *gyro; 34 | void * user_data; 35 | }; 36 | 37 | // Magnetometer 38 | typedef bool (*mgos_imu_mag_detect_fn)(struct mgos_imu_mag *dev, void *imu_user_data); 39 | typedef bool (*mgos_imu_mag_create_fn)(struct mgos_imu_mag *dev, void *imu_user_data); 40 | typedef bool (*mgos_imu_mag_destroy_fn)(struct mgos_imu_mag *dev, void *imu_user_data); 41 | typedef bool (*mgos_imu_mag_read_fn)(struct mgos_imu_mag *dev, void *imu_user_data); 42 | typedef bool (*mgos_imu_mag_get_odr_fn)(struct mgos_imu_mag *dev, void *imu_user_data, float *odr); 43 | typedef bool (*mgos_imu_mag_set_odr_fn)(struct mgos_imu_mag *dev, void *imu_user_data, float odr); 44 | typedef bool (*mgos_imu_mag_get_scale_fn)(struct mgos_imu_mag *dev, void *imu_user_data, float *scale); 45 | typedef bool (*mgos_imu_mag_set_scale_fn)(struct mgos_imu_mag *dev, void *imu_user_data, float scale); 46 | 47 | struct mgos_imu_mag { 48 | mgos_imu_mag_detect_fn detect; 49 | mgos_imu_mag_create_fn create; 50 | mgos_imu_mag_destroy_fn destroy; 51 | mgos_imu_mag_read_fn read; 52 | mgos_imu_mag_get_odr_fn get_odr; 53 | mgos_imu_mag_set_odr_fn set_odr; 54 | mgos_imu_mag_get_scale_fn get_scale; 55 | mgos_imu_mag_set_scale_fn set_scale; 56 | 57 | struct mgos_i2c * i2c; 58 | uint8_t i2caddr; 59 | struct mgos_imu_mag_opts opts; 60 | 61 | void * user_data; 62 | 63 | float scale; 64 | float bias[3]; 65 | float orientation[9]; 66 | int16_t mx, my, mz; 67 | }; 68 | 69 | // Accelerometer 70 | typedef bool (*mgos_imu_acc_detect_fn)(struct mgos_imu_acc *dev, void *imu_user_data); 71 | typedef bool (*mgos_imu_acc_create_fn)(struct mgos_imu_acc *dev, void *imu_user_data); 72 | typedef bool (*mgos_imu_acc_destroy_fn)(struct mgos_imu_acc *dev, void *imu_user_data); 73 | typedef bool (*mgos_imu_acc_read_fn)(struct mgos_imu_acc *dev, void *imu_user_data); 74 | typedef bool (*mgos_imu_acc_get_odr_fn)(struct mgos_imu_acc *dev, void *imu_user_data, float *odr); 75 | typedef bool (*mgos_imu_acc_set_odr_fn)(struct mgos_imu_acc *dev, void *imu_user_data, float odr); 76 | typedef bool (*mgos_imu_acc_get_scale_fn)(struct mgos_imu_acc *dev, void *imu_user_data, float *scale); 77 | typedef bool (*mgos_imu_acc_set_scale_fn)(struct mgos_imu_acc *dev, void *imu_user_data, float scale); 78 | 79 | struct mgos_imu_acc { 80 | mgos_imu_acc_detect_fn detect; 81 | mgos_imu_acc_create_fn create; 82 | mgos_imu_acc_destroy_fn destroy; 83 | mgos_imu_acc_read_fn read; 84 | mgos_imu_acc_get_odr_fn get_odr; 85 | mgos_imu_acc_set_odr_fn set_odr; 86 | mgos_imu_acc_get_scale_fn get_scale; 87 | mgos_imu_acc_set_scale_fn set_scale; 88 | 89 | struct mgos_i2c * i2c; 90 | uint8_t i2caddr; 91 | struct mgos_imu_acc_opts opts; 92 | 93 | void * user_data; 94 | 95 | float scale; 96 | float offset_ax, offset_ay, offset_az; 97 | int16_t ax, ay, az; 98 | }; 99 | 100 | // Gyroscope 101 | typedef bool (*mgos_imu_gyro_detect_fn)(struct mgos_imu_gyro *dev, void *imu_user_data); 102 | typedef bool (*mgos_imu_gyro_create_fn)(struct mgos_imu_gyro *dev, void *imu_user_data); 103 | typedef bool (*mgos_imu_gyro_destroy_fn)(struct mgos_imu_gyro *dev, void *imu_user_data); 104 | typedef bool (*mgos_imu_gyro_read_fn)(struct mgos_imu_gyro *dev, void *imu_user_data); 105 | typedef bool (*mgos_imu_gyro_get_odr_fn)(struct mgos_imu_gyro *dev, void *imu_user_data, float *odr); 106 | typedef bool (*mgos_imu_gyro_set_odr_fn)(struct mgos_imu_gyro *dev, void *imu_user_data, float odr); 107 | typedef bool (*mgos_imu_gyro_get_scale_fn)(struct mgos_imu_gyro *dev, void *imu_user_data, float *scale); 108 | typedef bool (*mgos_imu_gyro_set_scale_fn)(struct mgos_imu_gyro *dev, void *imu_user_data, float scale); 109 | 110 | struct mgos_imu_gyro { 111 | mgos_imu_gyro_detect_fn detect; 112 | mgos_imu_gyro_create_fn create; 113 | mgos_imu_gyro_destroy_fn destroy; 114 | mgos_imu_gyro_read_fn read; 115 | mgos_imu_gyro_get_odr_fn get_odr; 116 | mgos_imu_gyro_set_odr_fn set_odr; 117 | mgos_imu_gyro_get_scale_fn get_scale; 118 | mgos_imu_gyro_set_scale_fn set_scale; 119 | 120 | struct mgos_i2c * i2c; 121 | uint8_t i2caddr; 122 | struct mgos_imu_gyro_opts opts; 123 | 124 | void * user_data; 125 | 126 | float scale; 127 | float offset_gx, offset_gy, offset_gz; 128 | float orientation[9]; 129 | int16_t gx, gy, gz; 130 | }; 131 | 132 | #ifdef __cplusplus 133 | } 134 | #endif 135 | -------------------------------------------------------------------------------- /src/mgos_imu_lsm303d.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_lsm303d.h" 20 | 21 | static bool mgos_imu_lsm303d_detect(struct mgos_i2c *i2c, uint8_t i2caddr, uint8_t *devid) { 22 | int device_id; 23 | 24 | if (!i2c || !devid) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_LSM303D_REG_WHO_AM_I); 29 | switch (device_id) { 30 | case MGOS_LSM303D_DEVID: 31 | *devid = MGOS_LSM303D_DEVID; 32 | return true; 33 | 34 | case MGOS_LSM303DLM_DEVID: 35 | *devid = MGOS_LSM303DLM_DEVID; 36 | return true; 37 | 38 | default: 39 | break; 40 | } 41 | return false; 42 | } 43 | 44 | static bool mgos_imu_lsm303d_create(struct mgos_i2c *i2c, uint8_t i2caddr) { 45 | if (!i2c) { 46 | return false; 47 | } 48 | 49 | // Reset 50 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_LSM303D_REG_CTRL0, 0x80); 51 | mgos_usleep(5000); 52 | 53 | // Enable 54 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_LSM303D_REG_CTRL0, 0x00); 55 | 56 | return true; 57 | } 58 | 59 | bool mgos_imu_lsm303d_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data) { 60 | uint8_t devid; 61 | 62 | if (mgos_imu_lsm303d_detect(dev->i2c, dev->i2caddr, &devid)) { 63 | if (devid == MGOS_LSM303D_DEVID) { 64 | dev->opts.type = ACC_LSM303D; 65 | } else{ 66 | dev->opts.type = ACC_LSM303DLM; 67 | } 68 | return true; 69 | } 70 | return false; 71 | 72 | (void)imu_user_data; 73 | } 74 | 75 | bool mgos_imu_lsm303d_acc_create(struct mgos_imu_acc *dev, void *imu_user_data) { 76 | struct mgos_imu_lsm303d_userdata *iud = (struct mgos_imu_lsm303d_userdata *)imu_user_data; 77 | 78 | if (!dev) { 79 | return false; 80 | } 81 | 82 | // Only initialize the LSM303D if mag hasn't done so yet 83 | if (!iud->initialized) { 84 | if (!mgos_imu_lsm303d_create(dev->i2c, dev->i2caddr)) { 85 | return false; 86 | } 87 | iud->initialized = true; 88 | } 89 | 90 | // Accel register settings: 91 | // CTRL1: AODR=0110 (100 Hz ODR); BDU=1; A*EN=1 (enable all axes) 92 | // CTRL2: ABW=00 (773Hz); AFS=011 (8G); 0; AST=0; SIM=0 93 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_CTRL1, 0x6F); 94 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_CTRL2, 0x18); 95 | 96 | dev->scale = 8.f / 32767.5; 97 | return true; 98 | } 99 | 100 | bool mgos_imu_lsm303d_acc_read(struct mgos_imu_acc *dev, void *imu_user_data) { 101 | uint8_t data[6]; 102 | 103 | if (!dev) { 104 | return false; 105 | } 106 | // Low register is single-read, high register is streaming read. 107 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_OUT_X_L_A | 0x80, 6, data)) { 108 | return false; 109 | } 110 | dev->ax = (data[1] << 8) | (data[0]); 111 | dev->ay = (data[3] << 8) | (data[2]); 112 | dev->az = (data[5] << 8) | (data[4]); 113 | 114 | return true; 115 | 116 | (void)imu_user_data; 117 | } 118 | 119 | bool mgos_imu_lsm303d_mag_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 120 | uint8_t devid; 121 | 122 | if (mgos_imu_lsm303d_detect(dev->i2c, dev->i2caddr, &devid)) { 123 | if (devid == MGOS_LSM303D_DEVID) { 124 | dev->opts.type = MAG_LSM303D; 125 | } else{ 126 | dev->opts.type = MAG_LSM303DLM; 127 | } 128 | return true; 129 | } 130 | return false; 131 | 132 | (void)imu_user_data; 133 | } 134 | 135 | bool mgos_imu_lsm303d_mag_create(struct mgos_imu_mag *dev, void *imu_user_data) { 136 | struct mgos_imu_lsm303d_userdata *iud = (struct mgos_imu_lsm303d_userdata *)imu_user_data; 137 | 138 | if (!dev) { 139 | return false; 140 | } 141 | 142 | // Only initialize the LSM303D if acc hasn't done so yet 143 | if (!iud->initialized) { 144 | if (!mgos_imu_lsm303d_create(dev->i2c, dev->i2caddr)) { 145 | return false; 146 | } 147 | iud->initialized = true; 148 | } 149 | 150 | // Mag settings 151 | // CTRL5: TEMP_EN=0; M_RES=11 (hires); M_ODR=101 (100Hz); LIR2=0; LIR1=0 152 | // CTRL6: 0; MFS=11 (12gauss); 00000 153 | // CTRL7: AHPM=00; AFDS=0; T_ONLY=0; 0; MLP=0; MD=01 (singleshot) 154 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_CTRL5, 0xf4); 155 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_CTRL6, 0x60); 156 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_CTRL7, 0x00); 157 | 158 | dev->scale = 12.f / 32767.5; 159 | dev->bias[0] = 1.0f; 160 | dev->bias[1] = 1.0f; 161 | dev->bias[2] = 1.0f; 162 | 163 | return true; 164 | } 165 | 166 | bool mgos_imu_lsm303d_mag_read(struct mgos_imu_mag *dev, void *imu_user_data) { 167 | uint8_t data[6]; 168 | 169 | if (!dev) { 170 | return false; 171 | } 172 | 173 | // Low register is single-read, high register is streaming read. 174 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_LSM303D_REG_OUT_X_L_M | 0x80, 6, data)) { 175 | return false; 176 | } 177 | dev->mx = (data[1] << 8) | (data[0]); 178 | dev->my = (data[3] << 8) | (data[2]); 179 | dev->mz = (data[5] << 8) | (data[4]); 180 | 181 | return true; 182 | 183 | (void)imu_user_data; 184 | } 185 | 186 | struct mgos_imu_lsm303d_userdata *mgos_imu_lsm303d_userdata_create(void) { 187 | struct mgos_imu_lsm303d_userdata *iud; 188 | 189 | iud = calloc(1, sizeof(struct mgos_imu_lsm303d_userdata)); 190 | if (!iud) { 191 | return NULL; 192 | } 193 | iud->initialized = false; 194 | return iud; 195 | } 196 | -------------------------------------------------------------------------------- /src/mgos_imu_lsm9ds1.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_LSM9DS1_DEFAULT_I2CADDR (0x6b) 23 | #define MGOS_LSM9DS1_DEFAULT_M_I2CADDR (0x1e) 24 | #define MGOS_LSM9DS1_DEVID (0x68) 25 | #define MGOS_LSM9DS1_DEVID_M (0x3d) 26 | 27 | // LSM9DS1 -- Registers (Acc/Gyro) 28 | #define MGOS_LSM9DS1_REG_ACT_THS (0x04) 29 | #define MGOS_LSM9DS1_REG_ACT_DUR (0x05) 30 | #define MGOS_LSM9DS1_REG_INT_GEN_CFG_XL (0x06) 31 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_X_XL (0x07) 32 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_Y_XL (0x08) 33 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_Z_XL (0x09) 34 | #define MGOS_LSM9DS1_REG_INT_GEN_DUR_XL (0x0A) 35 | #define MGOS_LSM9DS1_REG_REFERENCE_G (0x0B) 36 | #define MGOS_LSM9DS1_REG_INT1_CTRL (0x0C) 37 | #define MGOS_LSM9DS1_REG_INT2_CTRL (0x0D) 38 | #define MGOS_LSM9DS1_REG_WHO_AM_I (0x0F) 39 | #define MGOS_LSM9DS1_REG_CTRL_REG1_G (0x10) 40 | #define MGOS_LSM9DS1_REG_CTRL_REG2_G (0x11) 41 | #define MGOS_LSM9DS1_REG_CTRL_REG3_G (0x12) 42 | #define MGOS_LSM9DS1_REG_ORIENT_CFG_G (0x13) 43 | #define MGOS_LSM9DS1_REG_INT_GEN_SRC_G (0x14) 44 | #define MGOS_LSM9DS1_REG_OUT_TEMP_L (0x15) 45 | #define MGOS_LSM9DS1_REG_OUT_TEMP_H (0x16) 46 | #define MGOS_LSM9DS1_REG_STATUS1_REG (0x17) 47 | #define MGOS_LSM9DS1_REG_OUT_X_L_G (0x18) 48 | #define MGOS_LSM9DS1_REG_OUT_X_H_G (0x19) 49 | #define MGOS_LSM9DS1_REG_OUT_Y_L_G (0x1A) 50 | #define MGOS_LSM9DS1_REG_OUT_Y_H_G (0x1B) 51 | #define MGOS_LSM9DS1_REG_OUT_Z_L_G (0x1C) 52 | #define MGOS_LSM9DS1_REG_OUT_Z_H_G (0x1D) 53 | #define MGOS_LSM9DS1_REG_CTRL_REG4 (0x1E) 54 | #define MGOS_LSM9DS1_REG_CTRL_REG5_XL (0x1F) 55 | #define MGOS_LSM9DS1_REG_CTRL_REG6_XL (0x20) 56 | #define MGOS_LSM9DS1_REG_CTRL_REG7_XL (0x21) 57 | #define MGOS_LSM9DS1_REG_CTRL_REG8 (0x22) 58 | #define MGOS_LSM9DS1_REG_CTRL_REG9 (0x23) 59 | #define MGOS_LSM9DS1_REG_CTRL_REG10 (0x24) 60 | #define MGOS_LSM9DS1_REG_INT_GEN_SRC_XL (0x26) 61 | #define MGOS_LSM9DS1_REG_STATUS2_REG (0x27) 62 | #define MGOS_LSM9DS1_REG_OUT_X_L_XL (0x28) 63 | #define MGOS_LSM9DS1_REG_OUT_X_H_XL (0x29) 64 | #define MGOS_LSM9DS1_REG_OUT_Y_L_XL (0x2A) 65 | #define MGOS_LSM9DS1_REG_OUT_Y_H_XL (0x2B) 66 | #define MGOS_LSM9DS1_REG_OUT_Z_L_XL (0x2C) 67 | #define MGOS_LSM9DS1_REG_OUT_Z_H_XL (0x2D) 68 | #define MGOS_LSM9DS1_REG_FIFO_CTRL (0x2E) 69 | #define MGOS_LSM9DS1_REG_FIFO_SRC (0x2F) 70 | #define MGOS_LSM9DS1_REG_INT_GEN_CFG_G (0x30) 71 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_XH_G (0x31) 72 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_XL_G (0x32) 73 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_YH_G (0x33) 74 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_YL_G (0x34) 75 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_ZH_G (0x35) 76 | #define MGOS_LSM9DS1_REG_INT_GEN_THS_ZL_G (0x36) 77 | #define MGOS_LSM9DS1_REG_INT_GEN_DUR_G (0x37) 78 | 79 | // LSM9DS1 -- Registers (Mag) 80 | #define MGOS_LSM9DS1_REG_OFFSET_X_REG_L_M (0x05) 81 | #define MGOS_LSM9DS1_REG_OFFSET_X_REG_H_M (0x06) 82 | #define MGOS_LSM9DS1_REG_OFFSET_Y_REG_L_M (0x07) 83 | #define MGOS_LSM9DS1_REG_OFFSET_Y_REG_H_M (0x08) 84 | #define MGOS_LSM9DS1_REG_OFFSET_Z_REG_L_M (0x09) 85 | #define MGOS_LSM9DS1_REG_OFFSET_Z_REG_H_M (0x0A) 86 | #define MGOS_LSM9DS1_REG_WHO_AM_I_M (0x0F) 87 | #define MGOS_LSM9DS1_REG_CTRL_REG1_M (0x20) 88 | #define MGOS_LSM9DS1_REG_CTRL_REG2_M (0x21) 89 | #define MGOS_LSM9DS1_REG_CTRL_REG3_M (0x22) 90 | #define MGOS_LSM9DS1_REG_CTRL_REG4_M (0x23) 91 | #define MGOS_LSM9DS1_REG_CTRL_REG5_M (0x24) 92 | #define MGOS_LSM9DS1_REG_STATUS_REG_M (0x27) 93 | #define MGOS_LSM9DS1_REG_OUT_X_L_M (0x28) 94 | #define MGOS_LSM9DS1_REG_OUT_X_H_M (0x29) 95 | #define MGOS_LSM9DS1_REG_OUT_Y_L_M (0x2A) 96 | #define MGOS_LSM9DS1_REG_OUT_Y_H_M (0x2B) 97 | #define MGOS_LSM9DS1_REG_OUT_Z_L_M (0x2C) 98 | #define MGOS_LSM9DS1_REG_OUT_Z_H_M (0x2D) 99 | #define MGOS_LSM9DS1_REG_INT_CFG_M (0x30) 100 | #define MGOS_LSM9DS1_REG_INT_SRC_M (0x31) 101 | #define MGOS_LSM9DS1_REG_INT_THS_L_M (0x32) 102 | #define MGOS_LSM9DS1_REG_INT_THS_H_M (0x33) 103 | 104 | struct mgos_imu_lsm9ds1_userdata { 105 | bool accgyro_initialized; 106 | }; 107 | 108 | struct mgos_imu_lsm9ds1_userdata *mgos_imu_lsm9ds1_userdata_create(void); 109 | 110 | bool mgos_imu_lsm9ds1_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 111 | bool mgos_imu_lsm9ds1_acc_create(struct mgos_imu_acc *dev, void *imu_user_data); 112 | bool mgos_imu_lsm9ds1_acc_read(struct mgos_imu_acc *dev, void *imu_user_data); 113 | 114 | bool mgos_imu_lsm9ds1_gyro_detect(struct mgos_imu_gyro *dev, void *imu_user_data); 115 | bool mgos_imu_lsm9ds1_gyro_create(struct mgos_imu_gyro *dev, void *imu_user_data); 116 | bool mgos_imu_lsm9ds1_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data); 117 | 118 | bool mgos_imu_lsm9ds1_mag_detect(struct mgos_imu_mag *dev, void *imu_user_data); 119 | bool mgos_imu_lsm9ds1_mag_create(struct mgos_imu_mag *dev, void *imu_user_data); 120 | bool mgos_imu_lsm9ds1_mag_read(struct mgos_imu_mag *dev, void *imu_user_data); 121 | -------------------------------------------------------------------------------- /src/mgos_imu_mpu60x0.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos_imu_mpu60x0.h" 18 | #include "mgos.h" 19 | #include "mgos_i2c.h" 20 | 21 | static bool mgos_imu_mpu60x0_detect(struct mgos_i2c *i2c, uint8_t i2caddr) { 22 | if (!i2c) { 23 | return false; 24 | } 25 | 26 | return MGOS_MPU60X0_DEVID == 27 | mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_MPU60X0_REG_WHO_AM_I); 28 | } 29 | 30 | static bool mgos_imu_mpu60x0_create(struct mgos_i2c *i2c, uint8_t i2caddr) { 31 | if (!i2c) { 32 | return false; 33 | } 34 | 35 | // Reset 36 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU60X0_REG_PWR_MGMT_1, 0x80); 37 | mgos_usleep(80000); 38 | 39 | // Enable IMU sensors 40 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU60X0_REG_PWR_MGMT_2, 0x00); 41 | 42 | // Set DPLF: -- EXT_SYNC_SET=000 DLPF_CFG=010 (94Hz accel, 98Hz gyro, Fs=1KHz) 43 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU60X0_REG_CONFIG, 0x02); 44 | 45 | // Exit sleep mode 46 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU60X0_REG_PWR_MGMT_1, 0x00); 47 | 48 | return true; 49 | } 50 | 51 | bool mgos_imu_mpu60x0_acc_detect(struct mgos_imu_acc *dev, 52 | void *imu_user_data) { 53 | return mgos_imu_mpu60x0_detect(dev->i2c, dev->i2caddr); 54 | 55 | (void)imu_user_data; 56 | } 57 | 58 | bool mgos_imu_mpu60x0_acc_create(struct mgos_imu_acc *dev, 59 | void *imu_user_data) { 60 | struct mgos_imu_mpu60x0_userdata *iud = 61 | (struct mgos_imu_mpu60x0_userdata *)imu_user_data; 62 | 63 | if (!dev) { 64 | return false; 65 | } 66 | 67 | // Only initialize the MPU60X0 if gyro hasn't done so yet 68 | if (!iud->initialized) { 69 | if (!mgos_imu_mpu60x0_create(dev->i2c, dev->i2caddr)) { 70 | return false; 71 | } 72 | iud->initialized = true; 73 | } 74 | // Accel Config: XA_ST=0 YG_AT=0 ZA_ST=0 FS_SEL=10 (8G) --- 75 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_ACCEL_CONFIG, 0x10); 76 | 77 | return true; 78 | } 79 | 80 | bool mgos_imu_mpu60x0_acc_read(struct mgos_imu_acc *dev, void *imu_user_data) { 81 | uint8_t data[6]; 82 | 83 | if (!dev) { 84 | return false; 85 | } 86 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_ACCEL_XOUT_H, 6, data)) { 87 | return false; 88 | } 89 | dev->ax = (data[0] << 8) | (data[1]); 90 | dev->ay = (data[2] << 8) | (data[3]); 91 | dev->az = (data[4] << 8) | (data[5]); 92 | 93 | return true; 94 | 95 | (void)imu_user_data; 96 | } 97 | 98 | bool mgos_imu_mpu60x0_gyro_detect(struct mgos_imu_gyro *dev, 99 | void *imu_user_data) { 100 | return mgos_imu_mpu60x0_detect(dev->i2c, dev->i2caddr); 101 | 102 | (void)imu_user_data; 103 | } 104 | 105 | bool mgos_imu_mpu60x0_gyro_create(struct mgos_imu_gyro *dev, 106 | void *imu_user_data) { 107 | struct mgos_imu_mpu60x0_userdata *iud = 108 | (struct mgos_imu_mpu60x0_userdata *)imu_user_data; 109 | 110 | if (!dev) { 111 | return false; 112 | } 113 | 114 | // Only initialize the MPU60X0 if acc hasn't done so yet 115 | if (!iud->initialized) { 116 | if (!mgos_imu_mpu60x0_create(dev->i2c, dev->i2caddr)) { 117 | return false; 118 | } 119 | iud->initialized = true; 120 | } 121 | // Gyro Config: XG_ST=0 YG_ST=0 ZG_ST=0 FS_SEL=11 (2000 dps) --- 122 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_GYRO_CONFIG, 0x18); 123 | 124 | return true; 125 | } 126 | 127 | bool mgos_imu_mpu60x0_gyro_read(struct mgos_imu_gyro *dev, 128 | void *imu_user_data) { 129 | uint8_t data[6]; 130 | 131 | if (!dev) { 132 | return false; 133 | } 134 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_GYRO_XOUT_H, 6, data)) { 135 | return false; 136 | } 137 | dev->gx = (data[0] << 8) | (data[1]); 138 | dev->gy = (data[2] << 8) | (data[3]); 139 | dev->gz = (data[4] << 8) | (data[5]); 140 | 141 | return true; 142 | 143 | (void)imu_user_data; 144 | } 145 | 146 | struct mgos_imu_mpu60x0_userdata *mgos_imu_mpu60x0_userdata_create(void) { 147 | struct mgos_imu_mpu60x0_userdata *iud; 148 | 149 | iud = calloc(1, sizeof(struct mgos_imu_mpu60x0_userdata)); 150 | if (!iud) { 151 | return NULL; 152 | } 153 | iud->initialized = false; 154 | return iud; 155 | } 156 | 157 | bool mgos_imu_mpu60x0_acc_get_scale(struct mgos_imu_acc *dev, 158 | void *imu_user_data, float *scale) { 159 | uint8_t sel; 160 | 161 | if (!mgos_i2c_getbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_ACCEL_CONFIG, 3, 2, &sel)) { 162 | return false; 163 | } 164 | switch (sel) { 165 | case 3: *scale = 16; break; 166 | 167 | case 2: *scale = 8; break; 168 | 169 | case 1: *scale = 4; break; 170 | 171 | default: *scale = 2; break; 172 | } 173 | 174 | return true; 175 | 176 | (void)imu_user_data; 177 | } 178 | 179 | bool mgos_imu_mpu60x0_acc_set_scale(struct mgos_imu_acc *dev, 180 | void *imu_user_data, float scale) { 181 | uint8_t sel; 182 | 183 | if (scale > 16) { 184 | return false; 185 | } else if (scale > 8) { 186 | sel = 3; // 16G 187 | scale = 16.f; 188 | } else if (scale > 4) { 189 | sel = 2; // 8G 190 | scale = 8.f; 191 | } else if (scale > 2) { 192 | sel = 1; // 4G 193 | scale = 4.f; 194 | } else { 195 | sel = 0; // 2G 196 | scale = 2.f; 197 | } 198 | if (!mgos_i2c_setbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_ACCEL_CONFIG, 3, 2, sel)) { 199 | return false; 200 | } 201 | dev->opts.scale = scale; 202 | dev->scale = scale / 32768.0f; 203 | 204 | return true; 205 | 206 | (void)imu_user_data; 207 | } 208 | 209 | bool mgos_imu_mpu60x0_gyro_get_scale(struct mgos_imu_gyro *dev, 210 | void *imu_user_data, float *scale) { 211 | uint8_t sel; 212 | 213 | if (!mgos_i2c_getbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_GYRO_CONFIG, 3, 2, &sel)) { 214 | return false; 215 | } 216 | switch (sel) { 217 | case 3: *scale = 2000; break; 218 | 219 | case 2: *scale = 1000; break; 220 | 221 | case 1: *scale = 500; break; 222 | 223 | default: *scale = 250; break; 224 | } 225 | 226 | return true; 227 | 228 | (void)imu_user_data; 229 | } 230 | 231 | bool mgos_imu_mpu60x0_gyro_set_scale(struct mgos_imu_gyro *dev, 232 | void *imu_user_data, float scale) { 233 | uint8_t sel; 234 | 235 | if (scale > 2000) { 236 | return false; 237 | } else if (scale > 1000) { 238 | sel = 3; // 2000DPS 239 | scale = 2000.f; 240 | } else if (scale > 500) { 241 | sel = 2; // 1000DPS 242 | scale = 1000.f; 243 | } else if (scale > 250) { 244 | sel = 1; // 500DPS 245 | scale = 500.f; 246 | } else { 247 | sel = 0; // 250DPS 248 | scale = 250.f; 249 | } 250 | if (!mgos_i2c_setbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU60X0_REG_GYRO_CONFIG, 3, 2, sel)) { 251 | return false; 252 | } 253 | dev->opts.scale = scale; 254 | dev->scale = scale / 32768.0f; 255 | 256 | return true; 257 | 258 | (void)imu_user_data; 259 | } 260 | -------------------------------------------------------------------------------- /src/mgos_imu_mpu925x.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_mpu925x.h" 20 | 21 | static bool mgos_imu_mpu925x_detect(struct mgos_i2c *i2c, uint8_t i2caddr, uint8_t *devid) { 22 | int device_id; 23 | 24 | if (!i2c || !devid) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_WHO_AM_I); 29 | switch (device_id) { 30 | case MGOS_MPU9250_DEVID_9250: 31 | LOG(LL_INFO, ("Detected MPU9250 at I2C 0x%02x", i2caddr)); 32 | *devid = MGOS_MPU9250_DEVID_9250; 33 | return true; 34 | 35 | case MGOS_MPU9250_DEVID_9255: 36 | LOG(LL_INFO, ("Detected MPU9255 at I2C 0x%02x", i2caddr)); 37 | *devid = MGOS_MPU9250_DEVID_9255; 38 | return true; 39 | } 40 | return false; 41 | } 42 | 43 | static bool mgos_imu_mpu925x_create(struct mgos_i2c *i2c, uint8_t i2caddr) { 44 | if (!i2c) { 45 | return false; 46 | } 47 | 48 | // Reset 49 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_1, 0x80); 50 | mgos_usleep(80000); 51 | 52 | // Enable IMU sensors 53 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_2, 0x00); 54 | 55 | // I2C Passthrough enable (exposes magnetometer to I2C bus) 56 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_INT_PIN_CFG, 0x02); 57 | 58 | return true; 59 | } 60 | 61 | bool mgos_imu_mpu925x_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data) { 62 | uint8_t devid; 63 | 64 | if (mgos_imu_mpu925x_detect(dev->i2c, dev->i2caddr, &devid)) { 65 | if (devid == MGOS_MPU9250_DEVID_9255) { 66 | dev->opts.type = ACC_MPU9255; 67 | } else{ 68 | dev->opts.type = ACC_MPU9250; 69 | } 70 | return true; 71 | } 72 | return false; 73 | 74 | (void)imu_user_data; 75 | } 76 | 77 | bool mgos_imu_mpu925x_acc_create(struct mgos_imu_acc *dev, void *imu_user_data) { 78 | struct mgos_imu_mpu925x_userdata *iud = (struct mgos_imu_mpu925x_userdata *)imu_user_data; 79 | 80 | if (!dev) { 81 | return false; 82 | } 83 | 84 | // Only initialize the MPU9250 if gyro hasn't done so yet 85 | if (!iud->initialized) { 86 | if (!mgos_imu_mpu925x_create(dev->i2c, dev->i2caddr)) { 87 | return false; 88 | } 89 | iud->initialized = true; 90 | } 91 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG2, MGOS_MPU9250_DLPF_41)) { 92 | return false; 93 | } 94 | return true; 95 | } 96 | 97 | bool mgos_imu_mpu925x_acc_read(struct mgos_imu_acc *dev, void *imu_user_data) { 98 | uint8_t data[6]; 99 | 100 | if (!dev) { 101 | return false; 102 | } 103 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_ACCEL_XOUT_H, 6, data)) { 104 | return false; 105 | } 106 | dev->ax = (data[0] << 8) | (data[1]); 107 | dev->ay = (data[2] << 8) | (data[3]); 108 | dev->az = (data[4] << 8) | (data[5]); 109 | return true; 110 | 111 | (void)imu_user_data; 112 | } 113 | 114 | bool mgos_imu_mpu925x_gyro_detect(struct mgos_imu_gyro *dev, void *imu_user_data) { 115 | uint8_t devid; 116 | 117 | if (mgos_imu_mpu925x_detect(dev->i2c, dev->i2caddr, &devid)) { 118 | if (devid == MGOS_MPU9250_DEVID_9255) { 119 | dev->opts.type = GYRO_MPU9255; 120 | } else{ 121 | dev->opts.type = GYRO_MPU9250; 122 | } 123 | return true; 124 | } 125 | return false; 126 | 127 | (void)imu_user_data; 128 | } 129 | 130 | bool mgos_imu_mpu925x_gyro_create(struct mgos_imu_gyro *dev, void *imu_user_data) { 131 | struct mgos_imu_mpu925x_userdata *iud = (struct mgos_imu_mpu925x_userdata *)imu_user_data; 132 | 133 | if (!dev) { 134 | return false; 135 | } 136 | 137 | // Only initialize the MPU9250 if acc hasn't done so yet 138 | if (!iud->initialized) { 139 | if (!mgos_imu_mpu925x_create(dev->i2c, dev->i2caddr)) { 140 | return false; 141 | } 142 | iud->initialized = true; 143 | } 144 | if (!mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_CONFIG, MGOS_MPU9250_DLPF_41)) { 145 | return false; 146 | } 147 | return true; 148 | } 149 | 150 | bool mgos_imu_mpu925x_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data) { 151 | uint8_t data[6]; 152 | 153 | if (!dev) { 154 | return false; 155 | } 156 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_GYRO_XOUT_H, 6, data)) { 157 | return false; 158 | } 159 | dev->gx = (data[0] << 8) | (data[1]); 160 | dev->gy = (data[2] << 8) | (data[3]); 161 | dev->gz = (data[4] << 8) | (data[5]); 162 | 163 | return true; 164 | 165 | (void)imu_user_data; 166 | } 167 | 168 | struct mgos_imu_mpu925x_userdata *mgos_imu_mpu925x_userdata_create(void) { 169 | struct mgos_imu_mpu925x_userdata *iud; 170 | 171 | iud = calloc(1, sizeof(struct mgos_imu_mpu925x_userdata)); 172 | if (!iud) { 173 | return NULL; 174 | } 175 | iud->initialized = false; 176 | return iud; 177 | } 178 | 179 | bool mgos_imu_mpu925x_acc_get_scale(struct mgos_imu_acc *dev, void *imu_user_data, float *scale) { 180 | uint8_t sel; 181 | 182 | if (!mgos_i2c_getbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG, 3, 2, &sel)) { 183 | return false; 184 | } 185 | switch (sel) { 186 | case 3: *scale = 16; break; 187 | 188 | case 2: *scale = 8; break; 189 | 190 | case 1: *scale = 4; break; 191 | 192 | default: *scale = 2; break; 193 | } 194 | 195 | return true; 196 | 197 | (void)imu_user_data; 198 | } 199 | 200 | bool mgos_imu_mpu925x_acc_set_scale(struct mgos_imu_acc *dev, void *imu_user_data, float scale) { 201 | uint8_t sel; 202 | 203 | if (scale > 16) { 204 | return false; 205 | } else if (scale > 8) { 206 | sel = 3; // 16G 207 | scale = 16.f; 208 | } else if (scale > 4) { 209 | sel = 2; // 8G 210 | scale = 8.f; 211 | } else if (scale > 2) { 212 | sel = 1; // 4G 213 | scale = 4.f; 214 | } else { 215 | sel = 0; // 2G 216 | scale = 2.f; 217 | } 218 | if (!mgos_i2c_setbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG, 3, 2, sel)) return false; 219 | dev->opts.scale = scale; 220 | dev->scale = scale / 32768.0f; 221 | return true; 222 | (void)imu_user_data; 223 | } 224 | 225 | bool mgos_imu_mpu925x_gyro_get_scale(struct mgos_imu_gyro *dev, void *imu_user_data, float *scale) { 226 | uint8_t sel; 227 | 228 | if (!mgos_i2c_getbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG, 3, 2, &sel)) { 229 | return false; 230 | } 231 | switch (sel) { 232 | case 3: *scale = 2000; break; 233 | 234 | case 2: *scale = 1000; break; 235 | 236 | case 1: *scale = 500; break; 237 | 238 | default: *scale = 250; break; 239 | } 240 | 241 | return true; 242 | 243 | (void)imu_user_data; 244 | } 245 | 246 | bool mgos_imu_mpu925x_gyro_set_scale(struct mgos_imu_gyro *dev, void *imu_user_data, float scale) { 247 | uint8_t sel; 248 | 249 | if (scale > 2000) { 250 | return false; 251 | } else if (scale > 1000) { 252 | sel = 3; // 2000DPS 253 | scale = 2000.f; 254 | } else if (scale > 500) { 255 | sel = 2; // 1000DPS 256 | scale = 1000.f; 257 | } else if (scale > 250) { 258 | sel = 1; // 500DPS 259 | scale = 500.f; 260 | } else { 261 | sel = 0; // 250DPS 262 | scale = 250.f; 263 | } 264 | if (!mgos_i2c_setbits_reg_b(dev->i2c, dev->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG, 3, 2, sel)) return false; 265 | dev->opts.scale = scale; 266 | dev->scale = scale / 32768.0f; 267 | return true; 268 | 269 | (void)imu_user_data; 270 | } 271 | -------------------------------------------------------------------------------- /src/mgos_imu_lsm9ds1.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_i2c.h" 19 | #include "mgos_imu_lsm9ds1.h" 20 | 21 | static bool mgos_imu_lsm9ds1_detect(struct mgos_i2c *i2c, uint8_t i2caddr) { 22 | int device_id; 23 | 24 | if (!i2c) { 25 | return false; 26 | } 27 | 28 | device_id = mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_LSM9DS1_REG_WHO_AM_I); 29 | if (device_id == MGOS_LSM9DS1_DEVID) { 30 | return true; 31 | } 32 | return false; 33 | } 34 | 35 | static bool mgos_imu_lsm9ds1_accgyro_create(struct mgos_i2c *i2c, uint8_t i2caddr) { 36 | if (!i2c) { 37 | return false; 38 | } 39 | 40 | // Reset acc/gyro 41 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_LSM9DS1_REG_CTRL_REG8, 0x81); 42 | mgos_usleep(10000); 43 | 44 | // FIFO_CTRL: FMODE=000 (FIFO off); FTH=00000 45 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_LSM9DS1_REG_FIFO_CTRL, 0x00); 46 | 47 | // CTRL_REG8: BOOT=0; BDU=1; H_LACTIVE=0; PP_OD=1; SIM=0; IF_ADD_INC=1; BLE=0; SW_RESET=0; 48 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_LSM9DS1_REG_CTRL_REG8, 0x54); 49 | 50 | // CTRL_REG9: 0; SLEEP_G=0; 0; FIFO_TEMP_EN=0; DRDY=1; I2C_DIS=0; FIFO_EN=0; STOP_ON_FTH=0; 51 | mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_LSM9DS1_REG_CTRL_REG9, 0x08); 52 | 53 | return true; 54 | 55 | (void)i2caddr; 56 | } 57 | 58 | bool mgos_imu_lsm9ds1_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data) { 59 | return mgos_imu_lsm9ds1_detect(dev->i2c, dev->i2caddr); 60 | 61 | (void)imu_user_data; 62 | } 63 | 64 | bool mgos_imu_lsm9ds1_acc_create(struct mgos_imu_acc *dev, void *imu_user_data) { 65 | struct mgos_imu_lsm9ds1_userdata *iud = (struct mgos_imu_lsm9ds1_userdata *)imu_user_data; 66 | 67 | if (!dev) { 68 | return false; 69 | } 70 | 71 | // Only initialize the LSM9DS1 if gyro hasn't done so yet 72 | if (!iud->accgyro_initialized) { 73 | if (!mgos_imu_lsm9ds1_accgyro_create(dev->i2c, dev->i2caddr)) { 74 | return false; 75 | } 76 | iud->accgyro_initialized = true; 77 | } 78 | 79 | // CTRL_REG5_XL: DEC=00 (no decimation); [ZYX]en_XL=111; 000 80 | // CTRL_REG6_XL: ODR_XL=011 (ODR 119Hz) FS_XL=11 (8G); BW_SCAL=0; BW_XL=00 (408Hz bandwidth) 81 | // CTRL_REG7_XL: HR=0 (lores); DCF=00 (50Hz cutoff); 00; FDS=0 (filter off); 0; HPIS1=0 (filter on interrupt) 82 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG7_XL, 0x00); 83 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG6_XL, 0x78); 84 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG5_XL, 0x68); 85 | 86 | /* 87 | #define SENSITIVITY_ACCELEROMETER_2 0.000061 88 | #define SENSITIVITY_ACCELEROMETER_4 0.000122 89 | #define SENSITIVITY_ACCELEROMETER_8 0.000244 90 | #define SENSITIVITY_ACCELEROMETER_16 0.000732 91 | */ 92 | dev->scale = 0.000244; 93 | return true; 94 | } 95 | 96 | bool mgos_imu_lsm9ds1_acc_read(struct mgos_imu_acc *dev, void *imu_user_data) { 97 | uint8_t data[6]; 98 | 99 | if (!dev) { 100 | return false; 101 | } 102 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_OUT_X_L_XL, 6, data)) { 103 | return false; 104 | } 105 | dev->ax = (data[1] << 8) | (data[0]); 106 | dev->ay = (data[3] << 8) | (data[2]); 107 | dev->az = (data[5] << 8) | (data[4]); 108 | return true; 109 | 110 | (void)imu_user_data; 111 | } 112 | 113 | bool mgos_imu_lsm9ds1_gyro_detect(struct mgos_imu_gyro *dev, void *imu_user_data) { 114 | return mgos_imu_lsm9ds1_detect(dev->i2c, dev->i2caddr); 115 | 116 | (void)imu_user_data; 117 | (void)imu_user_data; 118 | } 119 | 120 | bool mgos_imu_lsm9ds1_gyro_create(struct mgos_imu_gyro *dev, void *imu_user_data) { 121 | struct mgos_imu_lsm9ds1_userdata *iud = (struct mgos_imu_lsm9ds1_userdata *)imu_user_data; 122 | 123 | if (!dev) { 124 | return false; 125 | } 126 | 127 | // Only initialize the LSM9DS1 if acc hasn't done so yet 128 | if (!iud->accgyro_initialized) { 129 | if (!mgos_imu_lsm9ds1_accgyro_create(dev->i2c, dev->i2caddr)) { 130 | return false; 131 | } 132 | iud->accgyro_initialized = true; 133 | } 134 | 135 | // CTRL_REG1_G: ODR_G=011 (ODR 119Hz); FS_G=11 (2000dps); 0; BW_G=00 (14Hz bandwidth) 136 | // CTRL_REG2_G: 0000; INT_SEL=00; OUT_SEL=00; 137 | // CTRL_REG3_G: LP=0; HP_EN=0; HPCF_G=0000; 138 | // CTRL_REG4: 00; [ZYX]en_G=111; 0; LIR_XL1=0; 4D_XL1=0; 139 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG1_G, 0x78); 140 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG2_G, 0x00); 141 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG3_G, 0x00); 142 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG4, 0x38); 143 | 144 | /* 145 | #define SENSITIVITY_GYROSCOPE_245 0.00875 146 | #define SENSITIVITY_GYROSCOPE_500 0.0175 147 | #define SENSITIVITY_GYROSCOPE_2000 0.07 148 | */ 149 | dev->scale = 0.07f; 150 | return true; 151 | } 152 | 153 | bool mgos_imu_lsm9ds1_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data) { 154 | uint8_t data[6]; 155 | 156 | if (!dev) { 157 | return false; 158 | } 159 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_OUT_X_L_G, 6, data)) { 160 | return false; 161 | } 162 | dev->gx = (data[1] << 8) | (data[0]); 163 | dev->gy = (data[3] << 8) | (data[2]); 164 | dev->gz = (data[5] << 8) | (data[4]); 165 | 166 | return true; 167 | 168 | (void)imu_user_data; 169 | } 170 | 171 | bool mgos_imu_lsm9ds1_mag_detect(struct mgos_imu_mag *dev, void *imu_user_data) { 172 | int device_id; 173 | 174 | if (!dev->i2c) { 175 | return false; 176 | } 177 | 178 | device_id = mgos_i2c_read_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_WHO_AM_I_M); 179 | if (device_id == MGOS_LSM9DS1_DEVID_M) { 180 | return true; 181 | } 182 | return false; 183 | 184 | (void)imu_user_data; 185 | } 186 | 187 | bool mgos_imu_lsm9ds1_mag_create(struct mgos_imu_mag *dev, void *imu_user_data) { 188 | if (!dev) { 189 | return false; 190 | } 191 | 192 | // Reset mag 193 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG2_M, 0x0c); 194 | mgos_usleep(10000); 195 | 196 | 197 | // CTRL_REG1_M: TEMP_COMP=0; OM=10 (high performance); DO=110 (ODR 40Hz); FAST_ODR=0; ST=0 198 | // CTRL_REG2_M: 0; FS=10 (12Gauss); 0; REBOOT=0; SOFT_RST=0; 00 199 | // CTRL_REG3_M: I2C_DIS=0; 0; LP=0; 00; SIM=1 MD=00 (continuous) 200 | // CTRL_REG4_M: 0000; OMZ=10 (high performance); BLE=0; 0 201 | // CTRL_REG5_M: FAST_READ=0; BDU=1; 000000 202 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG1_M, 0x58); 203 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG2_M, 0x40); 204 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG3_M, 0x04); 205 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG4_M, 0x08); 206 | mgos_i2c_write_reg_b(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_CTRL_REG5_M, 0x40); 207 | 208 | /* 209 | #define SENSITIVITY_MAGNETOMETER_4 0.00014 210 | #define SENSITIVITY_MAGNETOMETER_8 0.00029 211 | #define SENSITIVITY_MAGNETOMETER_12 0.00043 212 | #define SENSITIVITY_MAGNETOMETER_16 0.00058 213 | */ 214 | dev->scale = 0.00043; 215 | dev->bias[0] = 1.f; 216 | dev->bias[1] = 1.f; 217 | dev->bias[2] = 1.f; 218 | return true; 219 | 220 | (void)imu_user_data; 221 | } 222 | 223 | bool mgos_imu_lsm9ds1_mag_read(struct mgos_imu_mag *dev, void *imu_user_data) { 224 | uint8_t data[6]; 225 | 226 | if (!dev) { 227 | return false; 228 | } 229 | if (!mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_LSM9DS1_REG_OUT_X_L_M, 6, data)) { 230 | return false; 231 | } 232 | dev->mx = (data[1] << 8) | (data[0]); 233 | dev->my = (data[3] << 8) | (data[2]); 234 | dev->mz = (data[5] << 8) | (data[4]); 235 | return true; 236 | 237 | (void)imu_user_data; 238 | } 239 | 240 | struct mgos_imu_lsm9ds1_userdata *mgos_imu_lsm9ds1_userdata_create(void) { 241 | struct mgos_imu_lsm9ds1_userdata *iud; 242 | 243 | iud = calloc(1, sizeof(struct mgos_imu_lsm9ds1_userdata)); 244 | if (!iud) { 245 | return NULL; 246 | } 247 | iud->accgyro_initialized = false; 248 | return iud; 249 | } 250 | -------------------------------------------------------------------------------- /include/mgos_imu.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "mgos.h" 20 | #include "mgos_i2c.h" 21 | 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | 26 | // Conversion constants between m/s/s and G's 27 | #define MSS2G (.10196798f) 28 | #define G2MSS (9.807f) 29 | 30 | // Conversion constants between Rad/sec and Deg/sec 31 | #define DEG2RAD (.017453292519943f) /* PI / 180 */ 32 | #define RAD2DEG (57.2957795130823f) /* 180 / PI */ 33 | 34 | // Conversion constants between uTesla and Gauss (1 tesla = 10^4 gauss) 35 | #define UTESLA2GAUSS (0.01f) 36 | #define GAUSS2UTESLA (100.f) 37 | 38 | 39 | enum mgos_imu_acc_type { 40 | ACC_NONE = 0, 41 | ACC_MPU9250, 42 | ACC_MPU9255, 43 | ACC_ADXL345, 44 | ACC_LSM303D, 45 | ACC_LSM303DLM, 46 | ACC_MMA8451, 47 | ACC_LSM9DS1, 48 | ACC_LSM6DSL, 49 | ACC_MPU6000, 50 | ACC_MPU6050, 51 | ACC_MPU6886, 52 | ACC_ICM20948 53 | }; 54 | 55 | enum mgos_imu_gyro_type { 56 | GYRO_NONE = 0, 57 | GYRO_MPU9250, 58 | GYRO_MPU9255, 59 | GYRO_L3GD20, 60 | GYRO_L3GD20H, 61 | GYRO_ITG3205, 62 | GYRO_LSM9DS1, 63 | GYRO_LSM6DSL, 64 | GYRO_MPU6000, 65 | GYRO_MPU6050, 66 | GYRO_MPU6886, 67 | GYRO_ICM20948 68 | }; 69 | 70 | enum mgos_imu_mag_type { 71 | MAG_NONE = 0, 72 | MAG_AK8963, 73 | MAG_AK8975, 74 | MAG_BMM150, 75 | MAG_MAG3110, 76 | MAG_LSM303D, 77 | MAG_LSM303DLM, 78 | MAG_HMC5883L, 79 | MAG_LSM9DS1, 80 | MAG_ICM20948 81 | }; 82 | 83 | struct mgos_imu; 84 | 85 | struct mgos_imu *mgos_imu_create(void); 86 | void mgos_imu_destroy(struct mgos_imu **imu); 87 | 88 | // Gyroscope functions 89 | struct mgos_imu_gyro_opts { 90 | enum mgos_imu_gyro_type type; // Gyroscope type. 91 | float odr; // Data rate, in Hz. See doc for set_odr(). 92 | float scale; // Scale. See doc for set_scale(). 93 | bool no_rst; // Do not perform reset of the device when configuring. 94 | }; 95 | 96 | bool mgos_imu_gyroscope_create_i2c(struct mgos_imu *imu, struct mgos_i2c *i2c, uint8_t i2caddr, const struct mgos_imu_gyro_opts *opts); 97 | bool mgos_imu_gyroscope_destroy(struct mgos_imu *imu); 98 | bool mgos_imu_gyroscope_present(struct mgos_imu *imu); 99 | 100 | // String representation of the sensor types, guaranteed to be less than or equal to 10 chars 101 | const char *mgos_imu_gyroscope_get_name(struct mgos_imu *imu); 102 | 103 | // Return gyroscope data in units of degrees/sec 104 | bool mgos_imu_gyroscope_get(struct mgos_imu *imu, float *x, float *y, float *z); 105 | 106 | // Get/set gyroscope offset in units of degrees/sec 107 | bool mgos_imu_gyroscope_get_offset(struct mgos_imu *imu, float *x, float *y, float *z); 108 | bool mgos_imu_gyroscope_set_offset(struct mgos_imu *imu, float x, float y, float z); 109 | 110 | // Get/set gyroscope scale in units of degrees/sec 111 | // The driver will set the scale to at least the given `scale` parameter, eg 1000 112 | // Will return true upon success, false if setting the scale is not feasible. 113 | bool mgos_imu_gyroscope_get_scale(struct mgos_imu *imu, float *scale); 114 | bool mgos_imu_gyroscope_set_scale(struct mgos_imu *imu, float scale); 115 | 116 | // Get/set gyroscope output data rate in units Hertz 117 | // The driver will set the data rate to at least the given `hertz` parameter, eg 100 118 | // Will return true upon success, false if setting the data rate is not feasible. 119 | bool mgos_imu_gyroscope_get_odr(struct mgos_imu *imu, float *hertz); 120 | bool mgos_imu_gyroscope_set_odr(struct mgos_imu *imu, float hertz); 121 | 122 | // Get/set gyroscope axes orientation relatve to accelerometer 123 | // *vector is a list of 9 floats which determine how much of a certain sensor axis 124 | // should be blended into calls to mgos_imu_gyroscope_get(). 125 | // Examples: swap x/y axes, and flip direction of z-axis 126 | // float v[9]={0, 1, 0, // x gets 0% of x-sensor, 100% of y-sensor, 0% of z-sensor 127 | // 1, 0, 0, // y gets 100% of x-sensor, 0% of y-sensor, 0% of z-sensor 128 | // 0, 0, -1}; // z gets 0% of x-sensor, 0% of y-sensor, -100% of z-sensor 129 | bool mgos_imu_gyroscope_get_orientation(struct mgos_imu *imu, float v[9]); 130 | bool mgos_imu_gyroscope_set_orientation(struct mgos_imu *imu, float v[9]); 131 | 132 | // Accelerometer functions 133 | struct mgos_imu_acc_opts { 134 | enum mgos_imu_acc_type type; // Accelerometer type. 135 | float odr; // Data rate, in Hz. See doc for set_odr(). 136 | float scale; // Scale. See doc for set_scale(). 137 | bool no_rst; // Do not perform reset of the device when configuring. 138 | }; 139 | 140 | bool mgos_imu_accelerometer_create_i2c(struct mgos_imu *imu, struct mgos_i2c *i2c, uint8_t i2caddr, const struct mgos_imu_acc_opts *opts); 141 | bool mgos_imu_accelerometer_destroy(struct mgos_imu *imu); 142 | bool mgos_imu_accelerometer_present(struct mgos_imu *imu); 143 | 144 | // String representation of the sensor types, guaranteed to be less than or equal to 10 chars 145 | const char *mgos_imu_accelerometer_get_name(struct mgos_imu *imu); 146 | 147 | // Return accelerometer data in units of G 148 | bool mgos_imu_accelerometer_get(struct mgos_imu *imu, float *x, float *y, float *z); 149 | 150 | // Get/set accelerometer offset in units of G 151 | bool mgos_imu_accelerometer_get_offset(struct mgos_imu *imu, float *x, float *y, float *z); 152 | bool mgos_imu_accelerometer_set_offset(struct mgos_imu *imu, float x, float y, float z); 153 | 154 | // Get/set accelerometer scale in units of G 155 | // The driver will set the scale to at least the given `scale` parameter, eg 20 m/s/s 156 | // Will return true upon success, false if setting the scale is not feasible. 157 | bool mgos_imu_accelerometer_get_scale(struct mgos_imu *imu, float *scale); 158 | bool mgos_imu_accelerometer_set_scale(struct mgos_imu *imu, float scale); 159 | 160 | // Get/set accelerometer output data rate in units Hertz 161 | // The driver will set the data rate to at least the given `hertz` parameter, eg 100 162 | // Will return true upon success, false if setting the data rate is not feasible. 163 | bool mgos_imu_accelerometer_get_odr(struct mgos_imu *imu, float *hertz); 164 | bool mgos_imu_accelerometer_set_odr(struct mgos_imu *imu, float hertz); 165 | 166 | 167 | // Magnetometer functions 168 | struct mgos_imu_mag_opts { 169 | enum mgos_imu_mag_type type; // Magnetometer type. 170 | float odr; // Data rate, in Hz. See doc for set_odr(). 171 | float scale; // Scale. See doc for set_scale(). 172 | bool no_rst; // Do not perform reset of the device when configuring. 173 | }; 174 | 175 | bool mgos_imu_magnetometer_create_i2c(struct mgos_imu *imu, struct mgos_i2c *i2c, uint8_t i2caddr, const struct mgos_imu_mag_opts *opts); 176 | bool mgos_imu_magnetometer_destroy(struct mgos_imu *imu); 177 | bool mgos_imu_magnetometer_present(struct mgos_imu *imu); 178 | 179 | // String representation of the sensor types, guaranteed to be less than or equal to 10 chars 180 | const char *mgos_imu_magnetometer_get_name(struct mgos_imu *imu); 181 | 182 | // Return magnetometer data in units of Gauss 183 | bool mgos_imu_magnetometer_get(struct mgos_imu *imu, float *x, float *y, float *z); 184 | 185 | // Get/set magnetometer scale in units of Gauss 186 | // The driver will set the scale to at least the given `scale` parameter, eg 400 187 | // Will return true upon success, false if setting the scale is not feasible. 188 | bool mgos_imu_magnetometer_get_scale(struct mgos_imu *imu, float *scale); 189 | bool mgos_imu_magnetometer_set_scale(struct mgos_imu *imu, float scale); 190 | 191 | // Get/set magnetometer output data rate in units Hertz 192 | // The driver will set the data rate to at least the given `hertz` parameter, eg 100 193 | // Will return true upon success, false if setting the data rate is not feasible. 194 | bool mgos_imu_magnetometer_get_odr(struct mgos_imu *imu, float *hertz); 195 | bool mgos_imu_magnetometer_set_odr(struct mgos_imu *imu, float hertz); 196 | 197 | // Get/set magnetometer axes orientation relative to accelerometer 198 | // *vector is a list of 9 floats which determine how much of a certain sensor axis 199 | // should be blended into calls to mgos_imu_magnetometer_get(). 200 | // Examples: swap x/y axes, and flip direction of z-axis 201 | // float v[9]={0, 1, 0, // x gets 0% of x-sensor, 100% of y-sensor, 0% of z-sensor 202 | // 1, 0, 0, // y gets 100% of x-sensor, 0% of y-sensor, 0% of z-sensor 203 | // 0, 0, -1}; // z gets 0% of x-sensor, 0% of y-sensor, -100% of z-sensor 204 | bool mgos_imu_magnetometer_get_orientation(struct mgos_imu *imu, float v[9]); 205 | bool mgos_imu_magnetometer_set_orientation(struct mgos_imu *imu, float v[9]); 206 | 207 | 208 | // Initialization function for MGOS -- currently a noop. 209 | bool mgos_imu_init(void); 210 | 211 | #ifdef __cplusplus 212 | } 213 | #endif 214 | -------------------------------------------------------------------------------- /src/mgos_imu_magnetometer.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_imu_internal.h" 19 | #include "mgos_imu_ak8963.h" 20 | #include "mgos_imu_ak8975.h" 21 | #include "mgos_imu_bmm150.h" 22 | #include "mgos_imu_mag3110.h" 23 | #include "mgos_imu_lsm303d.h" 24 | #include "mgos_imu_hmc5883l.h" 25 | #include "mgos_imu_lsm9ds1.h" 26 | #include "mgos_imu_icm20948.h" 27 | 28 | static struct mgos_imu_mag *mgos_imu_mag_create(void) { 29 | struct mgos_imu_mag *mag; 30 | 31 | mag = calloc(1, sizeof(struct mgos_imu_mag)); 32 | if (!mag) { 33 | return NULL; 34 | } 35 | memset(mag, 0, sizeof(struct mgos_imu_mag)); 36 | 37 | mag->opts.type = MAG_NONE; 38 | mag->orientation[0] = 1.f; 39 | mag->orientation[1] = 0.f; 40 | mag->orientation[2] = 0.f; 41 | mag->orientation[3] = 0.f; 42 | mag->orientation[4] = 1.f; 43 | mag->orientation[5] = 0.f; 44 | mag->orientation[6] = 0.f; 45 | mag->orientation[7] = 0.f; 46 | mag->orientation[8] = 1.f; 47 | return mag; 48 | } 49 | 50 | static bool mgos_imu_mag_destroy(struct mgos_imu_mag **mag, void *imu_user_data) { 51 | if (!*mag) { 52 | return false; 53 | } 54 | if ((*mag)->destroy) { 55 | (*mag)->destroy(*mag, imu_user_data); 56 | } 57 | if ((*mag)->user_data) { 58 | free((*mag)->user_data); 59 | } 60 | free(*mag); 61 | *mag = NULL; 62 | return true; 63 | } 64 | 65 | bool mgos_imu_magnetometer_destroy(struct mgos_imu *imu) { 66 | bool ret; 67 | 68 | if (!imu || !imu->mag) { 69 | return false; 70 | } 71 | ret = mgos_imu_mag_destroy(&(imu->mag), imu->user_data); 72 | imu->mag = NULL; 73 | return ret; 74 | } 75 | 76 | const char *mgos_imu_magnetometer_get_name(struct mgos_imu *imu) { 77 | if (!imu || !imu->mag) { 78 | return "VOID"; 79 | } 80 | 81 | switch (imu->mag->opts.type) { 82 | case MAG_NONE: return "NONE"; 83 | 84 | case MAG_AK8963: return "AK8963"; 85 | 86 | case MAG_AK8975: return "AK8975"; 87 | 88 | case MAG_BMM150: return "BMM150"; 89 | 90 | case MAG_MAG3110: return "MAG3110"; 91 | 92 | case MAG_LSM303D: return "LSM303D"; 93 | 94 | case MAG_LSM303DLM: return "LSM303DLM"; 95 | 96 | case MAG_HMC5883L: return "HMC5883L"; 97 | 98 | case MAG_LSM9DS1: return "LSM9DS1"; 99 | 100 | case MAG_ICM20948: return "ICM20948"; 101 | 102 | default: return "UNKNOWN"; 103 | } 104 | } 105 | 106 | bool mgos_imu_magnetometer_get(struct mgos_imu *imu, float *x, float *y, float *z) { 107 | float mxb, myb, mzb; 108 | 109 | if (!imu->mag || !imu->mag->read) { 110 | return false; 111 | } 112 | 113 | if (!imu->mag->read(imu->mag, imu->user_data)) { 114 | LOG(LL_ERROR, ("Could not read from magnetometer")); 115 | return false; 116 | } 117 | // LOG(LL_DEBUG, ("Raw: mx=%d my=%d mz=%d", imu->mag->mx, imu->mag->my, imu->mag->mz)); 118 | mxb = imu->mag->bias[0] * imu->mag->mx * imu->mag->scale; 119 | myb = imu->mag->bias[1] * imu->mag->my * imu->mag->scale; 120 | mzb = imu->mag->bias[2] * imu->mag->mz * imu->mag->scale; 121 | if (x) { 122 | *x = (mxb * imu->mag->orientation[0] + myb * imu->mag->orientation[1] + mzb * imu->mag->orientation[2]); 123 | } 124 | if (y) { 125 | *y = (mxb * imu->mag->orientation[3] + myb * imu->mag->orientation[4] + mzb * imu->mag->orientation[5]); 126 | } 127 | if (z) { 128 | *z = (mxb * imu->mag->orientation[6] + myb * imu->mag->orientation[7] + mzb * imu->mag->orientation[8]); 129 | } 130 | return true; 131 | } 132 | 133 | bool mgos_imu_magnetometer_create_i2c(struct mgos_imu *imu, struct mgos_i2c *i2c, uint8_t i2caddr, const struct mgos_imu_mag_opts *opts) { 134 | if (!imu || !i2c || !opts) { 135 | return false; 136 | } 137 | if (imu->mag) { 138 | mgos_imu_magnetometer_destroy(imu); 139 | } 140 | imu->mag = mgos_imu_mag_create(); 141 | if (!imu->mag) { 142 | return false; 143 | } 144 | imu->mag->i2c = i2c; 145 | imu->mag->i2caddr = i2caddr; 146 | imu->mag->opts = *opts; 147 | switch (opts->type) { 148 | case MAG_LSM9DS1: 149 | imu->mag->detect = mgos_imu_lsm9ds1_mag_detect; 150 | imu->mag->create = mgos_imu_lsm9ds1_mag_create; 151 | imu->mag->read = mgos_imu_lsm9ds1_mag_read; 152 | if (!imu->user_data) { 153 | imu->user_data = mgos_imu_lsm9ds1_userdata_create(); 154 | } 155 | break; 156 | 157 | case MAG_HMC5883L: 158 | imu->mag->detect = mgos_imu_hmc5883l_detect; 159 | imu->mag->create = mgos_imu_hmc5883l_create; 160 | imu->mag->read = mgos_imu_hmc5883l_read; 161 | break; 162 | 163 | case MAG_LSM303DLM: 164 | case MAG_LSM303D: 165 | imu->mag->detect = mgos_imu_lsm303d_mag_detect; 166 | imu->mag->create = mgos_imu_lsm303d_mag_create; 167 | imu->mag->read = mgos_imu_lsm303d_mag_read; 168 | if (!imu->user_data) { 169 | imu->user_data = mgos_imu_lsm303d_userdata_create(); 170 | } 171 | break; 172 | 173 | case MAG_AK8963: 174 | imu->mag->detect = mgos_imu_ak8963_detect; 175 | imu->mag->create = mgos_imu_ak8963_create; 176 | imu->mag->read = mgos_imu_ak8963_read; 177 | break; 178 | 179 | case MAG_AK8975: 180 | imu->mag->detect = mgos_imu_ak8975_detect; 181 | imu->mag->create = mgos_imu_ak8975_create; 182 | imu->mag->read = mgos_imu_ak8975_read; 183 | break; 184 | 185 | case MAG_BMM150: 186 | imu->mag->detect = mgos_imu_bmm150_detect; 187 | imu->mag->create = mgos_imu_bmm150_create; 188 | imu->mag->read = mgos_imu_bmm150_read; 189 | break; 190 | 191 | case MAG_MAG3110: 192 | imu->mag->detect = mgos_imu_mag3110_detect; 193 | imu->mag->create = mgos_imu_mag3110_create; 194 | imu->mag->read = mgos_imu_mag3110_read; 195 | break; 196 | 197 | case MAG_ICM20948: 198 | imu->mag->detect = mgos_imu_icm20948_mag_detect; 199 | imu->mag->create = mgos_imu_icm20948_mag_create; 200 | imu->mag->read = mgos_imu_icm20948_mag_read; 201 | imu->mag->get_odr = mgos_imu_icm20948_mag_get_odr; 202 | imu->mag->set_odr = mgos_imu_icm20948_mag_set_odr; 203 | if (!imu->user_data) { 204 | imu->user_data = mgos_imu_icm20948_userdata_create(); 205 | } 206 | break; 207 | 208 | default: 209 | LOG(LL_ERROR, ("Unknown magnetometer type %d", opts->type)); 210 | mgos_imu_magnetometer_destroy(imu); 211 | return false; 212 | } 213 | 214 | if (imu->mag->detect) { 215 | if (!imu->mag->detect(imu->mag, imu->user_data)) { 216 | LOG(LL_ERROR, ("Could not detect magnetometer type %d (%s) at I2C 0x%02x", 217 | opts->type, mgos_imu_magnetometer_get_name(imu), i2caddr)); 218 | mgos_imu_magnetometer_destroy(imu); 219 | return false; 220 | } else { 221 | LOG(LL_DEBUG, ("Successfully detected magnetometer type %d (%s) at I2C 0x%02x", 222 | opts->type, mgos_imu_magnetometer_get_name(imu), i2caddr)); 223 | } 224 | } 225 | 226 | if (imu->mag->create) { 227 | if (!imu->mag->create(imu->mag, imu->user_data)) { 228 | LOG(LL_ERROR, ("Could not create magnetometer type %d (%s) at I2C 0x%02x", 229 | opts->type, mgos_imu_magnetometer_get_name(imu), i2caddr)); 230 | mgos_imu_magnetometer_destroy(imu); 231 | return false; 232 | } else { 233 | LOG(LL_DEBUG, ("Successfully created magnetometer type %d (%s) at I2C 0x%02x", 234 | opts->type, mgos_imu_magnetometer_get_name(imu), i2caddr)); 235 | } 236 | } 237 | if (imu->mag->set_scale) { 238 | imu->mag->set_scale(imu->mag, imu->user_data, opts->scale); 239 | } 240 | 241 | if (imu->mag->set_odr) { 242 | imu->mag->set_odr(imu->mag, imu->user_data, opts->odr); 243 | } 244 | 245 | return true; 246 | } 247 | 248 | bool mgos_imu_magnetometer_get_orientation(struct mgos_imu *imu, float v[9]) { 249 | if (!imu || !imu->mag || !v) { 250 | return false; 251 | } 252 | memcpy(v, imu->mag->orientation, sizeof(float) * 9); 253 | return true; 254 | } 255 | 256 | bool mgos_imu_magnetometer_set_orientation(struct mgos_imu *imu, float v[9]) { 257 | if (!imu || !imu->mag || !v) { 258 | return false; 259 | } 260 | memcpy(imu->mag->orientation, v, sizeof(float) * 9); 261 | return true; 262 | } 263 | 264 | bool mgos_imu_magnetometer_get_scale(struct mgos_imu *imu, float *scale) { 265 | if (!imu || !imu->mag || !imu->mag->get_scale || !scale) { 266 | return false; 267 | } 268 | return imu->mag->get_scale(imu->mag, imu->user_data, scale); 269 | } 270 | 271 | bool mgos_imu_magnetometer_set_scale(struct mgos_imu *imu, float scale) { 272 | if (!imu || !imu->mag || !imu->mag->set_scale) { 273 | return false; 274 | } 275 | return imu->mag->set_scale(imu->mag, imu->user_data, scale); 276 | } 277 | 278 | bool mgos_imu_magnetometer_get_odr(struct mgos_imu *imu, float *hertz) { 279 | if (!imu || !imu->mag || !imu->mag->get_odr || !hertz) { 280 | return false; 281 | } 282 | return imu->mag->get_odr(imu->mag, imu->user_data, hertz); 283 | } 284 | 285 | bool mgos_imu_magnetometer_set_odr(struct mgos_imu *imu, float hertz) { 286 | if (!imu || !imu->mag || !imu->mag->set_odr) { 287 | return false; 288 | } 289 | return imu->mag->set_odr(imu->mag, imu->user_data, hertz); 290 | } 291 | -------------------------------------------------------------------------------- /third-party/bosch/src/bmm150.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. 3 | * 4 | * BSD-3-Clause 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived from 18 | * this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 28 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 29 | * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 30 | * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | */ 34 | 35 | /* Original code has been conveniently modified to fit current environment 36 | * requirements */ 37 | 38 | #include "bmm150.h" 39 | #include "mgos_imu_bmm150.h" 40 | 41 | bool read_trim_registers(struct mgos_imu_mag *dev) { 42 | struct mgos_imu_bmm150_trim_registers *trim_data = 43 | (struct mgos_imu_bmm150_trim_registers *)dev->user_data; 44 | uint8_t trim_x1y1[2]; 45 | uint8_t trim_xyz_data[4]; 46 | uint8_t trim_xy1xy2[10]; 47 | 48 | /* Trim register values are read */ 49 | if (mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_BMM150_DIG_X1, 2, 50 | trim_x1y1) && 51 | mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_BMM150_DIG_Z4_LSB, 4, 52 | trim_xyz_data) && 53 | mgos_i2c_read_reg_n(dev->i2c, dev->i2caddr, MGOS_BMM150_DIG_Z2_LSB, 10, 54 | trim_xy1xy2)) { 55 | /* Trim data which is read is updated 56 | * in the device structure 57 | */ 58 | uint16_t temp_msb = 0; 59 | trim_data->dig_x1 = (int8_t)trim_x1y1[0]; 60 | trim_data->dig_y1 = (int8_t)trim_x1y1[1]; 61 | trim_data->dig_x2 = (int8_t)trim_xyz_data[2]; 62 | trim_data->dig_y2 = (int8_t)trim_xyz_data[3]; 63 | temp_msb = ((uint16_t)trim_xy1xy2[3]) << 8; 64 | trim_data->dig_z1 = (uint16_t)(temp_msb | trim_xy1xy2[2]); 65 | temp_msb = ((uint16_t)trim_xy1xy2[1]) << 8; 66 | trim_data->dig_z2 = (int16_t)(temp_msb | trim_xy1xy2[0]); 67 | temp_msb = ((uint16_t)trim_xy1xy2[7]) << 8; 68 | trim_data->dig_z3 = (int16_t)(temp_msb | trim_xy1xy2[6]); 69 | temp_msb = ((uint16_t)trim_xyz_data[1]) << 8; 70 | trim_data->dig_z4 = (int16_t)(temp_msb | trim_xyz_data[0]); 71 | trim_data->dig_xy1 = trim_xy1xy2[9]; 72 | trim_data->dig_xy2 = (int8_t)trim_xy1xy2[8]; 73 | temp_msb = ((uint16_t)(trim_xy1xy2[5] & 0x7F)) << 8; 74 | trim_data->dig_xyz1 = (uint16_t)(temp_msb | trim_xy1xy2[4]); 75 | return true; 76 | } 77 | 78 | return false; 79 | } 80 | 81 | /*! 82 | * @brief This internal API is used to obtain the compensated 83 | * magnetometer X axis data(micro-tesla) in int16_t. 84 | */ 85 | int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall, 86 | const struct mgos_imu_bmm150_trim_registers *trim_data) { 87 | int16_t retval; 88 | uint16_t process_comp_x0 = 0; 89 | int32_t process_comp_x1; 90 | uint16_t process_comp_x2; 91 | int32_t process_comp_x3; 92 | int32_t process_comp_x4; 93 | int32_t process_comp_x5; 94 | int32_t process_comp_x6; 95 | int32_t process_comp_x7; 96 | int32_t process_comp_x8; 97 | int32_t process_comp_x9; 98 | int32_t process_comp_x10; 99 | 100 | /* Overflow condition check */ 101 | if (mag_data_x != MGOS_BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP) { 102 | if (data_rhall != 0) { 103 | /* Availability of valid data */ 104 | process_comp_x0 = data_rhall; 105 | } else if (trim_data->dig_xyz1 != 0) { 106 | process_comp_x0 = trim_data->dig_xyz1; 107 | } else { 108 | process_comp_x0 = 0; 109 | } 110 | 111 | if (process_comp_x0 != 0) { 112 | /* Processing compensation equations */ 113 | process_comp_x1 = ((int32_t)trim_data->dig_xyz1) * 16384; 114 | process_comp_x2 = 115 | ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); 116 | retval = ((int16_t)process_comp_x2); 117 | process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); 118 | process_comp_x4 = 119 | (((int32_t)trim_data->dig_xy2) * (process_comp_x3 / 128)); 120 | process_comp_x5 = (int32_t)(((int16_t)trim_data->dig_xy1) * 128); 121 | process_comp_x6 = ((int32_t)retval) * process_comp_x5; 122 | process_comp_x7 = 123 | (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); 124 | process_comp_x8 = 125 | ((int32_t)(((int16_t)trim_data->dig_x2) + ((int16_t)0xA0))); 126 | process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); 127 | process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; 128 | retval = ((int16_t)(process_comp_x10 / 8192)); 129 | retval = (retval + (((int16_t)trim_data->dig_x1) * 8)) / 16; 130 | } else { 131 | retval = MGOS_BMM150_OVERFLOW_OUTPUT; 132 | } 133 | } else { 134 | /* Overflow condition */ 135 | retval = MGOS_BMM150_OVERFLOW_OUTPUT; 136 | } 137 | 138 | return retval; 139 | } 140 | 141 | /*! 142 | * @brief This internal API is used to obtain the compensated 143 | * magnetometer Y axis data(micro-tesla) in int16_t. 144 | */ 145 | int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall, 146 | const struct mgos_imu_bmm150_trim_registers *trim_data) { 147 | int16_t retval; 148 | uint16_t process_comp_y0 = 0; 149 | int32_t process_comp_y1; 150 | uint16_t process_comp_y2; 151 | int32_t process_comp_y3; 152 | int32_t process_comp_y4; 153 | int32_t process_comp_y5; 154 | int32_t process_comp_y6; 155 | int32_t process_comp_y7; 156 | int32_t process_comp_y8; 157 | int32_t process_comp_y9; 158 | 159 | /* Overflow condition check */ 160 | if (mag_data_y != MGOS_BMM150_OVERFLOW_ADCVAL_XYAXES_FLIP) { 161 | if (data_rhall != 0) { 162 | /* Availability of valid data */ 163 | process_comp_y0 = data_rhall; 164 | } else if (trim_data->dig_xyz1 != 0) { 165 | process_comp_y0 = trim_data->dig_xyz1; 166 | } else { 167 | process_comp_y0 = 0; 168 | } 169 | 170 | if (process_comp_y0 != 0) { 171 | /* Processing compensation equations */ 172 | process_comp_y1 = 173 | (((int32_t)trim_data->dig_xyz1) * 16384) / process_comp_y0; 174 | process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); 175 | retval = ((int16_t)process_comp_y2); 176 | process_comp_y3 = ((int32_t)retval) * ((int32_t)retval); 177 | process_comp_y4 = ((int32_t)trim_data->dig_xy2) * (process_comp_y3 / 128); 178 | process_comp_y5 = ((int32_t)(((int16_t)trim_data->dig_xy1) * 128)); 179 | process_comp_y6 = 180 | ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); 181 | process_comp_y7 = 182 | ((int32_t)(((int16_t)trim_data->dig_y2) + ((int16_t)0xA0))); 183 | process_comp_y8 = 184 | (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); 185 | process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); 186 | retval = (int16_t)(process_comp_y9 / 8192); 187 | retval = (retval + (((int16_t)trim_data->dig_y1) * 8)) / 16; 188 | } else { 189 | retval = MGOS_BMM150_OVERFLOW_OUTPUT; 190 | } 191 | } else { 192 | /* Overflow condition */ 193 | retval = MGOS_BMM150_OVERFLOW_OUTPUT; 194 | } 195 | 196 | return retval; 197 | } 198 | 199 | /*! 200 | * @brief This internal API is used to obtain the compensated 201 | * magnetometer Z axis data(micro-tesla) in int16_t. 202 | */ 203 | int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall, 204 | const struct mgos_imu_bmm150_trim_registers *trim_data) { 205 | int32_t retval; 206 | int16_t process_comp_z0; 207 | int32_t process_comp_z1; 208 | int32_t process_comp_z2; 209 | int32_t process_comp_z3; 210 | int16_t process_comp_z4; 211 | 212 | if (mag_data_z != MGOS_BMM150_OVERFLOW_ADCVAL_ZAXIS_HALL) { 213 | if ((trim_data->dig_z2 != 0) && (trim_data->dig_z1 != 0) && 214 | (data_rhall != 0) && (trim_data->dig_xyz1 != 0)) { 215 | /*Processing compensation equations */ 216 | process_comp_z0 = ((int16_t)data_rhall) - ((int16_t)trim_data->dig_xyz1); 217 | process_comp_z1 = 218 | (((int32_t)trim_data->dig_z3) * ((int32_t)(process_comp_z0))) / 4; 219 | process_comp_z2 = (((int32_t)(mag_data_z - trim_data->dig_z4)) * 32768); 220 | process_comp_z3 = 221 | ((int32_t)trim_data->dig_z1) * (((int16_t)data_rhall) * 2); 222 | process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); 223 | retval = ((process_comp_z2 - process_comp_z1) / 224 | (trim_data->dig_z2 + process_comp_z4)); 225 | 226 | /* Saturate result to +/- 2 micro-tesla */ 227 | if (retval > MGOS_BMM150_POSITIVE_SATURATION_Z) { 228 | retval = MGOS_BMM150_POSITIVE_SATURATION_Z; 229 | } else if (retval < MGOS_BMM150_NEGATIVE_SATURATION_Z) { 230 | retval = MGOS_BMM150_NEGATIVE_SATURATION_Z; 231 | } 232 | 233 | /* Conversion of LSB to micro-tesla */ 234 | retval = retval / 16; 235 | } else { 236 | retval = MGOS_BMM150_OVERFLOW_OUTPUT; 237 | } 238 | } else { 239 | /* Overflow condition */ 240 | retval = MGOS_BMM150_OVERFLOW_OUTPUT; 241 | } 242 | 243 | return (int16_t)retval; 244 | } 245 | -------------------------------------------------------------------------------- /src/mgos_imu_lsm6dsl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include "mgos_imu_internal.h" 21 | 22 | #define MGOS_LSM6DSL_DEFAULT_I2CADDR (0x6b) 23 | #define MGOS_LSM6DSL_DEVID (0x6A) 24 | 25 | enum mgos_imu_lsm6dsl_fifo_mode { 26 | MGOS_LSM6DSL_FIFO_MODE_BYPASS = 0, 27 | MGOS_LSM6DSL_FIFO_MODE_FIFO = 1, 28 | MGOS_LSM6DSL_FIFO_MODE_CONT_FIFO = 3, 29 | MGOS_LSM6DSL_FIFO_MODE_BYPASS_CONT = 4, 30 | MGOS_LSM6DSL_FIFO_MODE_CONT = 6, 31 | }; 32 | 33 | // LSM6DSL -- Registers (Acc/Gyro) 34 | #define MGOS_LSM6DSL_REG_FUNC_CFG_ACCESS (0x01) 35 | #define MGOS_LSM6DSL_REG_SENSOR_SYNC_TIME_FRAME (0x04) 36 | #define MGOS_LSM6DSL_REG_SENSOR_SYNC_RES_RATIO (0x05) 37 | #define MGOS_LSM6DSL_REG_FIFO_CTRL1 (0x06) 38 | #define MGOS_LSM6DSL_REG_FIFO_CTRL2 (0x07) 39 | #define MGOS_LSM6DSL_REG_FIFO_CTRL3 (0x08) 40 | #define MGOS_LSM6DSL_REG_FIFO_CTRL4 (0x09) 41 | #define MGOS_LSM6DSL_REG_FIFO_CTRL5 (0x0A) 42 | #define MGOS_LSM6DSL_REG_DRDY_PULSE_CFG_G (0x0B) 43 | #define MGOS_LSM6DSL_REG_INT1_CTRL (0x0D) 44 | #define MGOS_LSM6DSL_REG_INT2_CTRL (0x0E) 45 | #define MGOS_LSM6DSL_REG_WHO_AM_I (0x0F) 46 | #define MGOS_LSM6DSL_REG_CTRL1_XL (0x10) 47 | #define MGOS_LSM6DSL_REG_CTRL2_G (0x11) 48 | #define MGOS_LSM6DSL_REG_CTRL3_C (0x12) 49 | #define MGOS_LSM6DSL_REG_CTRL4_C (0x13) 50 | #define MGOS_LSM6DSL_REG_CTRL5_C (0x14) 51 | #define MGOS_LSM6DSL_REG_CTRL6_C (0x15) 52 | #define MGOS_LSM6DSL_REG_CTRL7_G (0x16) 53 | #define MGOS_LSM6DSL_REG_CTRL8_XL (0x17) 54 | #define MGOS_LSM6DSL_REG_CTRL9_XL (0x18) 55 | #define MGOS_LSM6DSL_REG_CTRL10_C (0x19) 56 | #define MGOS_LSM6DSL_REG_WAKE_UP_SRC (0x1B) 57 | #define MGOS_LSM6DSL_REG_TAP_SRC (0x1C) 58 | #define MGOS_LSM6DSL_REG_D6D_SRC (0x1D) 59 | #define MGOS_LSM6DSL_REG_STATUS_REG (0x1E) 60 | #define MGOS_LSM6DSL_REG_OUT_TEMP_L (0x20) 61 | #define MGOS_LSM6DSL_REG_OUT_TEMP_H (0x21) 62 | #define MGOS_LSM6DSL_REG_OUTX_L_G (0x22) 63 | #define MGOS_LSM6DSL_REG_OUTX_H_G (0x23) 64 | #define MGOS_LSM6DSL_REG_OUTY_L_G (0x24) 65 | #define MGOS_LSM6DSL_REG_OUTY_H_G (0x25) 66 | #define MGOS_LSM6DSL_REG_OUTZ_L_G (0x26) 67 | #define MGOS_LSM6DSL_REG_OUTZ_H_G (0x27) 68 | #define MGOS_LSM6DSL_REG_OUTX_L_XL (0x28) 69 | #define MGOS_LSM6DSL_REG_OUTX_H_XL (0x29) 70 | #define MGOS_LSM6DSL_REG_OUTY_L_XL (0x2A) 71 | #define MGOS_LSM6DSL_REG_OUTY_H_XL (0x2B) 72 | #define MGOS_LSM6DSL_REG_OUTZ_L_XL (0x2C) 73 | #define MGOS_LSM6DSL_REG_OUTZ_H_XL (0x2D) 74 | #define MGOS_LSM6DSL_REG_SENSORHUB1_REG (0x2E) 75 | #define MGOS_LSM6DSL_REG_SENSORHUB2_REG (0x2F) 76 | #define MGOS_LSM6DSL_REG_SENSORHUB3_REG (0x30) 77 | #define MGOS_LSM6DSL_REG_SENSORHUB4_REG (0x31) 78 | #define MGOS_LSM6DSL_REG_SENSORHUB5_REG (0x32) 79 | #define MGOS_LSM6DSL_REG_SENSORHUB6_REG (0x33) 80 | #define MGOS_LSM6DSL_REG_SENSORHUB7_REG (0x34) 81 | #define MGOS_LSM6DSL_REG_SENSORHUB8_REG (0x35) 82 | #define MGOS_LSM6DSL_REG_SENSORHUB9_REG (0x36) 83 | #define MGOS_LSM6DSL_REG_SENSORHUB10_REG (0x37) 84 | #define MGOS_LSM6DSL_REG_SENSORHUB11_REG (0x38) 85 | #define MGOS_LSM6DSL_REG_SENSORHUB12_REG (0x39) 86 | #define MGOS_LSM6DSL_REG_FIFO_STATUS1 (0x3A) 87 | #define MGOS_LSM6DSL_REG_FIFO_STATUS2 (0x3B) 88 | #define MGOS_LSM6DSL_REG_FIFO_STATUS3 (0x3C) 89 | #define MGOS_LSM6DSL_REG_FIFO_STATUS4 (0x3D) 90 | #define MGOS_LSM6DSL_REG_FIFO_DATA_OUT_L (0x3E) 91 | #define MGOS_LSM6DSL_REG_FIFO_DATA_OUT_H (0x3F) 92 | #define MGOS_LSM6DSL_REG_TIMESTAMP0_REG (0x40) 93 | #define MGOS_LSM6DSL_REG_TIMESTAMP2_REG (0x42) 94 | #define MGOS_LSM6DSL_REG_STEP_TIMESTAMP_L (0x49) 95 | #define MGOS_LSM6DSL_REG_STEP_TIMESTAMP_H (0x4A) 96 | #define MGOS_LSM6DSL_REG_STEP_COUNTER_L (0x4B) 97 | #define MGOS_LSM6DSL_REG_STEP_COUNTER_H (0x4C) 98 | #define MGOS_LSM6DSL_REG_SENSORHUB13_REG (0x4D) 99 | #define MGOS_LSM6DSL_REG_SENSORHUB14_REG (0x4E) 100 | #define MGOS_LSM6DSL_REG_SENSORHUB15_REG (0x4F) 101 | #define MGOS_LSM6DSL_REG_SENSORHUB16_REG (0x50) 102 | #define MGOS_LSM6DSL_REG_SENSORHUB17_REG (0x51) 103 | #define MGOS_LSM6DSL_REG_SENSORHUB18_REG (0x52) 104 | #define MGOS_LSM6DSL_REG_FUNC_SRC1 (0x53) 105 | #define MGOS_LSM6DSL_REG_FUNC_SRC2 (0x54) 106 | #define MGOS_LSM6DSL_REG_WRIST_TILT_IA (0x55) 107 | #define MGOS_LSM6DSL_REG_TAP_CFG (0x58) 108 | #define MGOS_LSM6DSL_REG_TAP_THS_6D (0x59) 109 | #define MGOS_LSM6DSL_REG_INT_DUR2 (0x5A) 110 | #define MGOS_LSM6DSL_REG_WAKE_UP_THS (0x5B) 111 | #define MGOS_LSM6DSL_REG_WAKE_UP_DUR (0x5C) 112 | #define MGOS_LSM6DSL_REG_FREE_FALL (0x5D) 113 | #define MGOS_LSM6DSL_REG_MD1_CFG (0x5E) 114 | #define MGOS_LSM6DSL_REG_MD2_CFG (0x5F) 115 | #define MGOS_LSM6DSL_REG_MASTER_CMD_CODE (0x60) 116 | #define MGOS_LSM6DSL_REG_SENS_SYNC_SPI_ERROR_CODE (0x61) 117 | #define MGOS_LSM6DSL_REG_OUT_MAG_RAW_X_L (0x66) 118 | #define MGOS_LSM6DSL_REG_OUT_MAG_RAW_X_H (0x67) 119 | #define MGOS_LSM6DSL_REG_OUT_MAG_RAW_Y_L (0x68) 120 | #define MGOS_LSM6DSL_REG_OUT_MAG_RAW_Y_H (0x69) 121 | #define MGOS_LSM6DSL_REG_OUT_MAG_RAW_Z_L (0x6A) 122 | #define MGOS_LSM6DSL_REG_OUT_MAG_RAW_Z_H (0x6B) 123 | #define MGOS_LSM6DSL_REG_X_OFS_USR (0x73) 124 | #define MGOS_LSM6DSL_REG_Y_OFS_USR (0x74) 125 | #define MGOS_LSM6DSL_REG_Z_OFS_USR (0x75) 126 | 127 | // Interrupt handler will receive. 128 | // int_mask will be a combination of MGOS_LSM6DSL_INT* bits defined below. 129 | typedef void (*mgos_imu_lsm6dsl_int_cb)(struct mgos_imu *imu, uint32_t int_mask, void *user_data); 130 | 131 | struct mgos_imu_lsm6dsl_userdata { 132 | bool initialized; 133 | mgos_imu_lsm6dsl_int_cb int_cb; 134 | void * int_cb_user_data; 135 | int int_gpio; 136 | }; 137 | 138 | struct mgos_imu_lsm6dsl_userdata *mgos_imu_lsm6dsl_userdata_create(void); 139 | 140 | bool mgos_imu_lsm6dsl_acc_detect(struct mgos_imu_acc *dev, void *imu_user_data); 141 | bool mgos_imu_lsm6dsl_acc_create(struct mgos_imu_acc *dev, void *imu_user_data); 142 | bool mgos_imu_lsm6dsl_acc_read(struct mgos_imu_acc *dev, void *imu_user_data); 143 | bool mgos_imu_lsm6dsl_acc_get_scale(struct mgos_imu_acc *dev, void *imu_user_data, float *scale); 144 | bool mgos_imu_lsm6dsl_acc_set_scale(struct mgos_imu_acc *dev, void *imu_user_data, float scale); 145 | bool mgos_imu_lsm6dsl_acc_get_odr(struct mgos_imu_acc *dev, void *imu_user_data, float *odr); 146 | bool mgos_imu_lsm6dsl_acc_set_odr(struct mgos_imu_acc *dev, void *imu_user_data, float odr); 147 | 148 | bool mgos_imu_lsm6dsl_gyro_detect(struct mgos_imu_gyro *dev, void *imu_user_data); 149 | bool mgos_imu_lsm6dsl_gyro_create(struct mgos_imu_gyro *dev, void *imu_user_data); 150 | bool mgos_imu_lsm6dsl_gyro_read(struct mgos_imu_gyro *dev, void *imu_user_data); 151 | 152 | // Interrupts 153 | #define MGOS_LSM6DSL_INT_DRDY_XL (1 << 0) 154 | #define MGOS_LSM6DSL_INT_DRDY_G (1 << 1) 155 | #define MGOS_LSM6DSL_INT_FIFO_THR (1 << 3) 156 | #define MGOS_LSM6DSL_INT_FIFO_OVR (1 << 4) 157 | #define MGOS_LSM6DSL_INT_FIFO_FULL (1 << 5) 158 | #define MGOS_LSM6DSL_INT_TIMER (1 << 8) 159 | #define MGOS_LSM6DSL_INT_TILT (1 << 9) 160 | #define MGOS_LSM6DSL_INT_D6D (1 << 10) 161 | #define MGOS_LSM6DSL_INT_DBL_TAP (1 << 11) 162 | #define MGOS_LSM6DSL_INT_FF (1 << 12) 163 | #define MGOS_LSM6DSL_INT_WU (1 << 13) 164 | #define MGOS_LSM6DSL_INT_TAP (1 << 14) 165 | #define MGOS_LSM6DSL_INT_INACT (1 << 15) 166 | // These interrupt sources are only available on INT1 or INT2 pin. 167 | #define MGOS_LSM6DSL_INT1_BOOT (1 << 2) 168 | #define MGOS_LSM6DSL_INT1_SIGN_MOT (1 << 6) 169 | #define MGOS_LSM6DSL_INT1_STEP_DET (1 << 7) 170 | #define MGOS_LSM6DSL_INT2_DRDY_TEMP (1 << 18) 171 | #define MGOS_LSM6DSL_INT2_STEP_OVR (1 << 22) 172 | #define MGOS_LSM6DSL_INT2_STEP_DELTA (1 << 23) 173 | 174 | // Set INT1 and/or INT2 interrupt handlers. 175 | // One or both can be specified, unused should be set to -1. 176 | // If both pins are set and are the same, will configure the device to send INT2 interrupts to INT1. 177 | bool mgos_imu_lsm6dsl_set_int_handler( 178 | struct mgos_imu *imu, int int1_gpio, int int2_gpio, 179 | mgos_imu_lsm6dsl_int_cb cb, void *user_data); 180 | 181 | // Enable/disable specific interrupt sources. 182 | bool mgos_imu_lsm6dsl_int1_enable(struct mgos_imu *imu, uint32_t int1_mask); 183 | bool mgos_imu_lsm6dsl_int1_disable(struct mgos_imu *imu, uint32_t int1_mask); 184 | bool mgos_imu_lsm6dsl_int2_enable(struct mgos_imu *imu, uint32_t int2_mask); 185 | bool mgos_imu_lsm6dsl_int2_disable(struct mgos_imu *imu, uint32_t int2_mask); 186 | 187 | // Read and clear all the interrupt sources. 188 | // *res contains mask of MGOS_LSM6DSL_INT_* values. 189 | bool mgos_imu_lsm6dsl_get_and_clear_ints(struct mgos_imu *imu, uint32_t *res); 190 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | -------------------------------------------------------------------------------- /src/mgos_imu_accelerometer.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_imu_internal.h" 19 | #include "mgos_imu_mpu925x.h" 20 | #include "mgos_imu_adxl345.h" 21 | #include "mgos_imu_lsm303d.h" 22 | #include "mgos_imu_mma8451.h" 23 | #include "mgos_imu_lsm9ds1.h" 24 | #include "mgos_imu_lsm6dsl.h" 25 | #include "mgos_imu_mpu60x0.h" 26 | #include "mgos_imu_mpu6886.h" 27 | #include "mgos_imu_icm20948.h" 28 | 29 | static struct mgos_imu_acc *mgos_imu_acc_create(void) { 30 | struct mgos_imu_acc *acc; 31 | 32 | acc = calloc(1, sizeof(struct mgos_imu_acc)); 33 | if (!acc) { 34 | return NULL; 35 | } 36 | memset(acc, 0, sizeof(struct mgos_imu_acc)); 37 | 38 | acc->opts.type = ACC_NONE; 39 | return acc; 40 | } 41 | 42 | static bool mgos_imu_acc_destroy(struct mgos_imu_acc **acc, void *imu_user_data) { 43 | if (!*acc) { 44 | return false; 45 | } 46 | if ((*acc)->destroy) { 47 | (*acc)->destroy(*acc, imu_user_data); 48 | } 49 | if ((*acc)->user_data) { 50 | free((*acc)->user_data); 51 | } 52 | free(*acc); 53 | *acc = NULL; 54 | return true; 55 | } 56 | 57 | bool mgos_imu_accelerometer_destroy(struct mgos_imu *imu) { 58 | bool ret; 59 | 60 | if (!imu || !imu->acc) { 61 | return false; 62 | } 63 | ret = mgos_imu_acc_destroy(&(imu->acc), imu->user_data); 64 | imu->acc = NULL; 65 | return ret; 66 | } 67 | 68 | const char *mgos_imu_accelerometer_get_name(struct mgos_imu *imu) { 69 | if (!imu || !imu->acc) { 70 | return "VOID"; 71 | } 72 | 73 | switch (imu->acc->opts.type) { 74 | case ACC_NONE: return "NONE"; 75 | 76 | case ACC_MPU9250: return "MPU9250"; 77 | 78 | case ACC_MPU9255: return "MPU9255"; 79 | 80 | case ACC_ADXL345: return "ADXL345"; 81 | 82 | case ACC_LSM303D: return "LSM303D"; 83 | 84 | case ACC_LSM303DLM: return "LSM303DLM"; 85 | 86 | case ACC_MMA8451: return "MMA8451"; 87 | 88 | case ACC_LSM9DS1: return "LSM9DS1"; 89 | 90 | case ACC_LSM6DSL: return "LSM6DSL"; 91 | 92 | case ACC_MPU6000: return "MPU6000"; 93 | 94 | case ACC_MPU6050: return "MPU6050"; 95 | 96 | case ACC_MPU6886: return "MPU6886"; 97 | 98 | case ACC_ICM20948: return "ICM20948"; 99 | 100 | default: return "UNKNOWN"; 101 | } 102 | } 103 | 104 | bool mgos_imu_accelerometer_get(struct mgos_imu *imu, float *x, float *y, float *z) { 105 | if (!imu->acc || !imu->acc->read) { 106 | return false; 107 | } 108 | 109 | if (!imu->acc->read(imu->acc, imu->user_data)) { 110 | LOG(LL_ERROR, ("Could not read from accelerometer")); 111 | return false; 112 | } 113 | // LOG(LL_DEBUG, ("Raw: ax=%d ay=%d az=%d", imu->acc->ax, imu->acc->ay, imu->acc->az)); 114 | if (x) { 115 | *x = (imu->acc->scale * imu->acc->ax) + imu->acc->offset_ax; 116 | } 117 | if (y) { 118 | *y = (imu->acc->scale * imu->acc->ay) + imu->acc->offset_ay; 119 | } 120 | if (z) { 121 | *z = (imu->acc->scale * imu->acc->az) + imu->acc->offset_az; 122 | } 123 | return true; 124 | } 125 | 126 | bool mgos_imu_accelerometer_create_i2c(struct mgos_imu *imu, struct mgos_i2c *i2c, uint8_t i2caddr, const struct mgos_imu_acc_opts *opts) { 127 | if (!imu || !i2c || !opts) { 128 | return false; 129 | } 130 | if (imu->acc) { 131 | mgos_imu_accelerometer_destroy(imu); 132 | } 133 | imu->acc = mgos_imu_acc_create(); 134 | if (!imu->acc) { 135 | false; 136 | } 137 | imu->acc->i2c = i2c; 138 | imu->acc->i2caddr = i2caddr; 139 | imu->acc->opts = *opts; 140 | switch (opts->type) { 141 | case ACC_MPU6000: 142 | case ACC_MPU6050: 143 | imu->acc->detect = mgos_imu_mpu60x0_acc_detect; 144 | imu->acc->create = mgos_imu_mpu60x0_acc_create; 145 | imu->acc->read = mgos_imu_mpu60x0_acc_read; 146 | imu->acc->get_scale = mgos_imu_mpu60x0_acc_get_scale; 147 | imu->acc->set_scale = mgos_imu_mpu60x0_acc_set_scale; 148 | if (!imu->user_data) { 149 | imu->user_data = mgos_imu_mpu60x0_userdata_create(); 150 | } 151 | break; 152 | 153 | case ACC_MPU6886: 154 | imu->acc->detect = mgos_imu_mpu6886_acc_detect; 155 | imu->acc->create = mgos_imu_mpu60x0_acc_create; 156 | imu->acc->read = mgos_imu_mpu60x0_acc_read; 157 | imu->acc->get_scale = mgos_imu_mpu60x0_acc_get_scale; 158 | imu->acc->set_scale = mgos_imu_mpu60x0_acc_set_scale; 159 | if (!imu->user_data) { 160 | imu->user_data = mgos_imu_mpu60x0_userdata_create(); 161 | } 162 | break; 163 | 164 | case ACC_LSM6DSL: 165 | imu->acc->detect = mgos_imu_lsm6dsl_acc_detect; 166 | imu->acc->create = mgos_imu_lsm6dsl_acc_create; 167 | imu->acc->read = mgos_imu_lsm6dsl_acc_read; 168 | imu->acc->get_odr = mgos_imu_lsm6dsl_acc_get_odr; 169 | imu->acc->set_odr = mgos_imu_lsm6dsl_acc_set_odr; 170 | imu->acc->get_scale = mgos_imu_lsm6dsl_acc_get_scale; 171 | imu->acc->set_scale = mgos_imu_lsm6dsl_acc_set_scale; 172 | if (!imu->user_data) { 173 | imu->user_data = mgos_imu_lsm6dsl_userdata_create(); 174 | } 175 | break; 176 | 177 | case ACC_LSM9DS1: 178 | imu->acc->detect = mgos_imu_lsm9ds1_acc_detect; 179 | imu->acc->create = mgos_imu_lsm9ds1_acc_create; 180 | imu->acc->read = mgos_imu_lsm9ds1_acc_read; 181 | if (!imu->user_data) { 182 | imu->user_data = mgos_imu_lsm9ds1_userdata_create(); 183 | } 184 | break; 185 | 186 | case ACC_MMA8451: 187 | imu->acc->detect = mgos_imu_mma8451_detect; 188 | imu->acc->create = mgos_imu_mma8451_create; 189 | imu->acc->read = mgos_imu_mma8451_read; 190 | break; 191 | 192 | case ACC_LSM303DLM: 193 | case ACC_LSM303D: 194 | imu->acc->detect = mgos_imu_lsm303d_acc_detect; 195 | imu->acc->create = mgos_imu_lsm303d_acc_create; 196 | imu->acc->read = mgos_imu_lsm303d_acc_read; 197 | if (!imu->user_data) { 198 | imu->user_data = mgos_imu_lsm303d_userdata_create(); 199 | } 200 | break; 201 | 202 | case ACC_MPU9250: 203 | case ACC_MPU9255: 204 | imu->acc->detect = mgos_imu_mpu925x_acc_detect; 205 | imu->acc->create = mgos_imu_mpu925x_acc_create; 206 | imu->acc->read = mgos_imu_mpu925x_acc_read; 207 | imu->acc->get_scale = mgos_imu_mpu925x_acc_get_scale; 208 | imu->acc->set_scale = mgos_imu_mpu925x_acc_set_scale; 209 | if (!imu->user_data) { 210 | imu->user_data = mgos_imu_mpu925x_userdata_create(); 211 | } 212 | break; 213 | 214 | case ACC_ADXL345: 215 | imu->acc->detect = mgos_imu_adxl345_detect; 216 | imu->acc->create = mgos_imu_adxl345_create; 217 | imu->acc->read = mgos_imu_adxl345_read; 218 | break; 219 | 220 | case ACC_ICM20948: 221 | imu->acc->detect = mgos_imu_icm20948_acc_detect; 222 | imu->acc->create = mgos_imu_icm20948_acc_create; 223 | imu->acc->read = mgos_imu_icm20948_acc_read; 224 | imu->acc->get_odr = mgos_imu_icm20948_acc_get_odr; 225 | imu->acc->set_odr = mgos_imu_icm20948_acc_set_odr; 226 | imu->acc->get_scale = mgos_imu_icm20948_acc_get_scale; 227 | imu->acc->set_scale = mgos_imu_icm20948_acc_set_scale; 228 | if (!imu->user_data) { 229 | imu->user_data = mgos_imu_icm20948_userdata_create(); 230 | } 231 | break; 232 | 233 | default: 234 | LOG(LL_ERROR, ("Unknown accelerometer type %d", opts->type)); 235 | mgos_imu_accelerometer_destroy(imu); 236 | return false; 237 | } 238 | if (imu->acc->detect) { 239 | if (!imu->acc->detect(imu->acc, imu->user_data)) { 240 | LOG(LL_ERROR, ("Could not detect accelerometer type %d (%s) at I2C 0x%02x", 241 | opts->type, mgos_imu_accelerometer_get_name(imu), i2caddr)); 242 | mgos_imu_accelerometer_destroy(imu); 243 | return false; 244 | } else { 245 | LOG(LL_DEBUG, ("Successfully detected accelerometer type %d (%s) at I2C 0x%02x", 246 | opts->type, mgos_imu_accelerometer_get_name(imu), i2caddr)); 247 | } 248 | } 249 | 250 | if (imu->acc->create) { 251 | if (!imu->acc->create(imu->acc, imu->user_data)) { 252 | LOG(LL_ERROR, ("Could not create accelerometer type %d (%s) at I2C 0x%02x", 253 | opts->type, mgos_imu_accelerometer_get_name(imu), i2caddr)); 254 | mgos_imu_accelerometer_destroy(imu); 255 | return false; 256 | } else { 257 | LOG(LL_DEBUG, ("Successfully created accelerometer type %d (%s) at I2C 0x%02x", 258 | opts->type, mgos_imu_accelerometer_get_name(imu), i2caddr)); 259 | } 260 | } 261 | if (imu->acc->set_scale) { 262 | imu->acc->set_scale(imu->acc, imu->user_data, opts->scale); 263 | } 264 | 265 | if (imu->acc->set_odr) { 266 | imu->acc->set_odr(imu->acc, imu->user_data, opts->odr); 267 | } 268 | 269 | return true; 270 | } 271 | 272 | bool mgos_imu_accelerometer_set_offset(struct mgos_imu *imu, float x, float y, float z) { 273 | if (!imu || !imu->acc) { 274 | return false; 275 | } 276 | imu->acc->offset_ax = x; 277 | imu->acc->offset_ay = y; 278 | imu->acc->offset_az = z; 279 | return true; 280 | } 281 | 282 | bool mgos_imu_accelerometer_get_offset(struct mgos_imu *imu, float *x, float *y, float *z) { 283 | if (!imu || !imu->acc) { 284 | return false; 285 | } 286 | if (x) { 287 | *x = imu->acc->offset_ax; 288 | } 289 | if (y) { 290 | *y = imu->acc->offset_ay; 291 | } 292 | if (z) { 293 | *z = imu->acc->offset_az; 294 | } 295 | return true; 296 | } 297 | 298 | bool mgos_imu_accelerometer_get_scale(struct mgos_imu *imu, float *scale) { 299 | if (!imu || !imu->acc || !imu->acc->get_scale || !scale) { 300 | return false; 301 | } 302 | return imu->acc->get_scale(imu->acc, imu->user_data, scale); 303 | } 304 | 305 | bool mgos_imu_accelerometer_set_scale(struct mgos_imu *imu, float scale) { 306 | if (!imu || !imu->acc || !imu->acc->set_scale) { 307 | return false; 308 | } 309 | return imu->acc->set_scale(imu->acc, imu->user_data, scale); 310 | } 311 | 312 | bool mgos_imu_accelerometer_get_odr(struct mgos_imu *imu, float *hertz) { 313 | if (!imu || !imu->acc || !imu->acc->get_odr || !hertz) { 314 | return false; 315 | } 316 | return imu->acc->get_odr(imu->acc, imu->user_data, hertz); 317 | } 318 | 319 | bool mgos_imu_accelerometer_set_odr(struct mgos_imu *imu, float hertz) { 320 | if (!imu || !imu->acc || !imu->acc->set_odr) { 321 | return false; 322 | } 323 | return imu->acc->set_odr(imu->acc, imu->user_data, hertz); 324 | } 325 | -------------------------------------------------------------------------------- /src/mgos_imu_gyroscope.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Google Inc. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "mgos.h" 18 | #include "mgos_imu_internal.h" 19 | #include "mgos_imu_mpu925x.h" 20 | #include "mgos_imu_l3gd20.h" 21 | #include "mgos_imu_itg3205.h" 22 | #include "mgos_imu_lsm9ds1.h" 23 | #include "mgos_imu_lsm6dsl.h" 24 | #include "mgos_imu_mpu60x0.h" 25 | #include "mgos_imu_mpu6886.h" 26 | #include "mgos_imu_icm20948.h" 27 | 28 | static struct mgos_imu_gyro *mgos_imu_gyro_create(void) { 29 | struct mgos_imu_gyro *gyro; 30 | 31 | gyro = calloc(1, sizeof(struct mgos_imu_gyro)); 32 | if (!gyro) { 33 | return NULL; 34 | } 35 | memset(gyro, 0, sizeof(struct mgos_imu_gyro)); 36 | 37 | gyro->opts.type = GYRO_NONE; 38 | gyro->orientation[0] = 1.f; 39 | gyro->orientation[1] = 0.f; 40 | gyro->orientation[2] = 0.f; 41 | gyro->orientation[3] = 0.f; 42 | gyro->orientation[4] = 1.f; 43 | gyro->orientation[5] = 0.f; 44 | gyro->orientation[6] = 0.f; 45 | gyro->orientation[7] = 0.f; 46 | gyro->orientation[8] = 1.f; 47 | return gyro; 48 | } 49 | 50 | static bool mgos_imu_gyro_destroy(struct mgos_imu_gyro **gyro, void *imu_user_data) { 51 | if (!*gyro) { 52 | return false; 53 | } 54 | if ((*gyro)->destroy) { 55 | (*gyro)->destroy(*gyro, imu_user_data); 56 | } 57 | if ((*gyro)->user_data) { 58 | free((*gyro)->user_data); 59 | } 60 | free(*gyro); 61 | *gyro = NULL; 62 | return true; 63 | } 64 | 65 | bool mgos_imu_gyroscope_destroy(struct mgos_imu *imu) { 66 | bool ret; 67 | 68 | if (!imu || !imu->gyro) { 69 | return false; 70 | } 71 | ret = mgos_imu_gyro_destroy(&(imu->gyro), imu->user_data); 72 | imu->gyro = NULL; 73 | return ret; 74 | } 75 | 76 | const char *mgos_imu_gyroscope_get_name(struct mgos_imu *imu) { 77 | if (!imu || !imu->gyro) { 78 | return "VOID"; 79 | } 80 | 81 | switch (imu->gyro->opts.type) { 82 | case GYRO_NONE: return "NONE"; 83 | 84 | case GYRO_MPU9250: return "MPU9250"; 85 | 86 | case GYRO_MPU9255: return "MPU9255"; 87 | 88 | case GYRO_L3GD20: return "L3GD20"; 89 | 90 | case GYRO_L3GD20H: return "L3GD20H"; 91 | 92 | case GYRO_ITG3205: return "ITG3205"; 93 | 94 | case GYRO_LSM9DS1: return "LSM9DS1"; 95 | 96 | case GYRO_LSM6DSL: return "LSM6DSL"; 97 | 98 | case GYRO_MPU6000: return "MPU6000"; 99 | 100 | case GYRO_MPU6050: return "MPU6050"; 101 | 102 | case GYRO_MPU6886: return "MPU6886"; 103 | 104 | case GYRO_ICM20948: return "ICM20948"; 105 | 106 | default: return "UNKNOWN"; 107 | } 108 | } 109 | 110 | bool mgos_imu_gyroscope_get(struct mgos_imu *imu, float *x, float *y, float *z) { 111 | if (!imu->gyro || !imu->gyro->read) { 112 | return false; 113 | } 114 | 115 | if (!imu->gyro->read(imu->gyro, imu->user_data)) { 116 | LOG(LL_ERROR, ("Could not read from gyroscope")); 117 | return false; 118 | } 119 | // LOG(LL_DEBUG, ("Raw: gx=%d gy=%d gz=%d", imu->gyro->gx, imu->gyro->gy, imu->gyro->gz)); 120 | if (x) { 121 | *x = (imu->gyro->scale * 122 | (imu->gyro->gx * imu->gyro->orientation[0] + imu->gyro->gy * imu->gyro->orientation[1] + imu->gyro->gz * imu->gyro->orientation[2]) 123 | ) + imu->gyro->offset_gx; 124 | } 125 | if (y) { 126 | *y = (imu->gyro->scale * 127 | (imu->gyro->gx * imu->gyro->orientation[3] + imu->gyro->gy * imu->gyro->orientation[4] + imu->gyro->gz * imu->gyro->orientation[5]) 128 | ) + imu->gyro->offset_gy; 129 | } 130 | if (z) { 131 | *z = (imu->gyro->scale * 132 | (imu->gyro->gx * imu->gyro->orientation[6] + imu->gyro->gy * imu->gyro->orientation[7] + imu->gyro->gz * imu->gyro->orientation[8]) 133 | ) + imu->gyro->offset_gz; 134 | } 135 | return true; 136 | } 137 | 138 | bool mgos_imu_gyroscope_create_i2c(struct mgos_imu *imu, struct mgos_i2c *i2c, uint8_t i2caddr, const struct mgos_imu_gyro_opts *opts) { 139 | if (!imu || !i2c || !opts) { 140 | return false; 141 | } 142 | if (imu->gyro) { 143 | mgos_imu_gyroscope_destroy(imu); 144 | } 145 | imu->gyro = mgos_imu_gyro_create(); 146 | if (!imu->gyro) { 147 | false; 148 | } 149 | imu->gyro->i2c = i2c; 150 | imu->gyro->i2caddr = i2caddr; 151 | imu->gyro->opts = *opts; 152 | switch (opts->type) { 153 | case GYRO_MPU6000: 154 | case GYRO_MPU6050: 155 | imu->gyro->detect = mgos_imu_mpu60x0_gyro_detect; 156 | imu->gyro->create = mgos_imu_mpu60x0_gyro_create; 157 | imu->gyro->read = mgos_imu_mpu60x0_gyro_read; 158 | imu->gyro->get_scale = mgos_imu_mpu60x0_gyro_get_scale; 159 | imu->gyro->set_scale = mgos_imu_mpu60x0_gyro_set_scale; 160 | if (!imu->user_data) { 161 | imu->user_data = mgos_imu_mpu60x0_userdata_create(); 162 | } 163 | break; 164 | 165 | case GYRO_MPU6886: 166 | imu->gyro->detect = mgos_imu_mpu6886_gyro_detect; 167 | imu->gyro->create = mgos_imu_mpu60x0_gyro_create; 168 | imu->gyro->read = mgos_imu_mpu60x0_gyro_read; 169 | imu->gyro->get_scale = mgos_imu_mpu60x0_gyro_get_scale; 170 | imu->gyro->set_scale = mgos_imu_mpu60x0_gyro_set_scale; 171 | if (!imu->user_data) { 172 | imu->user_data = mgos_imu_mpu60x0_userdata_create(); 173 | } 174 | break; 175 | 176 | case GYRO_LSM6DSL: 177 | imu->gyro->detect = mgos_imu_lsm6dsl_gyro_detect; 178 | imu->gyro->create = mgos_imu_lsm6dsl_gyro_create; 179 | imu->gyro->read = mgos_imu_lsm6dsl_gyro_read; 180 | if (!imu->user_data) { 181 | imu->user_data = mgos_imu_lsm6dsl_userdata_create(); 182 | } 183 | break; 184 | 185 | case GYRO_LSM9DS1: 186 | imu->gyro->detect = mgos_imu_lsm9ds1_gyro_detect; 187 | imu->gyro->create = mgos_imu_lsm9ds1_gyro_create; 188 | imu->gyro->read = mgos_imu_lsm9ds1_gyro_read; 189 | if (!imu->user_data) { 190 | imu->user_data = mgos_imu_lsm9ds1_userdata_create(); 191 | } 192 | break; 193 | 194 | case GYRO_ITG3205: 195 | imu->gyro->detect = mgos_imu_itg3205_detect; 196 | imu->gyro->create = mgos_imu_itg3205_create; 197 | imu->gyro->read = mgos_imu_itg3205_read; 198 | break; 199 | 200 | case GYRO_L3GD20: 201 | case GYRO_L3GD20H: 202 | imu->gyro->detect = mgos_imu_l3gd20_detect; 203 | imu->gyro->create = mgos_imu_l3gd20_create; 204 | imu->gyro->read = mgos_imu_l3gd20_read; 205 | break; 206 | 207 | case GYRO_MPU9250: 208 | case GYRO_MPU9255: 209 | imu->gyro->detect = mgos_imu_mpu925x_gyro_detect; 210 | imu->gyro->create = mgos_imu_mpu925x_gyro_create; 211 | imu->gyro->read = mgos_imu_mpu925x_gyro_read; 212 | imu->gyro->get_scale = mgos_imu_mpu925x_gyro_get_scale; 213 | imu->gyro->set_scale = mgos_imu_mpu925x_gyro_set_scale; 214 | if (!imu->user_data) { 215 | imu->user_data = mgos_imu_mpu925x_userdata_create(); 216 | } 217 | break; 218 | 219 | case GYRO_ICM20948: 220 | imu->gyro->detect = mgos_imu_icm20948_gyro_detect; 221 | imu->gyro->create = mgos_imu_icm20948_gyro_create; 222 | imu->gyro->read = mgos_imu_icm20948_gyro_read; 223 | imu->gyro->get_odr = mgos_imu_icm20948_gyro_get_odr; 224 | imu->gyro->set_odr = mgos_imu_icm20948_gyro_set_odr; 225 | imu->gyro->get_scale = mgos_imu_icm20948_gyro_get_scale; 226 | imu->gyro->set_scale = mgos_imu_icm20948_gyro_set_scale; 227 | if (!imu->user_data) { 228 | imu->user_data = mgos_imu_icm20948_userdata_create(); 229 | } 230 | break; 231 | 232 | default: 233 | LOG(LL_ERROR, ("Unknown gyroscope type %d", opts->type)); 234 | mgos_imu_gyroscope_destroy(imu); 235 | return false; 236 | } 237 | if (imu->gyro->detect) { 238 | if (!imu->gyro->detect(imu->gyro, imu->user_data)) { 239 | LOG(LL_ERROR, ("Could not detect gyroscope type %d (%s) at I2C 0x%02x", 240 | opts->type, mgos_imu_gyroscope_get_name(imu), i2caddr)); 241 | mgos_imu_gyroscope_destroy(imu); 242 | return false; 243 | } else { 244 | LOG(LL_DEBUG, ("Successfully detected gyroscope type %d (%s) at I2C 0x%02x", 245 | opts->type, mgos_imu_gyroscope_get_name(imu), i2caddr)); 246 | } 247 | } 248 | 249 | if (imu->gyro->create) { 250 | if (!imu->gyro->create(imu->gyro, imu->user_data)) { 251 | LOG(LL_ERROR, ("Could not create gyroscope type %d (%s) at I2C 0x%02x", 252 | opts->type, mgos_imu_gyroscope_get_name(imu), i2caddr)); 253 | mgos_imu_gyroscope_destroy(imu); 254 | return false; 255 | } else { 256 | LOG(LL_DEBUG, ("Successfully created gyroscope type %d (%s) at I2C 0x%02x", 257 | opts->type, mgos_imu_gyroscope_get_name(imu), i2caddr)); 258 | } 259 | } 260 | if (imu->gyro->set_scale) { 261 | imu->gyro->set_scale(imu->gyro, imu->user_data, opts->scale); 262 | } 263 | 264 | if (imu->gyro->set_odr) { 265 | imu->gyro->set_odr(imu->gyro, imu->user_data, opts->odr); 266 | } 267 | 268 | 269 | return true; 270 | } 271 | 272 | bool mgos_imu_gyroscope_set_offset(struct mgos_imu *imu, float x, float y, float z) { 273 | if (!imu || !imu->gyro) { 274 | return false; 275 | } 276 | imu->gyro->offset_gx = x; 277 | imu->gyro->offset_gy = y; 278 | imu->gyro->offset_gz = z; 279 | return true; 280 | } 281 | 282 | bool mgos_imu_gyroscope_get_offset(struct mgos_imu *imu, float *x, float *y, float *z) { 283 | if (!imu || !imu->gyro) { 284 | return false; 285 | } 286 | if (x) { 287 | *x = imu->gyro->offset_gx; 288 | } 289 | if (y) { 290 | *y = imu->gyro->offset_gy; 291 | } 292 | if (z) { 293 | *z = imu->gyro->offset_gz; 294 | } 295 | return true; 296 | } 297 | 298 | bool mgos_imu_gyroscope_get_orientation(struct mgos_imu *imu, float v[9]) { 299 | if (!imu || !imu->gyro || !v) { 300 | return false; 301 | } 302 | memcpy(v, imu->gyro->orientation, sizeof(float) * 9); 303 | return true; 304 | } 305 | 306 | bool mgos_imu_gyroscope_set_orientation(struct mgos_imu *imu, float v[9]) { 307 | if (!imu || !imu->gyro || !v) { 308 | return false; 309 | } 310 | memcpy(imu->gyro->orientation, v, sizeof(float) * 9); 311 | return true; 312 | } 313 | 314 | bool mgos_imu_gyroscope_get_scale(struct mgos_imu *imu, float *scale) { 315 | if (!imu || !imu->gyro || !imu->gyro->get_scale || !scale) { 316 | return false; 317 | } 318 | return imu->gyro->get_scale(imu->gyro, imu->user_data, scale); 319 | } 320 | 321 | bool mgos_imu_gyroscope_set_scale(struct mgos_imu *imu, float scale) { 322 | if (!imu || !imu->gyro || !imu->gyro->set_scale) { 323 | return false; 324 | } 325 | return imu->gyro->set_scale(imu->gyro, imu->user_data, scale); 326 | } 327 | 328 | bool mgos_imu_gyroscope_get_odr(struct mgos_imu *imu, float *hertz) { 329 | if (!imu || !imu->gyro || !imu->gyro->get_odr || !hertz) { 330 | return false; 331 | } 332 | return imu->gyro->get_odr(imu->gyro, imu->user_data, hertz); 333 | } 334 | 335 | bool mgos_imu_gyroscope_set_odr(struct mgos_imu *imu, float hertz) { 336 | if (!imu || !imu->gyro || !imu->gyro->set_odr) { 337 | return false; 338 | } 339 | return imu->gyro->set_odr(imu->gyro, imu->user_data, hertz); 340 | } 341 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mongoose OS IMU Library 2 | 3 | This library provides a simple API that describes inertial measurement units. 4 | It implements various popular I2C and SPI IMUs behind that API. Callers of 5 | the library can rely on the API returning consistently typed data regardless 6 | of the choice of sensor. 7 | 8 | # Anatomy 9 | 10 | This library provides an API to expose various implementations of gyroscopes, 11 | accelerometers, and magnetometers. It offers a suitable abstraction that 12 | presents the sensor data in a consistent way, so application authors need 13 | not worry about the implementation details of the sensors. 14 | 15 | Users create a `struct mgos_imu` object, to which they add sensors (gyroscope, 16 | acellerometer and magnetometer) either via I2C or SPI. Some chips have all 17 | three sensor types, some have only two (typically accelerometer and gyroscope) 18 | and some have only one type. After successfully adding the sensors to the 19 | `mgos_imu` object, reads can be performed, mostly by performing 20 | `mgos_imu_*_get()` calls. 21 | 22 | All implementations offer the calls described below in the `IMU API`. 23 | Some implementations offer chip-specific addendums, mostly setting sensitivity 24 | and range options. 25 | 26 | ## IMU API 27 | 28 | ### IMU primitives 29 | 30 | `struct mgos_imu *mgos_imu_create()` -- This call creates a new (opaque) object 31 | which represents the IMU device. Upon success, a pointer to the object will be 32 | returned. If the creation fails, NULL is returned. 33 | 34 | `void mgos_imu_destroy()` -- This cleans up all resources associated with with 35 | the IMU device. The caller passes a pointer to the object pointer. If any 36 | sensors are attached (gyroscope, accelerometer, magnetometer), they will be 37 | destroyed in turn using `mgos_imu_*_destroy()` calls. 38 | 39 | `bool mgos_imu_read()` -- This call schedules all sensors attached to the IMU 40 | to be read from. It is not generally necessary to call this method directly, 41 | as the `mgos_imu_*_get()` calls internally schedule reads from the sensors as 42 | well. 43 | 44 | `bool mgos_imu_accelerometer_present()` -- This returns `true` if the IMU has an 45 | attached accelerometer sensor, or `false` otherwise. 46 | 47 | `bool mgos_imu_gyroscope_present()` -- This returns `true` if the IMU has an 48 | attached gyroscope sensor, or `false` otherwise. 49 | 50 | `bool mgos_imu_magnetometer_present()` -- This returns `true` if the IMU has an 51 | attached magnetometer, or `false` otherwise. 52 | 53 | ### IMU Sensor primitives 54 | 55 | For each of ***accelerometer***, ***gyroscope*** and ***magnetometer***, the 56 | following primitives exist: 57 | 58 | `bool mgos_imu_*_create_i2c()` -- This attaches a sensor to the IMU 59 | based on the `type` enum given, using the `i2c` bus and specified `i2caddr` 60 | address. The function will return `true` upon success and `false` in case 61 | either detection of the sensor, or creation of it failed. 62 | 63 | `bool mgos_imu_*_create_spi()` -- This attaches a sensor to the IMU 64 | based on the `type` enum given, using the `spi` bus and a specified cable select 65 | in `cs_gpio`. The function will return `true` upon success and `false` in case 66 | either detection of the sensor or creation of it failed. 67 | 68 | `bool mgos_imu_*_destroy()` -- This detaches a sensor from the IMU if it exists. 69 | It takes care of cleaning up all resources associated with the sensor, and 70 | detaches it from the `i2c` or `spi` bus. The higher level `mgos_imu_destroy()` 71 | call uses these lower level calls to clean up sensors as well. 72 | 73 | `bool mgos_imu_*_get()` -- This call returns sensor data after 74 | polling the sensor for the data. It returns `true` if the read succeeded, in 75 | which case the floats pointed to by `x`, `y` and `z` are filled in. If an 76 | error occurred, `false` is returned and the contents of `x`, `y` and `z` are 77 | unmodified. Note the units of the return values: 78 | 79 | * ***magnetometer*** returns units of `Gauss`. 80 | * ***accelerometer*** returns units of `G`. 81 | * ***gyroscope*** returns units of `degrees per second`. 82 | 83 | `const char *mgos_imu_*_get_name()` -- This returns a symbolic name of the 84 | attached sensor, which is guaranteed to be less than or equal to 10 characters 85 | and always exist. If there is no sensor of this type attached, `VOID` will be 86 | returned. If the sensor is not known, `UNKNOWN` will be returned. Otherwise, 87 | the chip manufacturer / type will be returned, for example `MPU9250` or 88 | `ADXL345` or `MAG3110`. 89 | 90 | `bool mgos_imu_*_set_orientation()` and `bool mgos_imu_*_get_orientation()` -- 91 | Often times a 9DOF sensor will have multiple chips, whose axes do not line up 92 | correctly. Even within a single chip the accelerometer, gyroscope and 93 | magnetometer axes might not line up (for an example of this, see [MPU9250 94 | chapter 9.1](http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf)). 95 | To ensure that the `x`, `y`, and `z` axes on all sensors are pointed in the same 96 | direction, we can set the orientation on the gyroscope and magnetometer. 97 | See `mgos_imu.h` for more details and an example of how to do this. 98 | 99 | ## Supported devices 100 | 101 | ### Accelerometer 102 | 103 | * MPU9250 and MPU9255 104 | * ADXL345 105 | * LSM303D and LSM303DLM 106 | * MMA8451 107 | * LSM9DS1 108 | * LSM6DSL 109 | * MPU6000 and MPU6050 110 | * MPU6886 ("just works") 111 | * ICM20948 112 | 113 | ### Gyroscope 114 | 115 | * MPU9250 and MPU9255 116 | * L3GD20 and L3GD20H 117 | * ITG3205 118 | * LSM9DS1 119 | * LSM6DSL 120 | * MPU6000 and MPU6050 121 | * MPU6886 (work in progress) 122 | * ICM20948 123 | 124 | ### Magnetometer 125 | 126 | * MAG3110 127 | * AK8963 (as found in MPU9250/MPU9255) 128 | * AK8975 129 | * BMM150 130 | * LSM303D and LSM303DLM 131 | * HMC5883L 132 | * LSM9DS1 133 | * ICM20948 134 | 135 | ## Adding devices 136 | 137 | This is a fairly straight forward process, taking the `ADXL345` accelerometer 138 | as example: 139 | 140 | 1. Add a new driver header and C file, say `src/mgos_imu_adxl345.[ch]`. The 141 | header file exposes all of the `#define`'s for registers and constants. 142 | 1. In the header file, declare a `detect`, `create`, `destroy` and `read` 143 | function. 144 | * `bool mgos_imu_adxl345_detect()` -- this function optionally attempts 145 | to detect the chip, often times by reading a register or set of 146 | registers which uniquely identifies it. Not all chips can actually be 147 | detected, so it's OK to not define this function at all. 148 | * `bool mgos_imu_adxl345_create()` -- this function has to perform the 149 | initialization of the chip, typically by setting the right registers 150 | and possibly creating a driver-specific memory structure (for, say, 151 | coefficients or some such). If used, that memory structure is attached 152 | to the `user_data` pointer, and if so, the implementation of the 153 | `_destroy()` function must clean up and free this memory again. 154 | * `bool mgos_imu_adxl345_read()` -- this function performs the chip 155 | specific read functionality. This will be called whenever the user asks 156 | for data, either by calling `mgos_imu_read()` or by calling 157 | `mgos_imu_*_get()`. 158 | * `bool mgos_imu_adxl345_destroy()` -- this function deinitializes the 159 | chip, and optionally clears and frees the driver-specific memory 160 | structure in `user_data`. Not all chips need additional memory 161 | structures or deinitialization code, in which case it's OK to not 162 | define this function at all. 163 | 1. Implement the `detect`, `create`, `destroy` and `read` functions in the 164 | source code file `src/mgos_imu_adxl345.c`. 165 | 1. Add the device to one of the `enum mgos_imu_*_type` in `include/mgos_imu.h`. 166 | In the example case of adxl345 (which is an accelerometer), add it to 167 | `enum mgos_imu_acc_type`. 168 | 1. Add a string version of this to function `mgos_imu_*_get_name()` so that 169 | callers can resolve the sensor to a human readable format. Make sure that 170 | the string name is not greater than 10 characters. 171 | 1. Add the type to the `switch()` in `mgos_imu_*_create_i2c()` (or `_spi()`) 172 | functions. 173 | 1. Update this document to add the driver to the list of supported drivers. 174 | 1. Test code on a working sample, and send a PR using the guidelines laid 175 | out in [contributing](CONTRIBUTING.md). 176 | 177 | It is important to note a few implementation rules when adding drivers: 178 | 179 | * New drivers MUST NOT change any semantics of the abstraction layer 180 | (`mgos_imu` and `mgos_imu_*` members) nor the `mgos_imu_*_get()` functions. 181 | * Implementations of drivers MUST provide bias, scaling and other 182 | normalization in the driver itself. What this means, in practice, is that 183 | the correct units must be produced (`m/s/s`, `deg/s` and `uT`). 184 | * Pull Requests MUST NOT mix driver and abstraction changes. Separate them. 185 | * Changes to the abstraction layer MUST be proven to work on all existing 186 | drivers, and will generally be scrutinized. 187 | 188 | ### Example driver (AK8975) 189 | 190 | Here's an example, for the magnetometer chip `AK8975`, showing a set of commits 191 | for each of the steps above (and honoring the driver implementation rules). 192 | 193 | 1. Add `src/mgos_imu_ak8975.[ch]` [commit](https://github.com/mongoose-os-libs/imu/commit/64d29e32f7633ec22c5296c27c3faf6df75f929d) 194 | 1. Extend `enum mgos_imu_mag_type` in `include/mgos_imu.h` [commit](https://github.com/mongoose-os-libs/imu/commit/67b121c9f0dd511e3f3b5279c310c2e135895d02) 195 | 1. Add to `mgos_imu_magnetometer_get_name()` in `src/mgos_imu_magnetometer.c` [commit](https://github.com/mongoose-os-libs/imu/commit/286d53a199df124e793e9e428657fcd28ea7b3c3) 196 | 1. Add to `mgos_imu_magnetometer_create_i2c()` in `src/mgos_imu_magnetometer.c` [commit](https://github.com/mongoose-os-libs/imu/commit/8e33a0fbd805b5f840761266c03163deeb1cc3f3) 197 | 198 | # Example Code 199 | 200 | An example program that creates an IMU of type `MPU9250` (which has an 201 | accelerometer, a gyroscope and a magnetometer all in one tiny package): 202 | 203 | ``` 204 | #include "mgos.h" 205 | #include "mgos_i2c.h" 206 | #include "mgos_imu.h" 207 | 208 | static void imu_cb(void *user_data) { 209 | struct mgos_imu *imu = (struct mgos_imu *)user_data; 210 | float ax, ay, az; 211 | float gx, gy, gz; 212 | float mx, my, mz; 213 | 214 | if (!imu) return; 215 | 216 | if (mgos_imu_accelerometer_get(imu, &ax, &ay, &az)) 217 | LOG(LL_INFO, ("type=%-10s Accel X=%.2f Y=%.2f Z=%.2f", 218 | mgos_imu_accelerometer_get_name(imu), ax, ay, az)); 219 | if (mgos_imu_gyroscope_get(imu, &gx, &gy, &gz)) 220 | LOG(LL_INFO, ("type=%-10s Gyro X=%.2f Y=%.2f Z=%.2f", 221 | mgos_imu_gyroscope_get_name(imu), gx, gy, gz)); 222 | if (mgos_imu_magnetometer_get(imu, &mx, &my, &mz)) 223 | LOG(LL_INFO, ("type=%-10s Mag X=%.2f Y=%.2f Z=%.2f", 224 | mgos_imu_magnetometer_get_name(imu), mx, my, mz)); 225 | } 226 | 227 | enum mgos_app_init_result mgos_app_init(void) { 228 | struct mgos_i2c *i2c = mgos_i2c_get_global(); 229 | struct mgos_imu *imu = mgos_imu_create(); 230 | struct mgos_imu_acc_opts acc_opts; 231 | struct mgos_imu_gyro_opts gyro_opts; 232 | struct mgos_imu_mag_opts mag_opts; 233 | 234 | if (!i2c) { 235 | LOG(LL_ERROR, ("I2C bus missing, set i2c.enable=true in mos.yml")); 236 | return false; 237 | } 238 | 239 | if (!imu) { 240 | LOG(LL_ERROR, ("Cannot create IMU")); 241 | return false; 242 | } 243 | 244 | acc_opts.type = ACC_MPU9250; 245 | acc_opts.scale = 16.0; // G 246 | acc_opts.odr = 100; // Hz 247 | if (!mgos_imu_accelerometer_create_i2c(imu, i2c, 0x68, &acc_opts)) 248 | LOG(LL_ERROR, ("Cannot create accelerometer on IMU")); 249 | 250 | gyro_opts.type = GYRO_MPU9250; 251 | gyro_opts.scale = 2000; // deg/sec 252 | gyro_opts.odr = 100; // Hz 253 | if (!mgos_imu_gyroscope_create_i2c(imu, i2c, 0x68, &gyro_opts)) 254 | LOG(LL_ERROR, ("Cannot create gyroscope on IMU")); 255 | 256 | mag_opts.type = MAG_AK8963; 257 | mag_opts.scale = 12.0; // Gauss 258 | mag_opts.odr = 10; // Hz 259 | if (!mgos_imu_magnetometer_create_i2c(imu, i2c, 0x0C, &mag_opts)) 260 | LOG(LL_ERROR, ("Cannot create magnetometer on IMU")); 261 | 262 | mgos_set_timer(1000, true, imu_cb, imu); 263 | return MGOS_APP_INIT_SUCCESS; 264 | } 265 | ``` 266 | 267 | # Demo Code 268 | 269 | For a cool demo, take a look at my [Demo Apps](https://github.com/mongoose-os-apps/imu-demo): 270 | 271 | ![Chrome Demo](docs/chrome-client.png) 272 | 273 | # Disclaimer 274 | 275 | This project is not an official Google project. It is not supported by Google 276 | and Google specifically disclaims all warranties as to its quality, 277 | merchantability, or fitness for a particular purpose. 278 | -------------------------------------------------------------------------------- /src/madgwick.c: -------------------------------------------------------------------------------- 1 | //============================================================================================= 2 | // madgwick.c 3 | //============================================================================================= 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 7 | // 8 | // From the x-io website "Open-source resources available on this website are 9 | // provided under the GNU General Public Licence unless an alternative licence 10 | // is provided in source." 11 | // 12 | // Date Author Notes 13 | // 29/09/2011 SOH Madgwick Initial release 14 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 15 | // 12/11/2018 Pim van Pelt Rewritten to C for Mongoose OS 16 | // 17 | //============================================================================================= 18 | #include "madgwick.h" 19 | 20 | //------------------------------------------------------------------------------------------- 21 | // Fast inverse square-root 22 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 23 | 24 | static float invSqrt(float x) { 25 | union { 26 | float f; 27 | uint32_t i; 28 | } conv; 29 | 30 | float x2; 31 | const float threehalfs = 1.5F; 32 | 33 | x2 = x * 0.5F; 34 | conv.f = x; 35 | conv.i = 0x5f3759df - (conv.i >> 1); 36 | conv.f = conv.f * (threehalfs - (x2 * conv.f * conv.f)); 37 | return conv.f; 38 | } 39 | 40 | struct mgos_imu_madgwick *mgos_imu_madgwick_create(void) { 41 | struct mgos_imu_madgwick *filter; 42 | 43 | filter = calloc(1, sizeof(struct mgos_imu_madgwick)); 44 | if (!filter) { 45 | return NULL; 46 | } 47 | mgos_imu_madgwick_set_params(filter, 100.0f, 0.1f); 48 | mgos_imu_madgwick_reset(filter); 49 | return filter; 50 | } 51 | 52 | bool mgos_imu_madgwick_destroy(struct mgos_imu_madgwick **filter) { 53 | if (!*filter) { 54 | return false; 55 | } 56 | free(*filter); 57 | *filter = NULL; 58 | return true; 59 | } 60 | 61 | bool mgos_imu_madgwick_set_params(struct mgos_imu_madgwick *filter, float freq, float beta) { 62 | if (!filter) { 63 | return false; 64 | } 65 | filter->beta = beta; 66 | filter->freq = freq; 67 | filter->inv_freq = 1.0f / freq; 68 | return true; 69 | } 70 | 71 | bool mgos_imu_madgwick_reset(struct mgos_imu_madgwick *filter) { 72 | if (!filter) { 73 | return false; 74 | } 75 | filter->q0 = 1.0f; 76 | filter->q1 = 0.0f; 77 | filter->q2 = 0.0f; 78 | filter->q3 = 0.0f; 79 | filter->counter = 0; 80 | return true; 81 | } 82 | 83 | static bool mgos_imu_madgwick_updateIMU(struct mgos_imu_madgwick *filter, float gx, float gy, float gz, float ax, float ay, float az) { 84 | float recipNorm; 85 | float s0, s1, s2, s3; 86 | float qDot1, qDot2, qDot3, qDot4; 87 | float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; 88 | 89 | // No need to check filter pointer -- it's checked by _update() 90 | 91 | // Rate of change of quaternion from gyroscope 92 | qDot1 = 0.5f * (-filter->q1 * gx - filter->q2 * gy - filter->q3 * gz); 93 | qDot2 = 0.5f * (filter->q0 * gx + filter->q2 * gz - filter->q3 * gy); 94 | qDot3 = 0.5f * (filter->q0 * gy - filter->q1 * gz + filter->q3 * gx); 95 | qDot4 = 0.5f * (filter->q0 * gz + filter->q1 * gy - filter->q2 * gx); 96 | 97 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 98 | if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 99 | // Normalise accelerometer measurement 100 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 101 | ax *= recipNorm; 102 | ay *= recipNorm; 103 | az *= recipNorm; 104 | 105 | // Auxiliary variables to avoid repeated arithmetic 106 | _2q0 = 2.0f * filter->q0; 107 | _2q1 = 2.0f * filter->q1; 108 | _2q2 = 2.0f * filter->q2; 109 | _2q3 = 2.0f * filter->q3; 110 | _4q0 = 4.0f * filter->q0; 111 | _4q1 = 4.0f * filter->q1; 112 | _4q2 = 4.0f * filter->q2; 113 | _8q1 = 8.0f * filter->q1; 114 | _8q2 = 8.0f * filter->q2; 115 | q0q0 = filter->q0 * filter->q0; 116 | q1q1 = filter->q1 * filter->q1; 117 | q2q2 = filter->q2 * filter->q2; 118 | q3q3 = filter->q3 * filter->q3; 119 | 120 | // Gradient decent algorithm corrective step 121 | s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; 122 | s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * filter->q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; 123 | s2 = 4.0f * q0q0 * filter->q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; 124 | s3 = 4.0f * q1q1 * filter->q3 - _2q1 * ax + 4.0f * q2q2 * filter->q3 - _2q2 * ay; 125 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 126 | s0 *= recipNorm; 127 | s1 *= recipNorm; 128 | s2 *= recipNorm; 129 | s3 *= recipNorm; 130 | 131 | // Apply feedback step 132 | qDot1 -= filter->beta * s0; 133 | qDot2 -= filter->beta * s1; 134 | qDot3 -= filter->beta * s2; 135 | qDot4 -= filter->beta * s3; 136 | } 137 | 138 | // Integrate rate of change of quaternion to yield quaternion 139 | filter->q0 += qDot1 * filter->inv_freq; 140 | filter->q1 += qDot2 * filter->inv_freq; 141 | filter->q2 += qDot3 * filter->inv_freq; 142 | filter->q3 += qDot4 * filter->inv_freq; 143 | 144 | // Normalise quaternion 145 | recipNorm = invSqrt(filter->q0 * filter->q0 + filter->q1 * filter->q1 + filter->q2 * filter->q2 + filter->q3 * filter->q3); 146 | filter->q0 *= recipNorm; 147 | filter->q1 *= recipNorm; 148 | filter->q2 *= recipNorm; 149 | filter->q3 *= recipNorm; 150 | 151 | filter->counter++; 152 | return true; 153 | } 154 | 155 | bool mgos_imu_madgwick_update(struct mgos_imu_madgwick *filter, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { 156 | if (!filter) { 157 | return false; 158 | } 159 | float recipNorm; 160 | float s0, s1, s2, s3; 161 | float qDot1, qDot2, qDot3, qDot4; 162 | float hx, hy; 163 | float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; 164 | 165 | // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) 166 | if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { 167 | mgos_imu_madgwick_updateIMU(filter, gx, gy, gz, ax, ay, az); 168 | return false; 169 | } 170 | 171 | // Rate of change of quaternion from gyroscope 172 | qDot1 = 0.5f * (-filter->q1 * gx - filter->q2 * gy - filter->q3 * gz); 173 | qDot2 = 0.5f * (filter->q0 * gx + filter->q2 * gz - filter->q3 * gy); 174 | qDot3 = 0.5f * (filter->q0 * gy - filter->q1 * gz + filter->q3 * gx); 175 | qDot4 = 0.5f * (filter->q0 * gz + filter->q1 * gy - filter->q2 * gx); 176 | 177 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 178 | if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 179 | // Normalise accelerometer measurement 180 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 181 | ax *= recipNorm; 182 | ay *= recipNorm; 183 | az *= recipNorm; 184 | 185 | // Normalise magnetometer measurement 186 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); 187 | mx *= recipNorm; 188 | my *= recipNorm; 189 | mz *= recipNorm; 190 | 191 | // Auxiliary variables to avoid repeated arithmetic 192 | _2q0mx = 2.0f * filter->q0 * mx; 193 | _2q0my = 2.0f * filter->q0 * my; 194 | _2q0mz = 2.0f * filter->q0 * mz; 195 | _2q1mx = 2.0f * filter->q1 * mx; 196 | _2q0 = 2.0f * filter->q0; 197 | _2q1 = 2.0f * filter->q1; 198 | _2q2 = 2.0f * filter->q2; 199 | _2q3 = 2.0f * filter->q3; 200 | _2q0q2 = 2.0f * filter->q0 * filter->q2; 201 | _2q2q3 = 2.0f * filter->q2 * filter->q3; 202 | q0q0 = filter->q0 * filter->q0; 203 | q0q1 = filter->q0 * filter->q1; 204 | q0q2 = filter->q0 * filter->q2; 205 | q0q3 = filter->q0 * filter->q3; 206 | q1q1 = filter->q1 * filter->q1; 207 | q1q2 = filter->q1 * filter->q2; 208 | q1q3 = filter->q1 * filter->q3; 209 | q2q2 = filter->q2 * filter->q2; 210 | q2q3 = filter->q2 * filter->q3; 211 | q3q3 = filter->q3 * filter->q3; 212 | 213 | // Reference direction of Earth's magnetic field 214 | hx = mx * q0q0 - _2q0my * filter->q3 + _2q0mz * filter->q2 + mx * q1q1 + _2q1 * my * filter->q2 + _2q1 * mz * filter->q3 - mx * q2q2 - mx * q3q3; 215 | hy = _2q0mx * filter->q3 + my * q0q0 - _2q0mz * filter->q1 + _2q1mx * filter->q2 - my * q1q1 + my * q2q2 + _2q2 * mz * filter->q3 - my * q3q3; 216 | _2bx = sqrtf(hx * hx + hy * hy); 217 | _2bz = -_2q0mx * filter->q2 + _2q0my * filter->q1 + mz * q0q0 + _2q1mx * filter->q3 - mz * q1q1 + _2q2 * my * filter->q3 - mz * q2q2 + mz * q3q3; 218 | _4bx = 2.0f * _2bx; 219 | _4bz = 2.0f * _2bz; 220 | 221 | // Gradient decent algorithm corrective step 222 | s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * filter->q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * filter->q3 + _2bz * filter->q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * filter->q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 223 | s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * filter->q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * filter->q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * filter->q2 + _2bz * filter->q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * filter->q3 - _4bz * filter->q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 224 | s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * filter->q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * filter->q2 - _2bz * filter->q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * filter->q1 + _2bz * filter->q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * filter->q0 - _4bz * filter->q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 225 | s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * filter->q3 + _2bz * filter->q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * filter->q0 + _2bz * filter->q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * filter->q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 226 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 227 | s0 *= recipNorm; 228 | s1 *= recipNorm; 229 | s2 *= recipNorm; 230 | s3 *= recipNorm; 231 | 232 | // Apply feedback step 233 | qDot1 -= filter->beta * s0; 234 | qDot2 -= filter->beta * s1; 235 | qDot3 -= filter->beta * s2; 236 | qDot4 -= filter->beta * s3; 237 | } 238 | 239 | // Integrate rate of change of quaternion to yield quaternion 240 | filter->q0 += qDot1 * filter->inv_freq; 241 | filter->q1 += qDot2 * filter->inv_freq; 242 | filter->q2 += qDot3 * filter->inv_freq; 243 | filter->q3 += qDot4 * filter->inv_freq; 244 | 245 | // Normalise quaternion 246 | recipNorm = invSqrt(filter->q0 * filter->q0 + filter->q1 * filter->q1 + filter->q2 * filter->q2 + filter->q3 * filter->q3); 247 | filter->q0 *= recipNorm; 248 | filter->q1 *= recipNorm; 249 | filter->q2 *= recipNorm; 250 | filter->q3 *= recipNorm; 251 | 252 | filter->counter++; 253 | return true; 254 | } 255 | 256 | bool mgos_imu_madgwick_get_quaternion(struct mgos_imu_madgwick *filter, float *q0, float *q1, float *q2, float *q3) { 257 | if (!filter) { 258 | return false; 259 | } 260 | if (q0) { 261 | *q0 = filter->q0; 262 | } 263 | if (q1) { 264 | *q1 = filter->q1; 265 | } 266 | if (q2) { 267 | *q2 = filter->q2; 268 | } 269 | if (q3) { 270 | *q3 = filter->q3; 271 | } 272 | return true; 273 | } 274 | 275 | bool mgos_imu_madgwick_get_angles(struct mgos_imu_madgwick *filter, float *roll, float *pitch, float *yaw) { 276 | if (!filter) { 277 | return false; 278 | } 279 | if (roll) { 280 | *roll = asinf(-2.0f * (filter->q1 * filter->q3 - filter->q0 * filter->q2)); 281 | } 282 | if (pitch) { 283 | *pitch = atan2f(filter->q0 * filter->q1 + filter->q2 * filter->q3, 0.5f - filter->q1 * filter->q1 - filter->q2 * filter->q2); 284 | } 285 | if (yaw) { 286 | *yaw = atan2f(filter->q1 * filter->q2 + filter->q0 * filter->q3, 0.5f - filter->q2 * filter->q2 - filter->q3 * filter->q3); 287 | } 288 | return true; 289 | } 290 | 291 | bool mgos_imu_madgwick_get_counter(struct mgos_imu_madgwick *filter, uint32_t *counter) { 292 | if (!filter || !counter) { 293 | return false; 294 | } 295 | *counter = filter->counter; 296 | return true; 297 | } 298 | --------------------------------------------------------------------------------