Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Accelerometer.cpp@40:8e852115fe55, 2015-05-26 (annotated)
- Committer:
- pvaibhav
- Date:
- Tue May 26 11:28:37 2015 +0000
- Revision:
- 40:8e852115fe55
- Parent:
- 32:d37447aec6b4
- Child:
- 43:6251c0169f4f
SensorFusion base class and 6 axis derived class. utils, filter and pid classes moved inside SML2.
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 | 31:d65576185bdf | 22 | write_reg(0x10, 12); // set bandwidth = 125 Hz (earlier comment was incorrect) |
pvaibhav | 7:604a8369b801 | 23 | return read_reg(0x00) == 0xfa; // verify chip ID |
pvaibhav | 5:b9f2f62a8f90 | 24 | } |
pvaibhav | 5:b9f2f62a8f90 | 25 | |
pvaibhav | 7:604a8369b801 | 26 | void Accelerometer::powerOff() |
pvaibhav | 7:604a8369b801 | 27 | { |
pvaibhav | 5:b9f2f62a8f90 | 28 | write_reg(0x11, 1); // deep suspend mode |
pvaibhav | 6:c12cea26842d | 29 | LOG("deep sleep"); |
pvaibhav | 5:b9f2f62a8f90 | 30 | } |
pvaibhav | 7:604a8369b801 | 31 | |
pvaibhav | 7:604a8369b801 | 32 | void Accelerometer::start() |
pvaibhav | 7:604a8369b801 | 33 | { |
pvaibhav | 8:cba37530d480 | 34 | // nothing to do right now |
pvaibhav | 7:604a8369b801 | 35 | } |
pvaibhav | 7:604a8369b801 | 36 | |
pvaibhav | 7:604a8369b801 | 37 | void Accelerometer::stop() |
pvaibhav | 7:604a8369b801 | 38 | { |
pvaibhav | 7:604a8369b801 | 39 | // nothing to do right now |
pvaibhav | 7:604a8369b801 | 40 | } |
pvaibhav | 7:604a8369b801 | 41 | |
pvaibhav | 8:cba37530d480 | 42 | Vector3 Accelerometer::read() |
pvaibhav | 7:604a8369b801 | 43 | { |
pvaibhav | 7:604a8369b801 | 44 | // Check comments in the read() function of the Gyroscope for more info why we read bytes one by one. |
pvaibhav | 12:1632d7391453 | 45 | uint8_t buffer[6]; |
pvaibhav | 40:8e852115fe55 | 46 | /* |
pvaibhav | 12:1632d7391453 | 47 | for (size_t i = 0; i < 6; i++) |
pvaibhav | 12:1632d7391453 | 48 | buffer[i] = read_reg(0x02 + i); |
pvaibhav | 40:8e852115fe55 | 49 | */ |
pvaibhav | 40:8e852115fe55 | 50 | |
pvaibhav | 40:8e852115fe55 | 51 | read_reg(0x02, buffer, sizeof buffer); |
pvaibhav | 40:8e852115fe55 | 52 | |
pvaibhav | 12:1632d7391453 | 53 | const int16_t x = *(reinterpret_cast<const int16_t*>(buffer + 0)) / 16; |
pvaibhav | 12:1632d7391453 | 54 | const int16_t y = *(reinterpret_cast<const int16_t*>(buffer + 2)) / 16; |
pvaibhav | 12:1632d7391453 | 55 | const int16_t z = *(reinterpret_cast<const int16_t*>(buffer + 4)) / 16; |
pvaibhav | 12:1632d7391453 | 56 | |
pvaibhav | 40:8e852115fe55 | 57 | const float accel_resolution = 0.9765625; |
pvaibhav | 12:1632d7391453 | 58 | |
pvaibhav | 40:8e852115fe55 | 59 | return Vector3(x, y, z);// * accel_resolution; |
pvaibhav | 7:604a8369b801 | 60 | } |