#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(){
