/* init filter --------------------------------------------------------------*/
void initFilterACC(vs32 * gyroAngle, vs32 * copterAngle)
{
// setup compass only if HW_Setup is 4
if (getParameter(PARA_HW) & PARA_HW_COMP)
{
initCompass();
}
zeroGyro();
setAngleFilterACC(gyroAngle, copterAngle);
}
/* do Command Command -------------------------------------------------------*/
void docComm()
{
switch (line[1])
{
case 'g':
zeroGyro();
setGyroAngleFilterACC(gyroAngle);
break;
case 'a':
zeroACC();
break;
}
}
void FreeIMU1::init(int acc_addr, int gyro_addr, bool fastmode) {
#else
void FreeIMU1::init(int accgyro_addr, bool fastmode) {
#endif
delay(5);
if(fastmode) {
#if defined(__86DUINO_ZERO) || defined(__86DUINO_ONE) || defined(__86DUINO_EDUCAKE)
i2c_SetSpeed(0, I2CMODE_AUTO, 400000L);
#endif
}
#if HAS_LSM330DLC()
accgyro.init(acc_addr, gyro_addr);
#endif
// zero gyro
zeroGyro();
#ifndef CALIBRATION_H
// load calibration from eeprom
calLoad();
#endif
}
void FreeIMU::init(int acc_addr, int gyro_addr, bool fastmode) {
#else
void FreeIMU::init(int accgyro_addr, bool fastmode) {
#endif
delay(5);
// disable internal pullups of the ATMEGA which Wire enable by default
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
// deactivate internal pull-ups for twi
// as per note from atmega8 manual pg167
cbi(PORTC, 4);
cbi(PORTC, 5);
#elif defined(__AVR__)
// deactivate internal pull-ups for twi
// as per note from atmega128 manual pg204
cbi(PORTD, 0);
cbi(PORTD, 1);
#endif
#if defined(__AVR__) // only valid on AVR, not on 32bit platforms (eg: Arduino 2, Teensy 3.0)
if(fastmode) { // switch to 400KHz I2C - eheheh
TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
}
#elif defined(__arm__)
if(fastmode) {
#if defined(CORE_TEENSY) && F_BUS == 48000000
I2C0_F = 0x1A; // Teensy 3.0 at 48 or 96 MHz
I2C0_FLT = 2;
#elif defined(CORE_TEENSY) && F_BUS == 24000000
I2C0_F = 0x45; // Teensy 3.0 at 24 MHz
I2C0_FLT = 1;
#endif
}
#endif
#if HAS_ADXL345()
// init ADXL345
acc.init(acc_addr);
#elif HAS_BMA180()
// init BMA180
acc.setAddress(acc_addr);
acc.SoftReset();
acc.enableWrite();
acc.SetFilter(acc.F10HZ);
acc.setGSensitivty(acc.G15);
acc.SetSMPSkip();
acc.SetISRMode();
acc.disableWrite();
#endif
#if HAS_ITG3200()
// init ITG3200
gyro.init(gyro_addr);
delay(1000);
// calibrate the ITG3200
gyro.zeroCalibrate(128,5);
#endif
#if HAS_MPU6050()
accgyro = MPU60X0(false, accgyro_addr);
accgyro.initialize();
accgyro.setI2CMasterModeEnabled(0);
accgyro.setI2CBypassEnabled(1);
accgyro.setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);
delay(5);
#endif
#if HAS_MPU6000()
accgyro = MPU60X0(true, accgyro_addr);
accgyro.initialize();
accgyro.setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);
delay(5);
#endif
#if HAS_HMC5883L()
// init HMC5843
magn.init(false); // Don't set mode yet, we'll do that later on.
// Calibrate HMC using self test, not recommended to change the gain after calibration.
magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended.
// Single mode conversion was used in calibration, now set continuous mode
magn.setMode(0);
delay(10);
magn.setDOR(B110);
#endif
#if HAS_MS5611()
baro.init(FIMU_BARO_ADDR);
#endif
// zero gyro
zeroGyro();
#ifndef CALIBRATION_H
// load calibration from eeprom
calLoad();
#endif
}
/* init filter --------------------------------------------------------------*/
void initFilterHH(vs32 * gyroAngle, vs32 * copterAngle)
{
zeroGyro();
setAngleFilterHH(gyroAngle, copterAngle);
}
