MPU9250.h 46 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954
  1. #pragma once
  2. #ifndef MPU9250_H
  3. #define MPU9250_H
  4. #include <Wire.h>
  5. #include "MPU9250/MPU9250RegisterMap.h"
  6. #include "MPU9250/QuaternionFilter.h"
  7. enum class AFS { A2G,
  8. A4G,
  9. A8G,
  10. A16G };
  11. enum class GFS { G250DPS,
  12. G500DPS,
  13. G1000DPS,
  14. G2000DPS };
  15. enum class MFS { M14BITS,
  16. M16BITS };
  17. static constexpr uint8_t MPU9250_WHOAMI_DEFAULT_VALUE{0x71};
  18. static constexpr uint8_t MPU9255_WHOAMI_DEFAULT_VALUE{0x73};
  19. template <typename WireType, uint8_t WHO_AM_I, AFS AFSSEL = AFS::A16G, GFS GFSSEL = GFS::G2000DPS, MFS MFSSEL = MFS::M16BITS>
  20. class MPU9250_ {
  21. static constexpr uint8_t MPU9250_DEFAULT_ADDRESS{0x68}; // Device address when ADO = 0
  22. static constexpr uint8_t AK8963_ADDRESS{0x0C}; // Address of magnetometer
  23. uint8_t MPU9250_ADDRESS{MPU9250_DEFAULT_ADDRESS};
  24. const uint8_t AK8963_WHOAMI_DEFAULT_VALUE{0x48};
  25. // Set initial input parameters
  26. // const uint8_t Mmode {0x02}; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
  27. const uint8_t Mmode{0x06}; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
  28. const float aRes{getAres()}; // scale resolutions per LSB for the sensors
  29. const float gRes{getGres()}; // scale resolutions per LSB for the sensors
  30. const float mRes{getMres()}; // scale resolutions per LSB for the sensors
  31. float magCalibration[3] = {0, 0, 0}; // factory mag calibration
  32. float magBias[3] = {0, 0, 0};
  33. float magScale[3] = {1.0, 1.0, 1.0}; // Bias corrections for gyro and accelerometer
  34. float gyroBias[3] = {0, 0, 0}; // bias corrections
  35. float accelBias[3] = {0, 0, 0}; // bias corrections
  36. int16_t tempCount; // temperature raw count output
  37. float temperature; // Stores the real internal chip temperature in degrees Celsius
  38. float SelfTestResult[6]; // holds results of gyro and accelerometer self test
  39. float a[3], g[3], m[3];
  40. float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
  41. float pitch, yaw, roll;
  42. float a12, a22, a31, a32, a33; // rotation matrix coefficients for Euler angles and gravity components
  43. float lin_ax, lin_ay, lin_az; // linear acceleration (acceleration with gravity component subtracted)
  44. QuaternionFilter qFilter;
  45. float magnetic_declination = -7.51; // Japan, 24th June
  46. bool b_verbose{false};
  47. public:
  48. MPU9250_() : aRes(getAres()), gRes(getGres()), mRes(getMres()) {}
  49. bool setup(const uint8_t addr, WireType& w = Wire) {
  50. // addr should be valid for MPU
  51. if ((addr < MPU9250_DEFAULT_ADDRESS) || (addr > MPU9250_DEFAULT_ADDRESS + 7)) {
  52. Serial.print("I2C address 0x");
  53. Serial.print(addr, HEX);
  54. Serial.println(" is not valid for MPU. Please check your I2C address.");
  55. return false;
  56. }
  57. wire = &w;
  58. MPU9250_ADDRESS = addr;
  59. if (isConnectedMPU9250()) {
  60. if (b_verbose)
  61. Serial.println("MPU9250 is online...");
  62. initMPU9250();
  63. if (isConnectedAK8963())
  64. initAK8963(magCalibration);
  65. else {
  66. if (b_verbose)
  67. Serial.println("Could not connect to AK8963");
  68. return false;
  69. }
  70. } else {
  71. if (b_verbose)
  72. Serial.println("Could not connect to MPU9250");
  73. return false;
  74. }
  75. return true;
  76. }
  77. void verbose(const bool b) { b_verbose = b; }
  78. void calibrateAccelGyro() {
  79. calibrateMPU9250(gyroBias, accelBias);
  80. }
  81. void calibrateMag() {
  82. magcalMPU9250(magBias, magScale);
  83. }
  84. bool isConnectedMPU9250() {
  85. byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
  86. if (b_verbose) {
  87. Serial.print("MPU9250 WHO AM I = ");
  88. Serial.println(c, HEX);
  89. }
  90. return (c == WHO_AM_I);
  91. }
  92. bool isConnectedAK8963() {
  93. byte c = readByte(AK8963_ADDRESS, AK8963_WHO_AM_I);
  94. if (b_verbose) {
  95. Serial.print("AK8963 WHO AM I = ");
  96. Serial.println(c, HEX);
  97. }
  98. return (c == AK8963_WHOAMI_DEFAULT_VALUE);
  99. }
  100. bool available() {
  101. return (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01);
  102. }
  103. bool update() {
  104. if (!available()) return false;
  105. updateAccelGyro();
  106. updateMag();
  107. // Madgwick function needs to be fed North, East, and Down direction like
  108. // (AN, AE, AD, GN, GE, GD, MN, ME, MD)
  109. // Accel and Gyro direction is Right-Hand, X-Forward, Z-Up
  110. // Magneto direction is Right-Hand, Y-Forward, Z-Down
  111. // So to adopt to the general Aircraft coordinate system (Right-Hand, X-Forward, Z-Down),
  112. // we need to feed (ax, -ay, -az, gx, -gy, -gz, my, -mx, mz)
  113. // but we pass (-ax, ay, az, gx, -gy, -gz, my, -mx, mz)
  114. // because gravity is by convention positive down, we need to ivnert the accel data
  115. // get quaternion based on aircraft coordinate (Right-Hand, X-Forward, Z-Down)
  116. // acc[mg], gyro[deg/s], mag [mG]
  117. // gyro will be convert from [deg/s] to [rad/s] inside of this function
  118. qFilter.update(-a[0], a[1], a[2], g[0], -g[1], -g[2], m[1], -m[0], m[2], q);
  119. if (!b_ahrs) {
  120. tempCount = readTempData(); // Read the adc values
  121. temperature = ((float)tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade
  122. } else {
  123. // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
  124. // In this coordinate system, the positive z-axis is down toward Earth.
  125. // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
  126. // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
  127. // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
  128. // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
  129. // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
  130. // applied in the correct order which for this configuration is yaw, pitch, and then roll.
  131. // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
  132. updateRPY();
  133. }
  134. return true;
  135. }
  136. // TODO: more efficient getter, const refrerence of struct??
  137. float getRoll() const { return roll; }
  138. float getPitch() const { return pitch; }
  139. float getYaw() const { return yaw; }
  140. float getQuaternion(const uint8_t i) const { return (i < 4) ? q[i] : 0.f; }
  141. float getQuaternionX() const { return q[0]; }
  142. float getQuaternionY() const { return q[1]; }
  143. float getQuaternionZ() const { return q[2]; }
  144. float getQuaternionW() const { return q[3]; }
  145. float getAcc(const uint8_t i) const { return (i < 3) ? a[i] : 0.f; }
  146. float getGyro(const uint8_t i) const { return (i < 3) ? g[i] : 0.f; }
  147. float getMag(const uint8_t i) const { return (i < 3) ? m[i] : 0.f; }
  148. float getAccX() const { return a[0]; }
  149. float getAccY() const { return a[1]; }
  150. float getAccZ() const { return a[2]; }
  151. float getGyroX() const { return g[0]; }
  152. float getGyroY() const { return g[1]; }
  153. float getGyroZ() const { return g[2]; }
  154. float getMagX() const { return m[0]; }
  155. float getMagY() const { return m[1]; }
  156. float getMagZ() const { return m[2]; }
  157. float getAccBias(const uint8_t i) const { return (i < 3) ? accelBias[i] : 0.f; }
  158. float getGyroBias(const uint8_t i) const { return (i < 3) ? gyroBias[i] : 0.f; }
  159. float getMagBias(const uint8_t i) const { return (i < 3) ? magBias[i] : 0.f; }
  160. float getMagScale(const uint8_t i) const { return (i < 3) ? magScale[i] : 0.f; }
  161. float getAccBiasX() const { return accelBias[0]; }
  162. float getAccBiasY() const { return accelBias[1]; }
  163. float getAccBiasZ() const { return accelBias[2]; }
  164. float getGyroBiasX() const { return gyroBias[0]; }
  165. float getGyroBiasY() const { return gyroBias[1]; }
  166. float getGyroBiasZ() const { return gyroBias[2]; }
  167. float getMagBiasX() const { return magBias[0]; }
  168. float getMagBiasY() const { return magBias[1]; }
  169. float getMagBiasZ() const { return magBias[2]; }
  170. float getMagScaleX() const { return magScale[0]; }
  171. float getMagScaleY() const { return magScale[1]; }
  172. float getMagScaleZ() const { return magScale[2]; }
  173. float getTemperature() const { return temperature; }
  174. void setAccBias(const uint8_t i, const float v) {
  175. if (i < 3) accelBias[i] = v;
  176. }
  177. void setGyroBias(const uint8_t i, const float v) {
  178. if (i < 3) gyroBias[i] = v;
  179. }
  180. void setMagBias(const uint8_t i, const float v) {
  181. if (i < 3) magBias[i] = v;
  182. }
  183. void setMagScale(const uint8_t i, const float v) {
  184. if (i < 3) magScale[i] = v;
  185. }
  186. void setAccBiasX(const float v) { accelBias[0] = v; }
  187. void setAccBiasY(const float v) { accelBias[1] = v; }
  188. void setAccBiasZ(const float v) { accelBias[2] = v; }
  189. void setGyroBiasX(const float v) { gyroBias[0] = v; }
  190. void setGyroBiasY(const float v) { gyroBias[1] = v; }
  191. void setGyroBiasZ(const float v) { gyroBias[2] = v; }
  192. void setMagBiasX(const float v) { magBias[0] = v; }
  193. void setMagBiasY(const float v) { magBias[1] = v; }
  194. void setMagBiasZ(const float v) { magBias[2] = v; }
  195. void setMagScaleX(const float v) { magScale[0] = v; }
  196. void setMagScaleY(const float v) { magScale[1] = v; }
  197. void setMagScaleZ(const float v) { magScale[2] = v; }
  198. void setMagneticDeclination(const float d) { magnetic_declination = d; }
  199. void print() const {
  200. printRawData();
  201. printRollPitchYaw();
  202. printCalibration();
  203. }
  204. void printRawData() const {
  205. // Print acceleration values in milligs!
  206. Serial.print("ax = ");
  207. Serial.print((int)1000 * a[0]);
  208. Serial.print(" ay = ");
  209. Serial.print((int)1000 * a[1]);
  210. Serial.print(" az = ");
  211. Serial.print((int)1000 * a[2]);
  212. Serial.println(" mg");
  213. // Print gyro values in degree/sec
  214. Serial.print("gx = ");
  215. Serial.print(g[0], 2);
  216. Serial.print(" gy = ");
  217. Serial.print(g[1], 2);
  218. Serial.print(" gz = ");
  219. Serial.print(g[2], 2);
  220. Serial.println(" deg/s");
  221. // Print mag values in degree/sec
  222. Serial.print("mx = ");
  223. Serial.print((int)m[0]);
  224. Serial.print(" my = ");
  225. Serial.print((int)m[1]);
  226. Serial.print(" mz = ");
  227. Serial.print((int)m[2]);
  228. Serial.println(" mG");
  229. Serial.print("q0 = ");
  230. Serial.print(q[0]);
  231. Serial.print(" qx = ");
  232. Serial.print(q[1]);
  233. Serial.print(" qy = ");
  234. Serial.print(q[2]);
  235. Serial.print(" qz = ");
  236. Serial.println(q[3]);
  237. }
  238. void printRollPitchYaw() const {
  239. Serial.print("Yaw, Pitch, Roll: ");
  240. Serial.print(yaw, 2);
  241. Serial.print(", ");
  242. Serial.print(pitch, 2);
  243. Serial.print(", ");
  244. Serial.println(roll, 2);
  245. }
  246. void printCalibration() const {
  247. Serial.println("< calibration parameters >");
  248. Serial.println("accel bias [g]: ");
  249. Serial.print(accelBias[0] * 1000.f);
  250. Serial.print(", ");
  251. Serial.print(accelBias[1] * 1000.f);
  252. Serial.print(", ");
  253. Serial.print(accelBias[2] * 1000.f);
  254. Serial.println();
  255. Serial.println("gyro bias [deg/s]: ");
  256. Serial.print(gyroBias[0]);
  257. Serial.print(", ");
  258. Serial.print(gyroBias[1]);
  259. Serial.print(", ");
  260. Serial.print(gyroBias[2]);
  261. Serial.println();
  262. Serial.println("mag bias [mG]: ");
  263. Serial.print(magBias[0]);
  264. Serial.print(", ");
  265. Serial.print(magBias[1]);
  266. Serial.print(", ");
  267. Serial.print(magBias[2]);
  268. Serial.println();
  269. Serial.println("mag scale []: ");
  270. Serial.print(magScale[0]);
  271. Serial.print(", ");
  272. Serial.print(magScale[1]);
  273. Serial.print(", ");
  274. Serial.print(magScale[2]);
  275. Serial.println();
  276. }
  277. private:
  278. float getAres() const {
  279. switch (AFSSEL) {
  280. // Possible accelerometer scales (and their register bit settings) are:
  281. // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
  282. // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
  283. case AFS::A2G:
  284. return 2.0 / 32768.0;
  285. case AFS::A4G:
  286. return 4.0 / 32768.0;
  287. case AFS::A8G:
  288. return 8.0 / 32768.0;
  289. case AFS::A16G:
  290. return 16.0 / 32768.0;
  291. }
  292. }
  293. float getGres() const {
  294. switch (GFSSEL) {
  295. // Possible gyro scales (and their register bit settings) are:
  296. // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
  297. // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
  298. case GFS::G250DPS:
  299. return 250.0 / 32768.0;
  300. case GFS::G500DPS:
  301. return 500.0 / 32768.0;
  302. case GFS::G1000DPS:
  303. return 1000.0 / 32768.0;
  304. case GFS::G2000DPS:
  305. return 2000.0 / 32768.0;
  306. }
  307. }
  308. float getMres() const {
  309. switch (MFSSEL) {
  310. // Possible magnetometer scales (and their register bit settings) are:
  311. // 14 bit resolution (0) and 16 bit resolution (1)
  312. // Proper scale to return milliGauss
  313. case MFS::M14BITS:
  314. return 10. * 4912. / 8190.0;
  315. case MFS::M16BITS:
  316. return 10. * 4912. / 32760.0;
  317. }
  318. }
  319. void updateAccelGyro() {
  320. int16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro
  321. readMPU9250Data(MPU9250Data); // INT cleared on any read
  322. // Now we'll calculate the accleration value into actual g's
  323. a[0] = (float)MPU9250Data[0] * aRes - accelBias[0]; // get actual g value, this depends on scale being set
  324. a[1] = (float)MPU9250Data[1] * aRes - accelBias[1];
  325. a[2] = (float)MPU9250Data[2] * aRes - accelBias[2];
  326. tempCount = MPU9250Data[3]; // Read the adc values
  327. temperature = ((float)tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade
  328. // Calculate the gyro value into actual degrees per second
  329. g[0] = (float)MPU9250Data[4] * gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
  330. g[1] = (float)MPU9250Data[5] * gRes - gyroBias[1];
  331. g[2] = (float)MPU9250Data[6] * gRes - gyroBias[2];
  332. }
  333. void readMPU9250Data(int16_t* destination) {
  334. uint8_t rawData[14]; // x/y/z accel register data stored here
  335. readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]); // Read the 14 raw data registers into data array
  336. destination[0] = ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value
  337. destination[1] = ((int16_t)rawData[2] << 8) | rawData[3];
  338. destination[2] = ((int16_t)rawData[4] << 8) | rawData[5];
  339. destination[3] = ((int16_t)rawData[6] << 8) | rawData[7];
  340. destination[4] = ((int16_t)rawData[8] << 8) | rawData[9];
  341. destination[5] = ((int16_t)rawData[10] << 8) | rawData[11];
  342. destination[6] = ((int16_t)rawData[12] << 8) | rawData[13];
  343. }
  344. void updateMag() {
  345. int16_t magCount[3] = {0, 0, 0}; // Stores the 16-bit signed magnetometer sensor output
  346. readMagData(magCount); // Read the x/y/z adc values
  347. // getMres();
  348. // Calculate the magnetometer values in milliGauss
  349. // Include factory calibration per data sheet and user environmental corrections
  350. m[0] = (float)(magCount[0] * mRes * magCalibration[0] - magBias[0]) * magScale[0]; // get actual magnetometer value, this depends on scale being set
  351. m[1] = (float)(magCount[1] * mRes * magCalibration[1] - magBias[1]) * magScale[1];
  352. m[2] = (float)(magCount[2] * mRes * magCalibration[2] - magBias[2]) * magScale[2];
  353. }
  354. void readMagData(int16_t* destination) {
  355. uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
  356. if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
  357. readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
  358. uint8_t c = rawData[6]; // End data read by reading ST2 register
  359. if (!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
  360. destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
  361. destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian
  362. destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
  363. }
  364. }
  365. }
  366. void updateRPY() {
  367. a12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
  368. a22 = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
  369. a31 = 2.0f * (q[0] * q[1] + q[2] * q[3]);
  370. a32 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
  371. a33 = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  372. pitch = -asinf(a32);
  373. roll = atan2f(a31, a33);
  374. yaw = atan2f(a12, a22);
  375. pitch *= 180.0f / PI;
  376. roll *= 180.0f / PI;
  377. yaw *= 180.0f / PI;
  378. yaw += magnetic_declination;
  379. if (yaw >= +180.f)
  380. yaw -= 360.f;
  381. else if (yaw < -180.f)
  382. yaw += 360.f;
  383. lin_ax = a[0] + a31;
  384. lin_ay = a[1] + a32;
  385. lin_az = a[2] - a33;
  386. }
  387. int16_t readTempData() {
  388. uint8_t rawData[2]; // x/y/z gyro register data stored here
  389. readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
  390. return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value
  391. }
  392. void initAK8963(float* destination) {
  393. // First extract the factory calibration for each magnetometer axis
  394. uint8_t rawData[3]; // x/y/z gyro calibration data stored here
  395. writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
  396. delay(10);
  397. writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
  398. delay(10);
  399. readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
  400. destination[0] = (float)(rawData[0] - 128) / 256. + 1.; // Return x-axis sensitivity adjustment values, etc.
  401. destination[1] = (float)(rawData[1] - 128) / 256. + 1.;
  402. destination[2] = (float)(rawData[2] - 128) / 256. + 1.;
  403. writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
  404. delay(10);
  405. // Configure the magnetometer for continuous read and highest resolution
  406. // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
  407. // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
  408. writeByte(AK8963_ADDRESS, AK8963_CNTL, (uint8_t)MFSSEL << 4 | Mmode); // Set magnetometer data resolution and sample ODR
  409. delay(10);
  410. if (b_verbose) {
  411. Serial.println("Calibration values: ");
  412. Serial.print("X-Axis sensitivity adjustment value ");
  413. Serial.println(destination[0], 2);
  414. Serial.print("Y-Axis sensitivity adjustment value ");
  415. Serial.println(destination[1], 2);
  416. Serial.print("Z-Axis sensitivity adjustment value ");
  417. Serial.println(destination[2], 2);
  418. Serial.print("X-Axis sensitivity offset value ");
  419. Serial.println(magBias[0], 2);
  420. Serial.print("Y-Axis sensitivity offset value ");
  421. Serial.println(magBias[1], 2);
  422. Serial.print("Z-Axis sensitivity offset value ");
  423. Serial.println(magBias[2], 2);
  424. }
  425. }
  426. void magcalMPU9250(float* dest1, float* dest2) {
  427. uint16_t ii = 0, sample_count = 0;
  428. int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
  429. int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
  430. if (b_verbose)
  431. Serial.println("Mag Calibration: Wave device in a figure eight until done!");
  432. delay(4000);
  433. // shoot for ~fifteen seconds of mag data
  434. if (Mmode == 0x02)
  435. sample_count = 128; // at 8 Hz ODR, new mag data is available every 125 ms
  436. else if (Mmode == 0x06)
  437. sample_count = 1500; // at 100 Hz ODR, new mag data is available every 10 ms
  438. for (ii = 0; ii < sample_count; ii++) {
  439. readMagData(mag_temp); // Read the mag data
  440. for (int jj = 0; jj < 3; jj++) {
  441. if (mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
  442. if (mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
  443. }
  444. if (Mmode == 0x02) delay(135); // at 8 Hz ODR, new mag data is available every 125 ms
  445. if (Mmode == 0x06) delay(12); // at 100 Hz ODR, new mag data is available every 10 ms
  446. }
  447. if (b_verbose) {
  448. Serial.println("mag x min/max:");
  449. Serial.println(mag_max[0]);
  450. Serial.println(mag_min[0]);
  451. Serial.println("mag y min/max:");
  452. Serial.println(mag_max[1]);
  453. Serial.println(mag_min[1]);
  454. Serial.println("mag z min/max:");
  455. Serial.println(mag_max[2]);
  456. Serial.println(mag_min[2]);
  457. }
  458. // Get hard iron correction
  459. mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts
  460. mag_bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts
  461. mag_bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts
  462. dest1[0] = (float)mag_bias[0] * mRes * magCalibration[0]; // save mag biases in G for main program
  463. dest1[1] = (float)mag_bias[1] * mRes * magCalibration[1];
  464. dest1[2] = (float)mag_bias[2] * mRes * magCalibration[2];
  465. // Get soft iron correction estimate
  466. mag_scale[0] = (mag_max[0] - mag_min[0]) / 2; // get average x axis max chord length in counts
  467. mag_scale[1] = (mag_max[1] - mag_min[1]) / 2; // get average y axis max chord length in counts
  468. mag_scale[2] = (mag_max[2] - mag_min[2]) / 2; // get average z axis max chord length in counts
  469. float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
  470. avg_rad /= 3.0;
  471. dest2[0] = avg_rad / ((float)mag_scale[0]);
  472. dest2[1] = avg_rad / ((float)mag_scale[1]);
  473. dest2[2] = avg_rad / ((float)mag_scale[2]);
  474. if (b_verbose) {
  475. Serial.println("Mag Calibration done!");
  476. Serial.println("AK8963 mag biases (mG)");
  477. Serial.print(magBias[0]);
  478. Serial.print(", ");
  479. Serial.print(magBias[1]);
  480. Serial.print(", ");
  481. Serial.print(magBias[2]);
  482. Serial.println();
  483. Serial.println("AK8963 mag scale (mG)");
  484. Serial.print(magScale[0]);
  485. Serial.print(", ");
  486. Serial.print(magScale[1]);
  487. Serial.print(", ");
  488. Serial.print(magScale[2]);
  489. Serial.println();
  490. }
  491. }
  492. void initMPU9250() {
  493. // wake up device
  494. writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
  495. delay(100); // Wait for all registers to reset
  496. // get stable time source
  497. writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
  498. delay(200);
  499. // Configure Gyro and Thermometer
  500. // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
  501. // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
  502. // be higher than 1 / 0.0059 = 170 Hz
  503. // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
  504. // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
  505. writeByte(MPU9250_ADDRESS, MPU_CONFIG, 0x03);
  506. // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
  507. writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate
  508. // determined inset in CONFIG above
  509. // Set gyroscope full scale range
  510. // Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
  511. uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
  512. // c = c & ~0xE0; // Clear self-test bits [7:5]
  513. c = c & ~0x03; // Clear Fchoice bits [1:0]
  514. c = c & ~0x18; // Clear GFS bits [4:3]
  515. c = c | (uint8_t)GFSSEL << 3; // Set full scale range for the gyro
  516. // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
  517. writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c); // Write new GYRO_CONFIG value to register
  518. // Set accelerometer full-scale range configuration
  519. c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
  520. // c = c & ~0xE0; // Clear self-test bits [7:5]
  521. c = c & ~0x18; // Clear AFS bits [4:3]
  522. c = c | (uint8_t)AFSSEL << 3; // Set full scale range for the accelerometer
  523. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
  524. // Set accelerometer sample rate configuration
  525. // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
  526. // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
  527. c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
  528. c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
  529. c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
  530. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
  531. // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
  532. // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
  533. // Configure Interrupts and Bypass Enable
  534. // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
  535. // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
  536. // can join the I2C bus and all can be controlled by the Arduino as master
  537. writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
  538. writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
  539. delay(100);
  540. }
  541. // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
  542. // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
  543. void calibrateMPU9250(float* dest1, float* dest2) {
  544. uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  545. uint16_t ii, packet_count, fifo_count;
  546. int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
  547. // reset device
  548. writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
  549. delay(100);
  550. // get stable time source; Auto select clock source to be PLL gyroscope reference if ready
  551. // else use the internal oscillator, bits 2:0 = 001
  552. writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
  553. writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
  554. delay(200);
  555. // Configure device for bias calculation
  556. writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
  557. writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
  558. writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
  559. writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
  560. writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
  561. writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
  562. delay(15);
  563. // Configure MPU6050 gyro and accelerometer for bias calculation
  564. writeByte(MPU9250_ADDRESS, MPU_CONFIG, 0x01); // Set low-pass filter to 188 Hz
  565. writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
  566. writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
  567. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
  568. uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
  569. uint16_t accelsensitivity = 16384; // = 16384 LSB/g
  570. // Configure FIFO to capture accelerometer and gyro data for bias calculation
  571. writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
  572. writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
  573. delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
  574. // At end of sample accumulation, turn off FIFO sensor read
  575. writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
  576. readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
  577. fifo_count = ((uint16_t)data[0] << 8) | data[1];
  578. packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging
  579. for (ii = 0; ii < packet_count; ii++) {
  580. int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
  581. readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
  582. accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]); // Form signed 16-bit integer for each sample in FIFO
  583. accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]);
  584. accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]);
  585. gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]);
  586. gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]);
  587. gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
  588. accel_bias[0] += (int32_t)accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
  589. accel_bias[1] += (int32_t)accel_temp[1];
  590. accel_bias[2] += (int32_t)accel_temp[2];
  591. gyro_bias[0] += (int32_t)gyro_temp[0];
  592. gyro_bias[1] += (int32_t)gyro_temp[1];
  593. gyro_bias[2] += (int32_t)gyro_temp[2];
  594. }
  595. accel_bias[0] /= (int32_t)packet_count; // Normalize sums to get average count biases
  596. accel_bias[1] /= (int32_t)packet_count;
  597. accel_bias[2] /= (int32_t)packet_count;
  598. gyro_bias[0] /= (int32_t)packet_count;
  599. gyro_bias[1] /= (int32_t)packet_count;
  600. gyro_bias[2] /= (int32_t)packet_count;
  601. if (accel_bias[2] > 0L) {
  602. accel_bias[2] -= (int32_t)accelsensitivity;
  603. } // Remove gravity from the z-axis accelerometer bias calculation
  604. else {
  605. accel_bias[2] += (int32_t)accelsensitivity;
  606. }
  607. // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
  608. data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
  609. data[1] = (-gyro_bias[0] / 4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
  610. data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
  611. data[3] = (-gyro_bias[1] / 4) & 0xFF;
  612. data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
  613. data[5] = (-gyro_bias[2] / 4) & 0xFF;
  614. // Push gyro biases to hardware registers
  615. writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
  616. writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
  617. writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
  618. writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
  619. writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
  620. writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
  621. // Output scaled gyro biases for display in the main program
  622. dest1[0] = (float)gyro_bias[0] / (float)gyrosensitivity;
  623. dest1[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
  624. dest1[2] = (float)gyro_bias[2] / (float)gyrosensitivity;
  625. // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
  626. // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
  627. // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
  628. // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
  629. // the accelerometer biases calculated above must be divided by 8.
  630. // int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
  631. // readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
  632. // accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  633. // readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
  634. // accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  635. // readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
  636. // accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  637. // uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
  638. // uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
  639. // for(ii = 0; ii < 3; ii++) {
  640. // if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
  641. // }
  642. // // Construct total accelerometer bias, including calculated average accelerometer bias from above
  643. // accel_bias_reg[0] -= (accel_bias[0] / 8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
  644. // accel_bias_reg[1] -= (accel_bias[1] / 8);
  645. // accel_bias_reg[2] -= (accel_bias[2] / 8);
  646. // data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
  647. // data[1] = (accel_bias_reg[0]) & 0xFF;
  648. // data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  649. // data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
  650. // data[3] = (accel_bias_reg[1]) & 0xFF;
  651. // data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  652. // data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
  653. // data[5] = (accel_bias_reg[2]) & 0xFF;
  654. // data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  655. // Apparently this is not working for the acceleration biases in the MPU-9250
  656. // Are we handling the temperature correction bit properly?
  657. // Push accelerometer biases to hardware registers
  658. // writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
  659. // writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
  660. // writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
  661. // writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
  662. // writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
  663. // writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
  664. // Output scaled accelerometer biases for display in the main program
  665. dest2[0] = (float)accel_bias[0] / (float)accelsensitivity;
  666. dest2[1] = (float)accel_bias[1] / (float)accelsensitivity;
  667. dest2[2] = (float)accel_bias[2] / (float)accelsensitivity;
  668. if (b_verbose) {
  669. Serial.println("MPU9250 bias");
  670. Serial.println(" x y z ");
  671. Serial.print((int)(1000 * accelBias[0]));
  672. Serial.print(" ");
  673. Serial.print((int)(1000 * accelBias[1]));
  674. Serial.print(" ");
  675. Serial.print((int)(1000 * accelBias[2]));
  676. Serial.print(" ");
  677. Serial.println("mg");
  678. Serial.print(gyroBias[0], 1);
  679. Serial.print(" ");
  680. Serial.print(gyroBias[1], 1);
  681. Serial.print(" ");
  682. Serial.print(gyroBias[2], 1);
  683. Serial.print(" ");
  684. Serial.println("o/s");
  685. }
  686. delay(100);
  687. initMPU9250();
  688. delay(1000);
  689. }
  690. // Accelerometer and gyroscope self test; check calibration wrt factory settings
  691. void SelfTest() // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
  692. {
  693. uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
  694. uint8_t selfTest[6];
  695. int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
  696. float factoryTrim[6];
  697. uint8_t FS = 0;
  698. writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
  699. writeByte(MPU9250_ADDRESS, MPU_CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
  700. writeByte(MPU9250_ADDRESS, GYRO_CONFIG, FS << 3); // Set full scale range for the gyro to 250 dps
  701. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
  702. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, FS << 3); // Set full scale range for the accelerometer to 2 g
  703. for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
  704. readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
  705. aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); // Turn the MSB and LSB into a signed 16-bit value
  706. aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
  707. aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
  708. readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
  709. gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); // Turn the MSB and LSB into a signed 16-bit value
  710. gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
  711. gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
  712. }
  713. for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
  714. aAvg[ii] /= 200;
  715. gAvg[ii] /= 200;
  716. }
  717. // Configure the accelerometer for self-test
  718. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
  719. writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
  720. delay(25); // Delay a while to let the device stabilize
  721. for (int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
  722. readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
  723. aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); // Turn the MSB and LSB into a signed 16-bit value
  724. aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
  725. aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
  726. readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
  727. gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); // Turn the MSB and LSB into a signed 16-bit value
  728. gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
  729. gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
  730. }
  731. for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
  732. aSTAvg[ii] /= 200;
  733. gSTAvg[ii] /= 200;
  734. }
  735. // Configure the gyro and accelerometer for normal operation
  736. writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
  737. writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
  738. delay(25); // Delay a while to let the device stabilize
  739. // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
  740. selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
  741. selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
  742. selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
  743. selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
  744. selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
  745. selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
  746. // Retrieve factory self-test value from self-test code reads
  747. factoryTrim[0] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)selfTest[0] - 1.0))); // FT[Xa] factory trim calculation
  748. factoryTrim[1] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)selfTest[1] - 1.0))); // FT[Ya] factory trim calculation
  749. factoryTrim[2] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)selfTest[2] - 1.0))); // FT[Za] factory trim calculation
  750. factoryTrim[3] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)selfTest[3] - 1.0))); // FT[Xg] factory trim calculation
  751. factoryTrim[4] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)selfTest[4] - 1.0))); // FT[Yg] factory trim calculation
  752. factoryTrim[5] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)selfTest[5] - 1.0))); // FT[Zg] factory trim calculation
  753. // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
  754. // To get percent, must multiply by 100
  755. for (int i = 0; i < 3; i++) {
  756. SelfTestResult[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) / factoryTrim[i] - 100.; // Report percent differences
  757. SelfTestResult[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3] - 100.; // Report percent differences
  758. }
  759. if (b_verbose) {
  760. Serial.print("x-axis self test: acceleration trim within : ");
  761. Serial.print(SelfTestResult[0], 1);
  762. Serial.println("% of factory value");
  763. Serial.print("y-axis self test: acceleration trim within : ");
  764. Serial.print(SelfTestResult[1], 1);
  765. Serial.println("% of factory value");
  766. Serial.print("z-axis self test: acceleration trim within : ");
  767. Serial.print(SelfTestResult[2], 1);
  768. Serial.println("% of factory value");
  769. Serial.print("x-axis self test: gyration trim within : ");
  770. Serial.print(SelfTestResult[3], 1);
  771. Serial.println("% of factory value");
  772. Serial.print("y-axis self test: gyration trim within : ");
  773. Serial.print(SelfTestResult[4], 1);
  774. Serial.println("% of factory value");
  775. Serial.print("z-axis self test: gyration trim within : ");
  776. Serial.print(SelfTestResult[5], 1);
  777. Serial.println("% of factory value");
  778. }
  779. delay(5000);
  780. }
  781. void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
  782. wire->beginTransmission(address); // Initialize the Tx buffer
  783. wire->write(subAddress); // Put slave register address in Tx buffer
  784. wire->write(data); // Put data in Tx buffer
  785. i2c_err_ = wire->endTransmission(); // Send the Tx buffer
  786. if (i2c_err_) pirntI2CError();
  787. }
  788. uint8_t readByte(uint8_t address, uint8_t subAddress) {
  789. uint8_t data = 0; // `data` will store the register data
  790. wire->beginTransmission(address); // Initialize the Tx buffer
  791. wire->write(subAddress); // Put slave register address in Tx buffer
  792. i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
  793. if (i2c_err_) pirntI2CError();
  794. wire->requestFrom(address, (size_t)1); // Read one byte from slave register address
  795. if (wire->available()) data = wire->read(); // Fill Rx buffer with result
  796. return data; // Return data read from slave register
  797. }
  798. void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t* dest) {
  799. wire->beginTransmission(address); // Initialize the Tx buffer
  800. wire->write(subAddress); // Put slave register address in Tx buffer
  801. i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
  802. if (i2c_err_) pirntI2CError();
  803. uint8_t i = 0;
  804. wire->requestFrom(address, count); // Read bytes from slave register address
  805. while (wire->available()) {
  806. dest[i++] = wire->read();
  807. } // Put read results in the Rx buffer
  808. }
  809. void pirntI2CError() {
  810. if (i2c_err_ == 7) return; // to avoid stickbreaker-i2c branch's error code
  811. Serial.print("I2C ERROR CODE : ");
  812. Serial.println(i2c_err_);
  813. }
  814. bool b_ahrs{true};
  815. WireType* wire;
  816. uint8_t i2c_err_;
  817. };
  818. using MPU9250 = MPU9250_<TwoWire, MPU9250_WHOAMI_DEFAULT_VALUE>;
  819. using MPU9255 = MPU9250_<TwoWire, MPU9255_WHOAMI_DEFAULT_VALUE>;
  820. #endif // MPU9250_H