Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Accelerometer.cpp
- Committer:
- pvaibhav
- Date:
- 2015-05-26
- Revision:
- 40:8e852115fe55
- Parent:
- 32:d37447aec6b4
- Child:
- 43:6251c0169f4f
File content as of revision 40:8e852115fe55:
#include "Accelerometer.h" #define DEBUG "BMX055-Acc" #include "Logger.h" #include "Vector3.h" Accelerometer::Accelerometer(I2C &i2c) : I2CPeripheral(i2c, 0x18 << 1 /* address */), int1(p2), int2(p0) { if (powerOn()) { INFO("Bosch Sensortec BMX055-Accel found"); powerOff(); } else { WARN("Bosch Sensortec BMX055-Accel not found"); } } 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, 12); // set bandwidth = 125 Hz (earlier comment was incorrect) return read_reg(0x00) == 0xfa; // verify chip ID } void Accelerometer::powerOff() { write_reg(0x11, 1); // deep suspend mode LOG("deep sleep"); } void Accelerometer::start() { // nothing to do right now } void Accelerometer::stop() { // nothing to do right now } Vector3 Accelerometer::read() { // Check comments in the read() function of the Gyroscope for more info why we read bytes one by one. 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 = *(reinterpret_cast<const int16_t*>(buffer + 0)) / 16; const int16_t y = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 16; const int16_t z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 16; const float accel_resolution = 0.9765625; return Vector3(x, y, z);// * accel_resolution; }