20160814
Fork of LSM9DS0 by
Diff: LSM9DS0.cpp
- Revision:
- 5:56ff956c499e
- Parent:
- 4:90f024c6b406
--- a/LSM9DS0.cpp Sat Jun 18 09:48:50 2016 +0000 +++ b/LSM9DS0.cpp Thu Jul 28 08:16:54 2016 +0000 @@ -45,14 +45,14 @@ { // interfaceMode will keep track of whether we're using SPI or I2C: interfaceMode = interface; - + // xmAddress and gAddress will store the 7-bit I2C address, if using I2C. // If we're using SPI, these variables store the chip-select pins. gAddress = gAddr; xmAddress = xmAddr; } -uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, +uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, gyro_odr gODR, accel_odr aODR, mag_odr mODR) { // Store the given scales in class variables. These scale variables @@ -60,45 +60,47 @@ gScale = gScl; aScale = aScl; mScale = mScl; - + // Once we have the scale values, we can calculate the resolution // of each sensor. That's what these functions are for. One for each sensor calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable calcaRes(); // Calculate g / ADC tick, stored in aRes variable - + // Now, initialize our hardware interface. if (interfaceMode == I2C_MODE) // If we're using I2C initI2C(); // Initialize I2C else if (interfaceMode == SPI_MODE) // else, if we're using SPI initSPI(); // Initialize SPI - + // To verify communication, we can read from the WHO_AM_I register of // each device. Store those in a variable so we can return them. uint8_t gTest = gReadByte(WHO_AM_I_G); // Read the gyro WHO_AM_I uint8_t xmTest = xmReadByte(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I - + // Gyro initialization stuff: initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc. setGyroODR(gODR); // Set the gyro output data rate and bandwidth. setGyroScale(gScale); // Set the gyro range - + // Accelerometer initialization stuff: initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc. setAccelODR(aODR); // Set the accel data rate. setAccelScale(aScale); // Set the accel range. - + // Magnetometer initialization stuff: initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc. setMagODR(mODR); // Set the magnetometer output data rate. setMagScale(mScale); // Set the magnetometer's range. - + setGyroOffset(0,0,0); setAccelOffset(0,0,0); setMagOffset(0,0,0); - + // Once everything is initialized, return the WHO_AM_I registers we read: return (xmTest << 8) | gTest; + + init = 0; } void LSM9DS0::initGyro() @@ -112,7 +114,7 @@ PD - Power down enable (0=power down mode, 1=normal or sleep mode) Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */ gWriteByte(CTRL_REG1_G, 0xFF); // Normal mode, enable all axes - + /* CTRL_REG2_G sets up the HPF Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0 HPM[1:0] - High pass filter mode selection @@ -122,7 +124,7 @@ Value depends on data rate. See datasheet table 26. */ gWriteByte(CTRL_REG2_G, 0x09); // Normal mode, high cutoff frequency - + /* CTRL_REG3_G sets up interrupt and DRDY_G pins Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable) @@ -134,8 +136,8 @@ I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable) I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */ // Int1 enabled (pp, active low), data read on DRDY_G: - gWriteByte(CTRL_REG3_G, 0x00); - + gWriteByte(CTRL_REG3_G, 0x00); + /* CTRL_REG4_G sets the scale, update mode Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM BDU - Block data update (0=continuous, 1=output not updated until read @@ -147,7 +149,7 @@ SIM - SPI serial interface mode select 0=4 wire, 1=3 wire */ gWriteByte(CTRL_REG4_G, 0x30); // Set scale to 245 dps - + /* CTRL_REG5_G sets up the FIFO, HPF, and INT1 Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0 BOOT - Reboot memory content (0=normal, 1=reboot) @@ -156,7 +158,7 @@ INT1_Sel[1:0] - Int 1 selection configuration Out_Sel[1:0] - Out selection configuration */ gWriteByte(CTRL_REG5_G, 0x00); - + // Temporary !!! For testing !!! Remove !!! Or make useful !!! configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS... } @@ -172,20 +174,20 @@ HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled) HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled) */ xmWriteByte(CTRL_REG0_XM, 0x00); - + /* CTRL_REG1_XM (0x20) (Default value: 0x07) Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN AODR[3:0] - select the acceleration data rate: - 0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, + 0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, 0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz, 1001=800Hz, 1010=1600Hz, (remaining combinations undefined). BDU - block data update for accel AND mag 0: Continuous update 1: Output registers aren't updated until MSB and LSB have been read. AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled. - 0: Axis disabled, 1: Axis enabled */ + 0: Axis disabled, 1: Axis enabled */ xmWriteByte(CTRL_REG1_XM, 0x97); // 100Hz data rate, x/y/z all enabled - + //Serial.println(xmReadByte(CTRL_REG1_XM)); /* CTRL_REG2_XM (0x21) (Default value: 0x00) Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM @@ -198,16 +200,16 @@ SIM - SPI mode selection 0=4-wire, 1=3-wire */ xmWriteByte(CTRL_REG2_XM, 0xD8); // Set scale to 2g - + /* CTRL_REG3_XM is used to set interrupt generators on INT1_XM Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY */ // Accelerometer data ready on INT1_XM (0x04) - xmWriteByte(CTRL_REG3_XM, 0x00); + xmWriteByte(CTRL_REG3_XM, 0x00); } void LSM9DS0::initMag() -{ +{ /* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1 TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled) @@ -219,17 +221,17 @@ LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC) 0=irq not latched, 1=irq latched */ xmWriteByte(CTRL_REG5_XM, 0x74); // Mag data rate - 100 Hz, disable temperature sensor - + /* CTRL_REG6_XM sets the magnetometer full-scale Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0 MFS[1:0] - Magnetic full-scale selection 00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs */ xmWriteByte(CTRL_REG6_XM, 0x40); // Mag scale to +/- 2GS - + /* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0 AHPM[1:0] - HPF mode selection - 00=normal (resets reference registers), 01=reference signal for filtering, + 00=normal (resets reference registers), 01=reference signal for filtering, 10=normal, 11=autoreset on interrupt event AFDS - Filtered acceleration data selection 0=internal filter bypassed, 1=data from internal filter sent to FIFO @@ -239,12 +241,12 @@ MD[1:0] - Magnetic sensor mode selection (default 10) 00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */ xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode - + /* CTRL_REG4_XM is used to set interrupt generators on INT2_XM Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM */ xmWriteByte(CTRL_REG4_XM, 0x00); // Magnetometer data ready on INT2_XM (0x08) - + /* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data @@ -267,69 +269,69 @@ // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner // is good practice. void LSM9DS0::calLSM9DS0(float * gbias, float * abias) -{ - uint8_t data[6] = {0, 0, 0, 0, 0, 0}; - int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - int samples, ii; - - // First get gyro bias - uint8_t c = gReadByte(CTRL_REG5_G); - gWriteByte(CTRL_REG5_G, c | 0x40); // Enable gyro FIFO - wait_ms(20); // Wait for change to take effect - gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples - wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples - - samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples +{ + uint8_t data[6] = {0, 0, 0, 0, 0, 0}; + int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + int samples, ii; + + // First get gyro bias + uint8_t c = gReadByte(CTRL_REG5_G); + gWriteByte(CTRL_REG5_G, c | 0x40); // Enable gyro FIFO + wait_ms(20); // Wait for change to take effect + gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples + wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples + + samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples - for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO - gReadBytes(OUT_X_L_G, &data[0], 6); - gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]); - gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]); - gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]); - } + for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO + gReadBytes(OUT_X_L_G, &data[0], 6); + gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]); + gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]); + gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]); + } + + gyro_bias[0] /= samples; // average the data + gyro_bias[1] /= samples; + gyro_bias[2] /= samples; + + gbias[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s + gbias[1] = (float)gyro_bias[1]*gRes; + gbias[2] = (float)gyro_bias[2]*gRes; - gyro_bias[0] /= samples; // average the data - gyro_bias[1] /= samples; - gyro_bias[2] /= samples; - - gbias[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s - gbias[1] = (float)gyro_bias[1]*gRes; - gbias[2] = (float)gyro_bias[2]*gRes; - - c = gReadByte(CTRL_REG5_G); - gWriteByte(CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO - wait_ms(20); - gWriteByte(FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode - + c = gReadByte(CTRL_REG5_G); + gWriteByte(CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO + wait_ms(20); + gWriteByte(FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode + - // Now get the accelerometer biases - c = xmReadByte(CTRL_REG0_XM); - xmWriteByte(CTRL_REG0_XM, c | 0x40); // Enable accelerometer FIFO - wait_ms(20); // Wait for change to take effect - xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples - wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples + // Now get the accelerometer biases + c = xmReadByte(CTRL_REG0_XM); + xmWriteByte(CTRL_REG0_XM, c | 0x40); // Enable accelerometer FIFO + wait_ms(20); // Wait for change to take effect + xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples + wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples + + samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples - samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples - - for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO - xmReadBytes(OUT_X_L_A, &data[0], 6); - accel_bias[0] += (((int16_t)data[1] << 8) | data[0]); - accel_bias[1] += (((int16_t)data[3] << 8) | data[2]); - accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/aRes); // Assumes sensor facing up! - } + for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO + xmReadBytes(OUT_X_L_A, &data[0], 6); + accel_bias[0] += (((int16_t)data[1] << 8) | data[0]); + accel_bias[1] += (((int16_t)data[3] << 8) | data[2]); + accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/aRes); // Assumes sensor facing up! + } - accel_bias[0] /= samples; // average the data - accel_bias[1] /= samples; - accel_bias[2] /= samples; - - abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs - abias[1] = (float)accel_bias[1]*aRes; - abias[2] = (float)accel_bias[2]*aRes; + accel_bias[0] /= samples; // average the data + accel_bias[1] /= samples; + accel_bias[2] /= samples; - c = xmReadByte(CTRL_REG0_XM); - xmWriteByte(CTRL_REG0_XM, c & ~0x40); // Disable accelerometer FIFO - wait_ms(20); - xmWriteByte(FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode + abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs + abias[1] = (float)accel_bias[1]*aRes; + abias[2] = (float)accel_bias[2]*aRes; + + c = xmReadByte(CTRL_REG0_XM); + xmWriteByte(CTRL_REG0_XM, c & ~0x40); // Disable accelerometer FIFO + wait_ms(20); + xmWriteByte(FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode } //********************** @@ -398,7 +400,7 @@ //********************** void LSM9DS0::readAccel() { - uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp + uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp xmReadBytes(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay @@ -459,7 +461,7 @@ //********************** void LSM9DS0::readMag() { - uint8_t temp[6]; // We'll read six bytes from the mag into temp + uint8_t temp[6]; // We'll read six bytes from the mag into temp xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx my = (temp[3] << 8) | temp[2]; // Store y-axis values into my @@ -520,7 +522,7 @@ //********************** void LSM9DS0::readTemp() { - uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp + uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer } @@ -528,7 +530,7 @@ float LSM9DS0::calcGyro(int16_t gyro) { // Return the gyro raw reading times our pre-calculated DPS / (ADC tick): - return gRes * gyro; + return gRes * gyro; } float LSM9DS0::calcAccel(int16_t accel) @@ -553,7 +555,7 @@ temp |= gScl << 4; // And write the new register value back into CTRL_REG4_G: gWriteByte(CTRL_REG4_G, temp); - + // We've updated the sensor, but we also need to update our class variables // First update gScale: gScale = gScl; @@ -571,7 +573,7 @@ temp |= aScl << 3; // And write the new register value back into CTRL_REG2_XM: xmWriteByte(CTRL_REG2_XM, temp); - + // We've updated the sensor, but we also need to update our class variables // First update aScale: aScale = aScl; @@ -589,7 +591,7 @@ temp |= mScl << 5; // And write the new register value back into CTRL_REG6_XM: xmWriteByte(CTRL_REG6_XM, temp); - + // We've updated the sensor, but we also need to update our class variables // First update mScale: mScale = mScl; @@ -662,26 +664,25 @@ // Possible gyro scales (and their register bit settings) are: // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm // to calculate DPS/(ADC tick) based on that 2-bit value: - switch (gScale) - { - case G_SCALE_245DPS: - gRes = 245.0 / 32768.0; - break; - case G_SCALE_500DPS: - gRes = 500.0 / 32768.0; - break; - case G_SCALE_2000DPS: - gRes = 2000.0 / 32768.0; - break; + switch (gScale) { + case G_SCALE_245DPS: + gRes = 245.0 / 32768.0; + break; + case G_SCALE_500DPS: + gRes = 500.0 / 32768.0; + break; + case G_SCALE_2000DPS: + gRes = 2000.0 / 32768.0; + break; } } void LSM9DS0::calcaRes() { // Possible accelerometer scales (and their register bit settings) are: - // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an + // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an // algorithm to calculate g/(ADC tick) based on that 3-bit value: - aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : + aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : (((float) aScale + 1.0f) * 2.0f) / 32768.0f; } @@ -690,10 +691,10 @@ // Possible magnetometer scales (and their register bit settings) are: // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm // to calculate Gs/(ADC tick) based on that 2-bit value: - mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : + mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : (float) (mScale << 2) / 32768.0f; } - + void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data) { // Whether we're using I2C or SPI, write a byte using the @@ -762,7 +763,7 @@ { csG_ = 1; csXM_= 1; - + // Maximum SPI frequency is 10MHz: // spi_.frequency(1000000); spi_.format(8,0b11); @@ -775,12 +776,12 @@ csG_ = 0; else if(csPin == xmAddress) csXM_= 0; - + // If write, bit 0 (MSB) should be 0 // If single write, bit 1 should be 0 spi_.write(subAddress & 0x3F); // Send Address spi_.write(data); // Send data - + csG_ = 1; // Close communication csXM_= 1; } @@ -788,14 +789,14 @@ uint8_t LSM9DS0::SPIreadByte(uint8_t csPin, uint8_t subAddress) { uint8_t temp; - // Use the multiple read function to read 1 byte. + // Use the multiple read function to read 1 byte. // Value is returned to `temp`. SPIreadBytes(csPin, subAddress, &temp, 1); return temp; } void LSM9DS0::SPIreadBytes(uint8_t csPin, uint8_t subAddress, - uint8_t * dest, uint8_t count) + uint8_t * dest, uint8_t count) { // Initiate communication if(csPin == gAddress) @@ -809,8 +810,7 @@ spi_.write(0xC0 | (subAddress & 0x3F)); else spi_.write(0x80 | (subAddress & 0x3F)); - for (int i=0; i<count; i++) - { + for (int i=0; i<count; i++) { dest[i] = spi_.write(0x00); // Read into destination array } csG_ = 1; // Close communication @@ -836,11 +836,11 @@ uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress) { return 0; -// uint8_t data; // `data` will store the register data +// uint8_t data; // `data` will store the register data // Wire.beginTransmission(address); // Initialize the Tx buffer // Wire.write(subAddress); // Put slave register address in Tx buffer // Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive -// Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address +// Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address // data = Wire.read(); // Fill Rx buffer with result // return data; // Return data read from slave register } @@ -853,8 +853,8 @@ // Wire.write(subAddress | 0x80); // Put slave register address in Tx buffer // Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive // uint8_t i = 0; -// Wire.requestFrom(address, count); // Read bytes from slave register address -// while (Wire.available()) +// Wire.requestFrom(address, count); // Read bytes from slave register address +// while (Wire.available()) // { // dest[i++] = Wire.read(); // Put read results in the Rx buffer // } @@ -862,19 +862,23 @@ void LSM9DS0::complementaryFilter(float * data, float dt) { - - float pitchAcc, rollAcc; - - /* Integrate the gyro data(deg/s) over time to get angle */ - pitch += data[5] * dt; // Angle around the Z-axis + if(init == 0) { + pitch = (float)atan2f(-data[0], -data[1])*180.0f/PI; + init = 1; + } else { + float pitchAcc, rollAcc; + + /* Integrate the gyro data(deg/s) over time to get angle */ + pitch += data[5] * dt; // Angle around the Z-axis // roll += data[3] * dt; // Angle around the X-axis - - /* Turning around the X-axis results in a vector on the Y-axis - whereas turning around the Y-axis results in a vector on the X-axis. */ - pitchAcc = (float)atan2f(-data[0], -data[1])*180.0f/PI; + + /* Turning around the X-axis results in a vector on the Y-axis + whereas turning around the Y-axis results in a vector on the X-axis. */ + pitchAcc = (float)atan2f(-data[0], -data[1])*180.0f/PI; // rollAcc = (float)atan2f(data[2], -data[1])*180.0f/PI; - - /* Apply Complementary Filter */ - pitch = pitch * 0.999 + pitchAcc * 0.001; + + /* Apply Complementary Filter */ + pitch = pitch * 0.9999 + pitchAcc * 0.0001; // roll = roll * 0.999 + rollAcc * 0.001; + } } \ No newline at end of file