├── 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 | ## Please note that this project is no longer active and is of historical interest only. 4 | 5 | 6 | -------------------------------------------------------------------------------- /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.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 "RTArduLink.h" 32 | #include "RTArduLinkHAL.h" 33 | #include "RTArduLinkUtils.h" 34 | 35 | #include 36 | 37 | RTArduLink::RTArduLink() 38 | { 39 | } 40 | 41 | RTArduLink::~RTArduLink() 42 | { 43 | } 44 | 45 | void RTArduLink::begin(const char *identitySuffix) 46 | { 47 | RTARDULINK_PORT *portInfo; 48 | 49 | if (!RTArduLinkHALEEPROMValid()) 50 | RTArduLinkHALEEPROMDefault(); 51 | 52 | m_identitySuffix = identitySuffix; 53 | 54 | // now set up host and subsystem ports based on EEPROM configuration 55 | 56 | for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++) { 57 | portInfo = m_ports + i; 58 | portInfo->index = i; 59 | portInfo->inUse = RTArduLinkHALConfigurePort(&(portInfo->portHAL), i); 60 | RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer)); 61 | } 62 | m_hostPort = m_ports; 63 | } 64 | 65 | 66 | void RTArduLink::background() 67 | { 68 | unsigned char index; 69 | RTARDULINK_PORT *portInfo; 70 | 71 | for (index = 0; index < RTARDULINKHAL_MAX_PORTS; index++) { 72 | portInfo = m_ports + index; 73 | if (!portInfo->inUse) 74 | continue; 75 | 76 | while (RTArduLinkHALPortAvailable(&(portInfo->portHAL))) { 77 | if (!RTArduLinkReassemble(&(portInfo->RXFrame), RTArduLinkHALPortRead(&(portInfo->portHAL)))) { 78 | sendDebugMessage("Reassembly error"); 79 | } else { 80 | if (portInfo->RXFrame.complete) { 81 | processReceivedMessage(portInfo); 82 | RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer)); 83 | } 84 | } 85 | } 86 | } 87 | } 88 | 89 | void RTArduLink::processReceivedMessage(RTARDULINK_PORT *portInfo) 90 | { 91 | RTARDULINK_MESSAGE *message; // a pointer to the message part of the frame 92 | unsigned int address; 93 | 94 | message = &(portInfo->RXFrameBuffer.message); // get the message pointer 95 | address = RTArduLinkConvertUC2ToUInt(message->messageAddress); 96 | 97 | switch (portInfo->index) { 98 | case RTARDULINK_HOST_PORT: 99 | processHostMessage(); // came from this upstream link 100 | return; 101 | 102 | case RTARDULINK_DAISY_PORT: // came from dasiy chain port 103 | if (address != RTARDULINK_HOST_PORT) // true if it came from a daisy chained subsystem, not a directly connected subsystem 104 | RTArduLinkConvertIntToUC2(address + RTARDULINKHAL_MAX_PORTS, message->messageAddress); 105 | else 106 | RTArduLinkConvertIntToUC2(RTARDULINK_DAISY_PORT, message->messageAddress); 107 | break; 108 | 109 | default: 110 | RTArduLinkConvertIntToUC2(address + portInfo->index, message->messageAddress); 111 | break; 112 | } 113 | 114 | // if get here, need to forward to host port 115 | 116 | sendFrame(m_hostPort, &(portInfo->RXFrameBuffer), portInfo->RXFrameBuffer.messageLength); 117 | } 118 | 119 | 120 | void RTArduLink::processHostMessage() 121 | { 122 | RTARDULINK_MESSAGE *message; // a pointer to the message part of the frame 123 | int identityLength; 124 | int suffixLength; 125 | unsigned int address; 126 | 127 | message = &(m_hostPort->RXFrameBuffer.message); // get the message pointer 128 | address = RTArduLinkConvertUC2ToUInt(message->messageAddress); 129 | 130 | if (address == RTARDULINK_BROADCAST_ADDRESS) { // need to forward to downstream ports also 131 | for (int i = RTARDULINK_HOST_PORT + 1; i < RTARDULINKHAL_MAX_PORTS; i++) { 132 | if (m_ports[i].inUse) 133 | sendFrame(m_ports + i, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength); 134 | } 135 | } 136 | if ((address == RTARDULINK_MY_ADDRESS) || (address == RTARDULINK_BROADCAST_ADDRESS)) { // it's for me 137 | switch (message->messageType) 138 | { 139 | case RTARDULINK_MESSAGE_POLL: 140 | case RTARDULINK_MESSAGE_ECHO: 141 | RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress); 142 | sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength); // just send the frame back as received 143 | break; 144 | 145 | case RTARDULINK_MESSAGE_IDENTITY: 146 | identityLength = strlen(RTArduLinkHALConfig.identity); 147 | suffixLength = strlen(m_identitySuffix); 148 | 149 | memcpy(message->data, RTArduLinkHALConfig.identity, identityLength + 1); // copy in identity 150 | 151 | if ((identityLength + suffixLength) < RTARDULINK_DATA_MAX_LEN - 1) { 152 | memcpy(message->data + identityLength, m_identitySuffix, suffixLength + 1); // copy in suffix 153 | } else { 154 | suffixLength = 0; 155 | } 156 | RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress); 157 | message->data[RTARDULINK_DATA_MAX_LEN - 1] = 0; // make sure zero terminated if it was truncated 158 | sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + identityLength + suffixLength + 1); 159 | break; 160 | 161 | default: 162 | if (message->messageType < RTARDULINK_MESSAGE_CUSTOM) { // illegal code 163 | message->data[0] = RTARDULINK_RESPONSE_ILLEGAL_COMMAND; 164 | message->data[1] = message->messageType; // this is the offending code 165 | message->messageType = RTARDULINK_MESSAGE_ERROR; 166 | RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress); 167 | sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + 2); 168 | break; 169 | } 170 | processCustomMessage(message->messageType, message->messageParam, message->data, 171 | m_hostPort->RXFrameBuffer.messageLength - RTARDULINK_MESSAGE_HEADER_LEN); // see if anyone wants to process it 172 | break; 173 | } 174 | return; 175 | } 176 | 177 | if (address >= RTARDULINKHAL_MAX_PORTS) { // need to pass it to the first subsystem 178 | if (!m_ports[RTARDULINK_DAISY_PORT].inUse) 179 | return; // there is no daisy chain port 180 | RTArduLinkConvertIntToUC2(address - RTARDULINKHAL_MAX_PORTS, message->messageAddress); // adjust the address 181 | sendFrame(m_ports +RTARDULINK_DAISY_PORT, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength); 182 | return; 183 | } 184 | 185 | // if get here, needs to go to a local subsystem port 186 | 187 | if (m_ports[address].inUse) { 188 | RTArduLinkConvertIntToUC2(0, message->messageAddress); // indicates that the target should process it 189 | sendFrame(m_ports + address, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength); 190 | } 191 | } 192 | 193 | void RTArduLink::sendDebugMessage(const char *debugMessage) 194 | { 195 | RTARDULINK_FRAME frame; 196 | int stringLength; 197 | 198 | stringLength = strlen(debugMessage); 199 | if (stringLength >= RTARDULINK_DATA_MAX_LEN) 200 | stringLength = RTARDULINK_DATA_MAX_LEN-1; 201 | memcpy(frame.message.data, debugMessage, stringLength); 202 | frame.message.data[stringLength] = 0; 203 | frame.message.messageType = RTARDULINK_MESSAGE_DEBUG; 204 | RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress); 205 | sendFrame(m_hostPort, &frame, RTARDULINK_MESSAGE_HEADER_LEN + stringLength + 1); 206 | } 207 | 208 | void RTArduLink::sendMessage(unsigned char messageType, unsigned char messageParam, unsigned char *data, int length) 209 | { 210 | RTARDULINK_FRAME frame; 211 | 212 | RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress); 213 | frame.message.messageType = messageType; 214 | frame.message.messageParam = messageParam; 215 | 216 | if (length > RTARDULINK_DATA_MAX_LEN) 217 | length = RTARDULINK_DATA_MAX_LEN; 218 | memcpy(frame.message.data, data, length); 219 | 220 | sendFrame(m_hostPort, &frame, length + RTARDULINK_MESSAGE_HEADER_LEN); 221 | } 222 | 223 | void RTArduLink::sendFrame(RTARDULINK_PORT *portInfo, RTARDULINK_FRAME *frame, int length) 224 | { 225 | frame->sync0 = RTARDULINK_MESSAGE_SYNC0; 226 | frame->sync1 = RTARDULINK_MESSAGE_SYNC1; 227 | frame->messageLength = length; // set length 228 | RTArduLinkSetChecksum(frame); // compute checksum 229 | RTArduLinkHALPortWrite(&(portInfo->portHAL), (unsigned char *)frame, frame->messageLength + RTARDULINK_FRAME_HEADER_LEN); 230 | } 231 | 232 | -------------------------------------------------------------------------------- /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 | --------------------------------------------------------------------------------