TiltyIMU::TiltyIMU() { // Instantiate sensors imu = MPU6050(); magn = HMC5883(); alt = MPL3115A2(); }
void GetHMC5883LRawMeasure(uint8_t* buffer) { MPU6050(); HMC5883L(); HMC5883L_SetScale(13); HMC5883L_SetMeasurementMode(Measurement_Continuous); IICReadNBytes(HMC5883L_Address,DataRegisterBegin,6,buffer); }
#include "application.h" #include "MPU6050.h" #include "Adafruit_NeoPixel.h" #include <math.h> #include "UDPLogger.h" #define PIXEL_COUNT 16 #define LED_MODULO 4 Adafruit_NeoPixel strip1 = Adafruit_NeoPixel(PIXEL_COUNT, D2, WS2812B); Adafruit_NeoPixel strip2 = Adafruit_NeoPixel(PIXEL_COUNT, D3, WS2812B); MPU6050 mpu = MPU6050(MPU6050::Address0); UDPLogger logger; static bool ballTurnedOn = false; uint32_t Wheel(byte WheelPos); void rainbow(uint8_t modulo, uint8_t wait); void ball(void); void blinkLED(unsigned int count, unsigned int mydelay) { int led = D7; pinMode(led, OUTPUT); while (count > 0) { digitalWrite(led, HIGH); delay(mydelay); // Wait for 1000mS = 1 second digitalWrite(led, LOW); delay(mydelay); // Wait for 1 second in off mode
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 = MPU6050(); // I2C #elif HAS_MPU6000() accgyro = MPU6050(); // 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 }
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 #ifndef I2C_T3_H #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 && !defined(I2C_T3_H) 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 #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 = MPU6050(accgyro_addr); accgyro.initialize(); accgyro.setI2CMasterModeEnabled(0); accgyro.setI2CBypassEnabled(1); accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); delay(5); #endif #if HAS_MPU6000() accgyro = MPU6050(true, accgyro_addr); accgyro.initialize(); accgyro.setFullScaleGyroRange(MPU6050_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 }