Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Gyroscope.cpp
- Revision:
- 6:c12cea26842d
- Parent:
- 5:b9f2f62a8f90
- Child:
- 7:604a8369b801
--- a/Gyroscope.cpp Thu Feb 12 17:17:35 2015 +0000 +++ b/Gyroscope.cpp Thu Feb 12 19:00:28 2015 +0000 @@ -3,40 +3,53 @@ #include "Logger.h" Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) { - powerOn(); - - const uint8_t chip_id = read_reg(0x00); - if (chip_id == 0x0f) { + if (powerOn()) { INFO("Bosch Sensortec BMX055-Gyro found"); - deepSuspend(); } else { - WARN("Bosch Sensortec BMX055-Gyro not found (chip ID=0x%02x, expected=0x0f)", chip_id); + WARN("Bosch Sensortec BMX055-Gyro not found"); } } -void Gyroscope::deepSuspend() { +void Gyroscope::powerOff() { write_reg(0x11, 1); - LOG("deep suspend"); + LOG("deep sleep"); } -uint32_t ticks = 0; -frame_t frame; - void Gyroscope::handleInterrupt(void) { - read_reg(0x3F, (uint8_t*)&frame, sizeof frame); - ticks++; + static uint32_t ticks = 0; + Sensor::Data frame = read(); + + if (ticks % 100 == 0) + sendData(frame); } -void Gyroscope::powerOn() { +bool Gyroscope::powerOn() { write_reg(0x14, 0xB6); // softreset wait_ms(30); - LOG("full power mode"); + LOG("powered on"); + return read_reg(0x00) == 0x0f; // verify Chip ID } -void Gyroscope::startDataCapture() { +void Gyroscope::start() { write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz 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); + timer.reset(); + timer.start(); write_reg(0x15, 1 << 7); // new data interrupt enabled -} \ No newline at end of file +} + +void Gyroscope::stop() { + timer.stop(); + write_reg(0x15, 0); // turn off new data interrupt +} + +Sensor::Data Gyroscope::read() { + Sensor::Data frame; + + read_reg(0x3F, (uint8_t*)&frame, sizeof frame); + frame.timestamp = timer.read(); + + return frame; +}