├── ArduinoAccel └── ArduinoAccel.ino ├── ArduinoBNO055 └── ArduinoBNO055.ino ├── ArduinoIMU └── ArduinoIMU.ino ├── ArduinoIMU10 └── ArduinoIMU10.ino ├── ArduinoMagCal └── ArduinoMagCal.ino ├── LICENSE ├── README.md ├── RTArduLinkIMU ├── RTArduLinkIMU.h ├── RTArduLinkIMU.ino └── RTArduLinkIMUDefs.h └── libraries ├── CalLib ├── CalLib.cpp └── CalLib.h ├── I2CDev ├── I2Cdev.cpp └── I2Cdev.h ├── RTArduLink ├── RTArduLink.cpp ├── RTArduLink.h ├── RTArduLinkDefs.h ├── RTArduLinkDemoDefs.h ├── RTArduLinkHAL.cpp ├── RTArduLinkHAL.h ├── RTArduLinkUtils.cpp └── RTArduLinkUtils.h └── RTIMULib ├── RTFusionRTQF.cpp ├── RTFusionRTQF.h ├── RTIMU.cpp ├── RTIMU.h ├── RTIMUBNO055.cpp ├── RTIMUBNO055.h ├── RTIMUGD20HM303D.cpp ├── RTIMUGD20HM303D.h ├── RTIMUGD20HM303DLHC.cpp ├── RTIMUGD20HM303DLHC.h ├── RTIMUGD20M303DLHC.cpp ├── RTIMUGD20M303DLHC.h ├── RTIMULSM9DS0.cpp ├── RTIMULSM9DS0.h ├── RTIMULibDefs.h ├── RTIMUMPU9150.cpp ├── RTIMUMPU9150.h ├── RTIMUMPU9250.cpp ├── RTIMUMPU9250.h ├── RTIMUSettings.cpp ├── RTIMUSettings.h ├── RTMath.cpp ├── RTMath.h ├── RTPressure.cpp ├── RTPressure.h ├── RTPressureBMP180.cpp ├── RTPressureBMP180.h ├── RTPressureDefs.h ├── RTPressureLPS25H.cpp ├── RTPressureLPS25H.h ├── RTPressureMS5611.cpp └── RTPressureMS5611.h /ArduinoAccel/ArduinoAccel.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include 25 | #include "I2Cdev.h" 26 | #include "RTIMUSettings.h" 27 | #include "RTIMU.h" 28 | #include "RTFusionRTQF.h" 29 | #include "CalLib.h" 30 | #include 31 | 32 | RTIMU *imu; // the IMU object 33 | RTFusionRTQF fusion; // the fusion object 34 | RTIMUSettings settings; // the settings object 35 | 36 | // DISPLAY_INTERVAL sets the rate at which results are displayed 37 | 38 | #define DISPLAY_INTERVAL 100 // interval between pose displays 39 | 40 | // SERIAL_PORT_SPEED defines the speed to use for the debug serial port 41 | 42 | #define SERIAL_PORT_SPEED 115200 43 | 44 | unsigned long lastDisplay; 45 | unsigned long lastRate; 46 | int sampleCount; 47 | RTQuaternion gravity; 48 | 49 | void setup() 50 | { 51 | int errcode; 52 | 53 | Serial.begin(SERIAL_PORT_SPEED); 54 | Wire.begin(); 55 | imu = RTIMU::createIMU(&settings); // create the imu object 56 | 57 | Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName()); 58 | if ((errcode = imu->IMUInit()) < 0) { 59 | Serial.print("Failed to init IMU: "); Serial.println(errcode); 60 | } 61 | 62 | if (imu->getCalibrationValid()) 63 | Serial.println("Using compass calibration"); 64 | else 65 | Serial.println("No valid compass calibration data"); 66 | 67 | lastDisplay = lastRate = millis(); 68 | sampleCount = 0; 69 | 70 | gravity.setScalar(0); 71 | gravity.setX(0); 72 | gravity.setY(0); 73 | gravity.setZ(1); 74 | } 75 | 76 | void loop() 77 | { 78 | unsigned long now = millis(); 79 | unsigned long delta; 80 | RTVector3 realAccel; 81 | RTQuaternion rotatedGravity; 82 | RTQuaternion fusedConjugate; 83 | RTQuaternion qTemp; 84 | int loopCount = 0; 85 | 86 | while (imu->IMURead()) { // get the latest data if ready yet 87 | // this flushes remaining data in case we are falling behind 88 | if (++loopCount >= 10) 89 | continue; 90 | fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp()); 91 | 92 | // do gravity rotation and subtraction 93 | 94 | // create the conjugate of the pose 95 | 96 | fusedConjugate = fusion.getFusionQPose().conjugate(); 97 | 98 | // now do the rotation - takes two steps with qTemp as the intermediate variable 99 | 100 | qTemp = gravity * fusion.getFusionQPose(); 101 | rotatedGravity = fusedConjugate * qTemp; 102 | 103 | // now adjust the measured accel and change the signs to make sense 104 | 105 | realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x())); 106 | realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y())); 107 | realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z())); 108 | 109 | sampleCount++; 110 | if ((delta = now - lastRate) >= 1000) { 111 | Serial.print("Sample rate: "); Serial.print(sampleCount); 112 | if (!imu->IMUGyroBiasValid()) 113 | Serial.println(", calculating gyro bias"); 114 | else 115 | Serial.println(); 116 | 117 | sampleCount = 0; 118 | lastRate = now; 119 | } 120 | if ((now - lastDisplay) >= DISPLAY_INTERVAL) { 121 | lastDisplay = now; 122 | 123 | RTMath::display("Accel:", realAccel); 124 | Serial.println(); 125 | } 126 | } 127 | } 128 | 129 | -------------------------------------------------------------------------------- /ArduinoBNO055/ArduinoBNO055.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | // *** Important *** 25 | // 26 | // To use this sketch, either BNO055_28 or BNO055_29 must be uncommented out and 27 | // all other IMUs commented out in libraries/RTIMULib/RTIMULibDefs.h. 28 | // This sketch uses the BNO055's fusion results. 29 | 30 | #include 31 | #include "I2Cdev.h" 32 | #include "RTIMUSettings.h" 33 | #include "RTIMUBNO055.h" 34 | #include "CalLib.h" 35 | #include 36 | 37 | #if !defined(BNO055_28) && !defined(BNO055_29) 38 | #error "One of BNO055_28 or BNO055_29 must be selected in RTIMULibdefs.h" 39 | #endif 40 | 41 | RTIMUBNO055 *imu; // the IMU object 42 | RTIMUSettings settings; // the settings object 43 | 44 | // DISPLAY_INTERVAL sets the rate at which results are displayed 45 | 46 | #define DISPLAY_INTERVAL 300 // interval between pose displays 47 | 48 | // SERIAL_PORT_SPEED defines the speed to use for the debug serial port 49 | 50 | #define SERIAL_PORT_SPEED 115200 51 | 52 | unsigned long lastDisplay; 53 | unsigned long lastRate; 54 | int sampleCount; 55 | 56 | void setup() 57 | { 58 | int errcode; 59 | 60 | Serial.begin(SERIAL_PORT_SPEED); 61 | Wire.begin(); 62 | imu = (RTIMUBNO055 *)RTIMU::createIMU(&settings); // create the imu object 63 | 64 | Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName()); 65 | if ((errcode = imu->IMUInit()) < 0) { 66 | Serial.print("Failed to init IMU: "); Serial.println(errcode); 67 | } 68 | 69 | lastDisplay = lastRate = millis(); 70 | sampleCount = 0; 71 | } 72 | 73 | void loop() 74 | { 75 | unsigned long now = millis(); 76 | unsigned long delta; 77 | int loopCount = 1; 78 | 79 | while (imu->IMURead()) { // get the latest data if ready yet 80 | // this flushes remaining data in case we are falling behind 81 | if (++loopCount >= 10) 82 | continue; 83 | sampleCount++; 84 | if ((delta = now - lastRate) >= 1000) { 85 | Serial.print("Sample rate: "); Serial.println(sampleCount); 86 | sampleCount = 0; 87 | lastRate = now; 88 | } 89 | if ((now - lastDisplay) >= DISPLAY_INTERVAL) { 90 | lastDisplay = now; 91 | // RTMath::display("Gyro:", (RTVector3&)imu->getGyro()); // gyro data 92 | // RTMath::display("Accel:", (RTVector3&)imu->getAccel()); // accel data 93 | // RTMath::display("Mag:", (RTVector3&)imu->getCompass()); // compass data 94 | RTMath::displayRollPitchYaw("Pose:", (RTVector3&)imu->getFusionPose()); // fused output 95 | Serial.println(); 96 | } 97 | } 98 | } 99 | 100 | -------------------------------------------------------------------------------- /ArduinoIMU/ArduinoIMU.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include 25 | #include "I2Cdev.h" 26 | #include "RTIMUSettings.h" 27 | #include "RTIMU.h" 28 | #include "RTFusionRTQF.h" 29 | #include "CalLib.h" 30 | #include 31 | 32 | RTIMU *imu; // the IMU object 33 | RTFusionRTQF fusion; // the fusion object 34 | RTIMUSettings settings; // the settings object 35 | 36 | // DISPLAY_INTERVAL sets the rate at which results are displayed 37 | 38 | #define DISPLAY_INTERVAL 300 // interval between pose displays 39 | 40 | // SERIAL_PORT_SPEED defines the speed to use for the debug serial port 41 | 42 | #define SERIAL_PORT_SPEED 115200 43 | 44 | unsigned long lastDisplay; 45 | unsigned long lastRate; 46 | int sampleCount; 47 | 48 | void setup() 49 | { 50 | int errcode; 51 | 52 | Serial.begin(SERIAL_PORT_SPEED); 53 | Wire.begin(); 54 | imu = RTIMU::createIMU(&settings); // create the imu object 55 | 56 | Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName()); 57 | if ((errcode = imu->IMUInit()) < 0) { 58 | Serial.print("Failed to init IMU: "); Serial.println(errcode); 59 | } 60 | 61 | if (imu->getCalibrationValid()) 62 | Serial.println("Using compass calibration"); 63 | else 64 | Serial.println("No valid compass calibration data"); 65 | 66 | lastDisplay = lastRate = millis(); 67 | sampleCount = 0; 68 | 69 | // Slerp power controls the fusion and can be between 0 and 1 70 | // 0 means that only gyros are used, 1 means that only accels/compass are used 71 | // In-between gives the fusion mix. 72 | 73 | fusion.setSlerpPower(0.02); 74 | 75 | // use of sensors in the fusion algorithm can be controlled here 76 | // change any of these to false to disable that sensor 77 | 78 | fusion.setGyroEnable(true); 79 | fusion.setAccelEnable(true); 80 | fusion.setCompassEnable(true); 81 | } 82 | 83 | void loop() 84 | { 85 | unsigned long now = millis(); 86 | unsigned long delta; 87 | int loopCount = 1; 88 | 89 | while (imu->IMURead()) { // get the latest data if ready yet 90 | // this flushes remaining data in case we are falling behind 91 | if (++loopCount >= 10) 92 | continue; 93 | fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp()); 94 | sampleCount++; 95 | if ((delta = now - lastRate) >= 1000) { 96 | Serial.print("Sample rate: "); Serial.print(sampleCount); 97 | if (imu->IMUGyroBiasValid()) 98 | Serial.println(", gyro bias valid"); 99 | else 100 | Serial.println(", calculating gyro bias"); 101 | 102 | sampleCount = 0; 103 | lastRate = now; 104 | } 105 | if ((now - lastDisplay) >= DISPLAY_INTERVAL) { 106 | lastDisplay = now; 107 | // RTMath::display("Gyro:", (RTVector3&)imu->getGyro()); // gyro data 108 | // RTMath::display("Accel:", (RTVector3&)imu->getAccel()); // accel data 109 | // RTMath::display("Mag:", (RTVector3&)imu->getCompass()); // compass data 110 | RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output 111 | Serial.println(); 112 | } 113 | } 114 | } 115 | 116 | -------------------------------------------------------------------------------- /ArduinoIMU10/ArduinoIMU10.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include 25 | #include "I2Cdev.h" 26 | #include "RTIMUSettings.h" 27 | #include "RTIMU.h" 28 | #include "RTFusionRTQF.h" 29 | #include "RTPressure.h" 30 | #include "CalLib.h" 31 | #include 32 | 33 | RTIMU *imu; // the IMU object 34 | RTPressure *pressure; // the pressure object 35 | RTFusionRTQF fusion; // the fusion object 36 | RTIMUSettings settings; // the settings object 37 | 38 | // DISPLAY_INTERVAL sets the rate at which results are displayed 39 | 40 | #define DISPLAY_INTERVAL 300 // interval between pose displays 41 | 42 | // SERIAL_PORT_SPEED defines the speed to use for the debug serial port 43 | 44 | #define SERIAL_PORT_SPEED 115200 45 | 46 | unsigned long lastDisplay; 47 | unsigned long lastRate; 48 | int sampleCount; 49 | 50 | void setup() 51 | { 52 | int errcode; 53 | 54 | Serial.begin(SERIAL_PORT_SPEED); 55 | Wire.begin(); 56 | imu = RTIMU::createIMU(&settings); // create the imu object 57 | pressure = RTPressure::createPressure(&settings); // create the pressure sensor 58 | 59 | if (pressure == 0) { 60 | Serial.println("No pressure sensor has been configured - terminating"); 61 | while (1) ; 62 | } 63 | 64 | Serial.print("ArduinoIMU10 starting using IMU "); Serial.print(imu->IMUName()); 65 | Serial.print(", pressure sensor "); Serial.println(pressure->pressureName()); 66 | if ((errcode = imu->IMUInit()) < 0) { 67 | Serial.print("Failed to init IMU: "); Serial.println(errcode); 68 | } 69 | 70 | if ((errcode = pressure->pressureInit()) < 0) { 71 | Serial.print("Failed to init pressure sensor: "); Serial.println(errcode); 72 | } 73 | 74 | if (imu->getCalibrationValid()) 75 | Serial.println("Using compass calibration"); 76 | else 77 | Serial.println("No valid compass calibration data"); 78 | 79 | lastDisplay = lastRate = millis(); 80 | sampleCount = 0; 81 | 82 | // Slerp power controls the fusion and can be between 0 and 1 83 | // 0 means that only gyros are used, 1 means that only accels/compass are used 84 | // In-between gives the fusion mix. 85 | 86 | fusion.setSlerpPower(0.02); 87 | 88 | // use of sensors in the fusion algorithm can be controlled here 89 | // change any of these to false to disable that sensor 90 | 91 | fusion.setGyroEnable(true); 92 | fusion.setAccelEnable(true); 93 | fusion.setCompassEnable(true); 94 | } 95 | 96 | void loop() 97 | { 98 | unsigned long now = millis(); 99 | unsigned long delta; 100 | float latestPressure; 101 | float latestTemperature; 102 | int loopCount = 1; 103 | 104 | while (imu->IMURead()) { // get the latest data if ready yet 105 | // this flushes remaining data in case we are falling behind 106 | if (++loopCount >= 10) 107 | continue; 108 | 109 | fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp()); 110 | sampleCount++; 111 | if ((delta = now - lastRate) >= 1000) { 112 | Serial.print("Sample rate: "); Serial.print(sampleCount); 113 | if (imu->IMUGyroBiasValid()) 114 | Serial.println(", gyro bias valid"); 115 | else 116 | Serial.println(", calculating gyro bias"); 117 | 118 | sampleCount = 0; 119 | lastRate = now; 120 | } 121 | if ((now - lastDisplay) >= DISPLAY_INTERVAL) { 122 | lastDisplay = now; 123 | // RTMath::display("Gyro:", (RTVector3&)imu->getGyro()); // gyro data 124 | // RTMath::display("Accel:", (RTVector3&)imu->getAccel()); // accel data 125 | // RTMath::display("Mag:", (RTVector3&)imu->getCompass()); // compass data 126 | RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output 127 | 128 | if (pressure->pressureRead(latestPressure, latestTemperature)) { 129 | Serial.print(", pressure: "); Serial.print(latestPressure); 130 | Serial.print(", temperature: "); Serial.print(latestTemperature); 131 | } 132 | Serial.println(); 133 | } 134 | } 135 | } 136 | 137 | -------------------------------------------------------------------------------- /ArduinoMagCal/ArduinoMagCal.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include 25 | #include "I2Cdev.h" 26 | #include "RTIMUSettings.h" 27 | #include "RTIMU.h" 28 | #include "CalLib.h" 29 | #include 30 | 31 | RTIMU *imu; // the IMU object 32 | RTIMUSettings settings; // the settings object 33 | CALLIB_DATA calData; // the calibration data 34 | 35 | // SERIAL_PORT_SPEED defines the speed to use for the debug serial port 36 | 37 | #define SERIAL_PORT_SPEED 115200 38 | 39 | void setup() 40 | { 41 | calLibRead(0, &calData); // pick up existing mag data if there 42 | 43 | calData.magValid = false; 44 | for (int i = 0; i < 3; i++) { 45 | calData.magMin[i] = 10000000; // init mag cal data 46 | calData.magMax[i] = -10000000; 47 | } 48 | 49 | Serial.begin(SERIAL_PORT_SPEED); 50 | Serial.println("ArduinoMagCal starting"); 51 | Serial.println("Enter s to save current data to EEPROM"); 52 | Wire.begin(); 53 | 54 | imu = RTIMU::createIMU(&settings); // create the imu object 55 | imu->IMUInit(); 56 | imu->setCalibrationMode(true); // make sure we get raw data 57 | Serial.print("ArduinoIMU calibrating device "); Serial.println(imu->IMUName()); 58 | } 59 | 60 | void loop() 61 | { 62 | boolean changed; 63 | RTVector3 mag; 64 | 65 | if (imu->IMURead()) { // get the latest data 66 | changed = false; 67 | mag = imu->getCompass(); 68 | for (int i = 0; i < 3; i++) { 69 | if (mag.data(i) < calData.magMin[i]) { 70 | calData.magMin[i] = mag.data(i); 71 | changed = true; 72 | } 73 | if (mag.data(i) > calData.magMax[i]) { 74 | calData.magMax[i] = mag.data(i); 75 | changed = true; 76 | } 77 | } 78 | 79 | if (changed) { 80 | Serial.println("-------"); 81 | Serial.print("minX: "); Serial.print(calData.magMin[0]); 82 | Serial.print(" maxX: "); Serial.print(calData.magMax[0]); Serial.println(); 83 | Serial.print("minY: "); Serial.print(calData.magMin[1]); 84 | Serial.print(" maxY: "); Serial.print(calData.magMax[1]); Serial.println(); 85 | Serial.print("minZ: "); Serial.print(calData.magMin[2]); 86 | Serial.print(" maxZ: "); Serial.print(calData.magMax[2]); Serial.println(); 87 | } 88 | } 89 | 90 | if (Serial.available()) { 91 | if (Serial.read() == 's') { // save the data 92 | calData.magValid = true; 93 | calLibWrite(0, &calData); 94 | Serial.print("Mag cal data saved for device "); Serial.println(imu->IMUName()); 95 | } 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RTIMULib-Arduino - a versatile 9-dof and 10-dof IMU library for the Arduino 2 | 3 | RTIMULib-Arduino is the simplest way to connect a 9-dof or 10-dof IMU to an Arduino (Uno or Mega) and obtain fully fused quaternion or Euler angle pose data. 4 | 5 | ## Please note that this library is no longer supported. 6 | 7 | ## Features 8 | 9 | RTIMULib-Arduino currently supports the following IMUs via I2C: 10 | 11 | * InvenSense MPU-9150 single chip IMU. 12 | * InvenSense MPU-6050 plus HMC5883 magnetometer on MPU-6050's aux bus (handled by the MPU-9150 driver). 13 | * InvenSense MPU-6050 gyros + acclerometers. Treated as MPU-9150 without magnetometers. 14 | * InvenSense MPU-9250 single chip IMU 15 | * STM LSM9DS0 single chip IMU 16 | * L3GD20H + LSM303D (optionally with the LPS25H) as used on the Pololu AltIMU-10 v4. 17 | * L3GD20 + LSM303DLHC as used on the Adafruit 9-dof (older version with GD20 gyro) IMU. 18 | * L3GD20H + LSM303DLHC (optionally with BMP180) as used on the new Adafruit 10-dof IMU. 19 | * Bosch BNO055 9-dof IMU with onchip fusion (see notes below). 20 | 21 | Pressure/temperature sensing is supported for the following pressure sensors: 22 | 23 | * BMP180 24 | * LPS25H 25 | * MS5611 26 | 27 | Select the IMU in use by editing libraries/RTIMULib/RTIMULibDefs.h and uncommenting one of the supported IMUs like this: 28 | 29 | #define MPU9150_68 // MPU9150 at address 0x68 30 | //#define MPU9150_69 // MPU9150 at address 0x69 31 | //#define MPU9250_68 // MPU9250 at address 0x68 32 | //#define MPU9250_69 // MPU9250 at address 0x69 33 | //#define LSM9DS0_6a // LSM9DS0 at address 0x6a 34 | //#define LSM9DS0_6b // LSM9DS0 at address 0x6b 35 | //#define GD20HM303D_6a // GD20H + M303D at address 0x6a 36 | //#define GD20HM303D_6b // GD20H + M303D at address 0x6b 37 | //#define GD20M303DLHC_6a // GD20 + M303DLHC at address 0x6a 38 | //#define GD20M303DLHC_6b // GD20 + M303DLHC at address 0x6b 39 | //#define GD20HM303DLHC_6a // GD20H + M303DLHC at address 0x6a 40 | //#define GD20HM303DLHC_6b // GD20H + M303DLHC at address 0x6b 41 | //#define BNO055_28 // BNO055 at address 0x28 42 | //#define BNO055_29 // BNO055 at address 0x29 43 | 44 | Once this has been done, all example sketches will build for the selected IMU. 45 | 46 | To enable a pressure sensor, uncomment one of the following lines in libraries/RTIMULib/RTIMULibDefs.h: 47 | 48 | //#define BMP180 // BMP180 49 | //#define LPS25H_5c // LPS25H at standard address 50 | //#define LPS25H_5d // LPS25H at option address 51 | //#define MS5611_76 // MS5611 at standard address 52 | //#define MS5611_77 // MS5611 at option address 53 | 54 | 55 | The actual RTIMULib and support libraries are in the library directory. The other top level directories contain example sketches. 56 | 57 | *** Important note *** 58 | It is essential to calibrate the magnetometers (except for the BNO055 IMU) or else very poor results will obtained, especially with the MPU-9150 and MPU-9250. If odd results are being obtained, suspect the magnetometer calibration! 59 | 60 | ### Special notes for the BNO055 61 | 62 | The Bosch BNO055 can perform onchip fusion and also handles magnetometer calibration. Therefore, ArduinoMagCal need not be used. If the ArduinoIMU sketch is used, RTFusion RTQF performs the fusion using the BNO055's sensors. If the ArduinoBNO055 sketch is used, the BNO055's onchip fusion results are used. This results in a small flash memory footprint of approximately 11.5k bytes. 63 | 64 | ## The Example Sketches 65 | 66 | ### Build and run 67 | 68 | To build and run the example sketches, start the Arduino IDE and use File --> Preferences and then set the sketchbook location to: 69 | 70 | .../RTIMULib-Arduino 71 | 72 | where "..." represents the path to the RTIMULib-Arduino directory. The directory is set up so that there's no need to copy the libraries into the main Arduino libraries directory although this can be done if desired. 73 | 74 | ### ArduinoMagCal 75 | 76 | This sketch can be used to calibrate the magnetometers and should be run before trying to generate fused pose data. It also needs to be rerun at any time that the configuration is changed (such as different IMU or different IMU reference orientation). Load the sketch and waggle the IMU around, making sure all axes reach their minima and maxima. The display will stop updating when this occurs. Then, enter 's' followed by enter into the IDE serial monitor to save the data. 77 | 78 | ### ArduinoIMU 79 | 80 | ArduinoIMU is the main demo sketch. It configures the IMU based on settings in RTIMUSettings.cpp. Change these to alter any of the parameters. The display is updated only 3 times per second regardless of IMU sample rate. 81 | 82 | Note that, prior to version 2.2.0, the gyro bias is being calculated during the first 5 seconds. If the IMU is moved during this period, the bias calculation may be incorrect and the code will need to be restarted. Starting at version 2.2.0 this is no longer a problem and gyro bias will be reported as valid after the required number of stable samples have been obtained. 83 | 84 | If using this sketch with the BNO055, RTFusionRTQF performs the fusion and the BNO055's internal fusion results are not used. Magnetometer calibration data, if present, is also not used as the BNO055 performs this onchip. 85 | 86 | ### ArduinoBNO055 87 | 88 | This is a special version of ArduinoIMU for the BNO055 that uses the IMU's internal fusion results. It is still necessary to uncomment the correct BNO055 IMU address option in RTIMULibDefs.h. No magnetometer calibration is required as this is performed by the BNO055. 89 | 90 | ### ArduinoIMU10 91 | 92 | This is exactly the same as ArduinoIMU except that it adds support for a pressure sensor. One of the pressure sensors in libraries/RTIMULib/RTIMULibDefs.h must be uncommented for this sketch to run. It will display the current pressure and height above standard sea level in addition to pose information from the IMU. 93 | 94 | ### ArduinoAccel 95 | 96 | This is similar to ArduinoIMU except that it subtracts the rotated gravity vector from the accelerometer outputs in order to obtain the residual accelerations - i.e. those not attributable to gravity. 97 | 98 | ### RTArduLinkIMU 99 | 100 | This sketch sends the fused data from the IMU over the Arduino's USB serial link to a host computer running either RTHostIMU or RTHostIMUGL (whcih can be found in the main RTIMULib repo). Basically just build and download the sketch and that's all that needs to be done. Magnetometer calibration can be performed either on the Arduino or within RTHostIMU/RTHostIMUGL. 101 | 102 | -------------------------------------------------------------------------------- /RTArduLinkIMU/RTArduLinkIMU.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | 25 | #ifndef RTARDULINKIMU_H_ 26 | #define RTARDULINKIMU_H_ 27 | 28 | #include "Arduino.h" 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | void loop(); 34 | void setup(); 35 | #ifdef __cplusplus 36 | } // extern "C" 37 | #endif 38 | 39 | class RTArduLinkIMU : public RTArduLink 40 | { 41 | protected: 42 | void processCustomMessage(unsigned char messageType, unsigned char messageParam, 43 | unsigned char *data, int length); 44 | }; 45 | 46 | 47 | #endif /* RTARDULINKIMU_H_ */ 48 | -------------------------------------------------------------------------------- /RTArduLinkIMU/RTArduLinkIMU.ino: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #define RTARDULINK_MODE 25 | 26 | #include 27 | #include 28 | #include "I2Cdev.h" 29 | 30 | #include "RTArduLinkDefs.h" 31 | #include "RTArduLinkHAL.h" 32 | #include "RTArduLink.h" 33 | #include "RTArduLinkUtils.h" 34 | 35 | #include "RTArduLinkIMUDefs.h" 36 | 37 | #include "RTArduLinkIMU.h" 38 | #include "RTIMUSettings.h" 39 | #include "RTIMU.h" 40 | #include "CalLib.h" 41 | 42 | RTIMU *imu; // the IMU object 43 | RTIMUSettings settings; // the settings object 44 | RTArduLinkIMU linkIMU; // the link object 45 | RTARDULINKIMU_MESSAGE linkMessage; // the message that is sent to the host 46 | 47 | // SERIAL_PORT_SPEED defines the speed to use for the serial port 48 | 49 | #define SERIAL_PORT_SPEED 115200 50 | 51 | void setup() 52 | { 53 | int errcode; 54 | 55 | Serial.begin(SERIAL_PORT_SPEED); 56 | Wire.begin(); 57 | linkIMU.begin(":RTArduLinkIMU"); 58 | 59 | imu = RTIMU::createIMU(&settings); // create the imu object 60 | 61 | Serial.print("RTArduLinkIMU starting using device "); Serial.println(imu->IMUName()); 62 | if ((errcode = imu->IMUInit()) < 0) { 63 | Serial.print("Failed to init IMU: "); Serial.println(errcode); 64 | } 65 | 66 | if (imu->getCalibrationValid()) 67 | Serial.println("Using compass calibration"); 68 | else 69 | Serial.println("No valid compass calibration data"); 70 | 71 | RTArduLinkHALEEPROMDisplay(); 72 | 73 | linkIMU.sendDebugMessage("RTArduLinkIMU starting"); 74 | } 75 | 76 | void loop() 77 | { 78 | unsigned char state; 79 | 80 | linkIMU.background(); 81 | if (imu->IMURead()) { // get the latest data if ready yet 82 | // build message 83 | RTArduLinkConvertLongToUC4(millis(), linkMessage.timestamp); 84 | linkMessage.gyro[0] = imu->getGyro().x(); 85 | linkMessage.gyro[1] = imu->getGyro().y(); 86 | linkMessage.gyro[2] = imu->getGyro().z(); 87 | linkMessage.accel[0] = imu->getAccel().x(); 88 | linkMessage.accel[1] = imu->getAccel().y(); 89 | linkMessage.accel[2] = imu->getAccel().z(); 90 | linkMessage.mag[0] = imu->getCompass().x(); 91 | linkMessage.mag[1] = imu->getCompass().y(); 92 | linkMessage.mag[2] = imu->getCompass().z(); 93 | 94 | state = 0; 95 | if (imu->IMUGyroBiasValid()) 96 | state |= RTARDULINKIMU_STATE_GYRO_BIAS_VALID; 97 | if (imu->getCalibrationValid()) 98 | state |= RTARDULINKIMU_STATE_MAG_CAL_VALID; 99 | 100 | // send the message 101 | linkIMU.sendMessage(RTARDULINK_MESSAGE_IMU, state, 102 | (unsigned char *)(&linkMessage), sizeof(RTARDULINKIMU_MESSAGE)); 103 | } 104 | } 105 | 106 | void RTArduLinkIMU::processCustomMessage(unsigned char messageType, unsigned char messageParam, 107 | unsigned char *data, int length) 108 | { 109 | } 110 | 111 | -------------------------------------------------------------------------------- /RTArduLinkIMU/RTArduLinkIMUDefs.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | 25 | #ifndef RTARDULINKIMUDEFS_H_ 26 | #define RTARDULINKIMUDEFS_H_ 27 | 28 | #include "RTArduLinkDefs.h" 29 | 30 | // Defines for the format of the IMU record passed between the Arduino and the 31 | // host system. Both Arduino and host system should use identical copies of this file. 32 | 33 | // RTARDULINKIMU_MESSAGE is used to send messages to the host. 34 | // Note: the gyro bias and mag cal state come back in the messageParam field 35 | 36 | typedef struct 37 | { 38 | RTARDULINK_UC4 timestamp; // timestamp in mS 39 | float gyro[3]; // the de-biased gyro data in rads/sec 40 | float accel[3]; // raw accel data in gs 41 | float mag[3]; // magnetometer data in uT 42 | } RTARDULINKIMU_MESSAGE; 43 | 44 | 45 | // Message type 46 | 47 | #define RTARDULINK_MESSAGE_IMU (RTARDULINK_MESSAGE_CUSTOM + 1) 48 | 49 | // Defines for the messageParam field 50 | 51 | #define RTARDULINKIMU_STATE_GYRO_BIAS_VALID 1 // bit 0 set if bias is valid 52 | #define RTARDULINKIMU_STATE_MAG_CAL_VALID 2 // bit 1 set if mag calibration is valid 53 | 54 | #endif /* RTARDULINKIMUDEFS_H_ */ 55 | -------------------------------------------------------------------------------- /libraries/CalLib/CalLib.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include "CalLib.h" 25 | #ifdef __SAM3X8E__ 26 | 27 | // Due version 28 | 29 | #include "DueFlash.h" 30 | 31 | DueFlash flash; 32 | 33 | void calLibErase(byte device) 34 | { 35 | uint32_t data = 0; 36 | 37 | flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, &data, 1); // just destroy the valid byte 38 | } 39 | 40 | void calLibWrite(byte device, CALLIB_DATA *calData) 41 | { 42 | calData->validL = CALLIB_DATA_VALID_LOW; 43 | calData->validH = CALLIB_DATA_VALID_HIGH; 44 | 45 | flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, (uint32_t *)calData, sizeof(CALLIB_DATA) / 4); 46 | } 47 | 48 | boolean calLibRead(byte device, CALLIB_DATA *calData) 49 | { 50 | memcpy(calData, CALLIB_START + sizeof(CALLIB_DATA) * device, sizeof(CALLIB_DATA)); 51 | return calData->valid == CALLIB_DATA_VALID; 52 | } 53 | 54 | #else 55 | 56 | // AVR version 57 | 58 | #include 59 | 60 | void calLibErase(byte device) 61 | { 62 | EEPROM.write(sizeof(CALLIB_DATA) * device, 0); // just destroy the valid byte 63 | } 64 | 65 | void calLibWrite(byte device, CALLIB_DATA *calData) 66 | { 67 | byte *ptr = (byte *)calData; 68 | byte length = sizeof(CALLIB_DATA); 69 | int eeprom = sizeof(CALLIB_DATA) * device; 70 | 71 | calData->validL = CALLIB_DATA_VALID_LOW; 72 | calData->validH = CALLIB_DATA_VALID_HIGH; 73 | 74 | for (byte i = 0; i < length; i++) 75 | EEPROM.write(eeprom + i, *ptr++); 76 | } 77 | 78 | boolean calLibRead(byte device, CALLIB_DATA *calData) 79 | { 80 | byte *ptr = (byte *)calData; 81 | byte length = sizeof(CALLIB_DATA); 82 | int eeprom = sizeof(CALLIB_DATA) * device; 83 | 84 | calData->magValid = false; 85 | 86 | if ((EEPROM.read(eeprom) != CALLIB_DATA_VALID_LOW) || 87 | (EEPROM.read(eeprom + 1) != CALLIB_DATA_VALID_HIGH)) { 88 | return false; // invalid data 89 | } 90 | 91 | for (byte i = 0; i < length; i++) 92 | *ptr++ = EEPROM.read(eeprom + i); 93 | return true; 94 | } 95 | #endif 96 | -------------------------------------------------------------------------------- /libraries/CalLib/CalLib.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _CALLIB_H_ 25 | #define _CALLIB_H_ 26 | 27 | #include 28 | 29 | #define CALLIB_DATA_VALID_LOW 0xfc // pattern to detect valid config - low byte 30 | #define CALLIB_DATA_VALID_HIGH 0x15 // pattern to detect valid config - high byte 31 | 32 | #ifdef __SAM3X8E__ 33 | #define CALLIB_START ((uint32_t *)(IFLASH1_ADDR + IFLASH1_SIZE - IFLASH1_PAGE_SIZE)) 34 | #endif 35 | 36 | typedef struct 37 | { 38 | unsigned char validL; // should contain the valid pattern if a good config 39 | unsigned char validH; // should contain the valid pattern if a good config 40 | unsigned char magValid; // true if data valid 41 | unsigned char pad; 42 | float magMin[3]; // min values 43 | float magMax[3]; // max values 44 | } CALLIB_DATA; 45 | 46 | // calLibErase() erases any current data in the EEPROM 47 | 48 | void calLibErase(byte device); 49 | 50 | // calLibWrite() writes new data to the EEPROM 51 | 52 | void calLibWrite(byte device, CALLIB_DATA * calData); 53 | 54 | // calLibRead() reads existing data and returns true if valid else false in not. 55 | 56 | boolean calLibRead(byte device, CALLIB_DATA * calData); 57 | 58 | #endif // _CALLIB_H_ 59 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLink.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | #ifndef _RTARDULINK_H 32 | #define _RTARDULINK_H 33 | 34 | #include "RTArduLinkDefs.h" 35 | #include "RTArduLinkHAL.h" 36 | 37 | #define RTARDULINK_HOST_PORT 0 // host port is always 0 38 | #define RTARDULINK_DAISY_PORT 1 // daisy chain port is always 1 39 | 40 | typedef struct 41 | { 42 | int index; // port index 43 | bool inUse; // true if in use 44 | RTARDULINK_RXFRAME RXFrame; // structure to maintain receive frame state 45 | RTARDULINK_FRAME RXFrameBuffer; // used to assemble received frames 46 | RTARDULINKHAL_PORT portHAL; // the actual hardware port interface 47 | } RTARDULINK_PORT; 48 | 49 | class RTArduLink 50 | { 51 | public: 52 | RTArduLink(); 53 | virtual ~RTArduLink(); 54 | 55 | void begin(const char *identitySuffix); // should be called in setup() code 56 | void background(); // should be called once per loop() 57 | void sendDebugMessage(const char *debugMesssage); // sends a debug message to the host port 58 | void sendMessage(unsigned char messageType, unsigned char messageParam, 59 | unsigned char *data, int length); // sends a message to the host port 60 | 61 | protected: 62 | // These are functions that can be overridden 63 | 64 | virtual void processCustomMessage(unsigned char messageType, 65 | unsigned char messageParam, unsigned char *data, int dataLength) {} 66 | 67 | RTARDULINK_PORT m_ports[RTARDULINKHAL_MAX_PORTS]; // port array 68 | RTARDULINK_PORT *m_hostPort; // a link to the entry for the host port 69 | 70 | 71 | private: 72 | void processReceivedMessage(RTARDULINK_PORT *port); // process a completed message 73 | void processHostMessage(); // special case for stuff received from the host port 74 | void sendFrame(RTARDULINK_PORT *portInfo, RTARDULINK_FRAME *frame, int length); // send a frame to the host. length is length of data field 75 | 76 | const char *m_identitySuffix; // what to add to the EEPROM identity string 77 | 78 | }; 79 | 80 | #endif // _RTARDULINK_H 81 | 82 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLinkDefs.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | // The RTArduLink communications protocol works by exchanging frames across the host <-> subsystem interface. 32 | // The structure RTARDULINK_FRAME defines the frame structure. There is a 4 byte header for frame level control 33 | // while the remainder is available for higher level message. Note that the structure implies fixed length 34 | // buffers which works well with the subsystem however only the number of bytes actually used are transferred 35 | // across the interface. 36 | // 37 | // Note that there is no flow control at the frame level - it is assumed that the higher level interactions host -> subsystem 38 | // are window 1 acknowledged transfers so that the maximum possible unprocessed frames is equal to the number 39 | // of higher level services. Subsystem -> host transfers are always either responses to host commands or else regular 40 | // status updates so the rate from subsystem to host is controlled by configuration. 41 | // 42 | // The frame integrity is protected by a single byte checksum. To keep things very simple, it is the 2s complement 43 | // of the 8 bit sum of all the bytes in the message array. This is used in conjunction with 0xAA and 0x55 bytes 44 | // to determine correct sync (in case of lost bytes which should not really happen!). 45 | // 46 | // Frame sync is obtained by reading bytes until the 0xAA pattern is seen. If the next byte is not 0x55, keep 47 | // scanning. If it is, assume this byte is messageLength and the next one is the frameCksm value. Read in the 48 | // message array based on messageLength and then calculate the checksum. If the checksum is correct, sync has been 49 | // obtained and the message is valid. Otherwise, start looking for an 0xAA value again. 50 | 51 | #ifndef _RTARDULINKDEFS_H 52 | #define _RTARDULINKDEFS_H 53 | 54 | // Some defines to cope with compiler differences 55 | 56 | #ifndef __cplusplus 57 | #ifndef false 58 | #define false 0 59 | #endif 60 | 61 | #ifndef true 62 | #define true 1 63 | #endif 64 | 65 | typedef unsigned char bool; 66 | #endif 67 | 68 | #ifndef NULL 69 | #define NULL 0 70 | #endif 71 | 72 | 73 | // Some general purpose typedefs - used especially for transferring values greater than 74 | // 8 bits across the link and avoids endian issues. Assumes processor has 32 bit ints! 75 | 76 | typedef unsigned char RTARDULINK_UC2[2]; // an array of two unsigned chars 77 | typedef unsigned char RTARDULINK_UC4[4]; // an array of four unsigned chars 78 | 79 | // Port speed codes 80 | 81 | #define RTARDULINK_PORT_SPEED_OFF 0 // port is unused 82 | #define RTARDULINK_PORT_SPEED_9600 1 // 9600 baud 83 | #define RTARDULINK_PORT_SPEED_19200 2 // 19200 baud 84 | #define RTARDULINK_PORT_SPEED_38400 3 // 38400 baud 85 | #define RTARDULINK_PORT_SPEED_57600 4 // 57600 baud 86 | #define RTARDULINK_PORT_SPEED_115200 5 // 115200 baud 87 | 88 | #define RTARDULINK_PORT_SPEED_COUNT 6 // six codes total 89 | 90 | extern unsigned long RTArduLinkSpeedMap[]; 91 | 92 | //------------------------------------------------------------------------------------------------------ 93 | // 94 | // Frame level defs and structure 95 | 96 | #define RTARDULINK_FRAME_MAX_LEN 64 // maximum possible length of a frame 97 | #define RTARDULINK_FRAME_HEADER_LEN 4 // 4 bytes in frame header (must correspond with the structure below!) 98 | #define RTARDULINK_MESSAGE_HEADER_LEN 4 // 4 bytes in message header (must correspond with the structure below!) 99 | #define RTARDULINK_MESSAGE_MAX_LEN (RTARDULINK_FRAME_MAX_LEN - RTARDULINK_FRAME_HEADER_LEN) // max length of message 100 | #define RTARDULINK_DATA_MAX_LEN (RTARDULINK_MESSAGE_MAX_LEN - RTARDULINK_MESSAGE_HEADER_LEN)// max length of data field 101 | 102 | #define RTARDULINK_MESSAGE_SYNC0 0xAA 103 | #define RTARDULINK_MESSAGE_SYNC1 0x55 104 | 105 | #define RTARDULINK_MY_ADDRESS 0 // the subsystem address for local processing 106 | #define RTARDULINK_BROADCAST_ADDRESS 0xffff // the subsystem address for all subsystems 107 | #define RTARDULINK_ADDRESSES 0x1000 // number of addresses 108 | 109 | // RTARDULINK_MESSAGE is carried in the RTARDULINK_FRAME 110 | // 111 | // The messageAddress field allows subsystems to be daisy-chained. Valid addresses are 0 to 65534. 112 | // Address 65535 is a broadcast and goes to all subsystems. 113 | // Every message has the messageType and messageParam bytes but there can be from 0 to 56 bytes of data 114 | 115 | typedef struct 116 | { 117 | RTARDULINK_UC2 messageAddress; // subsystem message address 118 | unsigned char messageType; // message type code 119 | unsigned char messageParam; // an optional parameter to the message type 120 | unsigned char data[RTARDULINK_DATA_MAX_LEN]; // the actual data! Length is computed from messageLength. 121 | } RTARDULINK_MESSAGE; 122 | 123 | // RTARDULINK_FRAME is the lowest level structure used across the RTArduLink 124 | 125 | typedef struct 126 | { 127 | unsigned char sync0; // sync0 code 128 | unsigned char sync1; // sync1 code 129 | unsigned char messageLength; // the length of the message in the message field - between 4 and 60 bytes 130 | unsigned char frameChecksum; // checksum for frame 131 | RTARDULINK_MESSAGE message; // the actual message 132 | } RTARDULINK_FRAME; 133 | 134 | // RTARDULINK_RXFRAME is a type that is used to reassemble a frame from a stream of bytes in conjunction with RTArduLinkReassemble() 135 | 136 | typedef struct 137 | { 138 | RTARDULINK_FRAME *frameBuffer; // the frame buffer pointer 139 | int length; // current length of frame 140 | int bytesLeft; // number of bytes needed to complete 141 | bool complete; // true if frame is complete and correct (as far as checksum goes) 142 | } RTARDULINK_RXFRAME; 143 | 144 | // Message types 145 | 146 | // RTARDULINK_MESSAGE_POLL 147 | // 148 | // The host should poll the RTArduLink at every RTARDULINK_POLL_INTERVAL. 149 | // The subsystem will respond by echoing the poll message as received. 150 | 151 | #define RTARDULINK_MESSAGE_POLL 0 // poll message 152 | 153 | // RTARDULINK_MESSAGE_IDENTIFY 154 | // 155 | // The host can send this message to request an identity string from the subsystem. 156 | // Only the messageType field is used in the request host -> subsystem. The subsystem 157 | // responds with an identity string in the data field. 158 | 159 | #define RTARDULINK_MESSAGE_IDENTITY 1 // identity message 160 | 161 | // RTARDULINK_MESSAGE_DEBUG 162 | // 163 | // This can be used to send a debug message up to the host. The data field contains a debug message 164 | 165 | #define RTARDULINK_MESSAGE_DEBUG 2 // debug message 166 | 167 | // RTARDULINK_MESSAGE_INFO 168 | // 169 | // This can be used to send an info message up to the host. The data field contains the message 170 | 171 | #define RTARDULINK_MESSAGE_INFO 3 // info message 172 | 173 | // RTARDULINK_MESSAGE_ERROR 174 | // 175 | // This code is returned by the subsystem if it received a message with an illegal message type 176 | // The first byte of the data is the error code. The rest of the data field depends on the error. 177 | 178 | #define RTARDULINK_MESSAGE_ERROR 4 // illegal message type response 179 | 180 | // RTARDULINK_MESSAGE_ECHO 181 | // 182 | // This message can be used to test link performance. The addressed subsystem just returns 183 | // the entire message to the host. 184 | 185 | #define RTARDULINK_MESSAGE_ECHO 5 // echo message 186 | 187 | // RTARDULINK_MESSAGE_CUSTOM 188 | // 189 | // This is the first message code that should be used for custom messages 16-255 are available. 190 | 191 | #define RTARDULINK_MESSAGE_CUSTOM 16 // start of custom messages 192 | 193 | // RTArduLink response codes 194 | 195 | #define RTARDULINK_RESPONSE_OK 0 // means things worked 196 | #define RTARDULINK_RESPONSE_ILLEGAL_COMMAND 1 // not a supported message type, data[1] has offending type 197 | 198 | 199 | #endif // _RTARDULINKDEFS_H 200 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLinkDemoDefs.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | #ifndef RTARDULINKDEMODEFS_H_ 32 | #define RTARDULINKDEMODEFS_H_ 33 | 34 | #include "RTArduLinkUtils.h" 35 | 36 | #define SERVO_COUNT 2 // 2 servo channels 37 | #define PWM_COUNT 3 // 3 pwm channels 38 | #define INPUT_COUNT 2 // 2 inputs 39 | #define OUTPUT_COUNT 2 // 2 outputs 40 | 41 | #define SERVO_MIN_VALUE 1000 // min servo value 42 | #define SERVO_CTR_VALUE 1500 // center servo value 43 | #define SERVO_MAX_VALUE 2000 // max servo value 44 | 45 | #define PWM_MIN_VALUE 0 // min pwm value 46 | #define PWM_CTR_VALUE 128 // center pwm value 47 | #define PWM_MAX_VALUE 255 // max pwm value 48 | 49 | // The command structure is sent from the host to the subsystem 50 | 51 | typedef struct 52 | { 53 | RTARDULINK_UC2 servoPos[SERVO_COUNT]; // the servo positions 54 | unsigned char pwmValue[PWM_COUNT]; // PWM values 55 | unsigned char outputValue[OUTPUT_COUNT]; // the output pin values (true=high, false=low) 56 | } RTARDULINKDEMO_COMMAND; 57 | 58 | // the response structure is sent from the subsystem to the host 59 | 60 | typedef struct 61 | { 62 | unsigned char inputValue[INPUT_COUNT]; // the input pin values (true=high, false=low) 63 | } RTARDULINKDEMO_RESPONSE; 64 | 65 | 66 | #endif /* RTARDULINKDEMODEFS_H_ */ 67 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLinkHAL.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | #include 32 | #include "RTArduLinkHAL.h" 33 | 34 | //---------------------------------------------------------- 35 | // 36 | // Arduino HAL 37 | 38 | 39 | // The global config structure 40 | 41 | RTARDULINKHAL_EEPROM RTArduLinkHALConfig; 42 | 43 | bool RTArduLinkHALAddHardwarePort(RTARDULINKHAL_PORT *port, long portSpeed, unsigned char hardwarePort); 44 | 45 | // Port speed map array 46 | 47 | unsigned long RTArduLinkHALSpeedMap[] = {0, 9600, 19200, 38400, 57600, 115200}; 48 | 49 | 50 | bool RTArduLinkHALConfigurePort(RTARDULINKHAL_PORT *port, int portIndex) 51 | { 52 | if (RTArduLinkHALConfig.portSpeed[portIndex] == RTARDULINK_PORT_SPEED_OFF) 53 | return false; // port is not enabled 54 | 55 | return RTArduLinkHALAddHardwarePort(port, RTArduLinkHALSpeedMap[RTArduLinkHALConfig.portSpeed[portIndex]], 56 | RTArduLinkHALConfig.hardwarePort[portIndex]); 57 | } 58 | 59 | int RTArduLinkHALPortAvailable(RTARDULINKHAL_PORT *port) 60 | { 61 | return port->serialPort->available(); 62 | } 63 | 64 | unsigned char RTArduLinkHALPortRead(RTARDULINKHAL_PORT *port) 65 | { 66 | return port->serialPort->read(); 67 | } 68 | 69 | void RTArduLinkHALPortWrite(RTARDULINKHAL_PORT *port, unsigned char *data, unsigned char length) 70 | { 71 | port->serialPort->write(data, length); 72 | } 73 | 74 | 75 | bool RTArduLinkHALAddHardwarePort(RTARDULINKHAL_PORT *port, long portSpeed, unsigned char hardwarePort) 76 | { 77 | HardwareSerial *hardPort; 78 | 79 | switch (hardwarePort) { 80 | case 0: 81 | #if defined(USBCON) 82 | /* Leonardo support */ 83 | hardPort = &Serial1; 84 | #else 85 | hardPort = &Serial; 86 | #endif 87 | break; 88 | 89 | case 1: 90 | #if defined(UBRR1H) 91 | hardPort = &Serial1; 92 | #else 93 | return false; 94 | #endif 95 | break; 96 | 97 | case 2: 98 | #if defined(UBRR2H) 99 | hardPort = &Serial2; 100 | #else 101 | return false; 102 | #endif 103 | break; 104 | 105 | case 3: 106 | #if defined(UBRR3H) 107 | hardPort = &Serial3; 108 | #else 109 | return false; 110 | #endif 111 | break; 112 | 113 | default: 114 | return false; 115 | } 116 | 117 | port->serialPort = hardPort; 118 | hardPort->begin(portSpeed); // start the port 119 | return true; 120 | } 121 | 122 | 123 | bool RTArduLinkHALEEPROMValid() 124 | { 125 | RTArduLinkHALEEPROMRead(); // see what it really is 126 | return (RTArduLinkHALConfig.sig0 == RTARDULINKHAL_SIG0) && 127 | (RTArduLinkHALConfig.sig1 == RTARDULINKHAL_SIG1); 128 | } 129 | 130 | void RTArduLinkHALEEPROMDisplay() 131 | { 132 | Serial.println(); 133 | 134 | if ((RTArduLinkHALConfig.sig0 != RTARDULINKHAL_SIG0) || 135 | (RTArduLinkHALConfig.sig1 != RTARDULINKHAL_SIG1)) { 136 | Serial.println("Invalid config"); 137 | return; 138 | } 139 | Serial.print("Identity: "); 140 | Serial.println(RTArduLinkHALConfig.identity); 141 | 142 | for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++) 143 | RTArduLinkHALEEPROMDisplayPort(i, true); 144 | } 145 | 146 | void RTArduLinkHALEEPROMDisplayPort(int index, bool suppress) 147 | { 148 | if (suppress && (RTArduLinkHALConfig.portSpeed[index] == RTARDULINK_PORT_SPEED_OFF)) 149 | return; 150 | Serial.print("Port index "); 151 | Serial.print(index); 152 | Serial.print(" speed="); 153 | Serial.print(RTArduLinkHALConfig.portSpeed[index]); 154 | Serial.print(", "); 155 | Serial.print("hardware port number="); 156 | Serial.println(RTArduLinkHALConfig.hardwarePort[index]); 157 | } 158 | 159 | 160 | void RTArduLinkHALEEPROMDefault() 161 | { 162 | RTArduLinkHALConfig.sig0 = RTARDULINKHAL_SIG0; // set to valid signature 163 | RTArduLinkHALConfig.sig1 = RTARDULINKHAL_SIG1; 164 | strcpy(RTArduLinkHALConfig.identity, "RTArduLink_Arduino"); 165 | 166 | RTArduLinkHALConfig.portSpeed[0] = RTARDULINK_PORT_SPEED_115200; 167 | for (int i = 1; i < RTARDULINKHAL_MAX_PORTS; i++) 168 | RTArduLinkHALConfig.portSpeed[i] = RTARDULINK_PORT_SPEED_OFF; 169 | 170 | for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++) 171 | RTArduLinkHALConfig.hardwarePort[i] = i; 172 | 173 | RTArduLinkHALEEPROMWrite(); 174 | } 175 | 176 | void RTArduLinkHALEEPROMRead() 177 | { 178 | unsigned char *data; 179 | 180 | data = (unsigned char *)&RTArduLinkHALConfig; 181 | 182 | for (int i = 0; i < (int)sizeof(RTARDULINKHAL_EEPROM); i++) 183 | *data++ = EEPROM.read(i + RTARDULINKHAL_EEPROM_OFFSET); 184 | } 185 | 186 | void RTArduLinkHALEEPROMWrite() 187 | { 188 | unsigned char *data; 189 | 190 | data = (unsigned char *)&RTArduLinkHALConfig; 191 | 192 | for (int i = 0; i < (int)sizeof(RTARDULINKHAL_EEPROM); i++) 193 | EEPROM.write(i + RTARDULINKHAL_EEPROM_OFFSET, *data++); 194 | } 195 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLinkHAL.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | #ifndef _RTARDULINKHAL_H 32 | #define _RTARDULINKHAL_H 33 | 34 | 35 | //---------------------------------------------------------- 36 | // Target-specific includes 37 | // 38 | // 39 | // Arduino HAL 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #define RTARDULINKHAL_MAX_SUBSYSTEM_PORTS 3 // maximum number of subsystem ports 46 | #define RTARDULINKHAL_MAX_PORTS (RTARDULINKHAL_MAX_SUBSYSTEM_PORTS + 1) // max total ports (including host) 47 | #define RTARDULINKHAL_EEPROM_OFFSET 256 // where the config starts in EEPROM 48 | 49 | // RTARDULINKHAL_PORT should be modified as appropriate for the target. 50 | // There is one copy of this per port. It contains all state needed about 51 | // a serial port. 52 | 53 | typedef struct 54 | { 55 | HardwareSerial *serialPort; // the serial port structure 56 | } RTARDULINKHAL_PORT; 57 | 58 | // RTARDULINKHAL_EEPROM is the target-specific structure used to 59 | // store configs in EEPROM 60 | 61 | // Signature bytes indicating valid config 62 | 63 | #define RTARDULINKHAL_SIG0 0x38 64 | #define RTARDULINKHAL_SIG1 0xc1 65 | 66 | typedef struct 67 | { 68 | unsigned char sig0; // signature byte 0 69 | unsigned char sig1; // signature byte 1 70 | char identity[RTARDULINK_DATA_MAX_LEN]; // identity string 71 | unsigned char portSpeed[RTARDULINKHAL_MAX_PORTS]; // port speed codes 72 | unsigned char hardwarePort[RTARDULINKHAL_MAX_PORTS]; // port number for hardware serial 73 | } RTARDULINKHAL_EEPROM; 74 | 75 | // The global config structure 76 | 77 | extern RTARDULINKHAL_EEPROM RTArduLinkHALConfig; 78 | 79 | 80 | 81 | //---------------------------------------------------------- 82 | // 83 | // These functions must be provided the RTArduLinkHAL for all implementations 84 | 85 | // RTArduLinkHALConfigurePort() activates the specified port configuration specified by portIndex in port structure port. 86 | 87 | bool RTArduLinkHALConfigurePort(RTARDULINKHAL_PORT *port, int portIndex); 88 | 89 | 90 | // RTArduLinkHALPortAvailable() returns the number of bytes availabel on the specified port. 91 | 92 | int RTArduLinkHALPortAvailable(RTARDULINKHAL_PORT *port); 93 | 94 | 95 | // RTArduLinkHALPortRead() returns the next available byte from a port. Always check available bytes first 96 | 97 | unsigned char RTArduLinkHALPortRead(RTARDULINKHAL_PORT *port); 98 | 99 | 100 | // RTArduLinkHALPortWrite() writes length bytes of the block pointed to by data to the specified port. 101 | 102 | void RTArduLinkHALPortWrite(RTARDULINKHAL_PORT *port, unsigned char *data, unsigned char length); 103 | 104 | 105 | // RTArduLinkHALEEPROMValid() returns true if the EEPROM contains a valid configuration, 106 | // false otherwise. 107 | 108 | bool RTArduLinkHALEEPROMValid(); // returns true if a valid config 109 | 110 | 111 | // RTArduLinkHALEEPROMDisplay() displays the current configuration 112 | 113 | void RTArduLinkHALEEPROMDisplay(); // display the config 114 | 115 | 116 | // RTArduLinkHALEEPROMDisplayPort() displays the configuration for a single port 117 | // If suppress is true, nothing is displayed if the port is not enabled. If false 118 | // the port's data will be displayed regardless. 119 | 120 | void RTArduLinkHALEEPROMDisplayPort(int port, bool suppress); // display the port config 121 | 122 | 123 | // RTArduLinkHALEEPROMDefault() writes a default config to EEPROM 124 | 125 | void RTArduLinkHALEEPROMDefault(); // write and load default settings 126 | 127 | 128 | // RTArduLinkHALEEPROMRead() loads the EEPROM config into the RTArduLinkHALConfig 129 | // global structure. 130 | 131 | void RTArduLinkHALEEPROMRead(); // to load the config 132 | 133 | 134 | // RTArduLinkHALEEPROMWrite() writes the config in the RTArduLinkHALConfig 135 | // global structure back to EEPROM. 136 | 137 | void RTArduLinkHALEEPROMWrite(); // to write the config 138 | 139 | 140 | #endif // _RTARDULINKHAL_H 141 | 142 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLinkUtils.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | #include "RTArduLinkUtils.h" 32 | 33 | // RTArduLinkRXFrameInit initializes the structure for a new frame using frameBuffer for storage 34 | 35 | void RTArduLinkRXFrameInit(RTARDULINK_RXFRAME *RXFrame, RTARDULINK_FRAME *frameBuffer) 36 | { 37 | RXFrame->complete = false; 38 | RXFrame->length = 0; 39 | RXFrame->bytesLeft = 0; 40 | RXFrame->frameBuffer = frameBuffer; 41 | } 42 | 43 | // RTArduLinkReassemble takes a sequence of received bytes and tries to complete a frame. It returns true if ok 44 | // false if error. The caller can determine if the frame is complete by checking the complete flag. 45 | 46 | bool RTArduLinkReassemble(RTARDULINK_RXFRAME *RXFrame, unsigned char data) 47 | { 48 | bool flag = true; 49 | 50 | ((unsigned char *)(RXFrame->frameBuffer))[RXFrame->length] = data; // save byte in correct place 51 | switch (RXFrame->length) { 52 | case 0: // waiting for sync0 53 | if (RXFrame->frameBuffer->sync0 == RTARDULINK_MESSAGE_SYNC0) { 54 | RXFrame->length = 1; 55 | } 56 | break; 57 | 58 | case 1: // waiting for sync1 59 | if (RXFrame->frameBuffer->sync1 == RTARDULINK_MESSAGE_SYNC1) { 60 | RXFrame->length = 2; 61 | } else { 62 | RXFrame->length = 0; // try again if not correct two byte sequence 63 | } 64 | break; 65 | 66 | case 2: // should be message length 67 | if (RXFrame->frameBuffer->messageLength <= RTARDULINK_MESSAGE_MAX_LEN) { 68 | RXFrame->length = 3; 69 | RXFrame->bytesLeft = RXFrame->frameBuffer->messageLength + 1; // +1 to allow for the checksum 70 | } else { 71 | RXFrame->length = 0; // discard this and resync frame 72 | flag = false; 73 | } 74 | break; 75 | 76 | default: 77 | RXFrame->length++; 78 | RXFrame->bytesLeft--; 79 | if (RXFrame->bytesLeft == 0) { // a complete frame! 80 | if (!RTArduLinkCheckChecksum(RXFrame->frameBuffer)) { 81 | RTArduLinkRXFrameInit(RXFrame, RXFrame->frameBuffer); 82 | flag = false; // flag the error 83 | } else { 84 | // this is a valid frame (so far) 85 | RXFrame->complete = true; 86 | } 87 | } 88 | break; 89 | } 90 | return flag; 91 | } 92 | 93 | // RTArduLinkSetChecksum correctly sets the checksum field on an RCP frame prior to transmission 94 | // 95 | 96 | void RTArduLinkSetChecksum(RTARDULINK_FRAME *frame) 97 | { 98 | int cksm; 99 | int i; 100 | unsigned char *data; 101 | 102 | for (i = 0, cksm = 0, data = (unsigned char *)&(frame->message); i < frame->messageLength; i++) 103 | cksm += *data++; // add up checksum 104 | frame->frameChecksum = (255 - cksm) + 1; // 2s complement 105 | } 106 | 107 | 108 | // RTArduLinkCheckChecksum checks a received frame's checksum. 109 | // 110 | // It adds up all the bytes from the nFrameCksm byte to the end of the frame. The result should be 0. 111 | 112 | bool RTArduLinkCheckChecksum(RTARDULINK_FRAME *frame) 113 | { 114 | int length; 115 | int i; 116 | unsigned char *data; 117 | unsigned char cksm; 118 | 119 | length = frame->messageLength + 1; 120 | cksm = 0; 121 | data = (unsigned char *)&(frame->frameChecksum); 122 | 123 | for (i = 0; i < length; i++) 124 | cksm += *data++; 125 | 126 | return cksm == 0; 127 | } 128 | 129 | // UC2 and UC4 Conversion routines 130 | // 131 | 132 | long RTArduLinkConvertUC4ToLong(RTARDULINK_UC4 UC4) 133 | { 134 | long val; 135 | 136 | val = UC4[3]; 137 | val += (long)UC4[2] << 8; 138 | val += (long)UC4[1] << 16; 139 | val += (long)UC4[0] << 24; 140 | return val; 141 | } 142 | 143 | void RTArduLinkConvertLongToUC4(long val, RTARDULINK_UC4 UC4) 144 | { 145 | UC4[3] = val & 0xff; 146 | UC4[2] = (val >> 8) & 0xff; 147 | UC4[1] = (val >> 16) & 0xff; 148 | UC4[0] = (val >> 24) & 0xff; 149 | } 150 | 151 | int RTArduLinkConvertUC2ToInt(RTARDULINK_UC2 UC2) 152 | { 153 | int val; 154 | 155 | val = UC2[1]; 156 | val += (int)UC2[0] << 8; 157 | return val; 158 | } 159 | 160 | unsigned int RTArduLinkConvertUC2ToUInt(RTARDULINK_UC2 UC2) 161 | { 162 | unsigned int val; 163 | 164 | val = UC2[1]; 165 | val += (unsigned int)UC2[0] << 8; 166 | return val; 167 | } 168 | 169 | 170 | void RTArduLinkConvertIntToUC2(int val, RTARDULINK_UC2 UC2) 171 | { 172 | UC2[1] = val & 0xff; 173 | UC2[0] = (val >> 8) & 0xff; 174 | } 175 | 176 | 177 | void RTArduLinkCopyUC2(RTARDULINK_UC2 destUC2, RTARDULINK_UC2 sourceUC2) 178 | { 179 | destUC2[0] = sourceUC2[0]; 180 | destUC2[1] = sourceUC2[1]; 181 | } 182 | -------------------------------------------------------------------------------- /libraries/RTArduLink/RTArduLinkUtils.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTArduLink 4 | // 5 | // Copyright (c) 2014-2015 richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, 8 | // to any person obtaining a copy of 9 | // this software and associated documentation files 10 | // (the "Software"), to deal in the Software without 11 | // restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, 13 | // sublicense, and/or sell copies of the Software, and 14 | // to permit persons to whom the Software is furnished 15 | // to do so, subject to the following conditions: 16 | // 17 | // The above copyright notice and this permission notice 18 | // shall be included in all copies or substantial portions 19 | // of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 22 | // ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 23 | // TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 24 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 25 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 26 | // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 27 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 28 | // IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 29 | // DEALINGS IN THE SOFTWARE. 30 | 31 | #ifndef _RTARDULINKUTILS_H 32 | #define _RTARDULINKUTILS_H 33 | 34 | #include "RTArduLinkDefs.h" 35 | 36 | // Function defs 37 | 38 | void RTArduLinkRXFrameInit(RTARDULINK_RXFRAME *RXFrame, RTARDULINK_FRAME *frameBuffer); // initializes RTARDULINK_RXFRAME for a new frame 39 | bool RTArduLinkReassemble(RTARDULINK_RXFRAME *RXFrame, unsigned char data); // adds a byte to the reassembly, returns false if error 40 | 41 | // Checksum utilities 42 | 43 | void RTArduLinkSetChecksum(RTARDULINK_FRAME *frame); // sets the checksum field prior to transmission 44 | bool RTArduLinkCheckChecksum(RTARDULINK_FRAME *frame); // checks the checksum field after reception - returns true if ok, false if error 45 | 46 | // Type conversion utilities 47 | 48 | long RTArduLinkConvertUC4ToLong(RTARDULINK_UC4 uc4); // converts a 4 byte array to a signed long 49 | void RTArduLinkConvertLongToUC4(long val, RTARDULINK_UC4 uc4); // converts a long to a four byte array 50 | int RTArduLinkConvertUC2ToInt(RTARDULINK_UC2 uc2); // converts a 2 byte array to a signed integer 51 | unsigned int RTArduLinkConvertUC2ToUInt(RTARDULINK_UC2 uc2);// converts a 2 byte array to an unsigned integer 52 | void RTArduLinkConvertIntToUC2(int val, RTARDULINK_UC2 uc2);// converts an integer to a two byte array 53 | void RTArduLinkCopyUC2(RTARDULINK_UC2 destUC2, RTARDULINK_UC2 sourceUC2); // copies a UC2 54 | 55 | #endif // _RTARDULINKUTILS_H 56 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTFusionRTQF.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef RTARDULINK_MODE 25 | 26 | #include "RTFusionRTQF.h" 27 | 28 | #ifdef USE_SLERP 29 | // The slerp power valule controls the influence of the measured state to correct the predicted state 30 | // 0 = measured state ignored (just gyros), 1 = measured state overrides predicted state. 31 | // In between 0 and 1 mixes the two conditions 32 | 33 | #define RTQF_SLERP_POWER (RTFLOAT)0.02; 34 | 35 | #else 36 | // The QVALUE affects the gyro response. 37 | 38 | #define RTQF_QVALUE (RTFLOAT)0.001 39 | 40 | // The RVALUE controls the influence of the accels and compass. 41 | // The bigger the value, the more sluggish the response. 42 | 43 | #define RTQF_RVALUE (RTFLOAT)0.0005 44 | #endif 45 | 46 | RTFusionRTQF::RTFusionRTQF() 47 | { 48 | #ifdef USE_SLERP 49 | m_slerpPower = RTQF_SLERP_POWER; 50 | #else 51 | m_Q = RTQF_QVALUE; 52 | m_R = RTQF_RVALUE; 53 | #endif 54 | m_enableGyro = true; 55 | m_enableAccel = true; 56 | m_enableCompass = true; 57 | reset(); 58 | } 59 | 60 | RTFusionRTQF::~RTFusionRTQF() 61 | { 62 | } 63 | 64 | void RTFusionRTQF::reset() 65 | { 66 | m_firstTime = true; 67 | m_fusionPose = RTVector3(); 68 | m_fusionQPose.fromEuler(m_fusionPose); 69 | m_measuredPose = RTVector3(); 70 | m_measuredQPose.fromEuler(m_measuredPose); 71 | } 72 | 73 | void RTFusionRTQF::newIMUData(const RTVector3& gyro, const RTVector3& accel, const RTVector3& compass, unsigned long timestamp) 74 | { 75 | RTVector3 fusionGyro; 76 | 77 | if (m_firstTime) { 78 | m_lastFusionTime = timestamp; 79 | calculatePose(accel, compass); 80 | 81 | // initialize the poses 82 | 83 | m_fusionQPose.fromEuler(m_measuredPose); 84 | m_fusionPose = m_measuredPose; 85 | m_firstTime = false; 86 | } else { 87 | m_timeDelta = (RTFLOAT)(timestamp - m_lastFusionTime) / (RTFLOAT)1000; 88 | m_lastFusionTime = timestamp; 89 | if (m_timeDelta <= 0) 90 | return; 91 | 92 | calculatePose(accel, compass); 93 | 94 | // predict(); 95 | 96 | RTFLOAT x2, y2, z2; 97 | RTFLOAT qs, qx, qy,qz; 98 | 99 | qs = m_fusionQPose.scalar(); 100 | qx = m_fusionQPose.x(); 101 | qy = m_fusionQPose.y(); 102 | qz = m_fusionQPose.z(); 103 | 104 | if (m_enableGyro) 105 | fusionGyro = gyro; 106 | else 107 | fusionGyro = RTVector3(); 108 | 109 | x2 = fusionGyro.x() / (RTFLOAT)2.0; 110 | y2 = fusionGyro.y() / (RTFLOAT)2.0; 111 | z2 = fusionGyro.z() / (RTFLOAT)2.0; 112 | 113 | // Predict new state 114 | 115 | m_fusionQPose.setScalar(qs + (-x2 * qx - y2 * qy - z2 * qz) * m_timeDelta); 116 | m_fusionQPose.setX(qx + (x2 * qs + z2 * qy - y2 * qz) * m_timeDelta); 117 | m_fusionQPose.setY(qy + (y2 * qs - z2 * qx + x2 * qz) * m_timeDelta); 118 | m_fusionQPose.setZ(qz + (z2 * qs + y2 * qx - x2 * qy) * m_timeDelta); 119 | 120 | // update(); 121 | 122 | #ifdef USE_SLERP 123 | if (m_enableCompass || m_enableAccel) { 124 | 125 | // calculate rotation delta 126 | 127 | m_rotationDelta = m_fusionQPose.conjugate() * m_measuredQPose; 128 | m_rotationDelta.normalize(); 129 | 130 | // take it to the power (0 to 1) to give the desired amount of correction 131 | 132 | RTFLOAT theta = acos(m_rotationDelta.scalar()); 133 | 134 | RTFLOAT sinPowerTheta = sin(theta * m_slerpPower); 135 | RTFLOAT cosPowerTheta = cos(theta * m_slerpPower); 136 | 137 | m_rotationUnitVector.setX(m_rotationDelta.x()); 138 | m_rotationUnitVector.setY(m_rotationDelta.y()); 139 | m_rotationUnitVector.setZ(m_rotationDelta.z()); 140 | m_rotationUnitVector.normalize(); 141 | 142 | m_rotationPower.setScalar(cosPowerTheta); 143 | m_rotationPower.setX(sinPowerTheta * m_rotationUnitVector.x()); 144 | m_rotationPower.setY(sinPowerTheta * m_rotationUnitVector.y()); 145 | m_rotationPower.setZ(sinPowerTheta * m_rotationUnitVector.z()); 146 | m_rotationPower.normalize(); 147 | 148 | // multiple this by predicted value to get result 149 | 150 | m_fusionQPose *= m_rotationPower; 151 | } 152 | #else 153 | if (m_enableCompass || m_enableAccel) { 154 | m_stateQError = m_measuredQPose - m_fusionQPose; 155 | } else { 156 | m_stateQError = RTQuaternion(); 157 | } 158 | // make new state estimate 159 | 160 | RTFLOAT qt = m_Q * m_timeDelta; 161 | 162 | m_fusionQPose += m_stateQError * (qt / (qt + m_R)); 163 | #endif 164 | 165 | m_fusionQPose.normalize(); 166 | 167 | m_fusionQPose.toEuler(m_fusionPose); 168 | } 169 | } 170 | 171 | void RTFusionRTQF::calculatePose(const RTVector3& accel, const RTVector3& mag) 172 | { 173 | RTQuaternion m; 174 | RTQuaternion q; 175 | 176 | bool compassValid = (mag.x() != 0) || (mag.y() != 0) || (mag.z() != 0); 177 | 178 | if (m_enableAccel) { 179 | accel.accelToEuler(m_measuredPose); 180 | } else { 181 | m_measuredPose = m_fusionPose; 182 | } 183 | 184 | if (m_enableCompass && compassValid) { 185 | RTFLOAT cosX2 = cos(m_measuredPose.x() / 2.0f); 186 | RTFLOAT sinX2 = sin(m_measuredPose.x() / 2.0f); 187 | RTFLOAT cosY2 = cos(m_measuredPose.y() / 2.0f); 188 | RTFLOAT sinY2 = sin(m_measuredPose.y() / 2.0f); 189 | 190 | q.setScalar(cosX2 * cosY2); 191 | q.setX(sinX2 * cosY2); 192 | q.setY(cosX2 * sinY2); 193 | q.setZ( - sinX2 * sinY2); 194 | 195 | // normalize(); 196 | 197 | m.setScalar(0); 198 | m.setX(mag.x()); 199 | m.setY(mag.y()); 200 | m.setZ(mag.z()); 201 | 202 | m = q * m * q.conjugate(); 203 | m_measuredPose.setZ(-atan2(m.y(), m.x())); 204 | } else { 205 | m_measuredPose.setZ(m_fusionPose.z()); 206 | } 207 | 208 | m_measuredQPose.fromEuler(m_measuredPose); 209 | 210 | // check for quaternion aliasing. If the quaternion has the wrong sign 211 | // the kalman filter will be very unhappy. 212 | 213 | int maxIndex = -1; 214 | RTFLOAT maxVal = -1000; 215 | 216 | for (int i = 0; i < 4; i++) { 217 | if (fabs(m_measuredQPose.data(i)) > maxVal) { 218 | maxVal = fabs(m_measuredQPose.data(i)); 219 | maxIndex = i; 220 | } 221 | } 222 | 223 | // if the biggest component has a different sign in the measured and kalman poses, 224 | // change the sign of the measured pose to match. 225 | 226 | if (((m_measuredQPose.data(maxIndex) < 0) && (m_fusionQPose.data(maxIndex) > 0)) || 227 | ((m_measuredQPose.data(maxIndex) > 0) && (m_fusionQPose.data(maxIndex) < 0))) { 228 | m_measuredQPose.setScalar(-m_measuredQPose.scalar()); 229 | m_measuredQPose.setX(-m_measuredQPose.x()); 230 | m_measuredQPose.setY(-m_measuredQPose.y()); 231 | m_measuredQPose.setZ(-m_measuredQPose.z()); 232 | m_measuredQPose.toEuler(m_measuredPose); 233 | } 234 | } 235 | #endif // #ifndef RTARDULINK_MODE -------------------------------------------------------------------------------- /libraries/RTIMULib/RTFusionRTQF.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTFUSIONRTQF_H 25 | #define _RTFUSIONRTQF_H 26 | 27 | #ifndef RTARDULINK_MODE 28 | 29 | #include "RTMath.h" 30 | 31 | // Define this symbol to use more scientific prediction correction 32 | 33 | #define USE_SLERP 34 | 35 | class RTFusionRTQF 36 | { 37 | public: 38 | RTFusionRTQF(); 39 | ~RTFusionRTQF(); 40 | 41 | // reset() resets the state but keeps any setting changes (such as enables) 42 | 43 | void reset(); 44 | 45 | // newIMUData() should be called for subsequent updates 46 | // deltaTime is in units of seconds 47 | 48 | void newIMUData(const RTVector3& gyro, const RTVector3& accel, const RTVector3& compass, unsigned long timestamp); 49 | 50 | // the following three functions control the influence of the gyro, accel and compass sensors 51 | 52 | void setGyroEnable(bool enable) { m_enableGyro = enable;} 53 | void setAccelEnable(bool enable) { m_enableAccel = enable; } 54 | void setCompassEnable(bool enable) { m_enableCompass = enable;} 55 | 56 | #ifdef USE_SLERP 57 | // the following function can be called to set the SLERP power 58 | void setSlerpPower(RTFLOAT power) { m_slerpPower = power; } 59 | #else 60 | // the following two functions can be called to customize the noise covariance 61 | 62 | void setQ(RTFLOAT Q) { m_Q = Q; reset();} 63 | void setR(RTFLOAT R) { if (R > 0) m_R = R; reset();} 64 | #endif 65 | inline const RTVector3& getMeasuredPose() {return m_measuredPose;} 66 | inline const RTQuaternion& getMeasuredQPose() {return m_measuredQPose;} 67 | inline const RTVector3& getFusionPose() {return m_fusionPose;} 68 | inline const RTQuaternion& getFusionQPose() {return m_fusionQPose;} 69 | 70 | private: 71 | void calculatePose(const RTVector3& accel, const RTVector3& mag); // generates pose from accels and heading 72 | 73 | RTFLOAT m_timeDelta; // time between predictions 74 | 75 | RTQuaternion m_stateQError; // difference between stateQ and measuredQ 76 | 77 | #ifdef USE_SLERP 78 | RTFLOAT m_slerpPower; // a value 0 to 1 that controls measured state influence 79 | RTQuaternion m_rotationDelta; // amount by which measured state differs from predicted 80 | RTQuaternion m_rotationPower; // delta raised to the appopriate power 81 | RTVector3 m_rotationUnitVector; // the vector part of the rotation delta 82 | #else 83 | RTFLOAT m_Q; // process noise covariance 84 | RTFLOAT m_R; // the measurement noise covariance 85 | #endif 86 | RTQuaternion m_measuredQPose; // quaternion form of pose from measurement 87 | RTVector3 m_measuredPose; // vector form of pose from measurement 88 | RTQuaternion m_fusionQPose; // quaternion form of pose from fusion 89 | RTVector3 m_fusionPose; // vector form of pose from fusion 90 | 91 | bool m_enableGyro; // enables gyro as input 92 | bool m_enableAccel; // enables accel as input 93 | bool m_enableCompass; // enables compass a input 94 | 95 | bool m_firstTime; // if first time after reset 96 | unsigned long m_lastFusionTime; // for delta time calculation 97 | }; 98 | 99 | #endif // #ifndef RTARDULINK_MODE 100 | 101 | #endif // _RTFUSIONRTQF_H 102 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMU.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMU_H 25 | #define _RTIMU_H 26 | 27 | #include "RTMath.h" 28 | #include "RTIMULibDefs.h" 29 | #include "I2Cdev.h" 30 | 31 | #define I2CWrite(x, y, z) I2Cdev::writeByte(x, y, z) 32 | #define I2CRead(w, x, y, z) I2Cdev::readBytes(w, x, y, z) 33 | 34 | class RTIMUSettings; 35 | 36 | class RTIMU 37 | { 38 | public: 39 | // IMUs should always be created with the following call 40 | 41 | static RTIMU *createIMU(RTIMUSettings *settings); 42 | 43 | // Constructor/destructor 44 | 45 | RTIMU(RTIMUSettings *settings); 46 | virtual ~RTIMU(); 47 | 48 | // These functions must be provided by sub classes 49 | 50 | virtual const char *IMUName() = 0; // the name of the IMU 51 | virtual int IMUType() = 0; // the type code of the IMU 52 | virtual int IMUInit() = 0; // set up the IMU 53 | virtual int IMUGetPollInterval() = 0; // returns the recommended poll interval in mS 54 | virtual bool IMURead() = 0; // get a sample 55 | 56 | // This one wanted a similar name but isn't pure virtual 57 | 58 | virtual bool IMUCompassCalValid() { return m_calibrationValid; } 59 | 60 | // setCalibrationMode() turns off use of cal data so that raw data can be accumulated 61 | // to derive calibration data 62 | 63 | void setCalibrationMode(bool enable) { m_calibrationMode = enable; } 64 | 65 | // setCalibrationData configured the cal data and also enables use if valid 66 | 67 | void setCalibrationData(); 68 | 69 | // getCalibrationValid() returns true if the calibration data is being used 70 | 71 | bool getCalibrationValid() { return !m_calibrationMode && m_calibrationValid; } 72 | 73 | // returns true if enough samples for valid data 74 | 75 | virtual bool IMUGyroBiasValid(); 76 | 77 | inline const RTVector3& getGyro() { return m_gyro; } // gets gyro rates in radians/sec 78 | inline const RTVector3& getAccel() { return m_accel; } // get accel data in gs 79 | inline const RTVector3& getCompass() { return m_compass; } // gets compass data in uT 80 | inline unsigned long getTimestamp() { return m_timestamp; } // and the timestamp for it 81 | 82 | protected: 83 | void gyroBiasInit(); // sets up gyro bias calculation 84 | void handleGyroBias(); // adjust gyro for bias 85 | void calibrateAverageCompass(); // calibrate and smooth compass 86 | bool m_calibrationMode; // true if cal mode so don't use cal data! 87 | bool m_calibrationValid; // tru if call data is valid and can be used 88 | 89 | RTVector3 m_gyro; // the gyro readings 90 | RTVector3 m_accel; // the accel readings 91 | RTVector3 m_compass; // the compass readings 92 | unsigned long m_timestamp; // the timestamp 93 | 94 | RTIMUSettings *m_settings; // the settings object pointer 95 | 96 | int m_sampleRate; // samples per second 97 | uint64_t m_sampleInterval; // interval between samples in microseonds 98 | 99 | RTFLOAT m_gyroAlpha; // gyro bias learning rate 100 | int m_gyroSampleCount; // number of gyro samples used 101 | bool m_gyroBiasValid; // true if the recorded gyro bias is valid 102 | RTVector3 m_gyroBias; // the recorded gyro bias 103 | 104 | RTVector3 m_previousAccel; // previous step accel for gyro learning 105 | 106 | RTFLOAT m_compassCalOffset[3]; 107 | RTFLOAT m_compassCalScale[3]; 108 | RTVector3 m_compassAverage; // a running average to smooth the mag outputs 109 | 110 | static RTFLOAT m_axisRotation[9]; // axis rotation matrix 111 | 112 | }; 113 | 114 | #endif // _RTIMU_H 115 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUBNO055.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | // Based on the Adafruit BNO055 driver: 25 | 26 | /*************************************************************************** 27 | This is a library for the BNO055 orientation sensor 28 | Designed specifically to work with the Adafruit BNO055 Breakout. 29 | Pick one up today in the adafruit shop! 30 | ------> http://www.adafruit.com/products 31 | These sensors use I2C to communicate, 2 pins are required to interface. 32 | Adafruit invests time and resources providing this open source code, 33 | please support Adafruit andopen-source hardware by purchasing products 34 | from Adafruit! 35 | Written by KTOWN for Adafruit Industries. 36 | MIT license, all text above must be included in any redistribution 37 | ***************************************************************************/ 38 | 39 | #include "RTIMUBNO055.h" 40 | #include "RTIMUSettings.h" 41 | #if defined(BNO055_28) || defined(BNO055_29) 42 | 43 | RTIMUBNO055::RTIMUBNO055(RTIMUSettings *settings) : RTIMU(settings) 44 | { 45 | m_sampleRate = 100; 46 | m_sampleInterval = (unsigned long)1000 / m_sampleRate; 47 | } 48 | 49 | RTIMUBNO055::~RTIMUBNO055() 50 | { 51 | } 52 | 53 | int RTIMUBNO055::IMUInit() 54 | { 55 | unsigned char result; 56 | 57 | m_slaveAddr = m_settings->m_I2CSlaveAddress; 58 | m_lastReadTime = millis(); 59 | 60 | if (!I2Cdev::readByte(m_slaveAddr, BNO055_WHO_AM_I, &result)) 61 | return -1; 62 | 63 | if (result != BNO055_ID) { 64 | return -2; 65 | } 66 | 67 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_CONFIG)) 68 | return -3; 69 | 70 | delay(50); 71 | 72 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_SYS_TRIGGER, 0x20)) 73 | return -4; 74 | 75 | delay(50); 76 | 77 | while (1) { 78 | if (!I2Cdev::readByte(m_slaveAddr, BNO055_WHO_AM_I, &result)) 79 | continue; 80 | if (result == BNO055_ID) 81 | break; 82 | delay(50); 83 | } 84 | 85 | delay(50); 86 | 87 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_PWR_MODE, BNO055_PWR_MODE_NORMAL)) 88 | return -5; 89 | 90 | delay(50); 91 | 92 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_PAGE_ID, 0)) 93 | return -6; 94 | 95 | delay(50); 96 | 97 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_SYS_TRIGGER, 0x00)) 98 | return -7; 99 | 100 | delay(50); 101 | 102 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_UNIT_SEL, 0x87)) 103 | return -8; 104 | 105 | delay(50); 106 | 107 | if (!I2Cdev::writeByte(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_NDOF)) 108 | return -9; 109 | 110 | delay(50); 111 | 112 | return 1; 113 | } 114 | 115 | int RTIMUBNO055::IMUGetPollInterval() 116 | { 117 | return (7); 118 | } 119 | 120 | bool RTIMUBNO055::IMURead() 121 | { 122 | unsigned char buffer[24]; 123 | 124 | if ((millis() - m_lastReadTime) < m_sampleInterval) 125 | return false; // too soon 126 | 127 | m_lastReadTime = millis(); 128 | if (!I2Cdev::readBytes(m_slaveAddr, BNO055_ACCEL_DATA, 24, buffer)) 129 | return false; 130 | 131 | int16_t x, y, z; 132 | 133 | // process accel data 134 | 135 | x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); 136 | y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); 137 | z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); 138 | 139 | m_accel.setX((RTFLOAT)y / 1000.0); 140 | m_accel.setY((RTFLOAT)x / 1000.0); 141 | m_accel.setZ((RTFLOAT)z / 1000.0); 142 | 143 | // process mag data 144 | 145 | x = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); 146 | y = (((uint16_t)buffer[9]) << 8) | ((uint16_t)buffer[8]); 147 | z = (((uint16_t)buffer[11]) << 8) | ((uint16_t)buffer[10]); 148 | 149 | m_compass.setX(-(RTFLOAT)y / 16.0); 150 | m_compass.setY(-(RTFLOAT)x / 16.0); 151 | m_compass.setZ(-(RTFLOAT)z / 16.0); 152 | 153 | // process gyro data 154 | 155 | x = (((uint16_t)buffer[13]) << 8) | ((uint16_t)buffer[12]); 156 | y = (((uint16_t)buffer[15]) << 8) | ((uint16_t)buffer[14]); 157 | z = (((uint16_t)buffer[17]) << 8) | ((uint16_t)buffer[16]); 158 | 159 | m_gyro.setX(-(RTFLOAT)y / 900.0); 160 | m_gyro.setY(-(RTFLOAT)x / 900.0); 161 | m_gyro.setZ(-(RTFLOAT)z / 900.0); 162 | 163 | // process euler angles 164 | 165 | x = (((uint16_t)buffer[19]) << 8) | ((uint16_t)buffer[18]); 166 | y = (((uint16_t)buffer[21]) << 8) | ((uint16_t)buffer[20]); 167 | z = (((uint16_t)buffer[23]) << 8) | ((uint16_t)buffer[22]); 168 | 169 | // put in structure and do axis remap 170 | 171 | m_fusionPose.setX((RTFLOAT)y / 900.0); 172 | m_fusionPose.setY((RTFLOAT)z / 900.0); 173 | m_fusionPose.setZ((RTFLOAT)x / 900.0); 174 | 175 | m_fusionQPose.fromEuler(m_fusionPose); 176 | 177 | m_timestamp = millis(); 178 | return true; 179 | } 180 | #endif -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUBNO055.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | 25 | #ifndef _RTIMUBNO055_H 26 | #define _RTIMUBNO055_H 27 | 28 | #include "RTIMU.h" 29 | 30 | // I2C Slave Addresses 31 | 32 | #define BNO055_ADDRESS0 0x28 33 | #define BNO055_ADDRESS1 0x29 34 | #define BNO055_ID 0xa0 35 | 36 | // Register map 37 | 38 | #define BNO055_WHO_AM_I 0x00 39 | #define BNO055_PAGE_ID 0x07 40 | #define BNO055_ACCEL_DATA 0x08 41 | #define BNO055_MAG_DATA 0x0e 42 | #define BNO055_GYRO_DATA 0x14 43 | #define BNO055_FUSED_EULER 0x1a 44 | #define BNO055_FUSED_QUAT 0x20 45 | #define BNO055_UNIT_SEL 0x3b 46 | #define BNO055_OPER_MODE 0x3d 47 | #define BNO055_PWR_MODE 0x3e 48 | #define BNO055_SYS_TRIGGER 0x3f 49 | #define BNO055_AXIS_MAP_CONFIG 0x41 50 | #define BNO055_AXIS_MAP_SIGN 0x42 51 | 52 | // Operation modes 53 | 54 | #define BNO055_OPER_MODE_CONFIG 0x00 55 | #define BNO055_OPER_MODE_NDOF 0x0c 56 | 57 | // Power modes 58 | 59 | #define BNO055_PWR_MODE_NORMAL 0x00 60 | 61 | class RTIMUBNO055 : public RTIMU 62 | { 63 | public: 64 | RTIMUBNO055(RTIMUSettings *settings); 65 | ~RTIMUBNO055(); 66 | 67 | inline const RTVector3& getFusionPose() { return m_fusionPose; } 68 | inline const RTQuaternion& getFusionQPose() { return m_fusionQPose; } 69 | 70 | virtual const char *IMUName() { return "BNO055"; } 71 | virtual int IMUType() { return RTIMU_TYPE_BNO055; } 72 | virtual int IMUInit(); 73 | virtual int IMUGetPollInterval(); 74 | virtual bool IMURead(); 75 | 76 | private: 77 | unsigned char m_slaveAddr; // I2C address of BNO055 78 | 79 | uint64_t m_lastReadTime; 80 | 81 | RTQuaternion m_fusionQPose; 82 | RTVector3 m_fusionPose; 83 | }; 84 | 85 | #endif // _RTIMUBNO055_H 86 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUGD20HM303D.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMUGD20HM303D_H 25 | #define _RTIMUGD20HM303D_H 26 | 27 | #include "RTIMU.h" 28 | 29 | // I2C Slave Addresses 30 | 31 | #define L3GD20H_ADDRESS0 0x6a 32 | #define L3GD20H_ADDRESS1 0x6b 33 | #define L3GD20H_ID 0xd7 34 | 35 | #define LSM303D_ADDRESS0 0x1e 36 | #define LSM303D_ADDRESS1 0x1d 37 | #define LSM303D_ID 0x49 38 | 39 | // L3GD20H Register map 40 | 41 | #define L3GD20H_WHO_AM_I 0x0f 42 | #define L3GD20H_CTRL1 0x20 43 | #define L3GD20H_CTRL2 0x21 44 | #define L3GD20H_CTRL3 0x22 45 | #define L3GD20H_CTRL4 0x23 46 | #define L3GD20H_CTRL5 0x24 47 | #define L3GD20H_OUT_TEMP 0x26 48 | #define L3GD20H_STATUS 0x27 49 | #define L3GD20H_OUT_X_L 0x28 50 | #define L3GD20H_OUT_X_H 0x29 51 | #define L3GD20H_OUT_Y_L 0x2a 52 | #define L3GD20H_OUT_Y_H 0x2b 53 | #define L3GD20H_OUT_Z_L 0x2c 54 | #define L3GD20H_OUT_Z_H 0x2d 55 | #define L3GD20H_FIFO_CTRL 0x2e 56 | #define L3GD20H_FIFO_SRC 0x2f 57 | #define L3GD20H_IG_CFG 0x30 58 | #define L3GD20H_IG_SRC 0x31 59 | #define L3GD20H_IG_THS_XH 0x32 60 | #define L3GD20H_IG_THS_XL 0x33 61 | #define L3GD20H_IG_THS_YH 0x34 62 | #define L3GD20H_IG_THS_YL 0x35 63 | #define L3GD20H_IG_THS_ZH 0x36 64 | #define L3GD20H_IG_THS_ZL 0x37 65 | #define L3GD20H_IG_DURATION 0x38 66 | #define L3GD20H_LOW_ODR 0x39 67 | 68 | // Gyro sample rate defines 69 | 70 | #define L3GD20H_SAMPLERATE_12_5 0 71 | #define L3GD20H_SAMPLERATE_25 1 72 | #define L3GD20H_SAMPLERATE_50 2 73 | #define L3GD20H_SAMPLERATE_100 3 74 | #define L3GD20H_SAMPLERATE_200 4 75 | #define L3GD20H_SAMPLERATE_400 5 76 | #define L3GD20H_SAMPLERATE_800 6 77 | 78 | // Gyro banwidth defines 79 | 80 | #define L3GD20H_BANDWIDTH_0 0 81 | #define L3GD20H_BANDWIDTH_1 1 82 | #define L3GD20H_BANDWIDTH_2 2 83 | #define L3GD20H_BANDWIDTH_3 3 84 | 85 | // Gyro FSR defines 86 | 87 | #define L3GD20H_FSR_245 0 88 | #define L3GD20H_FSR_500 1 89 | #define L3GD20H_FSR_2000 2 90 | 91 | // Gyro high pass filter defines 92 | 93 | #define L3GD20H_HPF_0 0 94 | #define L3GD20H_HPF_1 1 95 | #define L3GD20H_HPF_2 2 96 | #define L3GD20H_HPF_3 3 97 | #define L3GD20H_HPF_4 4 98 | #define L3GD20H_HPF_5 5 99 | #define L3GD20H_HPF_6 6 100 | #define L3GD20H_HPF_7 7 101 | #define L3GD20H_HPF_8 8 102 | #define L3GD20H_HPF_9 9 103 | 104 | // LSM303D Register Map 105 | 106 | #define LSM303D_TEMP_OUT_L 0x05 107 | #define LSM303D_TEMP_OUT_H 0x06 108 | #define LSM303D_STATUS_M 0x07 109 | #define LSM303D_OUT_X_L_M 0x08 110 | #define LSM303D_OUT_X_H_M 0x09 111 | #define LSM303D_OUT_Y_L_M 0x0a 112 | #define LSM303D_OUT_Y_H_M 0x0b 113 | #define LSM303D_OUT_Z_L_M 0x0c 114 | #define LSM303D_OUT_Z_H_M 0x0d 115 | #define LSM303D_WHO_AM_I 0x0f 116 | #define LSM303D_INT_CTRL_M 0x12 117 | #define LSM303D_INT_SRC_M 0x13 118 | #define LSM303D_INT_THS_L_M 0x14 119 | #define LSM303D_INT_THS_H_M 0x15 120 | #define LSM303D_OFFSET_X_L_M 0x16 121 | #define LSM303D_OFFSET_X_H_M 0x17 122 | #define LSM303D_OFFSET_Y_L_M 0x18 123 | #define LSM303D_OFFSET_Y_H_M 0x19 124 | #define LSM303D_OFFSET_Z_L_M 0x1a 125 | #define LSM303D_OFFSET_Z_H_M 0x1b 126 | #define LSM303D_REFERENCE_X 0x1c 127 | #define LSM303D_REFERENCE_Y 0x1d 128 | #define LSM303D_REFERENCE_Z 0x1e 129 | #define LSM303D_CTRL0 0x1f 130 | #define LSM303D_CTRL1 0x20 131 | #define LSM303D_CTRL2 0x21 132 | #define LSM303D_CTRL3 0x22 133 | #define LSM303D_CTRL4 0x23 134 | #define LSM303D_CTRL5 0x24 135 | #define LSM303D_CTRL6 0x25 136 | #define LSM303D_CTRL7 0x26 137 | #define LSM303D_STATUS_A 0x27 138 | #define LSM303D_OUT_X_L_A 0x28 139 | #define LSM303D_OUT_X_H_A 0x29 140 | #define LSM303D_OUT_Y_L_A 0x2a 141 | #define LSM303D_OUT_Y_H_A 0x2b 142 | #define LSM303D_OUT_Z_L_A 0x2c 143 | #define LSM303D_OUT_Z_H_A 0x2d 144 | #define LSM303D_FIFO_CTRL 0x2e 145 | #define LSM303D_FIFO_SRC 0x2f 146 | #define LSM303D_IG_CFG1 0x30 147 | #define LSM303D_IG_SRC1 0x31 148 | #define LSM303D_IG_THS1 0x32 149 | #define LSM303D_IG_DUR1 0x33 150 | #define LSM303D_IG_CFG2 0x34 151 | #define LSM303D_IG_SRC2 0x35 152 | #define LSM303D_IG_THS2 0x36 153 | #define LSM303D_IG_DUR2 0x37 154 | #define LSM303D_CLICK_CFG 0x38 155 | #define LSM303D_CLICK_SRC 0x39 156 | #define LSM303D_CLICK_THS 0x3a 157 | #define LSM303D_TIME_LIMIT 0x3b 158 | #define LSM303D_TIME_LATENCY 0x3c 159 | #define LSM303D_TIME_WINDOW 0x3d 160 | #define LSM303D_ACT_THIS 0x3e 161 | #define LSM303D_ACT_DUR 0x3f 162 | 163 | // Accel sample rate defines 164 | 165 | #define LSM303D_ACCEL_SAMPLERATE_3_125 1 166 | #define LSM303D_ACCEL_SAMPLERATE_6_25 2 167 | #define LSM303D_ACCEL_SAMPLERATE_12_5 3 168 | #define LSM303D_ACCEL_SAMPLERATE_25 4 169 | #define LSM303D_ACCEL_SAMPLERATE_50 5 170 | #define LSM303D_ACCEL_SAMPLERATE_100 6 171 | #define LSM303D_ACCEL_SAMPLERATE_200 7 172 | #define LSM303D_ACCEL_SAMPLERATE_400 8 173 | #define LSM303D_ACCEL_SAMPLERATE_800 9 174 | #define LSM303D_ACCEL_SAMPLERATE_1600 10 175 | 176 | // Accel FSR 177 | 178 | #define LSM303D_ACCEL_FSR_2 0 179 | #define LSM303D_ACCEL_FSR_4 1 180 | #define LSM303D_ACCEL_FSR_6 2 181 | #define LSM303D_ACCEL_FSR_8 3 182 | #define LSM303D_ACCEL_FSR_16 4 183 | 184 | // Accel filter bandwidth 185 | 186 | #define LSM303D_ACCEL_LPF_773 0 187 | #define LSM303D_ACCEL_LPF_194 1 188 | #define LSM303D_ACCEL_LPF_362 2 189 | #define LSM303D_ACCEL_LPF_50 3 190 | 191 | // Compass sample rate defines 192 | 193 | #define LSM303D_COMPASS_SAMPLERATE_3_125 0 194 | #define LSM303D_COMPASS_SAMPLERATE_6_25 1 195 | #define LSM303D_COMPASS_SAMPLERATE_12_5 2 196 | #define LSM303D_COMPASS_SAMPLERATE_25 3 197 | #define LSM303D_COMPASS_SAMPLERATE_50 4 198 | #define LSM303D_COMPASS_SAMPLERATE_100 5 199 | 200 | // Compass FSR 201 | 202 | #define LSM303D_COMPASS_FSR_2 0 203 | #define LSM303D_COMPASS_FSR_4 1 204 | #define LSM303D_COMPASS_FSR_8 2 205 | #define LSM303D_COMPASS_FSR_12 3 206 | 207 | class RTIMUGD20HM303D : public RTIMU 208 | { 209 | public: 210 | RTIMUGD20HM303D(RTIMUSettings *settings); 211 | ~RTIMUGD20HM303D(); 212 | 213 | virtual const char *IMUName() { return "L3GD20H + LSM303D"; } 214 | virtual int IMUType() { return RTIMU_TYPE_GD20HM303D; } 215 | virtual int IMUInit(); 216 | virtual int IMUGetPollInterval(); 217 | virtual bool IMURead(); 218 | 219 | private: 220 | bool setGyroSampleRate(); 221 | bool setGyroCTRL2(); 222 | bool setGyroCTRL4(); 223 | bool setGyroCTRL5(); 224 | bool setAccelCTRL1(); 225 | bool setAccelCTRL2(); 226 | bool setCompassCTRL5(); 227 | bool setCompassCTRL6(); 228 | bool setCompassCTRL7(); 229 | 230 | unsigned char m_gyroSlaveAddr; // I2C address of L3GD20H 231 | unsigned char m_accelCompassSlaveAddr; // I2C address of LSM303D 232 | 233 | RTFLOAT m_gyroScale; 234 | RTFLOAT m_accelScale; 235 | RTFLOAT m_compassScale; 236 | }; 237 | 238 | #endif // _RTIMUGD20HM303D_H 239 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUGD20HM303DLHC.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMUGD20HM303DLHC_H 25 | #define _RTIMUGD20HM303DLHC_H 26 | 27 | #include "RTIMU.h" 28 | 29 | // I2C Slave Addresses 30 | 31 | #define L3GD20H_ADDRESS0 0x6a 32 | #define L3GD20H_ADDRESS1 0x6b 33 | #define L3GD20H_ID 0xd7 34 | 35 | #define LSM303DLHC_ACCEL_ADDRESS 0x19 36 | #define LSM303DLHC_COMPASS_ADDRESS 0x1e 37 | 38 | // L3GD20H Register map 39 | 40 | #define L3GD20H_WHO_AM_I 0x0f 41 | #define L3GD20H_CTRL1 0x20 42 | #define L3GD20H_CTRL2 0x21 43 | #define L3GD20H_CTRL3 0x22 44 | #define L3GD20H_CTRL4 0x23 45 | #define L3GD20H_CTRL5 0x24 46 | #define L3GD20H_OUT_TEMP 0x26 47 | #define L3GD20H_STATUS 0x27 48 | #define L3GD20H_OUT_X_L 0x28 49 | #define L3GD20H_OUT_X_H 0x29 50 | #define L3GD20H_OUT_Y_L 0x2a 51 | #define L3GD20H_OUT_Y_H 0x2b 52 | #define L3GD20H_OUT_Z_L 0x2c 53 | #define L3GD20H_OUT_Z_H 0x2d 54 | #define L3GD20H_FIFO_CTRL 0x2e 55 | #define L3GD20H_FIFO_SRC 0x2f 56 | #define L3GD20H_IG_CFG 0x30 57 | #define L3GD20H_IG_SRC 0x31 58 | #define L3GD20H_IG_THS_XH 0x32 59 | #define L3GD20H_IG_THS_XL 0x33 60 | #define L3GD20H_IG_THS_YH 0x34 61 | #define L3GD20H_IG_THS_YL 0x35 62 | #define L3GD20H_IG_THS_ZH 0x36 63 | #define L3GD20H_IG_THS_ZL 0x37 64 | #define L3GD20H_IG_DURATION 0x38 65 | #define L3GD20H_LOW_ODR 0x39 66 | 67 | // Gyro sample rate defines 68 | 69 | #define L3GD20H_SAMPLERATE_12_5 0 70 | #define L3GD20H_SAMPLERATE_25 1 71 | #define L3GD20H_SAMPLERATE_50 2 72 | #define L3GD20H_SAMPLERATE_100 3 73 | #define L3GD20H_SAMPLERATE_200 4 74 | #define L3GD20H_SAMPLERATE_400 5 75 | #define L3GD20H_SAMPLERATE_800 6 76 | 77 | // Gyro banwidth defines 78 | 79 | #define L3GD20H_BANDWIDTH_0 0 80 | #define L3GD20H_BANDWIDTH_1 1 81 | #define L3GD20H_BANDWIDTH_2 2 82 | #define L3GD20H_BANDWIDTH_3 3 83 | 84 | // Gyro FSR defines 85 | 86 | #define L3GD20H_FSR_245 0 87 | #define L3GD20H_FSR_500 1 88 | #define L3GD20H_FSR_2000 2 89 | 90 | // Gyro high pass filter defines 91 | 92 | #define L3GD20H_HPF_0 0 93 | #define L3GD20H_HPF_1 1 94 | #define L3GD20H_HPF_2 2 95 | #define L3GD20H_HPF_3 3 96 | #define L3GD20H_HPF_4 4 97 | #define L3GD20H_HPF_5 5 98 | #define L3GD20H_HPF_6 6 99 | #define L3GD20H_HPF_7 7 100 | #define L3GD20H_HPF_8 8 101 | #define L3GD20H_HPF_9 9 102 | 103 | // LSM303DLHC Accel Register Map 104 | 105 | #define LSM303DLHC_CTRL1_A 0x20 106 | #define LSM303DLHC_CTRL2_A 0x21 107 | #define LSM303DLHC_CTRL3_A 0x22 108 | #define LSM303DLHC_CTRL4_A 0x23 109 | #define LSM303DLHC_CTRL5_A 0x24 110 | #define LSM303DLHC_CTRL6_A 0x25 111 | #define LSM303DLHC_REF_A 0x26 112 | #define LSM303DLHC_STATUS_A 0x27 113 | #define LSM303DLHC_OUT_X_L_A 0x28 114 | #define LSM303DLHC_OUT_X_H_A 0x29 115 | #define LSM303DLHC_OUT_Y_L_A 0x2a 116 | #define LSM303DLHC_OUT_Y_H_A 0x2b 117 | #define LSM303DLHC_OUT_Z_L_A 0x2c 118 | #define LSM303DLHC_OUT_Z_H_A 0x2d 119 | #define LSM303DLHC_FIFO_CTRL_A 0x2e 120 | #define LSM303DLHC_FIFO_SRC_A 0x2f 121 | 122 | // LSM303DLHC Compass Register Map 123 | 124 | #define LSM303DLHC_CRA_M 0x00 125 | #define LSM303DLHC_CRB_M 0x01 126 | #define LSM303DLHC_CRM_M 0x02 127 | #define LSM303DLHC_OUT_X_H_M 0x03 128 | #define LSM303DLHC_OUT_X_L_M 0x04 129 | #define LSM303DLHC_OUT_Y_H_M 0x05 130 | #define LSM303DLHC_OUT_Y_L_M 0x06 131 | #define LSM303DLHC_OUT_Z_H_M 0x07 132 | #define LSM303DLHC_OUT_Z_L_M 0x08 133 | #define LSM303DLHC_STATUS_M 0x09 134 | #define LSM303DLHC_TEMP_OUT_L_M 0x31 135 | #define LSM303DLHC_TEMP_OUT_H_M 0x32 136 | 137 | // Accel sample rate defines 138 | 139 | #define LSM303DLHC_ACCEL_SAMPLERATE_1 1 140 | #define LSM303DLHC_ACCEL_SAMPLERATE_10 2 141 | #define LSM303DLHC_ACCEL_SAMPLERATE_25 3 142 | #define LSM303DLHC_ACCEL_SAMPLERATE_50 4 143 | #define LSM303DLHC_ACCEL_SAMPLERATE_100 5 144 | #define LSM303DLHC_ACCEL_SAMPLERATE_200 6 145 | #define LSM303DLHC_ACCEL_SAMPLERATE_400 7 146 | 147 | // Accel FSR 148 | 149 | #define LSM303DLHC_ACCEL_FSR_2 0 150 | #define LSM303DLHC_ACCEL_FSR_4 1 151 | #define LSM303DLHC_ACCEL_FSR_8 2 152 | #define LSM303DLHC_ACCEL_FSR_16 3 153 | 154 | // Compass sample rate defines 155 | 156 | #define LSM303DLHC_COMPASS_SAMPLERATE_0_75 0 157 | #define LSM303DLHC_COMPASS_SAMPLERATE_1_5 1 158 | #define LSM303DLHC_COMPASS_SAMPLERATE_3 2 159 | #define LSM303DLHC_COMPASS_SAMPLERATE_7_5 3 160 | #define LSM303DLHC_COMPASS_SAMPLERATE_15 4 161 | #define LSM303DLHC_COMPASS_SAMPLERATE_30 5 162 | #define LSM303DLHC_COMPASS_SAMPLERATE_75 6 163 | #define LSM303DLHC_COMPASS_SAMPLERATE_220 7 164 | 165 | // Compass FSR 166 | 167 | #define LSM303DLHC_COMPASS_FSR_1_3 1 168 | #define LSM303DLHC_COMPASS_FSR_1_9 2 169 | #define LSM303DLHC_COMPASS_FSR_2_5 3 170 | #define LSM303DLHC_COMPASS_FSR_4 4 171 | #define LSM303DLHC_COMPASS_FSR_4_7 5 172 | #define LSM303DLHC_COMPASS_FSR_5_6 6 173 | #define LSM303DLHC_COMPASS_FSR_8_1 7 174 | 175 | class RTIMUGD20HM303DLHC : public RTIMU 176 | { 177 | public: 178 | RTIMUGD20HM303DLHC(RTIMUSettings *settings); 179 | ~RTIMUGD20HM303DLHC(); 180 | 181 | virtual const char *IMUName() { return "L3GD20H + LSM303DLHC"; } 182 | virtual int IMUType() { return RTIMU_TYPE_GD20HM303DLHC; } 183 | virtual int IMUInit(); 184 | virtual int IMUGetPollInterval(); 185 | virtual bool IMURead(); 186 | 187 | private: 188 | bool setGyroSampleRate(); 189 | bool setGyroCTRL2(); 190 | bool setGyroCTRL4(); 191 | bool setGyroCTRL5(); 192 | bool setAccelCTRL1(); 193 | bool setAccelCTRL4(); 194 | bool setCompassCRA(); 195 | bool setCompassCRB(); 196 | bool setCompassCRM(); 197 | 198 | unsigned char m_gyroSlaveAddr; // I2C address of L3GD20 199 | unsigned char m_accelSlaveAddr; // I2C address of LSM303DLHC accel 200 | unsigned char m_compassSlaveAddr; // I2C address of LSM303DLHC compass 201 | 202 | RTFLOAT m_gyroScale; 203 | RTFLOAT m_accelScale; 204 | RTFLOAT m_compassScaleXY; 205 | RTFLOAT m_compassScaleZ; 206 | }; 207 | 208 | #endif // _RTIMUGD20HM303DLHC_H 209 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUGD20M303DLHC.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMUGD20M303DLHC_H 25 | #define _RTIMUGD20M303DLHC_H 26 | 27 | #include "RTIMU.h" 28 | 29 | // I2C Slave Addresses 30 | 31 | #define L3GD20_ADDRESS0 0x6a 32 | #define L3GD20_ADDRESS1 0x6b 33 | #define L3GD20_ID 0xd4 34 | 35 | #define LSM303DLHC_ACCEL_ADDRESS 0x19 36 | #define LSM303DLHC_COMPASS_ADDRESS 0x1e 37 | 38 | // L3GD20 Register map 39 | 40 | #define L3GD20_WHO_AM_I 0x0f 41 | #define L3GD20_CTRL1 0x20 42 | #define L3GD20_CTRL2 0x21 43 | #define L3GD20_CTRL3 0x22 44 | #define L3GD20_CTRL4 0x23 45 | #define L3GD20_CTRL5 0x24 46 | #define L3GD20_OUT_TEMP 0x26 47 | #define L3GD20_STATUS 0x27 48 | #define L3GD20_OUT_X_L 0x28 49 | #define L3GD20_OUT_X_H 0x29 50 | #define L3GD20_OUT_Y_L 0x2a 51 | #define L3GD20_OUT_Y_H 0x2b 52 | #define L3GD20_OUT_Z_L 0x2c 53 | #define L3GD20_OUT_Z_H 0x2d 54 | #define L3GD20_FIFO_CTRL 0x2e 55 | #define L3GD20_FIFO_SRC 0x2f 56 | #define L3GD20_IG_CFG 0x30 57 | #define L3GD20_IG_SRC 0x31 58 | #define L3GD20_IG_THS_XH 0x32 59 | #define L3GD20_IG_THS_XL 0x33 60 | #define L3GD20_IG_THS_YH 0x34 61 | #define L3GD20_IG_THS_YL 0x35 62 | #define L3GD20_IG_THS_ZH 0x36 63 | #define L3GD20_IG_THS_ZL 0x37 64 | #define L3GD20_IG_DURATION 0x38 65 | 66 | // Gyro sample rate defines 67 | 68 | #define L3GD20_SAMPLERATE_95 0 69 | #define L3GD20_SAMPLERATE_190 1 70 | #define L3GD20_SAMPLERATE_380 2 71 | #define L3GD20_SAMPLERATE_760 3 72 | 73 | // Gyro banwidth defines 74 | 75 | #define L3GD20_BANDWIDTH_0 0 76 | #define L3GD20_BANDWIDTH_1 1 77 | #define L3GD20_BANDWIDTH_2 2 78 | #define L3GD20_BANDWIDTH_3 3 79 | 80 | // Gyro FSR defines 81 | 82 | #define L3GD20_FSR_250 0 83 | #define L3GD20_FSR_500 1 84 | #define L3GD20_FSR_2000 2 85 | 86 | // Gyro high pass filter defines 87 | 88 | #define L3GD20_HPF_0 0 89 | #define L3GD20_HPF_1 1 90 | #define L3GD20_HPF_2 2 91 | #define L3GD20_HPF_3 3 92 | #define L3GD20_HPF_4 4 93 | #define L3GD20_HPF_5 5 94 | #define L3GD20_HPF_6 6 95 | #define L3GD20_HPF_7 7 96 | #define L3GD20_HPF_8 8 97 | #define L3GD20_HPF_9 9 98 | 99 | // LSM303DLHC Accel Register Map 100 | 101 | #define LSM303DLHC_CTRL1_A 0x20 102 | #define LSM303DLHC_CTRL2_A 0x21 103 | #define LSM303DLHC_CTRL3_A 0x22 104 | #define LSM303DLHC_CTRL4_A 0x23 105 | #define LSM303DLHC_CTRL5_A 0x24 106 | #define LSM303DLHC_CTRL6_A 0x25 107 | #define LSM303DLHC_REF_A 0x26 108 | #define LSM303DLHC_STATUS_A 0x27 109 | #define LSM303DLHC_OUT_X_L_A 0x28 110 | #define LSM303DLHC_OUT_X_H_A 0x29 111 | #define LSM303DLHC_OUT_Y_L_A 0x2a 112 | #define LSM303DLHC_OUT_Y_H_A 0x2b 113 | #define LSM303DLHC_OUT_Z_L_A 0x2c 114 | #define LSM303DLHC_OUT_Z_H_A 0x2d 115 | #define LSM303DLHC_FIFO_CTRL_A 0x2e 116 | #define LSM303DLHC_FIFO_SRC_A 0x2f 117 | 118 | // LSM303DLHC Compass Register Map 119 | 120 | #define LSM303DLHC_CRA_M 0x00 121 | #define LSM303DLHC_CRB_M 0x01 122 | #define LSM303DLHC_CRM_M 0x02 123 | #define LSM303DLHC_OUT_X_H_M 0x03 124 | #define LSM303DLHC_OUT_X_L_M 0x04 125 | #define LSM303DLHC_OUT_Y_H_M 0x05 126 | #define LSM303DLHC_OUT_Y_L_M 0x06 127 | #define LSM303DLHC_OUT_Z_H_M 0x07 128 | #define LSM303DLHC_OUT_Z_L_M 0x08 129 | #define LSM303DLHC_STATUS_M 0x09 130 | #define LSM303DLHC_TEMP_OUT_L_M 0x31 131 | #define LSM303DLHC_TEMP_OUT_H_M 0x32 132 | 133 | // Accel sample rate defines 134 | 135 | #define LSM303DLHC_ACCEL_SAMPLERATE_1 1 136 | #define LSM303DLHC_ACCEL_SAMPLERATE_10 2 137 | #define LSM303DLHC_ACCEL_SAMPLERATE_25 3 138 | #define LSM303DLHC_ACCEL_SAMPLERATE_50 4 139 | #define LSM303DLHC_ACCEL_SAMPLERATE_100 5 140 | #define LSM303DLHC_ACCEL_SAMPLERATE_200 6 141 | #define LSM303DLHC_ACCEL_SAMPLERATE_400 7 142 | 143 | // Accel FSR 144 | 145 | #define LSM303DLHC_ACCEL_FSR_2 0 146 | #define LSM303DLHC_ACCEL_FSR_4 1 147 | #define LSM303DLHC_ACCEL_FSR_8 2 148 | #define LSM303DLHC_ACCEL_FSR_16 3 149 | 150 | // Compass sample rate defines 151 | 152 | #define LSM303DLHC_COMPASS_SAMPLERATE_0_75 0 153 | #define LSM303DLHC_COMPASS_SAMPLERATE_1_5 1 154 | #define LSM303DLHC_COMPASS_SAMPLERATE_3 2 155 | #define LSM303DLHC_COMPASS_SAMPLERATE_7_5 3 156 | #define LSM303DLHC_COMPASS_SAMPLERATE_15 4 157 | #define LSM303DLHC_COMPASS_SAMPLERATE_30 5 158 | #define LSM303DLHC_COMPASS_SAMPLERATE_75 6 159 | #define LSM303DLHC_COMPASS_SAMPLERATE_220 7 160 | 161 | // Compass FSR 162 | 163 | #define LSM303DLHC_COMPASS_FSR_1_3 1 164 | #define LSM303DLHC_COMPASS_FSR_1_9 2 165 | #define LSM303DLHC_COMPASS_FSR_2_5 3 166 | #define LSM303DLHC_COMPASS_FSR_4 4 167 | #define LSM303DLHC_COMPASS_FSR_4_7 5 168 | #define LSM303DLHC_COMPASS_FSR_5_6 6 169 | #define LSM303DLHC_COMPASS_FSR_8_1 7 170 | 171 | class RTIMUGD20M303DLHC : public RTIMU 172 | { 173 | public: 174 | RTIMUGD20M303DLHC(RTIMUSettings *settings); 175 | ~RTIMUGD20M303DLHC(); 176 | 177 | virtual const char *IMUName() { return "L3GD20 + LSM303DLHC"; } 178 | virtual int IMUType() { return RTIMU_TYPE_GD20M303DLHC; } 179 | virtual int IMUInit(); 180 | virtual int IMUGetPollInterval(); 181 | virtual bool IMURead(); 182 | 183 | private: 184 | bool setGyroSampleRate(); 185 | bool setGyroCTRL2(); 186 | bool setGyroCTRL4(); 187 | bool setGyroCTRL5(); 188 | bool setAccelCTRL1(); 189 | bool setAccelCTRL4(); 190 | bool setCompassCRA(); 191 | bool setCompassCRB(); 192 | bool setCompassCRM(); 193 | 194 | unsigned char m_gyroSlaveAddr; // I2C address of L3GD20 195 | unsigned char m_accelSlaveAddr; // I2C address of LSM303DLHC accel 196 | unsigned char m_compassSlaveAddr; // I2C address of LSM303DLHC compass 197 | 198 | RTFLOAT m_gyroScale; 199 | RTFLOAT m_accelScale; 200 | RTFLOAT m_compassScaleXY; 201 | RTFLOAT m_compassScaleZ; 202 | }; 203 | 204 | #endif // _RTIMUGD20M303DLHC_H 205 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMULSM9DS0.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include "RTIMULSM9DS0.h" 25 | #include "RTIMUSettings.h" 26 | 27 | #if defined(LSM9DS0_6a) || defined(LSM9DS0_6b) 28 | 29 | 30 | RTIMULSM9DS0::RTIMULSM9DS0(RTIMUSettings *settings) : RTIMU(settings) 31 | { 32 | m_sampleRate = 100; 33 | } 34 | 35 | RTIMULSM9DS0::~RTIMULSM9DS0() 36 | { 37 | } 38 | 39 | int RTIMULSM9DS0::IMUInit() 40 | { 41 | unsigned char result; 42 | 43 | // configure IMU 44 | 45 | m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; 46 | if (m_gyroSlaveAddr == LSM9DS0_GYRO_ADDRESS0) 47 | m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0; 48 | else 49 | m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1; 50 | 51 | setCalibrationData(); 52 | 53 | // Set up the gyro 54 | 55 | if (!I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80)) 56 | return -1; 57 | 58 | if (!I2Cdev::readByte(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, &result)) 59 | return -2; 60 | 61 | if (result != LSM9DS0_GYRO_ID) { 62 | return -3; 63 | } 64 | 65 | if (!setGyroSampleRate()) 66 | return -4; 67 | 68 | if (!setGyroCTRL2()) 69 | return -5; 70 | 71 | if (!setGyroCTRL4()) 72 | return -6; 73 | 74 | // Set up the accel 75 | 76 | if (!I2Cdev::readByte(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, &result)) 77 | return -7; 78 | 79 | if (result != LSM9DS0_ACCELMAG_ID) { 80 | return -8; 81 | } 82 | 83 | if (!setAccelCTRL1()) 84 | return 9; 85 | 86 | if (!setAccelCTRL2()) 87 | return -10; 88 | 89 | if (!setCompassCTRL5()) 90 | return 11; 91 | 92 | if (!setCompassCTRL6()) 93 | return -12; 94 | 95 | if (!setCompassCTRL7()) 96 | return -13; 97 | 98 | if (!setGyroCTRL5()) 99 | return -14; 100 | 101 | gyroBiasInit(); 102 | return 1; 103 | } 104 | 105 | bool RTIMULSM9DS0::setGyroSampleRate() 106 | { 107 | unsigned char ctrl1; 108 | 109 | switch (m_settings->m_LSM9DS0GyroSampleRate) { 110 | case LSM9DS0_GYRO_SAMPLERATE_95: 111 | ctrl1 = 0x0f; 112 | m_sampleRate = 95; 113 | break; 114 | 115 | case LSM9DS0_GYRO_SAMPLERATE_190: 116 | ctrl1 = 0x4f; 117 | m_sampleRate = 190; 118 | break; 119 | 120 | case LSM9DS0_GYRO_SAMPLERATE_380: 121 | ctrl1 = 0x8f; 122 | m_sampleRate = 380; 123 | break; 124 | 125 | case LSM9DS0_GYRO_SAMPLERATE_760: 126 | ctrl1 = 0xcf; 127 | m_sampleRate = 760; 128 | break; 129 | 130 | default: 131 | return false; 132 | } 133 | 134 | m_sampleInterval = (uint64_t)1000000 / m_sampleRate; 135 | 136 | switch (m_settings->m_LSM9DS0GyroBW) { 137 | case LSM9DS0_GYRO_BANDWIDTH_0: 138 | ctrl1 |= 0x00; 139 | break; 140 | 141 | case LSM9DS0_GYRO_BANDWIDTH_1: 142 | ctrl1 |= 0x10; 143 | break; 144 | 145 | case LSM9DS0_GYRO_BANDWIDTH_2: 146 | ctrl1 |= 0x20; 147 | break; 148 | 149 | case LSM9DS0_GYRO_BANDWIDTH_3: 150 | ctrl1 |= 0x30; 151 | break; 152 | 153 | } 154 | 155 | return (I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL1, ctrl1)); 156 | } 157 | 158 | bool RTIMULSM9DS0::setGyroCTRL2() 159 | { 160 | if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) { 161 | return false; 162 | } 163 | return I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf); 164 | } 165 | 166 | bool RTIMULSM9DS0::setGyroCTRL4() 167 | { 168 | unsigned char ctrl4; 169 | 170 | switch (m_settings->m_LSM9DS0GyroFsr) { 171 | case LSM9DS0_GYRO_FSR_250: 172 | ctrl4 = 0x00; 173 | m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD; 174 | break; 175 | 176 | case LSM9DS0_GYRO_FSR_500: 177 | ctrl4 = 0x10; 178 | m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD; 179 | break; 180 | 181 | case LSM9DS0_GYRO_FSR_2000: 182 | ctrl4 = 0x20; 183 | m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD; 184 | break; 185 | 186 | default: 187 | return false; 188 | } 189 | 190 | return I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL4, ctrl4); 191 | } 192 | 193 | 194 | bool RTIMULSM9DS0::setGyroCTRL5() 195 | { 196 | unsigned char ctrl5; 197 | 198 | // Turn on hpf 199 | 200 | ctrl5 = 0x10; 201 | 202 | return I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, ctrl5); 203 | } 204 | 205 | 206 | bool RTIMULSM9DS0::setAccelCTRL1() 207 | { 208 | unsigned char ctrl1; 209 | 210 | if ((m_settings->m_LSM9DS0AccelSampleRate < 0) || (m_settings->m_LSM9DS0AccelSampleRate > 10)) { 211 | return false; 212 | } 213 | 214 | ctrl1 = (m_settings->m_LSM9DS0AccelSampleRate << 4) | 0x07; 215 | 216 | return I2Cdev::writeByte(m_accelCompassSlaveAddr, LSM9DS0_CTRL1, ctrl1); 217 | } 218 | 219 | bool RTIMULSM9DS0::setAccelCTRL2() 220 | { 221 | unsigned char ctrl2; 222 | 223 | if ((m_settings->m_LSM9DS0AccelLpf < 0) || (m_settings->m_LSM9DS0AccelLpf > 3)) { 224 | return false; 225 | } 226 | 227 | switch (m_settings->m_LSM9DS0AccelFsr) { 228 | case LSM9DS0_ACCEL_FSR_2: 229 | m_accelScale = (RTFLOAT)0.000061; 230 | break; 231 | 232 | case LSM9DS0_ACCEL_FSR_4: 233 | m_accelScale = (RTFLOAT)0.000122; 234 | break; 235 | 236 | case LSM9DS0_ACCEL_FSR_6: 237 | m_accelScale = (RTFLOAT)0.000183; 238 | break; 239 | 240 | case LSM9DS0_ACCEL_FSR_8: 241 | m_accelScale = (RTFLOAT)0.000244; 242 | break; 243 | 244 | case LSM9DS0_ACCEL_FSR_16: 245 | m_accelScale = (RTFLOAT)0.000732; 246 | break; 247 | 248 | default: 249 | return false; 250 | } 251 | 252 | ctrl2 = (m_settings->m_LSM9DS0AccelLpf << 6) | (m_settings->m_LSM9DS0AccelFsr << 3); 253 | 254 | return I2Cdev::writeByte(m_accelCompassSlaveAddr, LSM9DS0_CTRL2, ctrl2); 255 | } 256 | 257 | 258 | bool RTIMULSM9DS0::setCompassCTRL5() 259 | { 260 | unsigned char ctrl5; 261 | 262 | if ((m_settings->m_LSM9DS0CompassSampleRate < 0) || (m_settings->m_LSM9DS0CompassSampleRate > 5)) { 263 | return false; 264 | } 265 | 266 | ctrl5 = (m_settings->m_LSM9DS0CompassSampleRate << 2); 267 | 268 | return I2Cdev::writeByte(m_accelCompassSlaveAddr, LSM9DS0_CTRL5, ctrl5); 269 | } 270 | 271 | bool RTIMULSM9DS0::setCompassCTRL6() 272 | { 273 | unsigned char ctrl6; 274 | 275 | // convert FSR to uT 276 | 277 | switch (m_settings->m_LSM9DS0CompassFsr) { 278 | case LSM9DS0_COMPASS_FSR_2: 279 | ctrl6 = 0; 280 | m_compassScale = (RTFLOAT)0.008; 281 | break; 282 | 283 | case LSM9DS0_COMPASS_FSR_4: 284 | ctrl6 = 0x20; 285 | m_compassScale = (RTFLOAT)0.016; 286 | break; 287 | 288 | case LSM9DS0_COMPASS_FSR_8: 289 | ctrl6 = 0x40; 290 | m_compassScale = (RTFLOAT)0.032; 291 | break; 292 | 293 | case LSM9DS0_COMPASS_FSR_12: 294 | ctrl6 = 0x60; 295 | m_compassScale = (RTFLOAT)0.0479; 296 | break; 297 | 298 | default: 299 | return false; 300 | } 301 | 302 | return I2Cdev::writeByte(m_accelCompassSlaveAddr, LSM9DS0_CTRL6, ctrl6); 303 | } 304 | 305 | bool RTIMULSM9DS0::setCompassCTRL7() 306 | { 307 | return I2Cdev::writeByte(m_accelCompassSlaveAddr, LSM9DS0_CTRL7, 0x60); 308 | } 309 | 310 | int RTIMULSM9DS0::IMUGetPollInterval() 311 | { 312 | return (400 / m_sampleRate); 313 | } 314 | 315 | bool RTIMULSM9DS0::IMURead() 316 | { 317 | unsigned char status; 318 | unsigned char gyroData[6]; 319 | unsigned char accelData[6]; 320 | unsigned char compassData[6]; 321 | 322 | if (!I2Cdev::readByte(m_gyroSlaveAddr, LSM9DS0_GYRO_STATUS, &status)) 323 | return false; 324 | 325 | if ((status & 0x8) == 0) 326 | return false; 327 | 328 | if (!I2Cdev::readBytes(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData)) 329 | return false; 330 | 331 | m_timestamp = millis(); 332 | 333 | if (!I2Cdev::readBytes(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData)) 334 | return false; 335 | 336 | if (!I2Cdev::readBytes(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData)) 337 | return false; 338 | 339 | RTMath::convertToVector(gyroData, m_gyro, m_gyroScale, false); 340 | RTMath::convertToVector(accelData, m_accel, m_accelScale, false); 341 | RTMath::convertToVector(compassData, m_compass, m_compassScale, false); 342 | 343 | // sort out gyro axes 344 | 345 | m_gyro.setY(-m_gyro.y()); 346 | m_gyro.setZ(-m_gyro.z()); 347 | 348 | // sort out accel data; 349 | 350 | m_accel.setX(-m_accel.x()); 351 | 352 | // sort out compass axes 353 | 354 | m_compass.setY(-m_compass.y()); 355 | 356 | // now do standard processing 357 | 358 | handleGyroBias(); 359 | calibrateAverageCompass(); 360 | 361 | return true; 362 | } 363 | #endif 364 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMULSM9DS0.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMULSM9DS0_H 25 | #define _RTIMULSM9DS0_H 26 | 27 | #include "RTIMU.h" 28 | 29 | // I2C Slave Addresses 30 | 31 | #define LSM9DS0_GYRO_ADDRESS0 0x6a 32 | #define LSM9DS0_GYRO_ADDRESS1 0x6b 33 | #define LSM9DS0_GYRO_ID 0xd4 34 | 35 | #define LSM9DS0_ACCELMAG_ADDRESS0 0x1e 36 | #define LSM9DS0_ACCELMAG_ADDRESS1 0x1d 37 | #define LSM9DS0_ACCELMAG_ID 0x49 38 | 39 | // L3GD20 Register map 40 | 41 | #define LSM9DS0_GYRO_WHO_AM_I 0x0f 42 | #define LSM9DS0_GYRO_CTRL1 0x20 43 | #define LSM9DS0_GYRO_CTRL2 0x21 44 | #define LSM9DS0_GYRO_CTRL3 0x22 45 | #define LSM9DS0_GYRO_CTRL4 0x23 46 | #define LSM9DS0_GYRO_CTRL5 0x24 47 | #define LSM9DS0_GYRO_OUT_TEMP 0x26 48 | #define LSM9DS0_GYRO_STATUS 0x27 49 | #define LSM9DS0_GYRO_OUT_X_L 0x28 50 | #define LSM9DS0_GYRO_OUT_X_H 0x29 51 | #define LSM9DS0_GYRO_OUT_Y_L 0x2a 52 | #define LSM9DS0_GYRO_OUT_Y_H 0x2b 53 | #define LSM9DS0_GYRO_OUT_Z_L 0x2c 54 | #define LSM9DS0_GYRO_OUT_Z_H 0x2d 55 | #define LSM9DS0_GYRO_FIFO_CTRL 0x2e 56 | #define LSM9DS0_GYRO_FIFO_SRC 0x2f 57 | #define LSM9DS0_GYRO_IG_CFG 0x30 58 | #define LSM9DS0_GYRO_IG_SRC 0x31 59 | #define LSM9DS0_GYRO_IG_THS_XH 0x32 60 | #define LSM9DS0_GYRO_IG_THS_XL 0x33 61 | #define LSM9DS0_GYRO_IG_THS_YH 0x34 62 | #define LSM9DS0_GYRO_IG_THS_YL 0x35 63 | #define LSM9DS0_GYRO_IG_THS_ZH 0x36 64 | #define LSM9DS0_GYRO_IG_THS_ZL 0x37 65 | #define LSM9DS0_GYRO_IG_DURATION 0x38 66 | 67 | // Gyro sample rate defines 68 | 69 | #define LSM9DS0_GYRO_SAMPLERATE_95 0 70 | #define LSM9DS0_GYRO_SAMPLERATE_190 1 71 | #define LSM9DS0_GYRO_SAMPLERATE_380 2 72 | #define LSM9DS0_GYRO_SAMPLERATE_760 3 73 | 74 | // Gyro banwidth defines 75 | 76 | #define LSM9DS0_GYRO_BANDWIDTH_0 0 77 | #define LSM9DS0_GYRO_BANDWIDTH_1 1 78 | #define LSM9DS0_GYRO_BANDWIDTH_2 2 79 | #define LSM9DS0_GYRO_BANDWIDTH_3 3 80 | 81 | // Gyro FSR defines 82 | 83 | #define LSM9DS0_GYRO_FSR_250 0 84 | #define LSM9DS0_GYRO_FSR_500 1 85 | #define LSM9DS0_GYRO_FSR_2000 2 86 | 87 | // Gyro high pass filter defines 88 | 89 | #define LSM9DS0_GYRO_HPF_0 0 90 | #define LSM9DS0_GYRO_HPF_1 1 91 | #define LSM9DS0_GYRO_HPF_2 2 92 | #define LSM9DS0_GYRO_HPF_3 3 93 | #define LSM9DS0_GYRO_HPF_4 4 94 | #define LSM9DS0_GYRO_HPF_5 5 95 | #define LSM9DS0_GYRO_HPF_6 6 96 | #define LSM9DS0_GYRO_HPF_7 7 97 | #define LSM9DS0_GYRO_HPF_8 8 98 | #define LSM9DS0_GYRO_HPF_9 9 99 | 100 | // Accel/Mag Register Map 101 | 102 | #define LSM9DS0_TEMP_OUT_L 0x05 103 | #define LSM9DS0_TEMP_OUT_H 0x06 104 | #define LSM9DS0_STATUS_M 0x07 105 | #define LSM9DS0_OUT_X_L_M 0x08 106 | #define LSM9DS0_OUT_X_H_M 0x09 107 | #define LSM9DS0_OUT_Y_L_M 0x0a 108 | #define LSM9DS0_OUT_Y_H_M 0x0b 109 | #define LSM9DS0_OUT_Z_L_M 0x0c 110 | #define LSM9DS0_OUT_Z_H_M 0x0d 111 | #define LSM9DS0_WHO_AM_I 0x0f 112 | #define LSM9DS0_INT_CTRL_M 0x12 113 | #define LSM9DS0_INT_SRC_M 0x13 114 | #define LSM9DS0_INT_THS_L_M 0x14 115 | #define LSM9DS0_INT_THS_H_M 0x15 116 | #define LSM9DS0_OFFSET_X_L_M 0x16 117 | #define LSM9DS0_OFFSET_X_H_M 0x17 118 | #define LSM9DS0_OFFSET_Y_L_M 0x18 119 | #define LSM9DS0_OFFSET_Y_H_M 0x19 120 | #define LSM9DS0_OFFSET_Z_L_M 0x1a 121 | #define LSM9DS0_OFFSET_Z_H_M 0x1b 122 | #define LSM9DS0_REFERENCE_X 0x1c 123 | #define LSM9DS0_REFERENCE_Y 0x1d 124 | #define LSM9DS0_REFERENCE_Z 0x1e 125 | #define LSM9DS0_CTRL0 0x1f 126 | #define LSM9DS0_CTRL1 0x20 127 | #define LSM9DS0_CTRL2 0x21 128 | #define LSM9DS0_CTRL3 0x22 129 | #define LSM9DS0_CTRL4 0x23 130 | #define LSM9DS0_CTRL5 0x24 131 | #define LSM9DS0_CTRL6 0x25 132 | #define LSM9DS0_CTRL7 0x26 133 | #define LSM9DS0_STATUS_A 0x27 134 | #define LSM9DS0_OUT_X_L_A 0x28 135 | #define LSM9DS0_OUT_X_H_A 0x29 136 | #define LSM9DS0_OUT_Y_L_A 0x2a 137 | #define LSM9DS0_OUT_Y_H_A 0x2b 138 | #define LSM9DS0_OUT_Z_L_A 0x2c 139 | #define LSM9DS0_OUT_Z_H_A 0x2d 140 | #define LSM9DS0_FIFO_CTRL 0x2e 141 | #define LSM9DS0_FIFO_SRC 0x2f 142 | #define LSM9DS0_IG_CFG1 0x30 143 | #define LSM9DS0_IG_SRC1 0x31 144 | #define LSM9DS0_IG_THS1 0x32 145 | #define LSM9DS0_IG_DUR1 0x33 146 | #define LSM9DS0_IG_CFG2 0x34 147 | #define LSM9DS0_IG_SRC2 0x35 148 | #define LSM9DS0_IG_THS2 0x36 149 | #define LSM9DS0_IG_DUR2 0x37 150 | #define LSM9DS0_CLICK_CFG 0x38 151 | #define LSM9DS0_CLICK_SRC 0x39 152 | #define LSM9DS0_CLICK_THS 0x3a 153 | #define LSM9DS0_TIME_LIMIT 0x3b 154 | #define LSM9DS0_TIME_LATENCY 0x3c 155 | #define LSM9DS0_TIME_WINDOW 0x3d 156 | #define LSM9DS0_ACT_THIS 0x3e 157 | #define LSM9DS0_ACT_DUR 0x3f 158 | 159 | // Accel sample rate defines 160 | 161 | #define LSM9DS0_ACCEL_SAMPLERATE_3_125 1 162 | #define LSM9DS0_ACCEL_SAMPLERATE_6_25 2 163 | #define LSM9DS0_ACCEL_SAMPLERATE_12_5 3 164 | #define LSM9DS0_ACCEL_SAMPLERATE_25 4 165 | #define LSM9DS0_ACCEL_SAMPLERATE_50 5 166 | #define LSM9DS0_ACCEL_SAMPLERATE_100 6 167 | #define LSM9DS0_ACCEL_SAMPLERATE_200 7 168 | #define LSM9DS0_ACCEL_SAMPLERATE_400 8 169 | #define LSM9DS0_ACCEL_SAMPLERATE_800 9 170 | #define LSM9DS0_ACCEL_SAMPLERATE_1600 10 171 | 172 | // Accel FSR 173 | 174 | #define LSM9DS0_ACCEL_FSR_2 0 175 | #define LSM9DS0_ACCEL_FSR_4 1 176 | #define LSM9DS0_ACCEL_FSR_6 2 177 | #define LSM9DS0_ACCEL_FSR_8 3 178 | #define LSM9DS0_ACCEL_FSR_16 4 179 | 180 | // Accel filter bandwidth 181 | 182 | #define LSM9DS0_ACCEL_LPF_773 0 183 | #define LSM9DS0_ACCEL_LPF_194 1 184 | #define LSM9DS0_ACCEL_LPF_362 2 185 | #define LSM9DS0_ACCEL_LPF_50 3 186 | 187 | // Compass sample rate defines 188 | 189 | #define LSM9DS0_COMPASS_SAMPLERATE_3_125 0 190 | #define LSM9DS0_COMPASS_SAMPLERATE_6_25 1 191 | #define LSM9DS0_COMPASS_SAMPLERATE_12_5 2 192 | #define LSM9DS0_COMPASS_SAMPLERATE_25 3 193 | #define LSM9DS0_COMPASS_SAMPLERATE_50 4 194 | #define LSM9DS0_COMPASS_SAMPLERATE_100 5 195 | 196 | // Compass FSR 197 | 198 | #define LSM9DS0_COMPASS_FSR_2 0 199 | #define LSM9DS0_COMPASS_FSR_4 1 200 | #define LSM9DS0_COMPASS_FSR_8 2 201 | #define LSM9DS0_COMPASS_FSR_12 3 202 | 203 | 204 | class RTIMULSM9DS0 : public RTIMU 205 | { 206 | public: 207 | RTIMULSM9DS0(RTIMUSettings *settings); 208 | ~RTIMULSM9DS0(); 209 | 210 | virtual const char *IMUName() { return "LSM9DS0"; } 211 | virtual int IMUType() { return RTIMU_TYPE_LSM9DS0; } 212 | virtual int IMUInit(); 213 | virtual int IMUGetPollInterval(); 214 | virtual bool IMURead(); 215 | 216 | private: 217 | bool setGyroSampleRate(); 218 | bool setGyroCTRL2(); 219 | bool setGyroCTRL4(); 220 | bool setGyroCTRL5(); 221 | bool setAccelCTRL1(); 222 | bool setAccelCTRL2(); 223 | bool setCompassCTRL5(); 224 | bool setCompassCTRL6(); 225 | bool setCompassCTRL7(); 226 | 227 | unsigned char m_gyroSlaveAddr; // I2C address of gyro 228 | unsigned char m_accelCompassSlaveAddr; // I2C address of accel and mag 229 | unsigned char m_bus; // I2C bus (usually 1 for Raspberry Pi for example) 230 | 231 | RTFLOAT m_gyroScale; 232 | RTFLOAT m_accelScale; 233 | RTFLOAT m_compassScale; 234 | 235 | #ifdef LSM9DS0_CACHE_MODE 236 | bool m_firstTime; // if first sample 237 | 238 | LSM9DS0_CACHE_BLOCK m_cache[LSM9DS0_CACHE_BLOCK_COUNT]; // the cache itself 239 | int m_cacheIn; // the in index 240 | int m_cacheOut; // the out index 241 | int m_cacheCount; // number of used cache blocks 242 | 243 | #endif 244 | }; 245 | 246 | #endif // _RTIMULSM9DS0_H 247 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMULibDefs.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMULIBDEFS_H 25 | #define _RTIMULIBDEFS_H 26 | 27 | #include "RTMath.h" 28 | #include "RTPressureDefs.h" 29 | 30 | // IMU enable defs - only one should be enabled, the rest commented out 31 | 32 | #define MPU9150_68 // MPU9150 at address 0x68 33 | //#define MPU9150_69 // MPU9150 at address 0x69 34 | //#define MPU9250_68 // MPU9250 at address 0x68 35 | //#define MPU9250_69 // MPU9250 at address 0x69 36 | //#define LSM9DS0_6a // LSM9DS0 at address 0x6a 37 | //#define LSM9DS0_6b // LSM9DS0 at address 0x6b 38 | //#define GD20HM303D_6a // GD20H + M303D at address 0x6a 39 | //#define GD20HM303D_6b // GD20H + M303D at address 0x6b 40 | //#define GD20HM303DLHC_6a // GD20H + M303DLHC at address 0x6a 41 | //#define GD20HM303DLHC_6b // GD20H + M303DLHC at address 0x6b 42 | //#define GD20M303DLHC_6a // GD20 + M303DLHC at address 0x6a 43 | //#define GD20M303DLHC_6b // GD20 + M303DLHC at address 0x6b 44 | //#define BNO055_28 // BNO055 at address 0x28 45 | //#define BNO055_29 // BNO055 at address 0x29 46 | 47 | // IMU type codes 48 | 49 | #define RTIMU_TYPE_MPU9150 1 // InvenSense MPU9150 50 | #define RTIMU_TYPE_LSM9DS0 2 // STM LSM9DS0 (eg Sparkfun IMU) 51 | #define RTIMU_TYPE_GD20HM303D 3 // STM L3GD20H/LSM303D (Pololu Altimu) 52 | #define RTIMU_TYPE_GD20M303DLHC 4 // STM L3GD20/LSM303DHLC (old Adafruit IMU) 53 | #define RTIMU_TYPE_MPU9250 5 // InvenSense MPU9250 54 | #define RTIMU_TYPE_GD20HM303DLHC 6 // STM L3GD20H/LSM303DHLC (new Adafruit IMU) 55 | #define RTIMU_TYPE_BNO055 7 // BNO055 56 | 57 | // Pressure enable defs - only one should be enabled, the rest commented out 58 | 59 | //#define BMP180 // BMP180 60 | //#define LPS25H_5c // LPS25H at standard address 61 | //#define LPS25H_5d // LPS25H at option address 62 | //#define MS5611_76 // MS5611 at standard address 63 | //#define MS5611_77 // MS5611 at option address 64 | 65 | // IMU Axis rotation defs 66 | // 67 | // These allow the IMU to be virtually repositioned if it is in a non-standard configuration 68 | // Standard configuration is X pointing at north, Y pointing east and Z pointing down 69 | // with the IMU horizontal. There are 24 different possible orientations as defined 70 | // below. Setting the axis rotation code to non-zero values performs the repositioning. 71 | // 72 | // Uncomment the one required 73 | 74 | #define RTIMU_XNORTH_YEAST 0 // this is the default identity matrix 75 | //#define RTIMU_XEAST_YSOUTH 1 76 | //#define RTIMU_XSOUTH_YWEST 2 77 | //#define RTIMU_XWEST_YNORTH 3 78 | //#define RTIMU_XNORTH_YWEST 4 79 | //#define RTIMU_XEAST_YNORTH 5 80 | //#define RTIMU_XSOUTH_YEAST 6 81 | //#define RTIMU_XWEST_YSOUTH 7 82 | //#define RTIMU_XUP_YNORTH 8 83 | //#define RTIMU_XUP_YEAST 9 84 | //#define RTIMU_XUP_YSOUTH 10 85 | //#define RTIMU_XUP_YWEST 11 86 | //#define RTIMU_XDOWN_YNORTH 12 87 | //#define RTIMU_XDOWN_YEAST 13 88 | //#define RTIMU_XDOWN_YSOUTH 14 89 | //#define RTIMU_XDOWN_YWEST 15 90 | //#define RTIMU_XNORTH_YUP 16 91 | //#define RTIMU_XEAST_YUP 17 92 | //#define RTIMU_XSOUTH_YUP 18 93 | //#define RTIMU_XWEST_YUP 19 94 | //#define RTIMU_XNORTH_YDOWN 20 95 | //#define RTIMU_XEAST_YDOWN 21 96 | //#define RTIMU_XSOUTH_YDOWN 22 97 | //#define RTIMU_XWEST_YDOWN 23 98 | 99 | #endif // _RTIMULIBDEFS_H 100 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUMPU9150.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMUMPU9150_H 25 | #define _RTIMUMPU9150_H 26 | 27 | #include "RTIMU.h" 28 | 29 | // MPU9150 I2C Slave Addresses 30 | 31 | #define MPU9150_ADDRESS0 0x68 32 | #define MPU9150_ADDRESS1 0x69 33 | #define MPU9150_ID 0x68 34 | 35 | // these magnetometers are on aux bus 36 | 37 | #define AK8975_ADDRESS 0x0c 38 | #define HMC5883_ADDRESS 0x1e 39 | 40 | // Register map 41 | 42 | #define MPU9150_YG_OFFS_TC 0x01 43 | #define MPU9150_SMPRT_DIV 0x19 44 | #define MPU9150_LPF_CONFIG 0x1a 45 | #define MPU9150_GYRO_CONFIG 0x1b 46 | #define MPU9150_ACCEL_CONFIG 0x1c 47 | #define MPU9150_FIFO_EN 0x23 48 | #define MPU9150_I2C_MST_CTRL 0x24 49 | #define MPU9150_I2C_SLV0_ADDR 0x25 50 | #define MPU9150_I2C_SLV0_REG 0x26 51 | #define MPU9150_I2C_SLV0_CTRL 0x27 52 | #define MPU9150_I2C_SLV1_ADDR 0x28 53 | #define MPU9150_I2C_SLV1_REG 0x29 54 | #define MPU9150_I2C_SLV1_CTRL 0x2a 55 | #define MPU9150_I2C_SLV4_CTRL 0x34 56 | #define MPU9150_INT_PIN_CFG 0x37 57 | #define MPU9150_INT_ENABLE 0x38 58 | #define MPU9150_INT_STATUS 0x3a 59 | #define MPU9150_ACCEL_XOUT_H 0x3b 60 | #define MPU9150_GYRO_XOUT_H 0x43 61 | #define MPU9150_EXT_SENS_DATA_00 0x49 62 | #define MPU9150_I2C_SLV1_DO 0x64 63 | #define MPU9150_I2C_MST_DELAY_CTRL 0x67 64 | #define MPU9150_USER_CTRL 0x6a 65 | #define MPU9150_PWR_MGMT_1 0x6b 66 | #define MPU9150_PWR_MGMT_2 0x6c 67 | #define MPU9150_FIFO_COUNT_H 0x72 68 | #define MPU9150_FIFO_R_W 0x74 69 | #define MPU9150_WHO_AM_I 0x75 70 | 71 | // sample rate defines (applies to gyros and accels, not mags) 72 | 73 | #define MPU9150_SAMPLERATE_MIN 5 // 5 samples per second is the lowest 74 | #define MPU9150_SAMPLERATE_MAX 1000 // 1000 samples per second is the absolute maximum 75 | 76 | // compass rate defines 77 | 78 | #define MPU9150_COMPASSRATE_MIN 1 // 1 samples per second is the lowest 79 | #define MPU9150_COMPASSRATE_MAX 100 // 100 samples per second is maximum 80 | 81 | // LPF options (gyros and accels) 82 | 83 | #define MPU9150_LPF_256 0 // gyro: 256Hz, accel: 260Hz 84 | #define MPU9150_LPF_188 1 // gyro: 188Hz, accel: 184Hz 85 | #define MPU9150_LPF_98 2 // gyro: 98Hz, accel: 98Hz 86 | #define MPU9150_LPF_42 3 // gyro: 42Hz, accel: 44Hz 87 | #define MPU9150_LPF_20 4 // gyro: 20Hz, accel: 21Hz 88 | #define MPU9150_LPF_10 5 // gyro: 10Hz, accel: 10Hz 89 | #define MPU9150_LPF_5 6 // gyro: 5Hz, accel: 5Hz 90 | 91 | // Gyro FSR options 92 | 93 | #define MPU9150_GYROFSR_250 0 // +/- 250 degrees per second 94 | #define MPU9150_GYROFSR_500 8 // +/- 500 degrees per second 95 | #define MPU9150_GYROFSR_1000 0x10 // +/- 1000 degrees per second 96 | #define MPU9150_GYROFSR_2000 0x18 // +/- 2000 degrees per second 97 | 98 | // Accel FSR options 99 | 100 | #define MPU9150_ACCELFSR_2 0 // +/- 2g 101 | #define MPU9150_ACCELFSR_4 8 // +/- 4g 102 | #define MPU9150_ACCELFSR_8 0x10 // +/- 8g 103 | #define MPU9150_ACCELFSR_16 0x18 // +/- 16g 104 | 105 | 106 | // AK8975 compass registers 107 | 108 | #define AK8975_DEVICEID 0x0 // the device ID 109 | #define AK8975_ST1 0x02 // status 1 110 | #define AK8975_CNTL 0x0a // control reg 111 | #define AK8975_ASAX 0x10 // start of the fuse ROM data 112 | 113 | // HMC5883 compass registers 114 | 115 | #define HMC5883_CONFIG_A 0x00 // configuration A 116 | #define HMC5883_CONFIG_B 0x01 // configuration B 117 | #define HMC5883_MODE 0x02 // mode 118 | #define HMC5883_DATA_X_HI 0x03 // data x msb 119 | #define HMC5883_STATUS 0x09 // status 120 | #define HMC5883_ID 0x0a // id 121 | 122 | // FIFO transfer size 123 | 124 | #define MPU9150_FIFO_CHUNK_SIZE 12 // gyro and accels take 12 bytes 125 | 126 | class RTIMUMPU9150 : public RTIMU 127 | { 128 | public: 129 | RTIMUMPU9150(RTIMUSettings *settings); 130 | ~RTIMUMPU9150(); 131 | 132 | bool setLpf(unsigned char lpf); 133 | bool setSampleRate(int rate); 134 | bool setCompassRate(int rate); 135 | bool setGyroFsr(unsigned char fsr); 136 | bool setAccelFsr(unsigned char fsr); 137 | 138 | virtual const char *IMUName() { return "MPU-9150"; } 139 | virtual int IMUType() { return RTIMU_TYPE_MPU9150; } 140 | virtual int IMUInit(); 141 | virtual bool IMURead(); 142 | virtual int IMUGetPollInterval(); 143 | 144 | private: 145 | bool configureCompass(); // configure the compass 146 | bool bypassOn(); // talk to compass 147 | bool bypassOff(); // talk to MPU9150 148 | bool setSampleRate(); 149 | bool setCompassRate(); 150 | bool resetFifo(); 151 | 152 | bool m_firstTime; // if first sample 153 | 154 | unsigned char m_slaveAddr; // I2C address of MPU9150 155 | unsigned char m_bus; // I2C bus (usually 1 for Raspberry Pi for example) 156 | 157 | unsigned char m_lpf; // low pass filter setting 158 | int m_compassRate; // compass sample rate in Hz 159 | unsigned char m_gyroFsr; 160 | unsigned char m_accelFsr; 161 | 162 | RTFLOAT m_gyroScale; 163 | RTFLOAT m_accelScale; 164 | 165 | RTFLOAT m_compassAdjust[3]; // the compass fuse ROM values converted for use 166 | bool m_compassPresent; // false for MPU-6050 167 | bool m_compassIs5883; // if it is an MPU-6050/HMC5883 combo 168 | int m_compassDataLength; // 8 for MPU-9150, 6 for HMC5883 169 | }; 170 | 171 | #endif // _RTIMUMPU9150_H 172 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUMPU9250.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMUMPU9250_H 25 | #define _RTIMUMPU9250_H 26 | 27 | #include "RTIMU.h" 28 | 29 | // MPU9250 I2C Slave Addresses 30 | 31 | #define MPU9250_ADDRESS0 0x68 32 | #define MPU9250_ADDRESS1 0x69 33 | #define MPU9250_ID 0x71 34 | 35 | #define AK8963_ADDRESS 0x0c 36 | 37 | // Register map 38 | 39 | #define MPU9250_SMPRT_DIV 0x19 40 | #define MPU9250_GYRO_LPF 0x1a 41 | #define MPU9250_GYRO_CONFIG 0x1b 42 | #define MPU9250_ACCEL_CONFIG 0x1c 43 | #define MPU9250_ACCEL_LPF 0x1d 44 | #define MPU9250_FIFO_EN 0x23 45 | #define MPU9250_I2C_MST_CTRL 0x24 46 | #define MPU9250_I2C_SLV0_ADDR 0x25 47 | #define MPU9250_I2C_SLV0_REG 0x26 48 | #define MPU9250_I2C_SLV0_CTRL 0x27 49 | #define MPU9250_I2C_SLV1_ADDR 0x28 50 | #define MPU9250_I2C_SLV1_REG 0x29 51 | #define MPU9250_I2C_SLV1_CTRL 0x2a 52 | #define MPU9250_I2C_SLV2_ADDR 0x2b 53 | #define MPU9250_I2C_SLV2_REG 0x2c 54 | #define MPU9250_I2C_SLV2_CTRL 0x2d 55 | #define MPU9250_I2C_SLV4_CTRL 0x34 56 | #define MPU9250_INT_PIN_CFG 0x37 57 | #define MPU9250_INT_ENABLE 0x38 58 | #define MPU9250_INT_STATUS 0x3a 59 | #define MPU9250_ACCEL_XOUT_H 0x3b 60 | #define MPU9250_GYRO_XOUT_H 0x43 61 | #define MPU9250_EXT_SENS_DATA_00 0x49 62 | #define MPU9250_I2C_SLV1_DO 0x64 63 | #define MPU9250_I2C_MST_DELAY_CTRL 0x67 64 | #define MPU9250_USER_CTRL 0x6a 65 | #define MPU9250_PWR_MGMT_1 0x6b 66 | #define MPU9250_PWR_MGMT_2 0x6c 67 | #define MPU9250_FIFO_COUNT_H 0x72 68 | #define MPU9250_FIFO_R_W 0x74 69 | #define MPU9250_WHO_AM_I 0x75 70 | 71 | // sample rate defines (applies to gyros and accels, not mags) 72 | 73 | #define MPU9250_SAMPLERATE_MIN 5 // 5 samples per second is the lowest 74 | #define MPU9250_SAMPLERATE_MAX 1000 // 1000 samples per second is the absolute maximum 75 | 76 | // compass rate defines 77 | 78 | #define MPU9250_COMPASSRATE_MIN 1 // 1 samples per second is the lowest 79 | #define MPU9250_COMPASSRATE_MAX 100 // 100 samples per second is maximum 80 | 81 | // Gyro LPF options 82 | 83 | #define MPU9250_GYRO_LPF_8800 0x11 // 8800Hz, 0.64mS delay 84 | #define MPU9250_GYRO_LPF_3600 0x10 // 3600Hz, 0.11mS delay 85 | #define MPU9250_GYRO_LPF_250 0x00 // 250Hz, 0.97mS delay 86 | #define MPU9250_GYRO_LPF_184 0x01 // 184Hz, 2.9mS delay 87 | #define MPU9250_GYRO_LPF_92 0x02 // 92Hz, 3.9mS delay 88 | #define MPU9250_GYRO_LPF_41 0x03 // 41Hz, 5.9mS delay 89 | #define MPU9250_GYRO_LPF_20 0x04 // 20Hz, 9.9mS delay 90 | #define MPU9250_GYRO_LPF_10 0x05 // 10Hz, 17.85mS delay 91 | #define MPU9250_GYRO_LPF_5 0x06 // 5Hz, 33.48mS delay 92 | 93 | // Gyro FSR options 94 | 95 | #define MPU9250_GYROFSR_250 0 // +/- 250 degrees per second 96 | #define MPU9250_GYROFSR_500 8 // +/- 500 degrees per second 97 | #define MPU9250_GYROFSR_1000 0x10 // +/- 1000 degrees per second 98 | #define MPU9250_GYROFSR_2000 0x18 // +/- 2000 degrees per second 99 | 100 | // Accel FSR options 101 | 102 | #define MPU9250_ACCELFSR_2 0 // +/- 2g 103 | #define MPU9250_ACCELFSR_4 8 // +/- 4g 104 | #define MPU9250_ACCELFSR_8 0x10 // +/- 8g 105 | #define MPU9250_ACCELFSR_16 0x18 // +/- 16g 106 | 107 | // Accel LPF options 108 | 109 | #define MPU9250_ACCEL_LPF_1130 0x08 // 1130Hz, 0.75mS delay 110 | #define MPU9250_ACCEL_LPF_460 0x00 // 460Hz, 1.94mS delay 111 | #define MPU9250_ACCEL_LPF_184 0x01 // 184Hz, 5.80mS delay 112 | #define MPU9250_ACCEL_LPF_92 0x02 // 92Hz, 7.80mS delay 113 | #define MPU9250_ACCEL_LPF_41 0x03 // 41Hz, 11.80mS delay 114 | #define MPU9250_ACCEL_LPF_20 0x04 // 20Hz, 19.80mS delay 115 | #define MPU9250_ACCEL_LPF_10 0x05 // 10Hz, 35.70mS delay 116 | #define MPU9250_ACCEL_LPF_5 0x06 // 5Hz, 66.96mS delay 117 | 118 | // AK8963 compass registers 119 | 120 | #define AK8963_DEVICEID 0x48 // the device ID 121 | #define AK8963_ST1 0x02 // status 1 122 | #define AK8963_CNTL 0x0a // control reg 123 | #define AK8963_ASAX 0x10 // start of the fuse ROM data 124 | 125 | // FIFO transfer size 126 | 127 | #define MPU9250_FIFO_CHUNK_SIZE 12 // gyro and accels take 12 bytes 128 | 129 | class RTIMUMPU9250 : public RTIMU 130 | { 131 | public: 132 | RTIMUMPU9250(RTIMUSettings *settings); 133 | ~RTIMUMPU9250(); 134 | 135 | bool setGyroLpf(unsigned char lpf); 136 | bool setAccelLpf(unsigned char lpf); 137 | bool setSampleRate(int rate); 138 | bool setCompassRate(int rate); 139 | bool setGyroFsr(unsigned char fsr); 140 | bool setAccelFsr(unsigned char fsr); 141 | 142 | virtual const char *IMUName() { return "MPU-9250"; } 143 | virtual int IMUType() { return RTIMU_TYPE_MPU9250; } 144 | virtual int IMUInit(); 145 | virtual bool IMURead(); 146 | virtual int IMUGetPollInterval(); 147 | 148 | private: 149 | bool setGyroConfig(); 150 | bool setAccelConfig(); 151 | bool setSampleRate(); 152 | bool compassSetup(); 153 | bool setCompassRate(); 154 | bool resetFifo(); 155 | bool bypassOn(); 156 | bool bypassOff(); 157 | 158 | bool m_firstTime; // if first sample 159 | 160 | unsigned char m_slaveAddr; // I2C address of MPU9250 161 | unsigned char m_bus; // I2C bus (usually 1 for Raspberry Pi for example) 162 | 163 | unsigned char m_gyroLpf; // gyro low pass filter setting 164 | unsigned char m_accelLpf; // accel low pass filter setting 165 | int m_compassRate; // compass sample rate in Hz 166 | unsigned char m_gyroFsr; 167 | unsigned char m_accelFsr; 168 | 169 | RTFLOAT m_gyroScale; 170 | RTFLOAT m_accelScale; 171 | 172 | RTFLOAT m_compassAdjust[3]; // the compass fuse ROM values converted for use 173 | }; 174 | 175 | #endif // _RTIMUMPU9250_H 176 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTIMUSettings.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTIMUSETTINGS_H 25 | #define _RTIMUSETTINGS_H 26 | 27 | #include "RTMath.h" 28 | #include "RTIMULibDefs.h" 29 | 30 | class RTIMUSettings 31 | { 32 | public: 33 | RTIMUSettings(); 34 | 35 | // These are the local variables 36 | 37 | int m_imuType; // type code of imu in use 38 | unsigned char m_I2CSlaveAddress; // I2C slave address of the imu 39 | int m_pressureType; // type code of pressure sensor in use 40 | unsigned char m_I2CPressureAddress; // I2C slave address of the pressure sensor 41 | 42 | // IMU-specific vars 43 | 44 | #if defined(MPU9150_68) || defined(MPU9150_69) 45 | // MPU9150 46 | 47 | int m_MPU9150GyroAccelSampleRate; // the sample rate (samples per second) for gyro and accel 48 | int m_MPU9150CompassSampleRate; // same for the compass 49 | int m_MPU9150GyroAccelLpf; // low pass filter code for the gyro and accel 50 | int m_MPU9150GyroFsr; // FSR code for the gyro 51 | int m_MPU9150AccelFsr; // FSR code for the accel 52 | #endif 53 | 54 | #if defined(MPU9250_68) || defined(MPU9250_69) 55 | // MPU9250 56 | 57 | int m_MPU9250GyroAccelSampleRate; // the sample rate (samples per second) for gyro and accel 58 | int m_MPU9250CompassSampleRate; // same for the compass 59 | int m_MPU9250GyroLpf; // low pass filter code for the gyro 60 | int m_MPU9250AccelLpf; // low pass filter code for the accel 61 | int m_MPU9250GyroFsr; // FSR code for the gyro 62 | int m_MPU9250AccelFsr; // FSR code for the accel 63 | #endif 64 | 65 | #if defined(LSM9DS0_6a) || defined(LSM9DS0_6b) 66 | // LSM9DS0 67 | 68 | int m_LSM9DS0GyroSampleRate; // the gyro sample rate 69 | int m_LSM9DS0GyroBW; // the gyro bandwidth code 70 | int m_LSM9DS0GyroHpf; // the gyro high pass filter cutoff code 71 | int m_LSM9DS0GyroFsr; // the gyro full scale range 72 | 73 | int m_LSM9DS0AccelSampleRate; // the accel sample rate 74 | int m_LSM9DS0AccelFsr; // the accel full scale range 75 | int m_LSM9DS0AccelLpf; // the accel low pass filter 76 | 77 | int m_LSM9DS0CompassSampleRate; // the compass sample rate 78 | int m_LSM9DS0CompassFsr; // the compass full scale range 79 | #endif 80 | 81 | #if defined(GD20HM303D_6a) || defined(GD20HM303D_6b) 82 | int m_GD20HM303DGyroSampleRate; // the gyro sample rate 83 | int m_GD20HM303DGyroBW; // the gyro bandwidth code 84 | int m_GD20HM303DGyroHpf; // the gyro high pass filter cutoff code 85 | int m_GD20HM303DGyroFsr; // the gyro full scale range 86 | 87 | int m_GD20HM303DAccelSampleRate; // the accel sample rate 88 | int m_GD20HM303DAccelFsr; // the accel full scale range 89 | int m_GD20HM303DAccelLpf; // the accel low pass filter 90 | 91 | int m_GD20HM303DCompassSampleRate; // the compass sample rate 92 | int m_GD20HM303DCompassFsr; // the compass full scale range 93 | #endif 94 | 95 | #if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b) 96 | int m_GD20M303DLHCGyroSampleRate; // the gyro sample rate 97 | int m_GD20M303DLHCGyroBW; // the gyro bandwidth code 98 | int m_GD20M303DLHCGyroHpf; // the gyro high pass filter cutoff code 99 | int m_GD20M303DLHCGyroFsr; // the gyro full scale range 100 | 101 | int m_GD20M303DLHCAccelSampleRate; // the accel sample rate 102 | int m_GD20M303DLHCAccelFsr; // the accel full scale range 103 | 104 | int m_GD20M303DLHCCompassSampleRate; // the compass sample rate 105 | int m_GD20M303DLHCCompassFsr; // the compass full scale range 106 | #endif 107 | 108 | #if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b) 109 | int m_GD20HM303DLHCGyroSampleRate; // the gyro sample rate 110 | int m_GD20HM303DLHCGyroBW; // the gyro bandwidth code 111 | int m_GD20HM303DLHCGyroHpf; // the gyro high pass filter cutoff code 112 | int m_GD20HM303DLHCGyroFsr; // the gyro full scale range 113 | 114 | int m_GD20HM303DLHCAccelSampleRate; // the accel sample rate 115 | int m_GD20HM303DLHCAccelFsr; // the accel full scale range 116 | 117 | int m_GD20HM303DLHCCompassSampleRate; // the compass sample rate 118 | int m_GD20HM303DLHCCompassFsr; // the compass full scale range 119 | #endif 120 | 121 | }; 122 | 123 | #endif // _RTIMUSETTINGS_H 124 | 125 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTMath.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTMATH_H_ 25 | #define _RTMATH_H_ 26 | 27 | #include 28 | #include 29 | 30 | // The fundamental float type 31 | 32 | #ifdef RTMATH_USE_DOUBLE 33 | typedef double RTFLOAT; 34 | #else 35 | typedef float RTFLOAT; 36 | #endif 37 | 38 | // Useful constants 39 | 40 | #define RTMATH_PI 3.1415926535 41 | #define RTMATH_DEGREE_TO_RAD (M_PI / 180.0) 42 | #define RTMATH_RAD_TO_DEGREE (180.0 / M_PI) 43 | 44 | class RTVector3; 45 | 46 | #ifndef RTARDULINK_MODE 47 | class RTQuaternion; 48 | #endif 49 | 50 | class RTMath 51 | { 52 | public: 53 | #ifndef RTARDULINK_MODE 54 | // convenient display routines 55 | 56 | static void display(const char *label, RTVector3& vec); 57 | static void displayDegrees(const char *label, RTVector3& vec); 58 | static void displayRollPitchYaw(const char *label, RTVector3& vec); 59 | static void display(const char *label, RTQuaternion& quat); 60 | 61 | // poseFromAccelMag generates pose Euler angles from measured settings 62 | 63 | static RTVector3 poseFromAccelMag(const RTVector3& accel, const RTVector3& mag); 64 | 65 | // Takes signed 16 bit data from a char array and converts it to a vector of scaled RTFLOATs 66 | 67 | static void convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian); 68 | 69 | #endif // #ifndef RTARDULINK_MODE 70 | }; 71 | 72 | 73 | class RTVector3 74 | { 75 | public: 76 | RTVector3(); 77 | RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z); 78 | 79 | const RTVector3& operator +=(RTVector3& vec); 80 | const RTVector3& operator -=(RTVector3& vec); 81 | 82 | RTVector3& operator =(const RTVector3& vec); 83 | 84 | RTFLOAT squareLength(); 85 | void zero(); 86 | 87 | inline RTFLOAT x() const { return m_data[0]; } 88 | inline RTFLOAT y() const { return m_data[1]; } 89 | inline RTFLOAT z() const { return m_data[2]; } 90 | inline RTFLOAT data(const int i) const { return m_data[i]; } 91 | 92 | inline void setX(const RTFLOAT val) { m_data[0] = val; } 93 | inline void setY(const RTFLOAT val) { m_data[1] = val; } 94 | inline void setZ(const RTFLOAT val) { m_data[2] = val; } 95 | inline void setData(const int i, RTFLOAT val) { m_data[i] = val; } 96 | 97 | #ifndef RTARDULINK_MODE 98 | RTFLOAT length(); 99 | void normalize(); 100 | 101 | const char *display(); 102 | const char *displayDegrees(); 103 | 104 | static RTFLOAT dotProduct(const RTVector3& a, const RTVector3& b); 105 | static void crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d); 106 | 107 | void accelToEuler(RTVector3& rollPitchYaw) const; 108 | void accelToQuaternion(RTQuaternion& qPose) const; 109 | #endif // #ifndef RTARDULINK_MODE 110 | 111 | private: 112 | RTFLOAT m_data[3]; 113 | }; 114 | 115 | #ifndef RTARDULINK_MODE 116 | class RTQuaternion 117 | { 118 | public: 119 | RTQuaternion(); 120 | RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z); 121 | 122 | RTQuaternion& operator +=(const RTQuaternion& quat); 123 | RTQuaternion& operator -=(const RTQuaternion& quat); 124 | RTQuaternion& operator *=(const RTQuaternion& qb); 125 | RTQuaternion& operator *=(const RTFLOAT val); 126 | RTQuaternion& operator -=(const RTFLOAT val); 127 | 128 | RTQuaternion& operator =(const RTQuaternion& quat); 129 | const RTQuaternion operator *(const RTQuaternion& qb) const; 130 | const RTQuaternion operator *(const RTFLOAT val) const; 131 | const RTQuaternion operator -(const RTQuaternion& qb) const; 132 | const RTQuaternion operator -(const RTFLOAT val) const; 133 | 134 | void normalize(); 135 | void toEuler(RTVector3& vec); 136 | void fromEuler(RTVector3& vec); 137 | RTQuaternion conjugate() const; 138 | void toAngleVector(RTFLOAT& angle, RTVector3& vec); 139 | void fromAngleVector(const RTFLOAT& angle, const RTVector3& vec); 140 | 141 | void zero(); 142 | const char *display(); 143 | 144 | inline RTFLOAT scalar() const { return m_data[0]; } 145 | inline RTFLOAT x() const { return m_data[1]; } 146 | inline RTFLOAT y() const { return m_data[2]; } 147 | inline RTFLOAT z() const { return m_data[3]; } 148 | inline RTFLOAT data(const int i) const { return m_data[i]; } 149 | 150 | inline void setScalar(const RTFLOAT val) { m_data[0] = val; } 151 | inline void setX(const RTFLOAT val) { m_data[1] = val; } 152 | inline void setY(const RTFLOAT val) { m_data[2] = val; } 153 | inline void setZ(const RTFLOAT val) { m_data[3] = val; } 154 | inline void setData(const int i, RTFLOAT val) { m_data[i] = val; } 155 | 156 | private: 157 | RTFLOAT m_data[4]; 158 | }; 159 | #endif // #ifndef RTARDULINK_MODE 160 | 161 | #endif /* _RTMATH_H_ */ 162 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressure.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | 25 | #include "RTPressure.h" 26 | 27 | #if defined(BMP180) 28 | #include "RTPressureBMP180.h" 29 | #endif 30 | #if defined(LPS25H_5c) || defined(LPS25H_5d) 31 | #include "RTPressureLPS25H.h" 32 | #endif 33 | #if defined(MS5611_76) || defined(MS5611_77) 34 | #include "RTPressureMS5611.h" 35 | #endif 36 | 37 | RTPressure *RTPressure::createPressure(RTIMUSettings *settings) 38 | { 39 | #if defined(BMP180) 40 | return new RTPressureBMP180(settings); 41 | #endif 42 | #if defined(LPS25H_5c) || defined(LPS25H_5d) 43 | return new RTPressureLPS25H(settings); 44 | #endif 45 | #if defined(MS5611_76) || defined(MS5611_77) 46 | return new RTPressureMS5611(settings); 47 | #endif 48 | return 0; 49 | } 50 | 51 | 52 | RTPressure::RTPressure(RTIMUSettings *settings) 53 | { 54 | m_settings = settings; 55 | } 56 | 57 | RTPressure::~RTPressure() 58 | { 59 | } 60 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressure.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTPRESSURE_H 25 | #define _RTPRESSURE_H 26 | 27 | #include "RTIMUSettings.h" 28 | #include "RTIMULibDefs.h" 29 | #include "RTPressureDefs.h" 30 | #include "I2Cdev.h" 31 | 32 | class RTPressure 33 | { 34 | public: 35 | // Pressure sensor objects should always be created with the following call 36 | 37 | static RTPressure *createPressure(RTIMUSettings *settings); 38 | 39 | // Constructor/destructor 40 | 41 | RTPressure(RTIMUSettings *settings); 42 | virtual ~RTPressure(); 43 | 44 | // These functions must be provided by sub classes 45 | 46 | virtual const char *pressureName() = 0; // the name of the pressure sensor 47 | virtual int pressureType() = 0; // the type code of the pressure sensor 48 | virtual bool pressureInit() = 0; // set up the pressure sensor 49 | virtual bool pressureRead(float &latestPressure, float &latestTemperature) = 0; // get latest value 50 | 51 | protected: 52 | RTIMUSettings *m_settings; // the settings object pointer 53 | 54 | }; 55 | 56 | #endif // _RTPRESSURE_H 57 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureBMP180.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include "RTPressureBMP180.h" 25 | 26 | #if defined(BMP180) 27 | 28 | RTPressureBMP180::RTPressureBMP180(RTIMUSettings *settings) : RTPressure(settings) 29 | { 30 | m_validReadings = false; 31 | } 32 | 33 | RTPressureBMP180::~RTPressureBMP180() 34 | { 35 | } 36 | 37 | bool RTPressureBMP180::pressureInit() 38 | { 39 | unsigned char result; 40 | unsigned char data[22]; 41 | 42 | m_pressureAddr = m_settings->m_I2CPressureAddress; 43 | 44 | // check ID of chip 45 | 46 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_ID, 1, &result)) 47 | return false; 48 | 49 | if (result != BMP180_ID) { 50 | return false; 51 | } 52 | 53 | // get calibration data 54 | 55 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_AC1, 22, data)) 56 | return false; 57 | 58 | m_AC1 = (int16_t)(((uint16_t)data[0]) << 8) + (uint16_t)data[1]; 59 | m_AC2 = (int16_t)(((uint16_t)data[2]) << 8) + (uint16_t)data[3]; 60 | m_AC3 = (int16_t)(((uint16_t)data[4]) << 8) + (uint16_t)data[4]; 61 | m_AC4 = (((uint16_t)data[6]) << 8) + (uint16_t)data[7]; 62 | m_AC5 = (((uint16_t)data[8]) << 8) + (uint16_t)data[9]; 63 | m_AC6 = (((uint16_t)data[10]) << 8) + (uint16_t)data[11]; 64 | m_B1 = (int16_t)(((uint16_t)data[12]) << 8) + (uint16_t)data[13]; 65 | m_B2 = (int16_t)(((uint16_t)data[14]) << 8) + (uint16_t)data[15]; 66 | m_MB = (int16_t)(((uint16_t)data[16]) << 8) + (uint16_t)data[17]; 67 | m_MC = (int16_t)(((uint16_t)data[18]) << 8) + (uint16_t)data[19]; 68 | m_MD = (int16_t)(((uint16_t)data[20]) << 8) + (uint16_t)data[21]; 69 | 70 | m_state = BMP180_STATE_IDLE; 71 | m_oss = BMP180_SCO_PRESSURECONV_UHR; 72 | return true; 73 | } 74 | 75 | bool RTPressureBMP180::pressureRead(float &latestPressure, float &latestTemperature) 76 | { 77 | latestPressure = 0; 78 | latestTemperature = 0; 79 | 80 | if (m_state == BMP180_STATE_IDLE) { 81 | // start a temperature conversion 82 | if (!I2Cdev::writeByte(m_pressureAddr, BMP180_REG_SCO, BMP180_SCO_TEMPCONV)) { 83 | return false; 84 | } else { 85 | m_state = BMP180_STATE_TEMPERATURE; 86 | } 87 | } 88 | 89 | pressureBackground(); 90 | 91 | if (m_validReadings) { 92 | latestTemperature = m_temperature; 93 | latestPressure = m_pressure; 94 | } 95 | return true; 96 | } 97 | 98 | 99 | void RTPressureBMP180::pressureBackground() 100 | { 101 | uint8_t data[2]; 102 | 103 | switch (m_state) { 104 | case BMP180_STATE_IDLE: 105 | break; 106 | 107 | case BMP180_STATE_TEMPERATURE: 108 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_SCO, 1, data)) { 109 | break; 110 | } 111 | if ((data[0] & 0x20) == 0x20) 112 | break; // conversion not finished 113 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_RESULT, 2, data)) { 114 | m_state = BMP180_STATE_IDLE; 115 | break; 116 | } 117 | m_rawTemperature = (((uint16_t)data[0]) << 8) | (uint16_t)data[1]; 118 | 119 | data[0] = 0x34 + (m_oss << 6); 120 | if (!I2Cdev::writeBytes(m_pressureAddr, BMP180_REG_SCO, 1, data)) { 121 | m_state = BMP180_STATE_IDLE; 122 | break; 123 | } 124 | m_state = BMP180_STATE_PRESSURE; 125 | break; 126 | 127 | case BMP180_STATE_PRESSURE: 128 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_SCO, 1, data)) { 129 | break; 130 | } 131 | if ((data[0] & 0x20) == 0x20) 132 | break; // conversion not finished 133 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_RESULT, 2, data)) { 134 | m_state = BMP180_STATE_IDLE; 135 | break; 136 | } 137 | m_rawPressure = (((uint16_t)data[0]) << 8) | (uint16_t)data[1]; 138 | 139 | if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_XLSB, 1, data)) { 140 | m_state = BMP180_STATE_IDLE; 141 | break; 142 | } 143 | 144 | // call this function for testing only 145 | // should give T = 150 (15.0C) and pressure 6996 (699.6hPa) 146 | 147 | // setTestData(); 148 | 149 | int32_t pressure = ((((uint32_t)(m_rawPressure)) << 8) + (uint32_t)(data[0])) >> (8 - m_oss); 150 | 151 | m_state = BMP180_STATE_IDLE; 152 | 153 | // calculate compensated temperature 154 | 155 | int32_t X1 = ((m_rawTemperature - (int32_t)m_AC6) * (int32_t)m_AC5) / 32768; 156 | 157 | if ((X1 + m_MD) == 0) { 158 | break; 159 | } 160 | 161 | int32_t X2 = ((int32_t)m_MC * 2048) / (X1 + (int32_t)m_MD); 162 | int32_t B5 = X1 + X2; 163 | m_temperature = (RTFLOAT)((B5 + 8) / 16) / (RTFLOAT)10; 164 | 165 | // calculate compensated pressure 166 | 167 | int32_t B6 = B5 - 4000; 168 | // printf("B6 = %d\n", B6); 169 | X1 = (m_B2 * ((B6 * B6) / 4096)) / 2048; 170 | // printf("X1 = %d\n", X1); 171 | X2 = (m_AC2 * B6) / 2048; 172 | // printf("X2 = %d\n", X2); 173 | int32_t X3 = X1 + X2; 174 | // printf("X3 = %d\n", X3); 175 | int32_t B3 = (((m_AC1 * 4 + X3) << m_oss) + 2) / 4; 176 | // printf("B3 = %d\n", B3); 177 | X1 = (m_AC3 * B6) / 8192; 178 | // printf("X1 = %d\n", X1); 179 | X2 = (m_B1 * ((B6 * B6) / 4096)) / 65536; 180 | // printf("X2 = %d\n", X2); 181 | X3 = ((X1 + X2) + 2) / 4; 182 | // printf("X3 = %d\n", X3); 183 | uint32_t B4 = (m_AC4 * (uint32_t)(X3 + 32768)) / 32768; 184 | // printf("B4 = %d\n", B4); 185 | uint32_t B7 = (uint32_t)(pressure - B3) * (50000 >> m_oss); 186 | // printf("B7 = %d\n", B7); 187 | 188 | int32_t p; 189 | if (B7 < 0x80000000) 190 | p = (B7 * 2) / B4; 191 | else 192 | p = (B7 / B4) * 2; 193 | 194 | // printf("p = %d\n", p); 195 | X1 = (p / 256) * (p / 256); 196 | // printf("X1 = %d\n", X1); 197 | X1 = (X1 * 3038) / 65536; 198 | // printf("X1 = %d\n", X1); 199 | X2 = (-7357 * p) / 65536; 200 | // printf("X2 = %d\n", X2); 201 | m_pressure = (RTFLOAT)(p + (X1 + X2 + 3791) / 16) / (RTFLOAT)100; // the extra 100 factor is to get 1hPa units 202 | 203 | m_validReadings = true; 204 | 205 | // printf("UP = %d, P = %f, UT = %d, T = %f\n", m_rawPressure, m_pressure, m_rawTemperature, m_temperature); 206 | break; 207 | } 208 | } 209 | 210 | void RTPressureBMP180::setTestData() 211 | { 212 | m_AC1 = 408; 213 | m_AC2 = -72; 214 | m_AC3 = -14383; 215 | m_AC4 = 32741; 216 | m_AC5 = 32757; 217 | m_AC6 = 23153; 218 | m_B1 = 6190; 219 | m_B2 = 4; 220 | m_MB = -32767; 221 | m_MC = -8711; 222 | m_MD = 2868; 223 | 224 | m_rawTemperature = 27898; 225 | m_rawPressure = 23843; 226 | } 227 | #endif -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureBMP180.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTPRESSUREBMP180_H_ 25 | #define _RTPRESSUREBMP180_H_ 26 | 27 | #include "RTPressure.h" 28 | 29 | // State definitions 30 | 31 | #define BMP180_STATE_IDLE 0 32 | #define BMP180_STATE_TEMPERATURE 1 33 | #define BMP180_STATE_PRESSURE 2 34 | 35 | // Conversion reg defs 36 | 37 | #define BMP180_SCO_TEMPCONV 0x2e // temperature conversion 38 | #define BMP180_SCO_PRESSURECONV_ULP 0 // ultra low power pressure conversion 39 | #define BMP180_SCO_PRESSURECONV_STD 1 // standard pressure conversion 40 | #define BMP180_SCO_PRESSURECONV_HR 2 // high res pressure conversion 41 | #define BMP180_SCO_PRESSURECONV_UHR 3 // ultra high res pressure conversion 42 | 43 | class RTIMUSettings; 44 | 45 | class RTPressureBMP180 : public RTPressure 46 | { 47 | public: 48 | RTPressureBMP180(RTIMUSettings *settings); 49 | ~RTPressureBMP180(); 50 | 51 | virtual const char *pressureName() { return "BMP180"; } 52 | virtual int pressureType() { return RTPRESSURE_TYPE_BMP180; } 53 | virtual bool pressureInit(); 54 | virtual bool pressureRead(float &latestPressure, float &latestTemperature); 55 | 56 | private: 57 | void pressureBackground(); 58 | void setTestData(); 59 | 60 | unsigned char m_pressureAddr; // I2C address 61 | RTFLOAT m_pressure; // the current pressure 62 | RTFLOAT m_temperature; // the current temperature 63 | 64 | // This is the calibration data read from the sensor 65 | 66 | int16_t m_AC1; 67 | int16_t m_AC2; 68 | int16_t m_AC3; 69 | uint16_t m_AC4; 70 | uint16_t m_AC5; 71 | uint16_t m_AC6; 72 | int16_t m_B1; 73 | int16_t m_B2; 74 | int16_t m_MB; 75 | int16_t m_MC; 76 | int16_t m_MD; 77 | 78 | int m_state; 79 | int m_oss; 80 | 81 | int32_t m_rawPressure; 82 | int32_t m_rawTemperature; 83 | 84 | bool m_validReadings; 85 | }; 86 | 87 | #endif // _RTPRESSUREBMP180_H_ 88 | 89 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureDefs.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTPRESSUREDEFS_H 25 | #define _RTPRESSUREDEFS_H 26 | 27 | // Pressure sensor type codes 28 | 29 | #define RTPRESSURE_TYPE_AUTODISCOVER 0 // audodiscover the pressure sensor 30 | #define RTPRESSURE_TYPE_NULL 1 // if no physical hardware 31 | #define RTPRESSURE_TYPE_BMP180 2 // BMP180 32 | #define RTPRESSURE_TYPE_LPS25H 3 // LPS25H 33 | #define RTPRESSURE_TYPE_MS5611 4 // MS5611 34 | 35 | //---------------------------------------------------------- 36 | // 37 | // BMP180 38 | 39 | // BMP180 I2C Slave Addresse 40 | 41 | #define BMP180_ADDRESS 0x77 42 | #define BMP180_REG_ID 0xd0 43 | #define BMP180_ID 0x55 44 | 45 | // Register map 46 | 47 | #define BMP180_REG_AC1 0xaa 48 | #define BMP180_REG_SCO 0xf4 49 | #define BMP180_REG_RESULT 0xf6 50 | #define BMP180_REG_XLSB 0xf8 51 | 52 | //---------------------------------------------------------- 53 | // 54 | // LPS25H 55 | 56 | // LPS25H I2C Slave Addresse 57 | 58 | #define LPS25H_ADDRESS0 0x5c 59 | #define LPS25H_ADDRESS1 0x5d 60 | #define LPS25H_REG_ID 0x0f 61 | #define LPS25H_ID 0xbd 62 | 63 | // Register map 64 | 65 | #define LPS25H_REF_P_XL 0x08 66 | #define LPS25H_REF_P_XH 0x09 67 | #define LPS25H_RES_CONF 0x10 68 | #define LPS25H_CTRL_REG_1 0x20 69 | #define LPS25H_CTRL_REG_2 0x21 70 | #define LPS25H_CTRL_REG_3 0x22 71 | #define LPS25H_CTRL_REG_4 0x23 72 | #define LPS25H_INT_CFG 0x24 73 | #define LPS25H_INT_SOURCE 0x25 74 | #define LPS25H_STATUS_REG 0x27 75 | #define LPS25H_PRESS_OUT_XL 0x28 76 | #define LPS25H_PRESS_OUT_L 0x29 77 | #define LPS25H_PRESS_OUT_H 0x2a 78 | #define LPS25H_TEMP_OUT_L 0x2b 79 | #define LPS25H_TEMP_OUT_H 0x2c 80 | #define LPS25H_FIFO_CTRL 0x2e 81 | #define LPS25H_FIFO_STATUS 0x2f 82 | #define LPS25H_THS_P_L 0x30 83 | #define LPS25H_THS_P_H 0x31 84 | #define LPS25H_RPDS_L 0x39 85 | #define LPS25H_RPDS_H 0x3a 86 | 87 | //---------------------------------------------------------- 88 | // 89 | // MS5611 90 | 91 | // MS5611 I2C Slave Addresses 92 | 93 | #define MS5611_ADDRESS0 0x76 94 | #define MS5611_ADDRESS1 0x77 95 | 96 | // commands 97 | 98 | #define MS5611_CMD_RESET 0x1e 99 | #define MS5611_CMD_CONV_D1 0x48 100 | #define MS5611_CMD_CONV_D2 0x58 101 | #define MS5611_CMD_PROM 0xa0 102 | #define MS5611_CMD_ADC 0x00 103 | 104 | #endif // _RTPRESSUREDEFS_H 105 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureLPS25H.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include "RTPressureLPS25H.h" 25 | #include "RTPressureDefs.h" 26 | 27 | #if defined(LPS25H_5c) || defined(LPS25H_5d) 28 | 29 | RTPressureLPS25H::RTPressureLPS25H(RTIMUSettings *settings) : RTPressure(settings) 30 | { 31 | m_pressureValid = false; 32 | m_temperatureValid = false; 33 | } 34 | 35 | RTPressureLPS25H::~RTPressureLPS25H() 36 | { 37 | } 38 | 39 | bool RTPressureLPS25H::pressureInit() 40 | { 41 | m_pressureAddr = m_settings->m_I2CPressureAddress; 42 | 43 | if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_CTRL_REG_1, 0xc4)) 44 | return false; 45 | 46 | if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_RES_CONF, 0x05)) 47 | return false; 48 | 49 | if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_FIFO_CTRL, 0xc0)) 50 | return false; 51 | 52 | if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_CTRL_REG_2, 0x40)) 53 | return false; 54 | 55 | return true; 56 | } 57 | 58 | 59 | bool RTPressureLPS25H::pressureRead(float &latestPressure, float &latestTemperature) 60 | { 61 | unsigned char rawData[3]; 62 | unsigned char status; 63 | 64 | latestPressure = 0; 65 | latestTemperature = 0; 66 | 67 | if (!I2Cdev::readBytes(m_pressureAddr, LPS25H_STATUS_REG, 1, &status)) 68 | return false; 69 | 70 | if (status & 2) { 71 | if (!I2Cdev::readBytes(m_pressureAddr, LPS25H_PRESS_OUT_XL + 0x80, 3, rawData)) 72 | return false; 73 | 74 | m_pressure = (RTFLOAT)((((unsigned long)rawData[2]) << 16) | (((unsigned long)rawData[1]) << 8) | (unsigned long)rawData[0]) / (RTFLOAT)4096; 75 | m_pressureValid = true; 76 | } 77 | if (status & 1) { 78 | if (!I2Cdev::readBytes(m_pressureAddr, LPS25H_TEMP_OUT_L + 0x80, 2, rawData)) 79 | return false; 80 | 81 | m_temperature = (int16_t)((((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]) / (RTFLOAT)480 + (RTFLOAT)42.5; 82 | m_temperatureValid = true; 83 | } 84 | 85 | latestPressure = m_pressure; 86 | latestTemperature = m_temperature; 87 | 88 | return true; 89 | } 90 | #endif -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureLPS25H.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTPRESSURELPS25H_H_ 25 | #define _RTPRESSURELPS25H_H_ 26 | 27 | #include "RTPressure.h" 28 | 29 | class RTIMUSettings; 30 | 31 | class RTPressureLPS25H : public RTPressure 32 | { 33 | public: 34 | RTPressureLPS25H(RTIMUSettings *settings); 35 | ~RTPressureLPS25H(); 36 | 37 | virtual const char *pressureName() { return "LPS25H"; } 38 | virtual int pressureType() { return RTPRESSURE_TYPE_LPS25H; } 39 | virtual bool pressureInit(); 40 | virtual bool pressureRead(float &latestPressure, float &latestTemperature); 41 | 42 | private: 43 | unsigned char m_pressureAddr; // I2C address 44 | 45 | RTFLOAT m_pressure; // the current pressure 46 | RTFLOAT m_temperature; // the current temperature 47 | bool m_pressureValid; 48 | bool m_temperatureValid; 49 | 50 | }; 51 | 52 | #endif // _RTPRESSURELPS25H_H_ 53 | 54 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureMS5611.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include "RTPressureMS5611.h" 25 | 26 | RTPressureMS5611::RTPressureMS5611(RTIMUSettings *settings) : RTPressure(settings) 27 | { 28 | m_validReadings = false; 29 | } 30 | 31 | RTPressureMS5611::~RTPressureMS5611() 32 | { 33 | } 34 | 35 | bool RTPressureMS5611::pressureInit() 36 | { 37 | unsigned char cmd = MS5611_CMD_PROM + 2; 38 | unsigned char data[2]; 39 | 40 | m_pressureAddr = m_settings->m_I2CPressureAddress; 41 | 42 | // get calibration data 43 | 44 | for (int i = 0; i < 6; i++) { 45 | if (!I2Cdev::readBytes(m_pressureAddr, cmd, 2, data)) 46 | return false; 47 | m_calData[i] = (((uint16_t)data[0]) << 8) + (uint16_t)data[1]; 48 | // printf("Cal index: %d, data: %d\n", i, m_calData[i]); 49 | cmd += 2; 50 | } 51 | 52 | m_state = MS5611_STATE_IDLE; 53 | return true; 54 | } 55 | 56 | bool RTPressureMS5611::pressureRead(float &latestPressure, float &latestTemperature) 57 | { 58 | if (m_state == MS5611_STATE_IDLE) { 59 | // start pressure conversion 60 | if (!I2Cdev::writeBytes(m_pressureAddr, MS5611_CMD_CONV_D1, 0, 0)) { 61 | return false; 62 | } else { 63 | m_state = MS5611_STATE_PRESSURE; 64 | m_timer = millis(); 65 | } 66 | } 67 | 68 | pressureBackground(); 69 | 70 | if (m_validReadings) { 71 | latestTemperature = m_temperature; 72 | latestPressure = m_pressure; 73 | } 74 | return true; 75 | } 76 | 77 | 78 | void RTPressureMS5611::pressureBackground() 79 | { 80 | uint8_t data[3]; 81 | 82 | switch (m_state) { 83 | case MS5611_STATE_IDLE: 84 | break; 85 | 86 | case MS5611_STATE_PRESSURE: 87 | if ((millis() - m_timer) < 10) 88 | break; // not time yet 89 | if (!I2Cdev::readBytes(m_pressureAddr, MS5611_CMD_ADC, 3, data)) { 90 | break; 91 | } 92 | m_D1 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2]; 93 | 94 | // start temperature conversion 95 | 96 | if (!I2Cdev::writeBytes(m_pressureAddr, MS5611_CMD_CONV_D2, 0, 0)) { 97 | break; 98 | } else { 99 | m_state = MS5611_STATE_TEMPERATURE; 100 | m_timer = millis(); 101 | } 102 | break; 103 | 104 | case MS5611_STATE_TEMPERATURE: 105 | if ((millis() - m_timer) < 10) 106 | break; // not time yet 107 | if (!I2Cdev::readBytes(m_pressureAddr, MS5611_CMD_ADC, 3, data)) { 108 | break; 109 | } 110 | m_D2 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2]; 111 | 112 | // call this function for testing only 113 | // should give T = 2007 (20.07C) and pressure 100009 (1000.09hPa) 114 | 115 | // setTestData(); 116 | 117 | // now calculate the real values 118 | 119 | int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8); 120 | 121 | int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this needs to be divided by 100 122 | 123 | int64_t offset = ((int64_t)m_calData[1] << 16) + (((int64_t)m_calData[3] * deltaT) >> 7); 124 | int64_t sens = ((int64_t)m_calData[0] << 15) + (((int64_t)m_calData[2] * deltaT) >> 8); 125 | 126 | // do second order temperature compensation 127 | 128 | if (temperature < 2000) { 129 | int64_t T2 = (deltaT * deltaT) >> 31; 130 | int64_t offset2 = 5 * ((temperature - 2000) * (temperature - 2000)) / 2; 131 | int64_t sens2 = offset2 / 2; 132 | if (temperature < -1500) { 133 | offset2 += 7 * (temperature + 1500) * (temperature + 1500); 134 | sens2 += 11 * ((temperature + 1500) * (temperature + 1500)) / 2; 135 | } 136 | temperature -= T2; 137 | offset -= offset2; 138 | sens -= sens2; 139 | } 140 | 141 | m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0; 142 | m_temperature = (RTFLOAT)temperature / (RTFLOAT)100; 143 | 144 | // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure); 145 | 146 | m_validReadings = true; 147 | m_state = MS5611_STATE_IDLE; 148 | break; 149 | } 150 | } 151 | 152 | void RTPressureMS5611::setTestData() 153 | { 154 | m_calData[0] = 40127; 155 | m_calData[1] = 36924; 156 | m_calData[2] = 23317; 157 | m_calData[3] = 23282; 158 | m_calData[4] = 33464; 159 | m_calData[5] = 28312; 160 | 161 | m_D1 = 9085466; 162 | m_D2 = 8569150; 163 | } 164 | -------------------------------------------------------------------------------- /libraries/RTIMULib/RTPressureMS5611.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of RTIMULib-Arduino 4 | // 5 | // Copyright (c) 2014-2015, richards-tech 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _RTPRESSUREMS5611_H_ 25 | #define _RTPRESSUREMS5611_H_ 26 | 27 | #include "RTPressure.h" 28 | 29 | // State definitions 30 | 31 | #define MS5611_STATE_IDLE 0 32 | #define MS5611_STATE_TEMPERATURE 1 33 | #define MS5611_STATE_PRESSURE 2 34 | 35 | class RTIMUSettings; 36 | 37 | class RTPressureMS5611 : public RTPressure 38 | { 39 | public: 40 | RTPressureMS5611(RTIMUSettings *settings); 41 | ~RTPressureMS5611(); 42 | 43 | virtual const char *pressureName() { return "MS5611"; } 44 | virtual int pressureType() { return RTPRESSURE_TYPE_MS5611; } 45 | virtual bool pressureInit(); 46 | virtual bool pressureRead(float &latestPressure, float &latestTemperature); 47 | 48 | private: 49 | void pressureBackground(); 50 | void setTestData(); 51 | 52 | unsigned char m_pressureAddr; // I2C address 53 | RTFLOAT m_pressure; // the current pressure 54 | RTFLOAT m_temperature; // the current temperature 55 | 56 | int m_state; 57 | 58 | uint16_t m_calData[6]; // calibration data 59 | 60 | uint32_t m_D1; 61 | uint32_t m_D2; 62 | 63 | long m_timer; // used to time conversions 64 | 65 | bool m_validReadings; 66 | }; 67 | 68 | #endif // _RTPRESSUREMS5611_H_ 69 | 70 | --------------------------------------------------------------------------------