#include "Arduino.h" #include <ADXL345.h> #include <math.h> #include <ITG3200.h> #include "Cfilter.h" ITG3200 gyro = ITG3200(); ADXL345 Accel; int gx = 0, gy = 0, gz = 0; //Compute Variables float theta = 0, psi = 0, phi = 0, normAngle = 0, normACC = 0, alpha = 0; //Compute Variables void Cfilterbegin(){ //Turning on the accelerometer Accel.init(ADXL345_ADDR_ALT_LOW); Accel.set_bw(ADXL345_BW_12); gyro.reset(); // Use ITG3200_ADDR_AD0_HIGH or ITG3200_ADDR_AD0_LOW as the ITG3200 address // depending on how AD0 is connected on your breakout board, check its schematics for details gyro.init(ITG3200_ADDR_AD0_LOW); Serial.print("zeroCalibrating..."); gyro.zeroCalibrate(2500,2); Serial.println("done."); alpha = float(TIME_CONSTANT) / (float(TIME_CONSTANT) + float(SAMPLE_RATE)); // calculate the scaling coefficent
FreeIMU::FreeIMU() { #if HAS_ADXL345() acc = ADXL345(); #elif HAS_BMA180() acc = BMA180(); #endif #if HAS_HMC5883L() magn = HMC58X3(); #endif #if HAS_ITG3200() gyro = ITG3200(); #elif HAS_MPU6050() accgyro = MPU60X0(); // I2C #elif HAS_MPU6000() accgyro = MPU60X0(); // SPI for Arduimu v3 #endif #if HAS_MS5611() baro = MS561101BA(); #endif // initialize quaternion q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; exInt = 0.0; eyInt = 0.0; ezInt = 0.0; twoKp = twoKpDef; twoKi = twoKiDef; integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; lastUpdate = 0; now = 0; #ifndef CALIBRATION_H // initialize scale factors to neutral values acc_scale_x = 1; acc_scale_y = 1; acc_scale_z = 1; magn_scale_x = 1; magn_scale_y = 1; magn_scale_z = 1; #else // get values from global variables of same name defined in calibration.h acc_off_x = ::acc_off_x; acc_off_y = ::acc_off_y; acc_off_z = ::acc_off_z; acc_scale_x = ::acc_scale_x; acc_scale_y = ::acc_scale_y; acc_scale_z = ::acc_scale_z; magn_off_x = ::magn_off_x; magn_off_y = ::magn_off_y; magn_off_z = ::magn_off_z; magn_scale_x = ::magn_scale_x; magn_scale_y = ::magn_scale_y; magn_scale_z = ::magn_scale_z; #endif }
* it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Loguino is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * You should have received a copy of the GNU General Public License * along with Loguino. If not, see "http://www.gnu.org/licenses/". * */ #ifdef ENABLE_ITG3200_POLLER ITG3200 itg = ITG3200(); void ITG3200_init(){ #ifdef DEBUG_ITG3200_POLLER DEBUG_1("Starting"); #endif Wire.begin(); delay(1000); itg.init(ITG3200_ADDR_AD0_HIGH); #ifdef DEBUG_ITG3200_POLLER DEBUG_1("Finished"); #endif } void ITG3200_sample(){