Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Gyroscope.cpp
- Revision:
- 44:5483079fa156
- Parent:
- 40:8e852115fe55
diff -r 6251c0169f4f -r 5483079fa156 Gyroscope.cpp --- a/Gyroscope.cpp Wed May 27 11:45:00 2015 +0000 +++ b/Gyroscope.cpp Fri May 29 12:21:31 2015 +0000 @@ -4,7 +4,15 @@ #include "Vector3.h" -Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), interruptSet(false), int1(p3), int2(p1), tick(0) +Gyroscope::Gyroscope(I2C &i2c) : + I2CPeripheral(i2c, 0x69 << 1 /* address */), + isCalibrated(false), + interruptSet(false), + numSamples(0), + offset(0.0, 0.0, 0.0), + int1(p3), + int2(p1), + tick(0) { if (powerOn()) { INFO("Bosch Sensortec BMX055-Gyro found"); @@ -49,13 +57,12 @@ write_reg(0x15, 0); // turn off new data interrupt } -Vector3 Gyroscope::read() +Vector3 Gyroscope::uncalibrated_read() { // This chip causes high spikes in the data if FIFO or auto-incremented buffer is read. // To work around this, we will read each register ourselves. Also keeping interrupt disabled // until we finish reading all bytes. //const float gyro_resolution = 0.0610351563; // deg/sec for 2000 deg/sec range - const float gyro_resolution = 0.0152587891; // deg/sec for 500 deg/sec range //write_reg(0x15, 0); // new data interrupt disabled @@ -71,9 +78,42 @@ const int16_t y = buffer[3] << 8 | buffer[2]; const int16_t z = buffer[5] << 8 | buffer[4]; - //write_reg(0x15, 1 << 7); // new data interrupt enabled - + //write_reg(0x15, 1 << 7); // new data interrupt enabled //static Vector3 const offsets(-0.16, -0.17, 0.015); - return (Vector3(x, y, z) * gyro_resolution);// - offsets; + return Vector3(x, y, z); +} + +Vector3 Gyroscope::read() +{ + const float gyro_resolution = 0.0152587891; + return (uncalibrated_read() - offset) * gyro_resolution; } + +//http://math.stackexchange.com/questions/106700/incremental-averageing +void Gyroscope::calibtrationInterrupt(void){ + const static size_t maxNumCalibrationSamples = 200; + + Vector3 rawGyroReading = uncalibrated_read(); + + //newMean = oldMean + (currentReading - oldMean) / newNumSamples; + offset = offset + (rawGyroReading - offset) / (++numSamples); + + //Turn off calibration once we read desired number of samples + if(numSamples == maxNumCalibrationSamples){ + numSamples = 0; + isCalibrated = true; + int1.rise(this, &Gyroscope::handleInterrupt); + } +} + +void Gyroscope::calibrate(){ + int1.rise(this, &Gyroscope::calibtrationInterrupt); +} + +void Gyroscope::reset(){ + offset.x = 0.0; + offset.y = 0.0; + offset.z = 0.0; + +} \ No newline at end of file