Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Gyroscope.cpp
- Committer:
- pvaibhav
- Date:
- 2015-05-26
- Revision:
- 40:8e852115fe55
- Parent:
- 39:1fa9c0e1ffde
- Child:
- 44:5483079fa156
- Child:
- 46:fd5a62296b12
File content as of revision 40:8e852115fe55:
#include "Gyroscope.h" #define DEBUG "BMX055-Gyr" #include "Logger.h" #include "Vector3.h" Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), interruptSet(false), int1(p3), int2(p1), tick(0) { if (powerOn()) { INFO("Bosch Sensortec BMX055-Gyro found"); powerOff(); } else { WARN("Bosch Sensortec BMX055-Gyro not found"); } } void Gyroscope::powerOff() { write_reg(0x11, 1); LOG("deep sleep"); } void Gyroscope::handleInterrupt(void) { delegate->sensorUpdate(read()); } bool Gyroscope::powerOn() { write_reg(0x14, 0xB6); // softreset wait_ms(30); LOG("powered on"); return read_reg(0x00) == 0x0f; // verify Chip ID } void Gyroscope::start() { write_reg(0x10, 5); // set capture rate: 100 Hz / filter: 12 Hz write_reg(0x0F, 2); // set range +/- 500 deg/sec write_reg(0x16, 5); // interrupts active high, push-pull write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro) int1.rise(this, &Gyroscope::handleInterrupt); tick = 1; write_reg(0x15, 1 << 7); // new data interrupt enabled } void Gyroscope::stop() { write_reg(0x15, 0); // turn off new data interrupt } Vector3 Gyroscope::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 uint8_t buffer[6]; /* for (size_t i = 0; i < 6; i++) buffer[i] = read_reg(0x02 + i); */ read_reg(0x02, buffer, sizeof buffer); const int16_t x = buffer[1] << 8 | buffer[0]; 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 //static Vector3 const offsets(-0.16, -0.17, 0.015); return (Vector3(x, y, z) * gyro_resolution);// - offsets; }