Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Accelerometer.cpp@46:fd5a62296b12, 2015-05-27 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed May 27 13:01:43 2015 +0000
- Revision:
- 46:fd5a62296b12
- Parent:
- 43:6251c0169f4f
Code reformatted
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pvaibhav | 1:c279bc3af90c | 1 | #include "Accelerometer.h" |
pvaibhav | 1:c279bc3af90c | 2 | #define DEBUG "BMX055-Acc" |
pvaibhav | 1:c279bc3af90c | 3 | #include "Logger.h" |
pvaibhav | 1:c279bc3af90c | 4 | |
pvaibhav | 8:cba37530d480 | 5 | #include "Vector3.h" |
pvaibhav | 8:cba37530d480 | 6 | |
pvaibhav | 32:d37447aec6b4 | 7 | Accelerometer::Accelerometer(I2C &i2c) : I2CPeripheral(i2c, 0x18 << 1 /* address */), int1(p2), int2(p0) |
pvaibhav | 7:604a8369b801 | 8 | { |
pvaibhav | 7:604a8369b801 | 9 | if (powerOn()) { |
pvaibhav | 5:b9f2f62a8f90 | 10 | INFO("Bosch Sensortec BMX055-Accel found"); |
pvaibhav | 7:604a8369b801 | 11 | powerOff(); |
pvaibhav | 1:c279bc3af90c | 12 | } else { |
pvaibhav | 7:604a8369b801 | 13 | WARN("Bosch Sensortec BMX055-Accel not found"); |
pvaibhav | 1:c279bc3af90c | 14 | } |
pvaibhav | 1:c279bc3af90c | 15 | } |
pvaibhav | 1:c279bc3af90c | 16 | |
pvaibhav | 7:604a8369b801 | 17 | bool Accelerometer::powerOn() |
pvaibhav | 7:604a8369b801 | 18 | { |
pvaibhav | 5:b9f2f62a8f90 | 19 | write_reg(0x14, 0xB6); // reset |
pvaibhav | 5:b9f2f62a8f90 | 20 | wait_ms(2); // page 11 says only 1.3ms, nothing for startup time, so assuming 2ms |
pvaibhav | 5:b9f2f62a8f90 | 21 | write_reg(0x11, 0); // set power normal mode |
pvaibhav | 43:6251c0169f4f | 22 | write_reg(0x0f, 12); // set range = 16G |
pvaibhav | 31:d65576185bdf | 23 | write_reg(0x10, 12); // set bandwidth = 125 Hz (earlier comment was incorrect) |
pvaibhav | 7:604a8369b801 | 24 | return read_reg(0x00) == 0xfa; // verify chip ID |
pvaibhav | 5:b9f2f62a8f90 | 25 | } |
pvaibhav | 5:b9f2f62a8f90 | 26 | |
pvaibhav | 7:604a8369b801 | 27 | void Accelerometer::powerOff() |
pvaibhav | 7:604a8369b801 | 28 | { |
pvaibhav | 5:b9f2f62a8f90 | 29 | write_reg(0x11, 1); // deep suspend mode |
pvaibhav | 6:c12cea26842d | 30 | LOG("deep sleep"); |
pvaibhav | 5:b9f2f62a8f90 | 31 | } |
pvaibhav | 7:604a8369b801 | 32 | |
pvaibhav | 7:604a8369b801 | 33 | void Accelerometer::start() |
pvaibhav | 7:604a8369b801 | 34 | { |
pvaibhav | 8:cba37530d480 | 35 | // nothing to do right now |
pvaibhav | 7:604a8369b801 | 36 | } |
pvaibhav | 7:604a8369b801 | 37 | |
pvaibhav | 7:604a8369b801 | 38 | void Accelerometer::stop() |
pvaibhav | 7:604a8369b801 | 39 | { |
pvaibhav | 7:604a8369b801 | 40 | // nothing to do right now |
pvaibhav | 7:604a8369b801 | 41 | } |
pvaibhav | 7:604a8369b801 | 42 | |
pvaibhav | 8:cba37530d480 | 43 | Vector3 Accelerometer::read() |
pvaibhav | 7:604a8369b801 | 44 | { |
pvaibhav | 7:604a8369b801 | 45 | // Check comments in the read() function of the Gyroscope for more info why we read bytes one by one. |
pvaibhav | 12:1632d7391453 | 46 | uint8_t buffer[6]; |
pvaibhav | 40:8e852115fe55 | 47 | /* |
pvaibhav | 12:1632d7391453 | 48 | for (size_t i = 0; i < 6; i++) |
pvaibhav | 12:1632d7391453 | 49 | buffer[i] = read_reg(0x02 + i); |
pvaibhav | 40:8e852115fe55 | 50 | */ |
pvaibhav | 46:fd5a62296b12 | 51 | |
pvaibhav | 40:8e852115fe55 | 52 | read_reg(0x02, buffer, sizeof buffer); |
pvaibhav | 46:fd5a62296b12 | 53 | |
pvaibhav | 12:1632d7391453 | 54 | const int16_t x = *(reinterpret_cast<const int16_t*>(buffer + 0)) / 16; |
pvaibhav | 12:1632d7391453 | 55 | const int16_t y = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 16; |
pvaibhav | 12:1632d7391453 | 56 | const int16_t z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 16; |
pvaibhav | 12:1632d7391453 | 57 | |
pvaibhav | 40:8e852115fe55 | 58 | const float accel_resolution = 0.9765625; |
pvaibhav | 12:1632d7391453 | 59 | |
pvaibhav | 40:8e852115fe55 | 60 | return Vector3(x, y, z);// * accel_resolution; |
pvaibhav | 7:604a8369b801 | 61 | } |