Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Accelerometer.cpp
- Revision:
- 7:604a8369b801
- Parent:
- 6:c12cea26842d
- Child:
- 8:cba37530d480
--- a/Accelerometer.cpp Thu Feb 12 19:00:28 2015 +0000 +++ b/Accelerometer.cpp Tue Feb 17 16:53:50 2015 +0000 @@ -2,26 +2,69 @@ #define DEBUG "BMX055-Acc" #include "Logger.h" -Accelerometer::Accelerometer(I2C &i2c) : I2CPeripheral(i2c, 0x18 << 1 /* address */) { - powerOn(); - const uint8_t chip_id = read_reg(0x00); - if (chip_id == 0xfa) { +Accelerometer::Accelerometer(I2C &i2c) : I2CPeripheral(i2c, 0x18 << 1 /* address */) +{ + if (powerOn()) { INFO("Bosch Sensortec BMX055-Accel found"); - deepSuspend(); + powerOff(); } else { - WARN("Bosch Sensortec BMX055-Accel not found (chip ID=0x%02x, expected=0x58)", chip_id); + WARN("Bosch Sensortec BMX055-Accel not found"); } } -void Accelerometer::powerOn() { +bool Accelerometer::powerOn() +{ write_reg(0x14, 0xB6); // reset wait_ms(2); // page 11 says only 1.3ms, nothing for startup time, so assuming 2ms write_reg(0x11, 0); // set power normal mode - write_reg(0x10, 8); // set bandwidth = 7.81 Hz - LOG("powered on"); + write_reg(0x10, 15); // set bandwidth = 250 Hz + return read_reg(0x00) == 0xfa; // verify chip ID } -void Accelerometer::deepSuspend() { +void Accelerometer::powerOff() +{ write_reg(0x11, 1); // deep suspend mode LOG("deep sleep"); } + +void Accelerometer::start() +{ + // nothing to do right now except reset the timestamp counter + timer.stop(); + timer.reset(); + timer.start(); +} + +void Accelerometer::stop() +{ + // nothing to do right now + timer.stop(); +} + +Sensor::Data Accelerometer::read() +{ + Sensor::Data frame; + + // Check comments in the read() function of the Gyroscope for more info why we read bytes one by one. + union { + uint8_t bytes[6]; + struct { + int16_t x; + int16_t y; + int16_t z; + } values; + } f; + + f.bytes[0] = read_reg(0x02) >> 4; + f.bytes[1] = read_reg(0x03); + f.bytes[2] = read_reg(0x04) >> 4; + f.bytes[3] = read_reg(0x05); + f.bytes[4] = read_reg(0x06) >> 4; + f.bytes[5] = read_reg(0x07); + + frame.x = f.values.x; + frame.y = f.values.y; + frame.z = f.values.z; + + return frame; +}