├── I2Cbus.cpp ├── I2Cbus.hpp ├── LICENSE ├── MPU9250.cpp ├── MPU9250.h ├── README.md ├── examples └── compass │ └── compass.ino ├── extras ├── AK8963C.pdf ├── Embedded-Masters-MPU-9250-Breakout.pdf ├── MPU-9250-AXIS.png ├── MPU-9250-Datasheet.pdf ├── MPU-9250-Register-Map.pdf ├── MPU-9255-Datasheet.pdf └── MPU-9255-Register-Map.pdf ├── keywords.txt └── library.properties /I2Cbus.cpp: -------------------------------------------------------------------------------- 1 | /* ========================================================================= 2 | I2Cbus library is placed under the MIT License 3 | Copyright 2017 Natanael Josue Rabello. All rights reserved. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to 7 | deal in the Software without restriction, including without limitation the 8 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | sell copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | IN THE SOFTWARE. 22 | ========================================================================= */ 23 | 24 | #include "I2Cbus.hpp" 25 | #include 26 | #include 27 | #include "sdkconfig.h" 28 | #include "esp_err.h" 29 | #include "esp_log.h" 30 | #include "driver/i2c.h" 31 | #include "driver/gpio.h" 32 | 33 | 34 | #if defined CONFIG_I2CBUS_LOG_RW_LEVEL_INFO 35 | #define I2CBUS_LOG_RW(format, ...) ESP_LOGI(TAG, format, ##__VA_ARGS__) 36 | #elif defined CONFIG_I2CBUS_LOG_RW_LEVEL_DEBUG 37 | #define I2CBUS_LOG_RW(format, ...) ESP_LOGD(TAG, format, ##__VA_ARGS__) 38 | #elif defined CONFIG_I2CBUS_LOG_RW_LEVEL_VERBOSE 39 | #define I2CBUS_LOG_RW(format, ...) ESP_LOGV(TAG, format, ##__VA_ARGS__) 40 | #endif 41 | #define I2CBUS_LOGE(format, ...) ESP_LOGE(TAG, format, ##__VA_ARGS__) 42 | 43 | 44 | #define I2C_MASTER_ACK_EN true /*!< Enable ack check for master */ 45 | #define I2C_MASTER_ACK_DIS false /*!< Disable ack check for master */ 46 | 47 | 48 | static const char* TAG __attribute__((unused)) = "I2Cbus"; 49 | 50 | /******************************************************************************* 51 | * OBJECTS 52 | ******************************************************************************/ 53 | I2C_t i2c0 = i2cbus::I2C(I2C_NUM_0); 54 | I2C_t i2c1 = i2cbus::I2C(I2C_NUM_1); 55 | 56 | 57 | /* ^^^^^^ 58 | * I2Cbus 59 | * ^^^^^^ */ 60 | namespace i2cbus { 61 | 62 | /******************************************************************************* 63 | * SETUP 64 | ******************************************************************************/ 65 | I2C::I2C(i2c_port_t port) : port{port}, ticksToWait{pdMS_TO_TICKS(kDefaultTimeout)} { 66 | } 67 | 68 | I2C::~I2C() { 69 | close(); 70 | } 71 | 72 | esp_err_t I2C::begin(gpio_num_t sda_io_num, gpio_num_t scl_io_num, uint32_t clk_speed) { 73 | return begin(sda_io_num, scl_io_num, GPIO_PULLUP_ENABLE, GPIO_PULLUP_ENABLE, clk_speed); 74 | } 75 | 76 | esp_err_t I2C::begin(gpio_num_t sda_io_num, gpio_num_t scl_io_num, gpio_pullup_t sda_pullup_en, 77 | gpio_pullup_t scl_pullup_en, uint32_t clk_speed) { 78 | i2c_config_t conf; 79 | conf.mode = I2C_MODE_MASTER; 80 | conf.sda_io_num = sda_io_num; 81 | conf.sda_pullup_en = sda_pullup_en; 82 | conf.scl_io_num = scl_io_num; 83 | conf.scl_pullup_en = scl_pullup_en; 84 | conf.master.clk_speed = clk_speed; 85 | esp_err_t err = i2c_param_config(port, &conf); 86 | if (!err) err = i2c_driver_install(port, conf.mode, 0, 0, 0); 87 | return err; 88 | } 89 | 90 | esp_err_t I2C::close() { 91 | return i2c_driver_delete(port); 92 | } 93 | 94 | void I2C::setTimeout(uint32_t ms) { 95 | ticksToWait = pdMS_TO_TICKS(ms); 96 | } 97 | 98 | 99 | 100 | /******************************************************************************* 101 | * WRITING 102 | ******************************************************************************/ 103 | esp_err_t I2C::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, int32_t timeout) { 104 | uint8_t buffer; 105 | esp_err_t err = readByte(devAddr, regAddr, &buffer, timeout); 106 | if (err) return err; 107 | buffer = data ? (buffer | (1 << bitNum)) : (buffer & ~(1 << bitNum)); 108 | return writeByte(devAddr, regAddr, buffer, timeout); 109 | } 110 | 111 | esp_err_t I2C::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, int32_t timeout) { 112 | uint8_t buffer; 113 | esp_err_t err = readByte(devAddr, regAddr, &buffer, timeout); 114 | if (err) return err; 115 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 116 | data <<= (bitStart - length + 1); 117 | data &= mask; 118 | buffer &= ~mask; 119 | buffer |= data; 120 | return writeByte(devAddr, regAddr, buffer, timeout); 121 | } 122 | 123 | esp_err_t I2C::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, int32_t timeout) { 124 | return writeBytes(devAddr, regAddr, 1, &data, timeout); 125 | } 126 | 127 | esp_err_t I2C::writeBytes(uint8_t devAddr, uint8_t regAddr, size_t length, const uint8_t *data, int32_t timeout) { 128 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 129 | i2c_master_start(cmd); 130 | i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, I2C_MASTER_ACK_EN); 131 | i2c_master_write_byte(cmd, regAddr, I2C_MASTER_ACK_EN); 132 | i2c_master_write(cmd, (uint8_t*) data, length, I2C_MASTER_ACK_EN); 133 | i2c_master_stop(cmd); 134 | esp_err_t err = i2c_master_cmd_begin(port, cmd, (timeout < 0 ? ticksToWait : pdMS_TO_TICKS(timeout))); 135 | i2c_cmd_link_delete(cmd); 136 | #if defined CONFIG_I2CBUS_LOG_READWRITES 137 | if (!err) { 138 | char str[length*5+1]; 139 | for (size_t i = 0; i < length; i++) 140 | sprintf(str+i*5, "0x%s%X ", (data[i] < 0x10 ? "0" : ""), data[i]); 141 | I2CBUS_LOG_RW("[port:%d, slave:0x%X] Write %d bytes to register 0x%X, data: %s", 142 | port, devAddr, length, regAddr, str); 143 | } 144 | #endif 145 | #if defined CONFIG_I2CBUS_LOG_ERRORS 146 | #if defined CONFIG_I2CBUS_LOG_READWRITES 147 | else { 148 | #else 149 | if (err) { 150 | #endif 151 | I2CBUS_LOGE("[port:%d, slave:0x%X] Failed to write %d bytes to__ register 0x%X, error: 0x%X", 152 | port, devAddr, length, regAddr, err); 153 | } 154 | #endif 155 | return err; 156 | } 157 | 158 | 159 | /******************************************************************************* 160 | * READING 161 | ******************************************************************************/ 162 | esp_err_t I2C::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, int32_t timeout) { 163 | return readBits(devAddr, regAddr, bitNum, 1, data, timeout); 164 | } 165 | 166 | esp_err_t I2C::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, int32_t timeout) { 167 | uint8_t buffer; 168 | esp_err_t err = readByte(devAddr, regAddr, &buffer, timeout); 169 | if (!err) { 170 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 171 | buffer &= mask; 172 | buffer >>= (bitStart - length + 1); 173 | *data = buffer; 174 | } 175 | return err; 176 | } 177 | 178 | esp_err_t I2C::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, int32_t timeout) { 179 | return readBytes(devAddr, regAddr, 1, data, timeout); 180 | } 181 | 182 | esp_err_t I2C::readBytes(uint8_t devAddr, uint8_t regAddr, size_t length, uint8_t *data, int32_t timeout) { 183 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 184 | i2c_master_start(cmd); 185 | i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, I2C_MASTER_ACK_EN); 186 | i2c_master_write_byte(cmd, regAddr, I2C_MASTER_ACK_EN); 187 | i2c_master_start(cmd); 188 | i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_READ, I2C_MASTER_ACK_EN); 189 | i2c_master_read(cmd, data, length, I2C_MASTER_LAST_NACK); 190 | i2c_master_stop(cmd); 191 | esp_err_t err = i2c_master_cmd_begin(port, cmd, (timeout < 0 ? ticksToWait : pdMS_TO_TICKS(timeout))); 192 | i2c_cmd_link_delete(cmd); 193 | #if defined CONFIG_I2CBUS_LOG_READWRITES 194 | if (!err) { 195 | char str[length*5+1]; 196 | for (size_t i = 0; i < length; i++) 197 | sprintf(str+i*5, "0x%s%X ", (data[i] < 0x10 ? "0" : ""), data[i]); 198 | I2CBUS_LOG_RW("[port:%d, slave:0x%X] Read_ %d bytes from register 0x%X, data: %s", port, devAddr, length, regAddr, str); 199 | } 200 | #endif 201 | #if defined CONFIG_I2CBUS_LOG_ERRORS 202 | #if defined CONFIG_I2CBUS_LOG_READWRITES 203 | else { 204 | #else 205 | if (err) { 206 | #endif 207 | I2CBUS_LOGE("[port:%d, slave:0x%X] Failed to read %d bytes from register 0x%X, error: 0x%X", 208 | port, devAddr, length, regAddr, err); 209 | } 210 | #endif 211 | return err; 212 | } 213 | 214 | 215 | /******************************************************************************* 216 | * UTILS 217 | ******************************************************************************/ 218 | esp_err_t I2C::testConnection(uint8_t devAddr, int32_t timeout) { 219 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 220 | i2c_master_start(cmd); 221 | i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, I2C_MASTER_ACK_EN); 222 | i2c_master_stop(cmd); 223 | esp_err_t err = i2c_master_cmd_begin(port, cmd, (timeout < 0 ? ticksToWait : pdMS_TO_TICKS(timeout))); 224 | i2c_cmd_link_delete(cmd); 225 | return err; 226 | } 227 | 228 | void I2C::scanner() { 229 | constexpr int32_t scanTimeout = 20; 230 | printf(LOG_COLOR_W "\n>> I2C scanning ..." LOG_RESET_COLOR "\n"); 231 | uint8_t count = 0; 232 | for (size_t i = 0x3; i < 0x78; i++) { 233 | if (testConnection(i, scanTimeout) == ESP_OK) { 234 | printf(LOG_COLOR_W "- Device found at address 0x%X%s", i, LOG_RESET_COLOR "\n"); 235 | count++; 236 | } 237 | } 238 | if (count == 0) 239 | printf(LOG_COLOR_E "- No I2C devices found!" LOG_RESET_COLOR "\n"); 240 | printf("\n"); 241 | } 242 | 243 | } // namespace i2cbus 244 | 245 | -------------------------------------------------------------------------------- /I2Cbus.hpp: -------------------------------------------------------------------------------- 1 | /* ========================================================================= 2 | I2Cbus library is placed under the MIT License 3 | Copyright 2017 Natanael Josue Rabello. All rights reserved. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to 7 | deal in the Software without restriction, including without limitation the 8 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | sell copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | IN THE SOFTWARE. 22 | ========================================================================= */ 23 | 24 | #ifndef _I2CBUS_HPP_ 25 | #define _I2CBUS_HPP_ 26 | 27 | #include 28 | #include "driver/i2c.h" 29 | #include "driver/gpio.h" 30 | #include "esp_err.h" 31 | 32 | 33 | /* ^^^^^^ 34 | * I2Cbus 35 | * ^^^^^^ */ 36 | namespace i2cbus { 37 | constexpr uint32_t kDefaultClockSpeed = 400000; /*!< Clock speed in Hz, default: 100KHz */ 38 | constexpr uint32_t kDefaultTimeout = 1000; /*!< Timeout in milliseconds, default: 1000ms */ 39 | class I2C; 40 | } // namespace i2cbus 41 | 42 | // I2Cbus type 43 | using I2C_t = i2cbus::I2C; 44 | 45 | // Default Objects 46 | extern I2C_t i2c0; /*!< port: I2C_NUM_0 */ 47 | extern I2C_t i2c1; /*!< port: I2C_NUM_1 */ 48 | 49 | 50 | // I2C class definition 51 | namespace i2cbus { 52 | class I2C { 53 | private: 54 | i2c_port_t port; /*!< I2C port: I2C_NUM_0 or I2C_NUM_1 */ 55 | uint32_t ticksToWait; /*!< Timeout in ticks for read and write */ 56 | 57 | public: 58 | explicit I2C(i2c_port_t port); 59 | ~I2C(); 60 | 61 | /** *** I2C Begin *** 62 | * @brief Config I2C bus and Install Driver 63 | * @param sda_io_num [GPIO number for SDA line] 64 | * @param scl_io_num [GPIO number for SCL line] 65 | * @param sda_pullup_en [Enable internal pullup on SDA line] 66 | * @param scl_pullup_en [Enable internal pullup on SCL line] 67 | * @param clk_speed [I2C clock frequency for master mode, (no higher than 1MHz for now), Default 100KHz] 68 | * @see "driver/i2c.h" 69 | * @return - ESP_OK Success 70 | * - ESP_ERR_INVALID_ARG Parameter error 71 | * - ESP_FAIL Driver install error 72 | */ 73 | esp_err_t begin(gpio_num_t sda_io_num = GPIO_NUM_21, gpio_num_t scl_io_num = GPIO_NUM_22, uint32_t clk_speed = kDefaultClockSpeed); 74 | 75 | esp_err_t begin(gpio_num_t sda_io_num, gpio_num_t scl_io_num, 76 | gpio_pullup_t sda_pullup_en, gpio_pullup_t scl_pullup_en, 77 | uint32_t clk_speed = kDefaultClockSpeed); 78 | 79 | /** 80 | * Stop I2C bus and unninstall driver 81 | */ 82 | esp_err_t close(); 83 | 84 | /** 85 | * Timeout read and write in milliseconds 86 | */ 87 | void setTimeout(uint32_t ms); 88 | 89 | 90 | /** 91 | * *** WRITING interface *** 92 | * @brief I2C commands for writing to a 8-bit slave device register. 93 | * All of them returns standard esp_err_t codes. So it can be used 94 | * with ESP_ERROR_CHECK(); 95 | * @param devAddr [I2C slave device register] 96 | * @param regAddr [Register address to write to] 97 | * @param bitNum [Bit position number to write to (bit 7~0)] 98 | * @param bitStart [Start bit number when writing a bit-sequence (MSB)] 99 | * @param data [Value(s) to be write to the register] 100 | * @param length [Number of bytes to write (should be within the data buffer size)] 101 | * [writeBits() -> Number of bits after bitStart (including)] 102 | * @param timeout [Custom timeout for the particular call] 103 | * @return - ESP_OK Success 104 | * - ESP_ERR_INVALID_ARG Parameter error 105 | * - ESP_FAIL Sending command error, slave doesn't ACK the transfer. 106 | * - ESP_ERR_INVALID_STATE I2C driver not installed or not in master mode. 107 | * - ESP_ERR_TIMEOUT Operation timeout because the bus is busy. 108 | */ 109 | esp_err_t writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, int32_t timeout = -1); 110 | esp_err_t writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, int32_t timeout = -1); 111 | esp_err_t writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, int32_t timeout = -1); 112 | esp_err_t writeBytes(uint8_t devAddr, uint8_t regAddr, size_t length, const uint8_t *data, int32_t timeout = -1); 113 | 114 | /** 115 | * *** READING interface *** 116 | * @breif I2C commands for reading a 8-bit slave device register. 117 | * All of them returns standard esp_err_t codes.So it can be used 118 | * with ESP_ERROR_CHECK(); 119 | * @param devAddr [I2C slave device register] 120 | * @param regAddr [Register address to read from] 121 | * @param bitNum [Bit position number to write to (bit 7~0)] 122 | * @param bitStart [Start bit number when writing a bit-sequence (MSB)] 123 | * @param data [Buffer to store the read value(s)] 124 | * @param length [Number of bytes to read (should be within the data buffer size)] 125 | * @param timeout [Custom timeout for the particular call] 126 | * @return - ESP_OK Success 127 | * - ESP_ERR_INVALID_ARG Parameter error 128 | * - ESP_FAIL Sending command error, slave doesn't ACK the transfer. 129 | * - ESP_ERR_INVALID_STATE I2C driver not installed or not in master mode. 130 | * - ESP_ERR_TIMEOUT Operation timeout because the bus is busy.] 131 | */ 132 | esp_err_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, int32_t timeout = -1); 133 | esp_err_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, int32_t timeout = -1); 134 | esp_err_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, int32_t timeout = -1); 135 | esp_err_t readBytes(uint8_t devAddr, uint8_t regAddr, size_t length, uint8_t *data, int32_t timeout = -1); 136 | 137 | /** 138 | * @brief Quick check to see if a slave device responds. 139 | * @param devAddr [I2C slave device register] 140 | * @param timeout [Custom timeout for the particular call] 141 | * @return - ESP_OK Success 142 | * - ESP_ERR_INVALID_ARG Parameter error 143 | * - ESP_FAIL Sending command error, slave doesn't ACK the transfer. 144 | * - ESP_ERR_INVALID_STATE I2C driver not installed or not in master mode. 145 | * - ESP_ERR_TIMEOUT Operation timeout because the bus is busy.] 146 | */ 147 | esp_err_t testConnection(uint8_t devAddr, int32_t timeout = -1); 148 | 149 | /** 150 | * I2C scanner utility, prints out all device addresses found on this I2C bus. 151 | */ 152 | void scanner(); 153 | }; 154 | 155 | } // namespace i2cbus 156 | 157 | 158 | /* Get default objects */ 159 | constexpr I2C_t& getI2C(i2c_port_t port) { 160 | return port == 0 ? i2c0 : i2c1; 161 | } 162 | 163 | 164 | 165 | #endif /* end of include guard: _I2CBUS_H_ */ 166 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 BananaPi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MPU9250.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MPU9250.cpp 3 | BananaPi Bit Team 4 | juwan@banana-pi.com (Juwan·C) 5 | wanghao@banana-pi.com (Hulk Wang) 6 | 7 | We modified it based on https://github.com/bolderflight/MPU9250 8 | The main purpose is to adapt the MPU9250 driver library of BPI-BIT (espressif32). 9 | 10 | Copyright (c) 2017 BananaPi Bit Team 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 13 | and associated documentation files (the "Software"), to deal in the Software without restriction, 14 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 15 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 16 | furnished to do so, subject to the following conditions: 17 | 18 | The above copyright notice and this permission notice shall be included in all copies or 19 | substantial portions of the Software. 20 | 21 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 22 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 23 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 24 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 25 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 26 | */ 27 | 28 | #include "Arduino.h" 29 | #include "MPU9250.h" 30 | 31 | /* MPU9250 object, input the I2C bus and address */ 32 | MPU9250::MPU9250(I2C_t &bus,uint8_t address){ 33 | _i2c = &bus; // I2C bus 34 | _address = address; // I2C address 35 | _useSPI = false; // set to use I2C 36 | } 37 | 38 | /* MPU9250 object, input the SPI bus and chip select pin */ 39 | MPU9250::MPU9250(SPIClass &bus,uint8_t csPin){ 40 | _spi = &bus; // SPI bus 41 | _csPin = csPin; // chip select pin 42 | _useSPI = true; // set to use SPI 43 | } 44 | 45 | /* starts communication with the MPU-9250 */ 46 | int MPU9250::begin(){ 47 | if( _useSPI ) { // using SPI for communication 48 | // use low speed SPI for register setting 49 | _useSPIHS = false; 50 | // setting CS pin to output 51 | pinMode(_csPin,OUTPUT); 52 | // setting CS pin high 53 | digitalWrite(_csPin,HIGH); 54 | // begin SPI communication 55 | _spi->begin(); 56 | } else { // using I2C for communication 57 | // starting the I2C bus 58 | _i2c->begin(); 59 | // setting the I2C clock 60 | // _i2c->setClock(_i2cRate); 61 | } 62 | while(true) 63 | { 64 | int res = whoAmI(); 65 | Serial.printf("whoAmI() %d\r\n", res); 66 | // _i2c->reset(); 67 | if(res == 113 || res == 115) 68 | { 69 | break; 70 | } 71 | } 72 | // select clock source to gyro 73 | if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ 74 | return -1; 75 | } 76 | // enable I2C master mode 77 | if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ 78 | return -2; 79 | } 80 | // set the I2C bus speed to 400 kHz 81 | if(writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ 82 | return -3; 83 | } 84 | // set AK8963 to Power Down 85 | writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); 86 | // reset the MPU9250 87 | writeRegister(PWR_MGMNT_1,PWR_RESET); 88 | // wait for MPU-9250 to come back up 89 | delay(1); 90 | // reset the AK8963 91 | writeAK8963Register(AK8963_CNTL2,AK8963_RESET); 92 | // select clock source to gyro 93 | if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ 94 | return -4; 95 | } 96 | // check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115) 97 | if((whoAmI() != 113)&&(whoAmI() != 115)){ 98 | return -5; 99 | } 100 | // enable accelerometer and gyro 101 | if(writeRegister(PWR_MGMNT_2,SEN_ENABLE) < 0){ 102 | return -6; 103 | } 104 | // setting accel range to 16G as default 105 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ 106 | return -7; 107 | } 108 | _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G 109 | _accelRange = ACCEL_RANGE_16G; 110 | // setting the gyro range to 2000DPS as default 111 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ 112 | return -8; 113 | } 114 | _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS 115 | _gyroRange = GYRO_RANGE_2000DPS; 116 | // setting bandwidth to 184Hz as default 117 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ 118 | return -9; 119 | } 120 | if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz 121 | return -10; 122 | } 123 | _bandwidth = DLPF_BANDWIDTH_184HZ; 124 | // setting the sample rate divider to 0 as default 125 | if(writeRegister(SMPDIV,0x00) < 0){ 126 | return -11; 127 | } 128 | _srd = 0; 129 | // enable I2C master mode 130 | if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ 131 | return -12; 132 | } 133 | // set the I2C bus speed to 400 kHz 134 | if( writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ 135 | return -13; 136 | } 137 | // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72) 138 | if( whoAmIAK8963() != 72 ){ 139 | esp_restart(); 140 | return -14; 141 | } 142 | /* get the magnetometer calibration */ 143 | // set AK8963 to Power Down 144 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ 145 | return -15; 146 | } 147 | delay(100); // long wait between AK8963 mode changes 148 | // set AK8963 to FUSE ROM access 149 | if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0){ 150 | return -16; 151 | } 152 | delay(100); // long wait between AK8963 mode changes 153 | // read the AK8963 ASA registers and compute magnetometer scale factors 154 | readAK8963Registers(AK8963_ASA,3,_buffer); 155 | _magScaleX = ((((float)_buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 156 | _magScaleY = ((((float)_buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 157 | _magScaleZ = ((((float)_buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 158 | printf("RawData (%hd, %hd, %hd) \r\n", _magScaleX, _magScaleY, _magScaleZ); 159 | 160 | // set AK8963 to Power Down 161 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ 162 | return -17; 163 | } 164 | delay(100); // long wait between AK8963 mode changes 165 | // set AK8963 to 16 bit resolution, 100 Hz update rate 166 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ 167 | return -18; 168 | } 169 | delay(100); // long wait between AK8963 mode changes 170 | // select clock source to gyro 171 | if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ 172 | return -19; 173 | } 174 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate 175 | readAK8963Registers(AK8963_HXL,7,_buffer); 176 | // estimate gyro bias 177 | if (calibrateGyro() < 0) { 178 | return -20; 179 | } 180 | // successful init, return 1 181 | return 1; 182 | } 183 | 184 | /* sets the accelerometer full scale range to values other than default */ 185 | int MPU9250::setAccelRange(AccelRange range) { 186 | // use low speed SPI for register setting 187 | _useSPIHS = false; 188 | switch(range) { 189 | case ACCEL_RANGE_2G: { 190 | // setting the accel range to 2G 191 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0){ 192 | return -1; 193 | } 194 | _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G 195 | break; 196 | } 197 | case ACCEL_RANGE_4G: { 198 | // setting the accel range to 4G 199 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0){ 200 | return -1; 201 | } 202 | _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G 203 | break; 204 | } 205 | case ACCEL_RANGE_8G: { 206 | // setting the accel range to 8G 207 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0){ 208 | return -1; 209 | } 210 | _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G 211 | break; 212 | } 213 | case ACCEL_RANGE_16G: { 214 | // setting the accel range to 16G 215 | if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ 216 | return -1; 217 | } 218 | _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G 219 | break; 220 | } 221 | } 222 | _accelRange = range; 223 | return 1; 224 | } 225 | 226 | /* sets the gyro full scale range to values other than default */ 227 | int MPU9250::setGyroRange(GyroRange range) { 228 | // use low speed SPI for register setting 229 | _useSPIHS = false; 230 | switch(range) { 231 | case GYRO_RANGE_250DPS: { 232 | // setting the gyro range to 250DPS 233 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0){ 234 | return -1; 235 | } 236 | _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS 237 | break; 238 | } 239 | case GYRO_RANGE_500DPS: { 240 | // setting the gyro range to 500DPS 241 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0){ 242 | return -1; 243 | } 244 | _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS 245 | break; 246 | } 247 | case GYRO_RANGE_1000DPS: { 248 | // setting the gyro range to 1000DPS 249 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0){ 250 | return -1; 251 | } 252 | _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS 253 | break; 254 | } 255 | case GYRO_RANGE_2000DPS: { 256 | // setting the gyro range to 2000DPS 257 | if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ 258 | return -1; 259 | } 260 | _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS 261 | break; 262 | } 263 | } 264 | _gyroRange = range; 265 | return 1; 266 | } 267 | 268 | /* sets the DLPF bandwidth to values other than default */ 269 | int MPU9250::setDlpfBandwidth(DlpfBandwidth bandwidth) { 270 | // use low speed SPI for register setting 271 | _useSPIHS = false; 272 | switch(bandwidth) { 273 | case DLPF_BANDWIDTH_184HZ: { 274 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz 275 | return -1; 276 | } 277 | if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz 278 | return -2; 279 | } 280 | break; 281 | } 282 | case DLPF_BANDWIDTH_92HZ: { 283 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0){ // setting accel bandwidth to 92Hz 284 | return -1; 285 | } 286 | if(writeRegister(CONFIG,GYRO_DLPF_92) < 0){ // setting gyro bandwidth to 92Hz 287 | return -2; 288 | } 289 | break; 290 | } 291 | case DLPF_BANDWIDTH_41HZ: { 292 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0){ // setting accel bandwidth to 41Hz 293 | return -1; 294 | } 295 | if(writeRegister(CONFIG,GYRO_DLPF_41) < 0){ // setting gyro bandwidth to 41Hz 296 | return -2; 297 | } 298 | break; 299 | } 300 | case DLPF_BANDWIDTH_20HZ: { 301 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0){ // setting accel bandwidth to 20Hz 302 | return -1; 303 | } 304 | if(writeRegister(CONFIG,GYRO_DLPF_20) < 0){ // setting gyro bandwidth to 20Hz 305 | return -2; 306 | } 307 | break; 308 | } 309 | case DLPF_BANDWIDTH_10HZ: { 310 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0){ // setting accel bandwidth to 10Hz 311 | return -1; 312 | } 313 | if(writeRegister(CONFIG,GYRO_DLPF_10) < 0){ // setting gyro bandwidth to 10Hz 314 | return -2; 315 | } 316 | break; 317 | } 318 | case DLPF_BANDWIDTH_5HZ: { 319 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0){ // setting accel bandwidth to 5Hz 320 | return -1; 321 | } 322 | if(writeRegister(CONFIG,GYRO_DLPF_5) < 0){ // setting gyro bandwidth to 5Hz 323 | return -2; 324 | } 325 | break; 326 | } 327 | } 328 | _bandwidth = bandwidth; 329 | return 1; 330 | } 331 | 332 | /* sets the sample rate divider to values other than default */ 333 | int MPU9250::setSrd(uint8_t srd) { 334 | // use low speed SPI for register setting 335 | _useSPIHS = false; 336 | /* setting the sample rate divider to 19 to facilitate setting up magnetometer */ 337 | if(writeRegister(SMPDIV,19) < 0){ // setting the sample rate divider 338 | return -1; 339 | } 340 | if(srd > 9){ 341 | // set AK8963 to Power Down 342 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ 343 | return -2; 344 | } 345 | delay(100); // long wait between AK8963 mode changes 346 | // set AK8963 to 16 bit resolution, 8 Hz update rate 347 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0){ 348 | return -3; 349 | } 350 | delay(100); // long wait between AK8963 mode changes 351 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate 352 | readAK8963Registers(AK8963_HXL,7,_buffer); 353 | } else { 354 | // set AK8963 to Power Down 355 | if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ 356 | return -2; 357 | } 358 | delay(100); // long wait between AK8963 mode changes 359 | // set AK8963 to 16 bit resolution, 100 Hz update rate 360 | if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ 361 | return -3; 362 | } 363 | delay(100); // long wait between AK8963 mode changes 364 | // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate 365 | readAK8963Registers(AK8963_HXL,7,_buffer); 366 | } 367 | /* setting the sample rate divider */ 368 | if(writeRegister(SMPDIV,srd) < 0){ // setting the sample rate divider 369 | return -4; 370 | } 371 | _srd = srd; 372 | return 1; 373 | } 374 | 375 | /* enables the data ready interrupt */ 376 | int MPU9250::enableDataReadyInterrupt() { 377 | // use low speed SPI for register setting 378 | _useSPIHS = false; 379 | /* setting the interrupt */ 380 | if (writeRegister(INT_PIN_CFG,INT_PULSE_50US) < 0){ // setup interrupt, 50 us pulse 381 | return -1; 382 | } 383 | if (writeRegister(INT_ENABLE,INT_RAW_RDY_EN) < 0){ // set to data ready 384 | return -2; 385 | } 386 | return 1; 387 | } 388 | 389 | /* disables the data ready interrupt */ 390 | int MPU9250::disableDataReadyInterrupt() { 391 | // use low speed SPI for register setting 392 | _useSPIHS = false; 393 | if(writeRegister(INT_ENABLE,INT_DISABLE) < 0){ // disable interrupt 394 | return -1; 395 | } 396 | return 1; 397 | } 398 | 399 | /* configures and enables wake on motion, low power mode */ 400 | int MPU9250::enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr) { 401 | // use low speed SPI for register setting 402 | _useSPIHS = false; 403 | // set AK8963 to Power Down 404 | writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); 405 | // reset the MPU9250 406 | writeRegister(PWR_MGMNT_1,PWR_RESET); 407 | // wait for MPU-9250 to come back up 408 | delay(1); 409 | if(writeRegister(PWR_MGMNT_1,0x00) < 0){ // cycle 0, sleep 0, standby 0 410 | return -1; 411 | } 412 | if(writeRegister(PWR_MGMNT_2,DIS_GYRO) < 0){ // disable gyro measurements 413 | return -2; 414 | } 415 | if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz 416 | return -3; 417 | } 418 | if(writeRegister(INT_ENABLE,INT_WOM_EN) < 0){ // enabling interrupt to wake on motion 419 | return -4; 420 | } 421 | if(writeRegister(MOT_DETECT_CTRL,(ACCEL_INTEL_EN | ACCEL_INTEL_MODE)) < 0){ // enabling accel hardware intelligence 422 | return -5; 423 | } 424 | _womThreshold = map(womThresh_mg, 0, 1020, 0, 255); 425 | if(writeRegister(WOM_THR,_womThreshold) < 0){ // setting wake on motion threshold 426 | return -6; 427 | } 428 | if(writeRegister(LP_ACCEL_ODR,(uint8_t)odr) < 0){ // set frequency of wakeup 429 | return -7; 430 | } 431 | if(writeRegister(PWR_MGMNT_1,PWR_CYCLE) < 0){ // switch to accel low power mode 432 | return -8; 433 | } 434 | return 1; 435 | } 436 | 437 | /* configures and enables the FIFO buffer */ 438 | int MPU9250FIFO::enableFifo(bool accel,bool gyro,bool mag,bool temp) { 439 | // use low speed SPI for register setting 440 | _useSPIHS = false; 441 | if(writeRegister(USER_CTRL, (0x40 | I2C_MST_EN)) < 0){ 442 | return -1; 443 | } 444 | if(writeRegister(FIFO_EN,(accel*FIFO_ACCEL)|(gyro*FIFO_GYRO)|(mag*FIFO_MAG)|(temp*FIFO_TEMP)) < 0){ 445 | return -2; 446 | } 447 | _enFifoAccel = accel; 448 | _enFifoGyro = gyro; 449 | _enFifoMag = mag; 450 | _enFifoTemp = temp; 451 | _fifoFrameSize = accel*6 + gyro*6 + mag*7 + temp*2; 452 | return 1; 453 | } 454 | 455 | /* reads the most current data from MPU9250 and stores in buffer */ 456 | int MPU9250::readSensor() { 457 | _useSPIHS = true; // use the high speed SPI for data readout 458 | // grab the data from the MPU9250 459 | if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) { 460 | return -1; 461 | } 462 | // combine into 16 bit values 463 | _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; 464 | _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; 465 | _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; 466 | _tcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7]; 467 | _gxcounts = (((int16_t)_buffer[8]) << 8) | _buffer[9]; 468 | _gycounts = (((int16_t)_buffer[10]) << 8) | _buffer[11]; 469 | _gzcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13]; 470 | _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14]; 471 | _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16]; 472 | _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18]; 473 | 474 | // transform and convert to float values 475 | _ax = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale) - _axb)*_axs; 476 | _ay = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale) - _ayb)*_ays; 477 | _az = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale) - _azb)*_azs; 478 | _gx = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; 479 | _gy = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; 480 | _gz = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; 481 | _hx = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; 482 | _hy = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; 483 | _hz = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; 484 | _t = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; 485 | return 1; 486 | } 487 | 488 | /* returns the accelerometer measurement in the x direction, m/s/s */ 489 | float MPU9250::getAccelX_mss() { 490 | return _ax; 491 | } 492 | 493 | /* returns the accelerometer measurement in the y direction, m/s/s */ 494 | float MPU9250::getAccelY_mss() { 495 | return _ay; 496 | } 497 | 498 | /* returns the accelerometer measurement in the z direction, m/s/s */ 499 | float MPU9250::getAccelZ_mss() { 500 | return _az; 501 | } 502 | 503 | /* returns the gyroscope measurement in the x direction, rad/s */ 504 | float MPU9250::getGyroX_rads() { 505 | return _gx; 506 | } 507 | 508 | /* returns the gyroscope measurement in the y direction, rad/s */ 509 | float MPU9250::getGyroY_rads() { 510 | return _gy; 511 | } 512 | 513 | /* returns the gyroscope measurement in the z direction, rad/s */ 514 | float MPU9250::getGyroZ_rads() { 515 | return _gz; 516 | } 517 | 518 | /* returns the magnetometer measurement in the x direction, uT */ 519 | float MPU9250::getMagX_uT() { 520 | return _hx; 521 | } 522 | 523 | /* returns the magnetometer measurement in the y direction, uT */ 524 | float MPU9250::getMagY_uT() { 525 | return _hy; 526 | } 527 | 528 | /* returns the magnetometer measurement in the z direction, uT */ 529 | float MPU9250::getMagZ_uT() { 530 | return _hz; 531 | } 532 | 533 | /* returns the die temperature, C */ 534 | float MPU9250::getTemperature_C() { 535 | return _t; 536 | } 537 | 538 | /* reads data from the MPU9250 FIFO and stores in buffer */ 539 | int MPU9250FIFO::readFifo() { 540 | _useSPIHS = true; // use the high speed SPI for data readout 541 | // get the fifo size 542 | readRegisters(FIFO_COUNT, 2, _buffer); 543 | _fifoSize = (((uint16_t) (_buffer[0]&0x0F)) <<8) + (((uint16_t) _buffer[1])); 544 | // read and parse the buffer 545 | for (size_t i=0; i < _fifoSize/_fifoFrameSize; i++) { 546 | // grab the data from the MPU9250 547 | if (readRegisters(FIFO_READ,_fifoFrameSize,_buffer) < 0) { 548 | return -1; 549 | } 550 | if (_enFifoAccel) { 551 | // combine into 16 bit values 552 | _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; 553 | _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; 554 | _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; 555 | // transform and convert to float values 556 | _axFifo[i] = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale)-_axb)*_axs; 557 | _ayFifo[i] = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale)-_ayb)*_ays; 558 | _azFifo[i] = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale)-_azb)*_azs; 559 | _aSize = _fifoSize/_fifoFrameSize; 560 | } 561 | if (_enFifoTemp) { 562 | // combine into 16 bit values 563 | _tcounts = (((int16_t)_buffer[0 + _enFifoAccel*6]) << 8) | _buffer[1 + _enFifoAccel*6]; 564 | // transform and convert to float values 565 | _tFifo[i] = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; 566 | _tSize = _fifoSize/_fifoFrameSize; 567 | } 568 | if (_enFifoGyro) { 569 | // combine into 16 bit values 570 | _gxcounts = (((int16_t)_buffer[0 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[1 + _enFifoAccel*6 + _enFifoTemp*2]; 571 | _gycounts = (((int16_t)_buffer[2 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[3 + _enFifoAccel*6 + _enFifoTemp*2]; 572 | _gzcounts = (((int16_t)_buffer[4 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[5 + _enFifoAccel*6 + _enFifoTemp*2]; 573 | // transform and convert to float values 574 | _gxFifo[i] = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; 575 | _gyFifo[i] = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; 576 | _gzFifo[i] = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; 577 | _gSize = _fifoSize/_fifoFrameSize; 578 | } 579 | if (_enFifoMag) { 580 | // combine into 16 bit values 581 | _hxcounts = (((int16_t)_buffer[1 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[0 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; 582 | _hycounts = (((int16_t)_buffer[3 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[2 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; 583 | _hzcounts = (((int16_t)_buffer[5 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[4 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; 584 | // transform and convert to float values 585 | _hxFifo[i] = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; 586 | _hyFifo[i] = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; 587 | _hzFifo[i] = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; 588 | _hSize = _fifoSize/_fifoFrameSize; 589 | } 590 | } 591 | return 1; 592 | } 593 | 594 | /* returns the accelerometer FIFO size and data in the x direction, m/s/s */ 595 | void MPU9250FIFO::getFifoAccelX_mss(size_t *size,float* data) { 596 | *size = _aSize; 597 | memcpy(data,_axFifo,_aSize*sizeof(float)); 598 | } 599 | 600 | /* returns the accelerometer FIFO size and data in the y direction, m/s/s */ 601 | void MPU9250FIFO::getFifoAccelY_mss(size_t *size,float* data) { 602 | *size = _aSize; 603 | memcpy(data,_ayFifo,_aSize*sizeof(float)); 604 | } 605 | 606 | /* returns the accelerometer FIFO size and data in the z direction, m/s/s */ 607 | void MPU9250FIFO::getFifoAccelZ_mss(size_t *size,float* data) { 608 | *size = _aSize; 609 | memcpy(data,_azFifo,_aSize*sizeof(float)); 610 | } 611 | 612 | /* returns the gyroscope FIFO size and data in the x direction, rad/s */ 613 | void MPU9250FIFO::getFifoGyroX_rads(size_t *size,float* data) { 614 | *size = _gSize; 615 | memcpy(data,_gxFifo,_gSize*sizeof(float)); 616 | } 617 | 618 | /* returns the gyroscope FIFO size and data in the y direction, rad/s */ 619 | void MPU9250FIFO::getFifoGyroY_rads(size_t *size,float* data) { 620 | *size = _gSize; 621 | memcpy(data,_gyFifo,_gSize*sizeof(float)); 622 | } 623 | 624 | /* returns the gyroscope FIFO size and data in the z direction, rad/s */ 625 | void MPU9250FIFO::getFifoGyroZ_rads(size_t *size,float* data) { 626 | *size = _gSize; 627 | memcpy(data,_gzFifo,_gSize*sizeof(float)); 628 | } 629 | 630 | /* returns the magnetometer FIFO size and data in the x direction, uT */ 631 | void MPU9250FIFO::getFifoMagX_uT(size_t *size,float* data) { 632 | *size = _hSize; 633 | memcpy(data,_hxFifo,_hSize*sizeof(float)); 634 | } 635 | 636 | /* returns the magnetometer FIFO size and data in the y direction, uT */ 637 | void MPU9250FIFO::getFifoMagY_uT(size_t *size,float* data) { 638 | *size = _hSize; 639 | memcpy(data,_hyFifo,_hSize*sizeof(float)); 640 | } 641 | 642 | /* returns the magnetometer FIFO size and data in the z direction, uT */ 643 | void MPU9250FIFO::getFifoMagZ_uT(size_t *size,float* data) { 644 | *size = _hSize; 645 | memcpy(data,_hzFifo,_hSize*sizeof(float)); 646 | } 647 | 648 | /* returns the die temperature FIFO size and data, C */ 649 | void MPU9250FIFO::getFifoTemperature_C(size_t *size,float* data) { 650 | *size = _tSize; 651 | memcpy(data,_tFifo,_tSize*sizeof(float)); 652 | } 653 | 654 | /* estimates the gyro biases */ 655 | int MPU9250::calibrateGyro() { 656 | // set the range, bandwidth, and srd 657 | if (setGyroRange(GYRO_RANGE_250DPS) < 0) { 658 | return -1; 659 | } 660 | if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { 661 | return -2; 662 | } 663 | if (setSrd(19) < 0) { 664 | return -3; 665 | } 666 | 667 | // take samples and find bias 668 | _gxbD = 0; 669 | _gybD = 0; 670 | _gzbD = 0; 671 | for (size_t i=0; i < _numSamples; i++) { 672 | readSensor(); 673 | _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples); 674 | _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples); 675 | _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples); 676 | delay(20); 677 | } 678 | _gxb = (float)_gxbD; 679 | _gyb = (float)_gybD; 680 | _gzb = (float)_gzbD; 681 | 682 | // set the range, bandwidth, and srd back to what they were 683 | if (setGyroRange(_gyroRange) < 0) { 684 | return -4; 685 | } 686 | if (setDlpfBandwidth(_bandwidth) < 0) { 687 | return -5; 688 | } 689 | if (setSrd(_srd) < 0) { 690 | return -6; 691 | } 692 | return 1; 693 | } 694 | 695 | /* returns the gyro bias in the X direction, rad/s */ 696 | float MPU9250::getGyroBiasX_rads() { 697 | return _gxb; 698 | } 699 | 700 | /* returns the gyro bias in the Y direction, rad/s */ 701 | float MPU9250::getGyroBiasY_rads() { 702 | return _gyb; 703 | } 704 | 705 | /* returns the gyro bias in the Z direction, rad/s */ 706 | float MPU9250::getGyroBiasZ_rads() { 707 | return _gzb; 708 | } 709 | 710 | /* sets the gyro bias in the X direction to bias, rad/s */ 711 | void MPU9250::setGyroBiasX_rads(float bias) { 712 | _gxb = bias; 713 | } 714 | 715 | /* sets the gyro bias in the Y direction to bias, rad/s */ 716 | void MPU9250::setGyroBiasY_rads(float bias) { 717 | _gyb = bias; 718 | } 719 | 720 | /* sets the gyro bias in the Z direction to bias, rad/s */ 721 | void MPU9250::setGyroBiasZ_rads(float bias) { 722 | _gzb = bias; 723 | } 724 | 725 | /* finds bias and scale factor calibration for the accelerometer, 726 | this should be run for each axis in each direction (6 total) to find 727 | the min and max values along each */ 728 | int MPU9250::calibrateAccel() { 729 | // set the range, bandwidth, and srd 730 | if (setAccelRange(ACCEL_RANGE_2G) < 0) { 731 | return -1; 732 | } 733 | if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { 734 | return -2; 735 | } 736 | if (setSrd(19) < 0) { 737 | return -3; 738 | } 739 | 740 | // take samples and find min / max 741 | _axbD = 0; 742 | _aybD = 0; 743 | _azbD = 0; 744 | for (size_t i=0; i < _numSamples; i++) { 745 | readSensor(); 746 | _axbD += (getAccelX_mss()/_axs + _axb)/((double)_numSamples); 747 | _aybD += (getAccelY_mss()/_ays + _ayb)/((double)_numSamples); 748 | _azbD += (getAccelZ_mss()/_azs + _azb)/((double)_numSamples); 749 | delay(20); 750 | } 751 | if (_axbD > 9.0f) { 752 | _axmax = (float)_axbD; 753 | } 754 | if (_aybD > 9.0f) { 755 | _aymax = (float)_aybD; 756 | } 757 | if (_azbD > 9.0f) { 758 | _azmax = (float)_azbD; 759 | } 760 | if (_axbD < -9.0f) { 761 | _axmin = (float)_axbD; 762 | } 763 | if (_aybD < -9.0f) { 764 | _aymin = (float)_aybD; 765 | } 766 | if (_azbD < -9.0f) { 767 | _azmin = (float)_azbD; 768 | } 769 | 770 | // find bias and scale factor 771 | if ((abs(_axmin) > 9.0f) && (abs(_axmax) > 9.0f)) { 772 | _axb = (_axmin + _axmax) / 2.0f; 773 | _axs = G/((abs(_axmin) + abs(_axmax)) / 2.0f); 774 | } 775 | if ((abs(_aymin) > 9.0f) && (abs(_aymax) > 9.0f)) { 776 | _ayb = (_axmin + _axmax) / 2.0f; 777 | _ays = G/((abs(_aymin) + abs(_aymax)) / 2.0f); 778 | } 779 | if ((abs(_azmin) > 9.0f) && (abs(_azmax) > 9.0f)) { 780 | _azb = (_azmin + _azmax) / 2.0f; 781 | _azs = G/((abs(_azmin) + abs(_azmax)) / 2.0f); 782 | } 783 | 784 | // set the range, bandwidth, and srd back to what they were 785 | if (setAccelRange(_accelRange) < 0) { 786 | return -4; 787 | } 788 | if (setDlpfBandwidth(_bandwidth) < 0) { 789 | return -5; 790 | } 791 | if (setSrd(_srd) < 0) { 792 | return -6; 793 | } 794 | return 1; 795 | } 796 | 797 | /* returns the accelerometer bias in the X direction, m/s/s */ 798 | float MPU9250::getAccelBiasX_mss() { 799 | return _axb; 800 | } 801 | 802 | /* returns the accelerometer scale factor in the X direction */ 803 | float MPU9250::getAccelScaleFactorX() { 804 | return _axs; 805 | } 806 | 807 | /* returns the accelerometer bias in the Y direction, m/s/s */ 808 | float MPU9250::getAccelBiasY_mss() { 809 | return _ayb; 810 | } 811 | 812 | /* returns the accelerometer scale factor in the Y direction */ 813 | float MPU9250::getAccelScaleFactorY() { 814 | return _ays; 815 | } 816 | 817 | /* returns the accelerometer bias in the Z direction, m/s/s */ 818 | float MPU9250::getAccelBiasZ_mss() { 819 | return _azb; 820 | } 821 | 822 | /* returns the accelerometer scale factor in the Z direction */ 823 | float MPU9250::getAccelScaleFactorZ() { 824 | return _azs; 825 | } 826 | 827 | /* sets the accelerometer bias (m/s/s) and scale factor in the X direction */ 828 | void MPU9250::setAccelCalX(float bias,float scaleFactor) { 829 | _axb = bias; 830 | _axs = scaleFactor; 831 | } 832 | 833 | /* sets the accelerometer bias (m/s/s) and scale factor in the Y direction */ 834 | void MPU9250::setAccelCalY(float bias,float scaleFactor) { 835 | _ayb = bias; 836 | _ays = scaleFactor; 837 | } 838 | 839 | /* sets the accelerometer bias (m/s/s) and scale factor in the Z direction */ 840 | void MPU9250::setAccelCalZ(float bias,float scaleFactor) { 841 | _azb = bias; 842 | _azs = scaleFactor; 843 | } 844 | 845 | /* finds bias and scale factor calibration for the magnetometer, 846 | the sensor should be rotated in a figure 8 motion until complete */ 847 | int MPU9250::calibrateMag() { 848 | // set the srd 849 | if (setSrd(19) < 0) { 850 | return -1; 851 | } 852 | 853 | // get a starting set of data 854 | readSensor(); 855 | _hxmax = getMagX_uT(); 856 | _hxmin = getMagX_uT(); 857 | _hymax = getMagY_uT(); 858 | _hymin = getMagY_uT(); 859 | _hzmax = getMagZ_uT(); 860 | _hzmin = getMagZ_uT(); 861 | 862 | // collect data to find max / min in each channel 863 | _counter = 0; 864 | while (_counter < _maxCounts) { 865 | _delta = 0.0f; 866 | _framedelta = 0.0f; 867 | readSensor(); 868 | _hxfilt = (_hxfilt*((float)_coeff-1)+(getMagX_uT()/_hxs+_hxb))/((float)_coeff); 869 | _hyfilt = (_hyfilt*((float)_coeff-1)+(getMagY_uT()/_hys+_hyb))/((float)_coeff); 870 | _hzfilt = (_hzfilt*((float)_coeff-1)+(getMagZ_uT()/_hzs+_hzb))/((float)_coeff); 871 | if (_hxfilt > _hxmax) { 872 | _delta = _hxfilt - _hxmax; 873 | _hxmax = _hxfilt; 874 | } 875 | if (_delta > _framedelta) { 876 | _framedelta = _delta; 877 | } 878 | if (_hyfilt > _hymax) { 879 | _delta = _hyfilt - _hymax; 880 | _hymax = _hyfilt; 881 | } 882 | if (_delta > _framedelta) { 883 | _framedelta = _delta; 884 | } 885 | if (_hzfilt > _hzmax) { 886 | _delta = _hzfilt - _hzmax; 887 | _hzmax = _hzfilt; 888 | } 889 | if (_delta > _framedelta) { 890 | _framedelta = _delta; 891 | } 892 | if (_hxfilt < _hxmin) { 893 | _delta = abs(_hxfilt - _hxmin); 894 | _hxmin = _hxfilt; 895 | } 896 | if (_delta > _framedelta) { 897 | _framedelta = _delta; 898 | } 899 | if (_hyfilt < _hymin) { 900 | _delta = abs(_hyfilt - _hymin); 901 | _hymin = _hyfilt; 902 | } 903 | if (_delta > _framedelta) { 904 | _framedelta = _delta; 905 | } 906 | if (_hzfilt < _hzmin) { 907 | _delta = abs(_hzfilt - _hzmin); 908 | _hzmin = _hzfilt; 909 | } 910 | if (_delta > _framedelta) { 911 | _framedelta = _delta; 912 | } 913 | if (_framedelta > _deltaThresh) { 914 | _counter = 0; 915 | } else { 916 | _counter++; 917 | } 918 | delay(20); 919 | } 920 | 921 | // find the magnetometer bias 922 | _hxb = (_hxmax + _hxmin) / 2.0f; 923 | _hyb = (_hymax + _hymin) / 2.0f; 924 | _hzb = (_hzmax + _hzmin) / 2.0f; 925 | 926 | // find the magnetometer scale factor 927 | _hxs = (_hxmax - _hxmin) / 2.0f; 928 | _hys = (_hymax - _hymin) / 2.0f; 929 | _hzs = (_hzmax - _hzmin) / 2.0f; 930 | _avgs = (_hxs + _hys + _hzs) / 3.0f; 931 | _hxs = _avgs/_hxs; 932 | _hys = _avgs/_hys; 933 | _hzs = _avgs/_hzs; 934 | 935 | // set the srd back to what it was 936 | if (setSrd(_srd) < 0) { 937 | return -2; 938 | } 939 | return 1; 940 | } 941 | 942 | /* returns the magnetometer bias in the X direction, uT */ 943 | float MPU9250::getMagBiasX_uT() { 944 | return _hxb; 945 | } 946 | 947 | /* returns the magnetometer scale factor in the X direction */ 948 | float MPU9250::getMagScaleFactorX() { 949 | return _hxs; 950 | } 951 | 952 | /* returns the magnetometer bias in the Y direction, uT */ 953 | float MPU9250::getMagBiasY_uT() { 954 | return _hyb; 955 | } 956 | 957 | /* returns the magnetometer scale factor in the Y direction */ 958 | float MPU9250::getMagScaleFactorY() { 959 | return _hys; 960 | } 961 | 962 | /* returns the magnetometer bias in the Z direction, uT */ 963 | float MPU9250::getMagBiasZ_uT() { 964 | return _hzb; 965 | } 966 | 967 | /* returns the magnetometer scale factor in the Z direction */ 968 | float MPU9250::getMagScaleFactorZ() { 969 | return _hzs; 970 | } 971 | 972 | /* sets the magnetometer bias (uT) and scale factor in the X direction */ 973 | void MPU9250::setMagCalX(float bias,float scaleFactor) { 974 | _hxb = bias; 975 | _hxs = scaleFactor; 976 | } 977 | 978 | /* sets the magnetometer bias (uT) and scale factor in the Y direction */ 979 | void MPU9250::setMagCalY(float bias,float scaleFactor) { 980 | _hyb = bias; 981 | _hys = scaleFactor; 982 | } 983 | 984 | /* sets the magnetometer bias (uT) and scale factor in the Z direction */ 985 | void MPU9250::setMagCalZ(float bias,float scaleFactor) { 986 | _hzb = bias; 987 | _hzs = scaleFactor; 988 | } 989 | 990 | /* writes a byte to MPU9250 register given a register address and data */ 991 | int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ 992 | /* write data to device */ 993 | if( _useSPI ){ 994 | _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction 995 | digitalWrite(_csPin,LOW); // select the MPU9250 chip 996 | _spi->transfer(subAddress); // write the register address 997 | _spi->transfer(data); // write the data 998 | digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip 999 | _spi->endTransaction(); // end the transaction 1000 | } 1001 | else{ 1002 | _i2c->writeByte(_address, subAddress, data); 1003 | } 1004 | 1005 | delay(10); 1006 | 1007 | /* read back the register */ 1008 | readRegisters(subAddress,1,_buffer); 1009 | /* check the read back register against the written register */ 1010 | if(_buffer[0] == data) { 1011 | return 1; 1012 | } 1013 | else{ 1014 | return -1; 1015 | } 1016 | } 1017 | 1018 | /* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ 1019 | int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ 1020 | if( _useSPI ){ 1021 | // begin the transaction 1022 | if(_useSPIHS){ 1023 | _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); 1024 | } 1025 | else{ 1026 | _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); 1027 | } 1028 | digitalWrite(_csPin,LOW); // select the MPU9250 chip 1029 | _spi->transfer(subAddress | SPI_READ); // specify the starting register address 1030 | for(uint8_t i = 0; i < count; i++){ 1031 | dest[i] = _spi->transfer(0x00); // read the data 1032 | } 1033 | digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip 1034 | _spi->endTransaction(); // end the transaction 1035 | return 1; 1036 | } 1037 | else{ 1038 | 1039 | if(ESP_OK == _i2c->readBytes(_address, subAddress, count, dest)) 1040 | { 1041 | return 1; 1042 | } 1043 | else 1044 | { 1045 | return -1; 1046 | } 1047 | } 1048 | } 1049 | 1050 | /* writes a register to the AK8963 given a register address and data */ 1051 | int MPU9250::writeAK8963Register(uint8_t subAddress, uint8_t data){ 1052 | // set slave 0 to the AK8963 and set for write 1053 | if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) { 1054 | return -1; 1055 | } 1056 | // set the register to the desired AK8963 sub address 1057 | if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { 1058 | return -2; 1059 | } 1060 | // store the data for write 1061 | if (writeRegister(I2C_SLV0_DO,data) < 0) { 1062 | return -3; 1063 | } 1064 | // enable I2C and send 1 byte 1065 | if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) { 1066 | return -4; 1067 | } 1068 | // read the register and confirm 1069 | if (readAK8963Registers(subAddress,1,_buffer) < 0) { 1070 | return -5; 1071 | } 1072 | if(_buffer[0] == data) { 1073 | return 1; 1074 | } else{ 1075 | return -6; 1076 | } 1077 | } 1078 | 1079 | /* reads registers from the AK8963 */ 1080 | int MPU9250::readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){ 1081 | // set slave 0 to the AK8963 and set for read 1082 | if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) { 1083 | return -1; 1084 | } 1085 | // set the register to the desired AK8963 sub address 1086 | if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { 1087 | return -2; 1088 | } 1089 | // enable I2C and request the bytes 1090 | if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) { 1091 | return -3; 1092 | } 1093 | delay(1); // takes some time for these registers to fill 1094 | // read the bytes off the MPU9250 EXT_SENS_DATA registers 1095 | _status = readRegisters(EXT_SENS_DATA_00,count,dest); 1096 | return _status; 1097 | } 1098 | 1099 | /* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */ 1100 | int MPU9250::whoAmI(){ 1101 | // read the WHO AM I register 1102 | if (readRegisters(WHO_AM_I,1,_buffer) < 0) { 1103 | return -1; 1104 | } 1105 | // return the register value 1106 | return _buffer[0]; 1107 | } 1108 | 1109 | /* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */ 1110 | int MPU9250::whoAmIAK8963(){ 1111 | // read the WHO AM I register 1112 | if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) { 1113 | return -1; 1114 | } 1115 | // return the register value 1116 | return _buffer[0]; 1117 | } 1118 | -------------------------------------------------------------------------------- /MPU9250.h: -------------------------------------------------------------------------------- 1 | /* 2 | MPU9250.h 3 | BananaPi Bit Team 4 | juwan@banana-pi.com (Juwan·C) 5 | wanghao@banana-pi.com (Hulk Wang) 6 | 7 | We modified it based on https://github.com/bolderflight/MPU9250 8 | The main purpose is to adapt the MPU9250 driver library of BPI-BIT (espressif32). 9 | 10 | Copyright (c) 2017 BananaPi Bit Team 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 13 | and associated documentation files (the "Software"), to deal in the Software without restriction, 14 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 15 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 16 | furnished to do so, subject to the following conditions: 17 | 18 | The above copyright notice and this permission notice shall be included in all copies or 19 | substantial portions of the Software. 20 | 21 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 22 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 23 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 24 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 25 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 26 | */ 27 | 28 | #ifndef MPU9250_h 29 | #define MPU9250_h 30 | 31 | #include "Arduino.h" 32 | #include "I2Cbus.hpp" 33 | // #include "Wire.h" // I2C library 34 | #include "SPI.h" // SPI library 35 | 36 | class MPU9250{ 37 | public: 38 | enum GyroRange 39 | { 40 | GYRO_RANGE_250DPS, 41 | GYRO_RANGE_500DPS, 42 | GYRO_RANGE_1000DPS, 43 | GYRO_RANGE_2000DPS 44 | }; 45 | enum AccelRange 46 | { 47 | ACCEL_RANGE_2G, 48 | ACCEL_RANGE_4G, 49 | ACCEL_RANGE_8G, 50 | ACCEL_RANGE_16G 51 | }; 52 | enum DlpfBandwidth 53 | { 54 | DLPF_BANDWIDTH_184HZ, 55 | DLPF_BANDWIDTH_92HZ, 56 | DLPF_BANDWIDTH_41HZ, 57 | DLPF_BANDWIDTH_20HZ, 58 | DLPF_BANDWIDTH_10HZ, 59 | DLPF_BANDWIDTH_5HZ 60 | }; 61 | enum LpAccelOdr 62 | { 63 | LP_ACCEL_ODR_0_24HZ = 0, 64 | LP_ACCEL_ODR_0_49HZ = 1, 65 | LP_ACCEL_ODR_0_98HZ = 2, 66 | LP_ACCEL_ODR_1_95HZ = 3, 67 | LP_ACCEL_ODR_3_91HZ = 4, 68 | LP_ACCEL_ODR_7_81HZ = 5, 69 | LP_ACCEL_ODR_15_63HZ = 6, 70 | LP_ACCEL_ODR_31_25HZ = 7, 71 | LP_ACCEL_ODR_62_50HZ = 8, 72 | LP_ACCEL_ODR_125HZ = 9, 73 | LP_ACCEL_ODR_250HZ = 10, 74 | LP_ACCEL_ODR_500HZ = 11 75 | }; 76 | MPU9250(I2C_t &bus,uint8_t address); 77 | MPU9250(SPIClass &bus,uint8_t csPin); 78 | int begin(); 79 | int setAccelRange(AccelRange range); 80 | int setGyroRange(GyroRange range); 81 | int setDlpfBandwidth(DlpfBandwidth bandwidth); 82 | int setSrd(uint8_t srd); 83 | int enableDataReadyInterrupt(); 84 | int disableDataReadyInterrupt(); 85 | int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr); 86 | int readSensor(); 87 | float getAccelX_mss(); 88 | float getAccelY_mss(); 89 | float getAccelZ_mss(); 90 | float getGyroX_rads(); 91 | float getGyroY_rads(); 92 | float getGyroZ_rads(); 93 | float getMagX_uT(); 94 | float getMagY_uT(); 95 | float getMagZ_uT(); 96 | float getTemperature_C(); 97 | 98 | int calibrateGyro(); 99 | float getGyroBiasX_rads(); 100 | float getGyroBiasY_rads(); 101 | float getGyroBiasZ_rads(); 102 | void setGyroBiasX_rads(float bias); 103 | void setGyroBiasY_rads(float bias); 104 | void setGyroBiasZ_rads(float bias); 105 | int calibrateAccel(); 106 | float getAccelBiasX_mss(); 107 | float getAccelScaleFactorX(); 108 | float getAccelBiasY_mss(); 109 | float getAccelScaleFactorY(); 110 | float getAccelBiasZ_mss(); 111 | float getAccelScaleFactorZ(); 112 | void setAccelCalX(float bias,float scaleFactor); 113 | void setAccelCalY(float bias,float scaleFactor); 114 | void setAccelCalZ(float bias,float scaleFactor); 115 | int calibrateMag(); 116 | float getMagBiasX_uT(); 117 | float getMagScaleFactorX(); 118 | float getMagBiasY_uT(); 119 | float getMagScaleFactorY(); 120 | float getMagBiasZ_uT(); 121 | float getMagScaleFactorZ(); 122 | void setMagCalX(float bias,float scaleFactor); 123 | void setMagCalY(float bias,float scaleFactor); 124 | void setMagCalZ(float bias,float scaleFactor); 125 | protected: 126 | // i2c 127 | uint8_t _address; 128 | I2C_t *_i2c; 129 | const uint32_t _i2cRate = 400000; // 400 kHz 130 | size_t _numBytes; // number of bytes received from I2C 131 | // spi 132 | SPIClass *_spi; 133 | uint8_t _csPin; 134 | bool _useSPI; 135 | bool _useSPIHS; 136 | const uint8_t SPI_READ = 0x80; 137 | const uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz 138 | const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz 139 | // track success of interacting with sensor 140 | int _status; 141 | // buffer for reading from sensor 142 | uint8_t _buffer[21]; 143 | // data counts 144 | int16_t _axcounts,_aycounts,_azcounts; 145 | int16_t _gxcounts,_gycounts,_gzcounts; 146 | int16_t _hxcounts,_hycounts,_hzcounts; 147 | int16_t _tcounts; 148 | // data buffer 149 | float _ax, _ay, _az; 150 | float _gx, _gy, _gz; 151 | float _hx, _hy, _hz; 152 | float _t; 153 | // wake on motion 154 | uint8_t _womThreshold; 155 | // scale factors 156 | float _accelScale; 157 | float _gyroScale; 158 | float _magScaleX, _magScaleY, _magScaleZ; 159 | const float _tempScale = 333.87f; 160 | const float _tempOffset = 21.0f; 161 | // configuration 162 | AccelRange _accelRange; 163 | GyroRange _gyroRange; 164 | DlpfBandwidth _bandwidth; 165 | uint8_t _srd; 166 | // gyro bias estimation 167 | size_t _numSamples = 100; 168 | double _gxbD, _gybD, _gzbD; 169 | float _gxb, _gyb, _gzb; 170 | // accel bias and scale factor estimation 171 | double _axbD, _aybD, _azbD; 172 | float _axmax, _aymax, _azmax; 173 | float _axmin, _aymin, _azmin; 174 | float _axb, _ayb, _azb; 175 | float _axs = 1.0f; 176 | float _ays = 1.0f; 177 | float _azs = 1.0f; 178 | // magnetometer bias and scale factor estimation 179 | uint16_t _maxCounts = 100; 180 | float _deltaThresh = 0.3f; 181 | uint8_t _coeff = 8; 182 | uint16_t _counter; 183 | float _framedelta, _delta; 184 | float _hxfilt, _hyfilt, _hzfilt; 185 | float _hxmax, _hymax, _hzmax; 186 | float _hxmin, _hymin, _hzmin; 187 | float _hxb, _hyb, _hzb; 188 | float _hxs = 1.0f; 189 | float _hys = 1.0f; 190 | float _hzs = 1.0f; 191 | float _avgs; 192 | // transformation matrix 193 | /* transform the accel and gyro axes to match the magnetometer axes */ 194 | const int16_t tX[3] = {0, 1, 0}; 195 | const int16_t tY[3] = {1, 0, 0}; 196 | const int16_t tZ[3] = {0, 0, -1}; 197 | // constants 198 | const float G = 9.807f; 199 | const float _d2r = 3.14159265359f/180.0f; 200 | // MPU9250 registers 201 | const uint8_t ACCEL_OUT = 0x3B; 202 | const uint8_t GYRO_OUT = 0x43; 203 | const uint8_t TEMP_OUT = 0x41; 204 | const uint8_t EXT_SENS_DATA_00 = 0x49; 205 | const uint8_t ACCEL_CONFIG = 0x1C; 206 | const uint8_t ACCEL_FS_SEL_2G = 0x00; 207 | const uint8_t ACCEL_FS_SEL_4G = 0x08; 208 | const uint8_t ACCEL_FS_SEL_8G = 0x10; 209 | const uint8_t ACCEL_FS_SEL_16G = 0x18; 210 | const uint8_t GYRO_CONFIG = 0x1B; 211 | const uint8_t GYRO_FS_SEL_250DPS = 0x00; 212 | const uint8_t GYRO_FS_SEL_500DPS = 0x08; 213 | const uint8_t GYRO_FS_SEL_1000DPS = 0x10; 214 | const uint8_t GYRO_FS_SEL_2000DPS = 0x18; 215 | const uint8_t ACCEL_CONFIG2 = 0x1D; 216 | const uint8_t ACCEL_DLPF_184 = 0x01; 217 | const uint8_t ACCEL_DLPF_92 = 0x02; 218 | const uint8_t ACCEL_DLPF_41 = 0x03; 219 | const uint8_t ACCEL_DLPF_20 = 0x04; 220 | const uint8_t ACCEL_DLPF_10 = 0x05; 221 | const uint8_t ACCEL_DLPF_5 = 0x06; 222 | const uint8_t CONFIG = 0x1A; 223 | const uint8_t GYRO_DLPF_184 = 0x01; 224 | const uint8_t GYRO_DLPF_92 = 0x02; 225 | const uint8_t GYRO_DLPF_41 = 0x03; 226 | const uint8_t GYRO_DLPF_20 = 0x04; 227 | const uint8_t GYRO_DLPF_10 = 0x05; 228 | const uint8_t GYRO_DLPF_5 = 0x06; 229 | const uint8_t SMPDIV = 0x19; 230 | const uint8_t INT_PIN_CFG = 0x37; 231 | const uint8_t INT_ENABLE = 0x38; 232 | const uint8_t INT_DISABLE = 0x00; 233 | const uint8_t INT_PULSE_50US = 0x00; 234 | const uint8_t INT_WOM_EN = 0x40; 235 | const uint8_t INT_RAW_RDY_EN = 0x01; 236 | const uint8_t PWR_MGMNT_1 = 0x6B; 237 | const uint8_t PWR_CYCLE = 0x20; 238 | const uint8_t PWR_RESET = 0x80; 239 | const uint8_t CLOCK_SEL_PLL = 0x01; 240 | const uint8_t PWR_MGMNT_2 = 0x6C; 241 | const uint8_t SEN_ENABLE = 0x00; 242 | const uint8_t DIS_GYRO = 0x07; 243 | const uint8_t USER_CTRL = 0x6A; 244 | const uint8_t I2C_MST_EN = 0x20; 245 | const uint8_t I2C_MST_CLK = 0x0D; 246 | const uint8_t I2C_MST_CTRL = 0x24; 247 | const uint8_t I2C_SLV0_ADDR = 0x25; 248 | const uint8_t I2C_SLV0_REG = 0x26; 249 | const uint8_t I2C_SLV0_DO = 0x63; 250 | const uint8_t I2C_SLV0_CTRL = 0x27; 251 | const uint8_t I2C_SLV0_EN = 0x80; 252 | const uint8_t I2C_READ_FLAG = 0x80; 253 | const uint8_t MOT_DETECT_CTRL = 0x69; 254 | const uint8_t ACCEL_INTEL_EN = 0x80; 255 | const uint8_t ACCEL_INTEL_MODE = 0x40; 256 | const uint8_t LP_ACCEL_ODR = 0x1E; 257 | const uint8_t WOM_THR = 0x1F; 258 | const uint8_t WHO_AM_I = 0x75; 259 | const uint8_t FIFO_EN = 0x23; 260 | const uint8_t FIFO_TEMP = 0x80; 261 | const uint8_t FIFO_GYRO = 0x70; 262 | const uint8_t FIFO_ACCEL = 0x08; 263 | const uint8_t FIFO_MAG = 0x01; 264 | const uint8_t FIFO_COUNT = 0x72; 265 | const uint8_t FIFO_READ = 0x74; 266 | // AK8963 registers 267 | const uint8_t AK8963_I2C_ADDR = 0x0C; 268 | const uint8_t AK8963_HXL = 0x03; 269 | const uint8_t AK8963_CNTL1 = 0x0A; 270 | const uint8_t AK8963_PWR_DOWN = 0x00; 271 | const uint8_t AK8963_CNT_MEAS1 = 0x12; 272 | const uint8_t AK8963_CNT_MEAS2 = 0x16; 273 | const uint8_t AK8963_FUSE_ROM = 0x0F; 274 | const uint8_t AK8963_CNTL2 = 0x0B; 275 | const uint8_t AK8963_RESET = 0x01; 276 | const uint8_t AK8963_ASA = 0x10; 277 | const uint8_t AK8963_WHO_AM_I = 0x00; 278 | // private functions 279 | int writeRegister(uint8_t subAddress, uint8_t data); 280 | int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); 281 | int writeAK8963Register(uint8_t subAddress, uint8_t data); 282 | int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest); 283 | int whoAmI(); 284 | int whoAmIAK8963(); 285 | }; 286 | 287 | class MPU9250FIFO: public MPU9250 { 288 | public: 289 | using MPU9250::MPU9250; 290 | int enableFifo(bool accel,bool gyro,bool mag,bool temp); 291 | int readFifo(); 292 | void getFifoAccelX_mss(size_t *size,float* data); 293 | void getFifoAccelY_mss(size_t *size,float* data); 294 | void getFifoAccelZ_mss(size_t *size,float* data); 295 | void getFifoGyroX_rads(size_t *size,float* data); 296 | void getFifoGyroY_rads(size_t *size,float* data); 297 | void getFifoGyroZ_rads(size_t *size,float* data); 298 | void getFifoMagX_uT(size_t *size,float* data); 299 | void getFifoMagY_uT(size_t *size,float* data); 300 | void getFifoMagZ_uT(size_t *size,float* data); 301 | void getFifoTemperature_C(size_t *size,float* data); 302 | protected: 303 | // fifo 304 | bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp; 305 | size_t _fifoSize,_fifoFrameSize; 306 | float _axFifo[85], _ayFifo[85], _azFifo[85]; 307 | size_t _aSize; 308 | float _gxFifo[85], _gyFifo[85], _gzFifo[85]; 309 | size_t _gSize; 310 | float _hxFifo[73], _hyFifo[73], _hzFifo[73]; 311 | size_t _hSize; 312 | float _tFifo[256]; 313 | size_t _tSize; 314 | }; 315 | 316 | #endif 317 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MPU9250 2 | Arduino library for communicating with the [MPU-9250](https://www.invensense.com/products/motion-tracking/9-axis/mpu-9250/) and MPU-9255 nine-axis Inertial Measurement Units (IMU). 3 | 4 | # Description 5 | The InvenSense MPU-9250 is a System in Package (SiP) that combines two chips: the MPU-6500 three-axis gyroscope and three-axis accelerometer; and the AK8963 three-axis magnetometer. The MPU-9250 supports I2C, up to 400 kHz, and SPI communication, up to 1 MHz for register setup and 20 MHz for data reading. The following selectable full scale sensor ranges are available: 6 | 7 | | Gyroscope Full Scale Range | Accelerometer Full Scale Range | Magnetometer Full Scale Range | 8 | | --- | --- | --- | 9 | | +/- 250 (deg/s) | +/- 2 (g) | +/- 4800 (uT) | 10 | | +/- 500 (deg/s) | +/- 4 (g) | | 11 | | +/- 1000 (deg/s) | +/- 8 (g) | | 12 | | +/- 2000 (deg/s) | +/- 16 (g) | | 13 | 14 | The MPU-9250 samples the gyroscopes, accelerometers, and magnetometers with 16 bit analog to digital converters. It also features programmable digital filters, a precision clock, an embedded temperature sensor, programmable interrupts (including wake on motion), and a 512 byte FIFO buffer. 15 | 16 | # Usage 17 | This library supports both I2C and SPI commmunication with the MPU-9250. 18 | 19 | ## Installation 20 | Simply clone or download this library into your Arduino/libraries folder. 21 | 22 | ## Function Description 23 | This library supports both I2C and SPI communication with the MPU-9250. The *MPU9250* object declaration is overloaded with different declarations for I2C and SPI communication. All other functions remain the same. Additionally, a derived class, *MPU250FIFO*, is included, which provides FIFO setup and data collection functionality in addition to all of the functionality included in the base *MPU9250* class. 24 | 25 | ## MPU9250 Class 26 | 27 | ### I2C Object Declaration 28 | 29 | **MPU9250(TwoWire &bus,uint8_t address)** 30 | An MPU9250 object should be declared, specifying the I2C bus and MPU-9250 I2C address. The MPU-9250 I2C address will be 0x68 if the AD0 pin is grounded or 0x69 if the AD0 pin is pulled high. For example, the following code declares an MPU9250 object called *IMU* with an MPU-9250 sensor located on I2C bus 0 with a sensor address of 0x68 (AD0 grounded). 31 | 32 | ```C++ 33 | MPU9250 IMU(Wire,0x68); 34 | ``` 35 | 36 | ### SPI Object Declaratioon 37 | 38 | **MPU9250(SPIClass &bus,uint8_t csPin)** 39 | An MPU9250 object should be declared, specifying the SPI bus and chip select pin used. Multiple MPU-9250 or other SPI objects could be used on the same SPI bus, each with their own chip select pin. The chip select pin can be any available digital pin. For example, the following code declares an MPU9250 object called *IMU* with an MPU-9250 sensor located on SPI bus 0 with chip select pin 10. 40 | 41 | ```C++ 42 | MPU9250 IMU(SPI,10); 43 | ``` 44 | 45 | ### Common Setup Functions 46 | The following functions are used to setup the MPU-9250 sensor. These should be called once before data collection, typically this is done in the Arduino *void setup()* function. The *begin* function should always be used. Optionally, the *setAccelRange* and *setGyroRange*, *setDlpfBandwidth*, and *setSrd* functions can be used to set the accelerometer and gyroscope full scale ranges, DLPF bandwidth, and SRD to values other than default. The *enableDataReadyInterrupt* and *disableDataReadyInterrupt* control whether the MPU-9250 generates an interrupt on data ready. The *enableWakeOnMotion* puts the MPU-9250 into a low power mode and enables an interrupt when motion detected is above a given threshold. Finally, *enableFifo* sets up and enables the FIFO buffer. These functions are described in detail, below. 47 | 48 | **int begin()** 49 | This should be called in your setup function. It initializes communication with the MPU-9250, sets up the sensor for reading data, and estimates the gyro bias, which is removed from the sensor data. This function returns a positive value on a successful initialization and returns a negative value on an unsuccesful initialization. If unsuccessful, please check your wiring or try resetting power to the sensor. The following is an example of setting up the MPU-9250. 50 | 51 | ```C++ 52 | int status; 53 | status = IMU.begin(); 54 | ``` 55 | 56 | #### Configuration Functions 57 | 58 | **(optional) int setAccelRange(AccelRange range)** 59 | This function sets the accelerometer full scale range to the given value. By default, if this function is not called, a full scale range of +/- 16 g will be used. The enumerated accelerometer full scale ranges are: 60 | 61 | | Accelerometer Name | Accelerometer Full Scale Range | 62 | | ------------------ | ------------------------------ | 63 | | ACCEL_RANGE_2G | +/- 2 (g) | 64 | | ACCEL_RANGE_4G | +/- 4 (g) | 65 | | ACCEL_RANGE_8G | +/- 8 (g) | 66 | | ACCEL_RANGE_16G | +/- 16 (g) | 67 | 68 | This function returns a positive value on success and a negative value on failure. Please see the *Advanced_I2C example*. The following is an example of selecting an accelerometer full scale range of +/- 8g. 69 | 70 | ```C++ 71 | status = IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G); 72 | ``` 73 | 74 | **(optional) int setGyroRange(GyroRange range)** 75 | This function sets the gyroscope full scale range to the given value. By default, if this function is not called, a full scale range of +/- 2000 deg/s will be used. The enumerated gyroscope full scale ranges are: 76 | 77 | | Gyroscope Name | Gyroscope Full Scale Range | 78 | | ------------------ | -------------------------- | 79 | | GYRO_RANGE_250DPS | +/- 250 (deg/s) | 80 | | GYRO_RANGE_500DPS | +/- 500 (deg/s) | 81 | | GYRO_RANGE_1000DPS | +/- 1000 (deg/s) | 82 | | GYRO_RANGE_2000DPS | +/- 2000 (deg/s) | 83 | 84 | This function returns a positive value on success and a negative value on failure. Please see the *Advanced_I2C example*. The following is an example of selecting an gyroscope full scale range of +/- 250 deg/s. 85 | 86 | ```C++ 87 | status = IMU.setGyroRange(MPU9250::GYRO_RANGE_250DPS); 88 | ``` 89 | 90 | **(optional) int setDlpfBandwidth(DlpfBandwidth bandwidth)** 91 | This is an optional function to set the programmable Digital Low Pass Filter (DLPF) bandwidth. By default, if this function is not called, a DLPF bandwidth of 184 Hz is used. The following DLPF bandwidths are supported: 92 | 93 | | Bandwidth Name | DLPF Bandwidth | Gyroscope Delay | Accelerometer Delay | Temperature Bandwidth | Temperature Delay | 94 | | --- | --- | --- | --- | --- | --- | 95 | | DLPF_BANDWIDTH_184HZ | 184 Hz | 2.9 ms | 5.8 ms | 188 Hz | 1.9 ms | 96 | | DLPF_BANDWIDTH_92HZ | 92 Hz | 3.9 ms | 7.8 ms | 98 Hz | 2.8 ms | 97 | | DLPF_BANDWIDTH_41HZ | 41 Hz | 5.9 ms | 11.8 ms | 42 Hz | 4.8 ms | 98 | | DLPF_BANDWIDTH_20HZ | 20 Hz | 9.9 ms | 19.8 ms | 20 Hz | 8.3 ms | 99 | | DLPF_BANDWIDTH_10HZ | 10 Hz | 17.85 ms | 35.7 ms | 10 Hz | 13.4 ms | 100 | | DLPF_BANDWIDTH_5HZ | 5 Hz | 33.48 ms | 66.96 ms | 5 Hz | 18.6 ms | 101 | 102 | This function returns a positive value on success and a negative value on failure. Please see the *Advanced_I2C example*. The following is an example of selecting a DLPF bandwidth of 20 Hz. 103 | 104 | ```C++ 105 | status = IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ); 106 | ``` 107 | 108 | **(optional) int setSrd(uint8_t srd)** 109 | This is an optional function to set the data output rate. The data output rate is set by a sample rate divider, *uint8_t SRD*. The data output rate is then given by: 110 | 111 | *Data Output Rate = 1000 / (1 + SRD)* 112 | 113 | By default, if this function is not called, an SRD of 0 is used resulting in a data output rate of 1000 Hz. This allows the data output rate for the gyroscopes, accelerometers, and temperature sensor to be set between 3.9 Hz and 1000 Hz. Note that data should be read at or above the selected rate. In order to prevent aliasing, the data should be sampled at twice the frequency of the DLPF bandwidth or higher. For example, this means for a DLPF bandwidth set to 41 Hz, the data output rate and data collection should be at frequencies of 82 Hz or higher. 114 | 115 | The magnetometer is fixed to an output rate of: 116 | * 100 Hz for frequencies of 100 Hz or above (SRD less than or equal to 9) 117 | * 8 Hz for frequencies below 100 Hz (SRD greater than 9) 118 | 119 | When the data is read above the selected output rate, the read data will be stagnant. For example, when the output rate is selected to 1000 Hz, the magnetometer data will be the same for 10 sequential frames. 120 | 121 | This function returns a positive value on success and a negative value on failure. Please see the *Advanced_I2C example*. The following is an example of selecting an SRD of 9, resulting in a data output rate of 100 Hz. 122 | 123 | ```C++ 124 | status = IMU.setSrd(9); 125 | ``` 126 | 127 | **(optional) int enableDataReadyInterrupt()** 128 | An interrupt is tied to the data output rate. The MPU-9250 *INT* pin will issue a 50us pulse when data is ready. This is extremely useful for using interrupts to clock data collection that should occur at a regular interval. Please see the *Interrupt_SPI example*. This function enables this interrupt, which will occur at a frequency given by the SRD. This function returns a positive value on success and a negative value on failure. The following is an example of enabling the data ready interrupt. 129 | 130 | ```C++ 131 | status = IMU.enableDataReadyInterrupt(); 132 | ``` 133 | 134 | **(optional) int disableDataReadyInterrupt()** 135 | This function disables the data ready interrupt, described above. This function returns a positive value on success and a negative value on failure. The following is an example of disabling the data ready interrupt. 136 | 137 | ```C++ 138 | status = IMU.disableDataReadyInterrupt(); 139 | ``` 140 | 141 | #### Calibration Functions 142 | 143 | **(optional) int calibrateGyro()** 144 | The gyro bias is automatically estimated during the *begin()* function and removed from sensor measurements. This function will re-estimate the gyro bias and remove the new bias from future sensor measurements. The sensor should be stationary during this process. This function returns a positive value on success and a negative value on failure. The following is an example of estimating new gyro biases. 145 | 146 | ```C++ 147 | status = IMU.calibrateGyro(); 148 | ``` 149 | 150 | **(optional) float getGyroBiasX_rads()** 151 | This function returns the current gyro bias in the X direction in units of rad/s. 152 | 153 | ```C++ 154 | float gxb; 155 | gxb = IMU.getGyroBiasX_rads(); 156 | ``` 157 | 158 | **(optional) float getGyroBiasY_rads()** 159 | This function returns the current gyro bias in the Y direction in units of rad/s. 160 | 161 | ```C++ 162 | float gyb; 163 | gyb = IMU.getGyroBiasY_rads(); 164 | ``` 165 | 166 | **(optional) float getGyroBiasZ_rads()** 167 | This function returns the current gyro bias in the Z direction in units of rad/s. 168 | 169 | ```C++ 170 | float gzb; 171 | gzb = IMU.getGyroBiasZ_rads(); 172 | ``` 173 | 174 | **(optional) void setGyroBiasX_rads(float bias)** 175 | This function sets the gyro bias being used in the X direction to the input value in units of rad/s. 176 | 177 | ```C++ 178 | float gxb = 0.001; // gyro bias of 0.001 rad/s 179 | IMU.setGyroBiasX_rads(gxb); 180 | ``` 181 | 182 | **(optional) void setGyroBiasY_rads(float bias)** 183 | This function sets the gyro bias being used in the Y direction to the input value in units of rad/s. 184 | 185 | ```C++ 186 | float gyb = 0.001; // gyro bias of 0.001 rad/s 187 | IMU.setGyroBiasY_rads(gyb); 188 | ``` 189 | 190 | **(optional) void setGyroBiasZ_rads(float bias)** 191 | This function sets the gyro bias being used in the Z direction to the input value in units of rad/s. 192 | 193 | ```C++ 194 | float gzb = 0.001; // gyro bias of 0.001 rad/s 195 | IMU.setGyroBiasZ_rads(gzb); 196 | ``` 197 | 198 | **(optional) int calibrateAccel()** 199 | This function will estimate the bias and scale factor needed to calibrate the accelerometers. This function works one axis at a time and needs to be run for all 6 sensor orientations. After it has collected enough sensor data, it will estimate the bias and scale factor for all three accelerometer channels and apply these corrections to the measured data. Accelerometer calibration only needs to be performed once on the IMU, the get and set functions detailed below can be used to retrieve the estimated bias and scale factors and use them during future power cycles or operations with the IMU. This function returns a positive value on success and a negative value on failure. 200 | 201 | ```C++ 202 | status = IMU.calibrateAccel(); 203 | ``` 204 | 205 | **(optional) float getAccelBiasX_mss()** 206 | This function returns the current accelerometer bias in the X direction in units of m/s/s. 207 | 208 | ```C++ 209 | float axb; 210 | axb = IMU.getAccelBiasX_mss(); 211 | ``` 212 | 213 | **(optional) float getAccelScaleFactorX()** 214 | This function returns the current accelerometer scale factor in the X direction. 215 | 216 | ```C++ 217 | float axs; 218 | axs = IMU.getAccelScaleFactorX(); 219 | ``` 220 | 221 | **(optional) float getAccelBiasY_mss()** 222 | This function returns the current accelerometer bias in the Y direction in units of m/s/s. 223 | 224 | ```C++ 225 | float ayb; 226 | ayb = IMU.getAccelBiasY_mss(); 227 | ``` 228 | 229 | **(optional) float getAccelScaleFactorY()** 230 | This function returns the current accelerometer scale factor in the Y direction. 231 | 232 | ```C++ 233 | float ays; 234 | ays = IMU.getAccelScaleFactorY(); 235 | ``` 236 | 237 | **(optional) float getAccelBiasZ_mss()** 238 | This function returns the current accelerometer bias in the Z direction in units of m/s/s. 239 | 240 | ```C++ 241 | float azb; 242 | azb = IMU.getAccelBiasZ_mss(); 243 | ``` 244 | 245 | **(optional) float getAccelScaleFactorZ()** 246 | This function returns the current accelerometer scale factor in the Z direction. 247 | 248 | ```C++ 249 | float azs; 250 | azs = IMU.getAccelScaleFactorZ(); 251 | ``` 252 | 253 | **(optional) void setAccelCalX(float bias,float scaleFactor)** 254 | This function sets the accelerometer bias (m/s/s) and scale factor being used in the X direction to the input values. 255 | 256 | ```C++ 257 | float axb = 0.01; // accel bias of 0.01 m/s/s 258 | float axs = 0.97; // accel scale factor of 0.97 259 | IMU.setAccelCalX(axb,axs); 260 | ``` 261 | 262 | **(optional) void setAccelCalY(float bias,float scaleFactor)** 263 | This function sets the accelerometer bias (m/s/s) and scale factor being used in the Y direction to the input values. 264 | 265 | ```C++ 266 | float ayb = 0.01; // accel bias of 0.01 m/s/s 267 | float ays = 0.97; // accel scale factor of 0.97 268 | IMU.setAccelCalY(ayb,ays); 269 | ``` 270 | 271 | **(optional) void setAccelCalZ(float bias,float scaleFactor)** 272 | This function sets the accelerometer bias (m/s/s) and scale factor being used in the Z direction to the input values. 273 | 274 | ```C++ 275 | float azb = 0.01; // accel bias of 0.01 m/s/s 276 | float azs = 0.97; // accel scale factor of 0.97 277 | IMU.setAccelCalZ(azb,azs); 278 | ``` 279 | 280 | **(optional) int calibrateMag()** 281 | This function will estimate the bias and scale factor needed to calibrate the magnetometers. This function works on all the sensor axes at once, you should continuously and slowly move the sensor in a figure 8 while the function is running. After it has collected enough sensor data, it will estimate the bias and scale factor for all three magnetometer channels and apply these corrections to the measured data. Magnetometer calibration only needs to be performed once on the IMU, unless the eletrical or magnetic environment changes. The get and set functions detailed below can be used to retrieve the estimated bias and scale factors and use them during future power cycles or operations with the IMU. This function returns a positive value on success and a negative value on failure. 282 | 283 | ```C++ 284 | status = IMU.calibrateMag(); 285 | ``` 286 | 287 | **(optional) float getMagBiasX_uT()** 288 | This function returns the current magnetometer bias in the X direction in units of uT. 289 | 290 | ```C++ 291 | float hxb; 292 | hxb = IMU.getMagBiasX_uT(); 293 | ``` 294 | 295 | **(optional) float getMagScaleFactorX()** 296 | This function returns the current magnetometer scale factor in the X direction. 297 | 298 | ```C++ 299 | float hxs; 300 | hxs = IMU.getMagScaleFactorX(); 301 | ``` 302 | 303 | **(optional) float getMagBiasY_uT()** 304 | This function returns the current magnetometer bias in the Y direction in units of uT. 305 | 306 | ```C++ 307 | float hyb; 308 | hyb = IMU.getMagBiasY_uT(); 309 | ``` 310 | 311 | **(optional) float getMagScaleFactorY()** 312 | This function returns the current magnetometer scale factor in the Y direction. 313 | 314 | ```C++ 315 | float hys; 316 | hys = IMU.getMagScaleFactorY(); 317 | ``` 318 | 319 | **(optional) float getMagBiasZ_uT()** 320 | This function returns the current magnetometer bias in the Z direction in units of uT. 321 | 322 | ```C++ 323 | float hzb; 324 | hzb = IMU.getMagBiasZ_uT(); 325 | ``` 326 | 327 | **(optional) float getMagScaleFactorZ()** 328 | This function returns the current magnetometer scale factor in the Z direction. 329 | 330 | ```C++ 331 | float hzs; 332 | hzs = IMU.getMagScaleFactorZ(); 333 | ``` 334 | 335 | **(optional) void setMagCalX(float bias,float scaleFactor)** 336 | This function sets the magnetometer bias (uT) and scale factor being used in the X direction to the input values. 337 | 338 | ```C++ 339 | float hxb = 10.0; // mag bias of 10 uT 340 | float hxs = 0.97; // mag scale factor of 0.97 341 | IMU.setMagCalX(hxb,hxs); 342 | ``` 343 | 344 | **(optional) void setMagCalY(float bias,float scaleFactor)** 345 | This function sets the magnetometer bias (uT) and scale factor being used in the Y direction to the input values. 346 | 347 | ```C++ 348 | float hyb = 10.0; // mag bias of 10 uT 349 | float hys = 0.97; // mag scale factor of 0.97 350 | IMU.setMagCalY(hyb,hys); 351 | ``` 352 | 353 | **(optional) void setMagCalZ(float bias,float scaleFactor)** 354 | This function sets the magnetometer bias (uT) and scale factor being used in the Z direction to the input values. 355 | 356 | ```C++ 357 | float hzb = 10.0; // mag bias of 10 uT 358 | float hzs = 0.97; // mag scale factor of 0.97 359 | IMU.setMagCalZ(hzb,hzs); 360 | ``` 361 | 362 | #### Wake on Motion Setup 363 | 364 | **(optional) int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr)** 365 | This function enables the MPU-9250 wake on motion interrupt functionality. It places the MPU-9250 into a low power state, with the MPU-9250 waking up at an interval determined by the Low Power Accelerometer Output Data Rate. If the accelerometer detects motion in excess of the threshold given, it generates a 50us pulse from the MPU-9250 INT pin. The following enumerated Low Power Accelerometer Output Data Rates are supported: 366 | 367 | | LpAccelOdr Name | Output Data Rate | 368 | | ------------------ | ---------------- | 369 | | LP_ACCEL_ODR_0_24HZ | 0.24 Hz | 370 | | LP_ACCEL_ODR_0_49HZ | 0.49 Hz | 371 | | LP_ACCEL_ODR_0_98HZ | 0.98 Hz | 372 | | LP_ACCEL_ODR_1_95HZ | 1.95 Hz | 373 | | LP_ACCEL_ODR_3_91HZ | 3.91 Hz | 374 | | LP_ACCEL_ODR_7_81HZ | 7.81 Hz | 375 | | LP_ACCEL_ODR_15_63HZ | 15.63 Hz | 376 | | LP_ACCEL_ODR_31_25HZ | 31.25 Hz | 377 | | LP_ACCEL_ODR_62_50HZ | 62.50 Hz | 378 | | LP_ACCEL_ODR_125HZ | 125 Hz | 379 | | LP_ACCEL_ODR_250HZ | 250 Hz | 380 | | LP_ACCEL_ODR_500HZ | 500 Hz | 381 | 382 | The motion threshold is given as a float value between 0 and 1020 mg mapped, which is internally mapped to a single byte, 0-255 value. This function returns a positive value on success and a negative value on failure. Please see the *WOM_I2C example*. The following is an example of enabling the wake on motion with a 400 mg threshold and a ODR of 31.25 Hz. 383 | 384 | ```C++ 385 | status = IMU.enableWakeOnMotion(400,MPU9250::LP_ACCEL_ODR_31_25HZ); 386 | ``` 387 | 388 | ### Common Data Collection Functions 389 | The functions below are used to collect data from the MPU-9250 sensor. Data is returned scaled to engineering units and transformed to a [common axis system](#sensor-orientation). 390 | 391 | #### Real-Time Data Collection 392 | **int readSensor()** reads the sensor and stores the newest data in a buffer, it should be called every time you would like to retrieve data from the sensor. This function returns a positive value on success and a negative value on failure. 393 | 394 | ```C++ 395 | IMU.readSensor(); 396 | ``` 397 | 398 | **float getAccelX_mss()** gets the accelerometer value from the data buffer in the X direction and returns it in units of m/s/s. 399 | 400 | ```C++ 401 | float ax; 402 | ax = IMU.getAccelX_mss(); 403 | ``` 404 | 405 | **float getAccelY_mss()** gets the accelerometer value from the data buffer in the Y direction and returns it in units of m/s/s. 406 | 407 | ```C++ 408 | float ay; 409 | ay = IMU.getAccelY_mss(); 410 | ``` 411 | 412 | **float getAccelZ_mss()** gets the accelerometer value from the data buffer in the Z direction and returns it in units of m/s/s. 413 | 414 | ```C++ 415 | float az; 416 | az = IMU.getAccelZ_mss(); 417 | ``` 418 | 419 | **float getGyroX_rads()** gets the gyroscope value from the data buffer in the X direction and returns it in units of rad/s. 420 | 421 | ```C++ 422 | float gx; 423 | gx = IMU.getGyroX_rads(); 424 | ``` 425 | 426 | **float getGyroY_rads()** gets the gyroscope value from the data buffer in the Y direction and returns it in units of rad/s. 427 | 428 | ```C++ 429 | float gy; 430 | gy = IMU.getGyroY_rads(); 431 | ``` 432 | 433 | **float getGyroZ_rads()** gets the gyroscope value from the data buffer in the Z direction and returns it in units of rad/s. 434 | 435 | ```C++ 436 | float gz; 437 | gz = IMU.getGyroZ_rads(); 438 | ``` 439 | 440 | **float getMagX_uT()** gets the magnetometer value from the data buffer in the X direction and returns it in units of uT. 441 | 442 | ```C++ 443 | float hx; 444 | hx = IMU.getMagX_uT(); 445 | ``` 446 | 447 | **float getMagY_uT()** gets the magnetometer value from the data buffer in the Y direction and returns it in units of uT. 448 | 449 | ```C++ 450 | float hy; 451 | hy = IMU.getMagY_uT(); 452 | ``` 453 | 454 | **float getMagZ_uT()** gets the magnetometer value from the data buffer in the Z direction and returns it in units of uT. 455 | 456 | ```C++ 457 | float hz; 458 | hz = IMU.getMagZ_uT(); 459 | ``` 460 | 461 | **float getTemperature_C()** gets the die temperature value from the data buffer and returns it in units of C. 462 | 463 | ```C++ 464 | float temperature; 465 | temperature = IMU.getTemperature_C(); 466 | ``` 467 | 468 | ## MPU9250FIFO Class 469 | The *MPU9250FIFO* derived class extends the functionality provided by the *MPU9250* base class by providing support for setting up and reading the MPU-9250 FIFO buffer. All of the functions described above, as part of the *MPU9250* class are also available to the *MPU9250FIFO* class. 470 | 471 | ### I2C Object Declaration 472 | 473 | **MPU9250FIFO(TwoWire &bus,uint8_t address)** 474 | An MPU9250FIFO object should be declared, specifying the I2C bus and MPU-9250 I2C address. The MPU-9250 I2C address will be 0x68 if the AD0 pin is grounded or 0x69 if the AD0 pin is pulled high. For example, the following code declares an MPU9250FIFO object called *IMU* with an MPU-9250 sensor located on I2C bus 0 with a sensor address of 0x68 (AD0 grounded). 475 | 476 | ```C++ 477 | MPU9250FIFO IMU(Wire,0x68); 478 | ``` 479 | 480 | ### SPI Object Declaratioon 481 | 482 | **MPU9250FIFO(SPIClass &bus,uint8_t csPin)** 483 | An MPU9250FIFO object should be declared, specifying the SPI bus and chip select pin used. Multiple MPU-9250 or other SPI objects could be used on the same SPI bus, each with their own chip select pin. The chip select pin can be any available digital pin. For example, the following code declares an MPU9250FIFO object called *IMU* with an MPU-9250 sensor located on SPI bus 0 with chip select pin 10. 484 | 485 | ```C++ 486 | MPU9250FIFO IMU(SPI,10); 487 | ``` 488 | 489 | ### FIFO Setup 490 | **(optional) int enableFifo(bool accel,bool gyro,bool mag,bool temp)** 491 | This function configures and enables the MPU-9250 FIFO buffer. This 512 byte buffer samples data at the data output rate set by the SRD and enables the microcontroller to bulk read the data, reducing microcontroller workload for certain applications. It is configured with a set of boolean values describing which data to buffer in the FIFO: accelerometer, gyroscope, magnetometer, or temperature. The accelerometer and gyroscope data each take 6 bytes of space per sample while the magnetometer takes 7 bytes of space and the temperature 2 bytes. It's important to select only the data sources desired to ensure that the FIFO does not overrun between reading it. For example, enabling all of the data sources would take 21 bytes per sample allowing the FIFO to hold only 24 samples before overflowing. If only the accelerometer data is needed, this increases to 85 samples before overflowing. This function returns a positive value on success and a negative value on failure. Please see the *FIFO_SPI example*. The following is an example of enabling the FIFO to buffer accelerometer and gyroscope data. 492 | 493 | ```C++ 494 | status = IMU.enableFifo(true,true,false,false); 495 | ``` 496 | 497 | ### FIFO Data Collection 498 | **int readFifo()** reads the FIFO buffer from the MPU-9250, parses it and stores the data in buffers on the microcontroller. It should be called every time you would like to retrieve data from the FIFO buffer. This function returns a positive value on success and a negative value on failure. 499 | 500 | ```C++ 501 | IMU.readFifo(); 502 | ``` 503 | 504 | **void getFifoAccelX_mss(size_t *size,float* data)** gets the accelerometer value from the data buffer in the X direction and returns it in units of m/s/s. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 505 | 506 | ```C++ 507 | float ax[100]; 508 | size_t samples; 509 | IMU.getFifoAccelX_mss(&samples,ax); 510 | ``` 511 | 512 | **void getFifoAccelY_mss(size_t *size,float* data)** gets the accelerometer value from the data buffer in the Y direction and returns it in units of m/s/s. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 513 | 514 | ```C++ 515 | float ay[100]; 516 | size_t samples; 517 | IMU.getFifoAccelY_mss(&samples,ay); 518 | ``` 519 | 520 | **void getFifoAccelZ_mss(size_t *size,float* data)** gets the accelerometer value from the data buffer in the Z direction and returns it in units of m/s/s. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 521 | 522 | ```C++ 523 | float az[100]; 524 | size_t samples; 525 | IMU.getFifoAccelZ_mss(&samples,az); 526 | ``` 527 | 528 | **void getFifoGyroX_rads(size_t *size,float* data)** gets the gyroscope value from the data buffer in the X direction and returns it in units of rad/s. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 529 | 530 | ```C++ 531 | float gx[100]; 532 | size_t samples; 533 | IMU.getFifoGyroX_rads(&samples,gx); 534 | ``` 535 | 536 | **void getFifoGyroY_rads(size_t *size,float* data)** gets the gyroscope value from the data buffer in the Y direction and returns it in units of rad/s. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 537 | 538 | ```C++ 539 | float gy[100]; 540 | size_t samples; 541 | IMU.getFifoGyroY_rads(&samples,gy); 542 | ``` 543 | 544 | **void getFifoGyroZ_rads(size_t *size,float* data)** gets the gyroscope value from the data buffer in the Z direction and returns it in units of rad/s. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 545 | 546 | ```C++ 547 | float gz[100]; 548 | size_t samples; 549 | IMU.getFifoGyroZ_rads(&samples,gx); 550 | ``` 551 | 552 | **void getFifoMagX_uT(size_t *size,float* data)** gets the magnetometer value from the data buffer in the X direction and returns it in units of uT. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 553 | 554 | ```C++ 555 | float hx[100]; 556 | size_t samples; 557 | IMU.getFifoMagX_uT(&samples,hx); 558 | ``` 559 | 560 | **void getFifoMagY_uT(size_t *size,float* data)** gets the magnetometer value from the data buffer in the Y direction and returns it in units of uT. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 561 | 562 | ```C++ 563 | float hy[100]; 564 | size_t samples; 565 | IMU.getFifoMagY_uT(&samples,hy); 566 | ``` 567 | 568 | **void getFifoMagZ_uT(size_t *size,float* data)** gets the magnetometer value from the data buffer in the Z direction and returns it in units of uT. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 569 | 570 | ```C++ 571 | float hz[100]; 572 | size_t samples; 573 | IMU.getFifoMagZ_uT(&samples,hz); 574 | ``` 575 | 576 | **void getFifoTemperature_C(size_t *size,float* data)** gets the die temperature value from the data buffer and returns it in units of C. The data is returned as an array along with the number of elements within that array. Ensure that the buffer you are transfering to has enough capacity to store the data. 577 | 578 | ```C++ 579 | float temp[100]; 580 | size_t samples; 581 | IMU.getFifoTemperature_C(&samples,temp); 582 | ``` 583 | 584 | ## Sensor Orientation 585 | This library transforms all data to a common axis system before it is returned. This axis system is shown below. It is a right handed coordinate system with the z-axis positive down, common in aircraft dynamics. 586 | 587 | Common Axis System 588 | 589 | **Caution!** This axis system is shown relative to the MPU-9250 sensor. The sensor may be rotated relative to the breakout board. 590 | 591 | ## Example List 592 | 593 | * **Basic_I2C**: demonstrates declaring an *MPU9250* object, initializing the sensor, and collecting data. I2C is used to communicate with the MPU-9250 sensor. 594 | * **Basic_SPI**: demonstrates declaring an *MPU9250* object, initializing the sensor, and collecting data. SPI is used to communicate with the MPU-9250 sensor. 595 | * **Advanced_I2C**: demonstrates a more advanced setup. In this case, the accelerometer and gyroscope full scale ranges, DLPF, and SRD are set to non-default values. I2C is used to communicate with the MPU-9250 sensor. 596 | * **Interrupt_SPI**: demonstrates having the MPU-9250 sensor create an interrupt pulse when data is ready, which is used to drive data collection at the specified rate. SPI is used to communicate with the MPU-9250 sensor. 597 | * **WOM_I2C**: demonstrates setting up and using the wake on motion interrupt. I2C is used to communicate with the MPU-9250 sensor. 598 | * **FIFO_SPI**: demonstrates setting up and using the FIFO buffer. SPI is used to communicate with the MPU-9250 sensor. 599 | 600 | # Wiring and Pullups 601 | 602 | Please refer to the [MPU-9250 datasheet](https://github.com/BPI-STEAM/MPU9250/blob/master/docs/MPU-9250-Datasheet.pdf) and your microcontroller's pinout diagram. This library was developed using the [Embedded Masters breakout board](https://store.invensense.com/Controls/www.embeddedmasters.com/ProductDetail/EMSENSRMPU9250-Embedded-Masters/552444/) v1.1 for the MPU-9250. The data sheet for this breakout board is located [here](https://github.com/BPI-STEAM/MPU9250/blob/master/docs/Embedded-Masters-MPU-9250-Breakout.pdf). This library should work well for other breakout boards or embedded sensors, please refer to your vendor's pinout diagram. 603 | 604 | ## I2C 605 | 606 | The MPU-9250 pins should be connected as: 607 | * VDD: this should be a 2.4V to 3.6V power source. 608 | * GND: ground. 609 | * VDDI: digital I/O supply voltage. This should be between 1.71V and VDD. 610 | * FSYNC: not used, should be grounded. 611 | * INT: (optional) used for the interrupt output setup in *enableDataReadyInterrupt* and *enableWakeOnMotion*. Connect to interruptable pin on microcontroller. 612 | * SDA / SDI: connect to SDA. 613 | * SCL / SCLK: connect to SCL. 614 | * AD0 / SDO: ground to select I2C address 0x68. Pull high to VDD to select I2C address 0x69. 615 | * nCS: no connect. 616 | * AUXDA: not used. 617 | * AUXCL: not used. 618 | 619 | 4.7 kOhm resistors should be used as pullups on SDA and SCL, these resistors should pullup with a 3.3V source. 620 | 621 | ## SPI 622 | 623 | The MPU-9250 pins should be connected as: 624 | * VDD: this should be a 2.4V to 3.6V power source. 625 | * GND: ground. 626 | * VDDI: digital I/O supply voltage. This should be between 1.71V and VDD. 627 | * FSYNC: not used, should be grounded. 628 | * INT: (optional) used for the interrupt output setup in *enableDataReadyInterrupt* and *enableWakeOnMotion*. Connect to interruptable pin on microcontroller. 629 | * SDA / SDI: connect to MOSI. 630 | * SCL / SCLK: connect to SCK. 631 | * AD0 / SDO: connect to MISO. 632 | * nCS: connect to chip select pin. Pin 10 was used in the code snippets in this document and the included examples, but any digital I/O pin can be used. 633 | * AUXDA: not used. 634 | * AUXCL: not used. 635 | 636 | Some breakout boards, including the Embedded Masters breakout board, require slight modification to enable SPI. Please refer to your vendor's documentation. 637 | -------------------------------------------------------------------------------- /examples/compass/compass.ino: -------------------------------------------------------------------------------- 1 | 2 | #include "MPU9250.h" 3 | 4 | MPU9250 IMU(i2c0, 0x68); 5 | int status; 6 | 7 | void setup() 8 | { 9 | // serial to display data 10 | Serial.begin(9600); 11 | while (!Serial) 12 | { 13 | } 14 | // start communication with IMU 15 | status = IMU.begin(); 16 | if (status < 0) 17 | { 18 | Serial.println("IMU initialization unsuccessful"); 19 | Serial.println("Check IMU wiring or try cycling power"); 20 | Serial.print("Status: "); 21 | Serial.println(status); 22 | while (1) 23 | { 24 | } 25 | } 26 | // setting the accelerometer full scale range to +/-8G 27 | IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G); 28 | // setting the gyroscope full scale range to +/-500 deg/s 29 | IMU.setGyroRange(MPU9250::GYRO_RANGE_250DPS); 30 | // setting DLPF bandwidth to 20 Hz 31 | IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_41HZ); 32 | // setting SRD to 19 for a 50 Hz update rate 33 | // IMU.setSrd(19); 34 | IMU.calibrateMag(); 35 | } 36 | 37 | #define RAD_TO_DEG 57.295779513082320876798154814105 38 | 39 | void loop() 40 | { 41 | // read the sensor 42 | IMU.readSensor(); 43 | 44 | // display the data Accel(%.6lf, %.6lf, %.6lf) Gyro(%.6lf, %.6lf, %.6lf) 45 | // printf("\r\n Mag(%.6f, %.6f, %.6f) Yaw %.6f ", 46 | // // IMU.getAccelX_mss(), IMU.getAccelX_mss(), IMU.getAccelY_mss(), IMU.getAccelZ_mss(), IMU.getGyroX_rads(), IMU.getGyroY_rads(), IMU.getGyroZ_rads(), 47 | // IMU.getMagX_uT(), IMU.getMagY_uT(), IMU.getMagZ_uT(), float(atan2(IMU.getMagY_uT(), IMU.getMagX_uT())) * RAD_TO_DEG 48 | // ); 49 | Serial.printf("\r\n Yaw %.6f ", 50 | float(atan2(IMU.getMagY_uT(), IMU.getMagX_uT())) * RAD_TO_DEG 51 | ); 52 | delay(20); 53 | } 54 | -------------------------------------------------------------------------------- /extras/AK8963C.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/AK8963C.pdf -------------------------------------------------------------------------------- /extras/Embedded-Masters-MPU-9250-Breakout.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/Embedded-Masters-MPU-9250-Breakout.pdf -------------------------------------------------------------------------------- /extras/MPU-9250-AXIS.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/MPU-9250-AXIS.png -------------------------------------------------------------------------------- /extras/MPU-9250-Datasheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/MPU-9250-Datasheet.pdf -------------------------------------------------------------------------------- /extras/MPU-9250-Register-Map.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/MPU-9250-Register-Map.pdf -------------------------------------------------------------------------------- /extras/MPU-9255-Datasheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/MPU-9255-Datasheet.pdf -------------------------------------------------------------------------------- /extras/MPU-9255-Register-Map.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yelvlab/ESP32-Arduino-MPU9250/2124a5d8eecb90aea90d8163011271a9d07243e0/extras/MPU-9255-Register-Map.pdf -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map BPI-BIT MPU9250 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | MPU9250 KEYWORD1 10 | MPU9250FIFO KEYWORD1 11 | 12 | 13 | ####################################### 14 | # Methods and Functions (KEYWORD2) 15 | ####################################### 16 | 17 | begin KEYWORD2 18 | setAccelRange KEYWORD2 19 | setGyroRange KEYWORD2 20 | setDlpfBandwidth KEYWORD2 21 | setSrd KEYWORD2 22 | enableDataReadyInterrupt KEYWORD2 23 | disableDataReadyInterrupt KEYWORD2 24 | enableWakeOnMotion KEYWORD2 25 | enableFifo KEYWORD2 26 | readSensor KEYWORD2 27 | getAccelX_mss KEYWORD2 28 | getAccelY_mss KEYWORD2 29 | getAccelZ_mss KEYWORD2 30 | getGyroX_rads KEYWORD2 31 | getGyroY_rads KEYWORD2 32 | getGyroZ_rads KEYWORD2 33 | getMagX_uT KEYWORD2 34 | getMagY_uT KEYWORD2 35 | getMagZ_uT KEYWORD2 36 | getTemp_C KEYWORD2 37 | readFifo KEYWORD2 38 | getFifoAccelX_mss KEYWORD2 39 | getFifoAccelY_mss KEYWORD2 40 | getFifoAccelZ_mss KEYWORD2 41 | getFifoGyroX_rads KEYWORD2 42 | getFifoGyroY_rads KEYWORD2 43 | getFifoGyroZ_rads KEYWORD2 44 | getFifoMagX_uT KEYWORD2 45 | getFifoMagY_uT KEYWORD2 46 | getFifoMagZ_uT KEYWORD2 47 | getFifoTemp_C KEYWORD2 48 | calibrateGyro KEYWORD2 49 | getGyroBiasX_rads KEYWORD2 50 | getGyroBiasY_rads KEYWORD2 51 | getGyroBiasZ_rads KEYWORD2 52 | setGyroBiasX_rads KEYWORD2 53 | setGyroBiasY_rads KEYWORD2 54 | setGyroBiasZ_rads KEYWORD2 55 | calibrateAccel KEYWORD2 56 | getAccelBiasX_mss KEYWORD2 57 | getAccelScaleFactorX KEYWORD2 58 | getAccelBiasY_mss KEYWORD2 59 | getAccelScaleFactorY KEYWORD2 60 | getAccelBiasZ_mss KEYWORD2 61 | getAccelScaleFactorZ KEYWORD2 62 | setAccelCalX KEYWORD2 63 | setAccelCalY KEYWORD2 64 | setAccelCalZ KEYWORD2 65 | calibrateMag KEYWORD2 66 | getMagBiasX_uT KEYWORD2 67 | getMagScaleFactorX KEYWORD2 68 | getMagBiasY_uT KEYWORD2 69 | getMagScaleFactorY KEYWORD2 70 | getMagBiasZ_uT KEYWORD2 71 | getMagScaleFactorZ KEYWORD2 72 | setMagCalX KEYWORD2 73 | setMagCalY KEYWORD2 74 | setMagCalZ KEYWORD2 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=BPI-BIT MPU9250 Master Mode 2 | version=1.0.0 3 | author=BPI-BIT Team 4 | maintainer=BPI-BIT Team 5 | sentence=BPI-BIT MPU9250 Master Mode Library 6 | paragraph=BPI-BIT MPU9250 Master Mode Library 7 | category=Device Control 8 | url=https://github.com/BPI-STEAM/MPU9250 9 | architectures=* 10 | includes=MPU9250.h --------------------------------------------------------------------------------