Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Accelerometer.cpp@7:604a8369b801, 2015-02-17 (annotated)
- Committer:
- pvaibhav
- Date:
- Tue Feb 17 16:53:50 2015 +0000
- Revision:
- 7:604a8369b801
- Parent:
- 6:c12cea26842d
- Child:
- 8:cba37530d480
Changed reading method for sensors to avoid high spikes
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 | 7:604a8369b801 | 5 | Accelerometer::Accelerometer(I2C &i2c) : I2CPeripheral(i2c, 0x18 << 1 /* address */) |
pvaibhav | 7:604a8369b801 | 6 | { |
pvaibhav | 7:604a8369b801 | 7 | if (powerOn()) { |
pvaibhav | 5:b9f2f62a8f90 | 8 | INFO("Bosch Sensortec BMX055-Accel found"); |
pvaibhav | 7:604a8369b801 | 9 | powerOff(); |
pvaibhav | 1:c279bc3af90c | 10 | } else { |
pvaibhav | 7:604a8369b801 | 11 | WARN("Bosch Sensortec BMX055-Accel not found"); |
pvaibhav | 1:c279bc3af90c | 12 | } |
pvaibhav | 1:c279bc3af90c | 13 | } |
pvaibhav | 1:c279bc3af90c | 14 | |
pvaibhav | 7:604a8369b801 | 15 | bool Accelerometer::powerOn() |
pvaibhav | 7:604a8369b801 | 16 | { |
pvaibhav | 5:b9f2f62a8f90 | 17 | write_reg(0x14, 0xB6); // reset |
pvaibhav | 5:b9f2f62a8f90 | 18 | wait_ms(2); // page 11 says only 1.3ms, nothing for startup time, so assuming 2ms |
pvaibhav | 5:b9f2f62a8f90 | 19 | write_reg(0x11, 0); // set power normal mode |
pvaibhav | 7:604a8369b801 | 20 | write_reg(0x10, 15); // set bandwidth = 250 Hz |
pvaibhav | 7:604a8369b801 | 21 | return read_reg(0x00) == 0xfa; // verify chip ID |
pvaibhav | 5:b9f2f62a8f90 | 22 | } |
pvaibhav | 5:b9f2f62a8f90 | 23 | |
pvaibhav | 7:604a8369b801 | 24 | void Accelerometer::powerOff() |
pvaibhav | 7:604a8369b801 | 25 | { |
pvaibhav | 5:b9f2f62a8f90 | 26 | write_reg(0x11, 1); // deep suspend mode |
pvaibhav | 6:c12cea26842d | 27 | LOG("deep sleep"); |
pvaibhav | 5:b9f2f62a8f90 | 28 | } |
pvaibhav | 7:604a8369b801 | 29 | |
pvaibhav | 7:604a8369b801 | 30 | void Accelerometer::start() |
pvaibhav | 7:604a8369b801 | 31 | { |
pvaibhav | 7:604a8369b801 | 32 | // nothing to do right now except reset the timestamp counter |
pvaibhav | 7:604a8369b801 | 33 | timer.stop(); |
pvaibhav | 7:604a8369b801 | 34 | timer.reset(); |
pvaibhav | 7:604a8369b801 | 35 | timer.start(); |
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 | timer.stop(); |
pvaibhav | 7:604a8369b801 | 42 | } |
pvaibhav | 7:604a8369b801 | 43 | |
pvaibhav | 7:604a8369b801 | 44 | Sensor::Data Accelerometer::read() |
pvaibhav | 7:604a8369b801 | 45 | { |
pvaibhav | 7:604a8369b801 | 46 | Sensor::Data frame; |
pvaibhav | 7:604a8369b801 | 47 | |
pvaibhav | 7:604a8369b801 | 48 | // Check comments in the read() function of the Gyroscope for more info why we read bytes one by one. |
pvaibhav | 7:604a8369b801 | 49 | union { |
pvaibhav | 7:604a8369b801 | 50 | uint8_t bytes[6]; |
pvaibhav | 7:604a8369b801 | 51 | struct { |
pvaibhav | 7:604a8369b801 | 52 | int16_t x; |
pvaibhav | 7:604a8369b801 | 53 | int16_t y; |
pvaibhav | 7:604a8369b801 | 54 | int16_t z; |
pvaibhav | 7:604a8369b801 | 55 | } values; |
pvaibhav | 7:604a8369b801 | 56 | } f; |
pvaibhav | 7:604a8369b801 | 57 | |
pvaibhav | 7:604a8369b801 | 58 | f.bytes[0] = read_reg(0x02) >> 4; |
pvaibhav | 7:604a8369b801 | 59 | f.bytes[1] = read_reg(0x03); |
pvaibhav | 7:604a8369b801 | 60 | f.bytes[2] = read_reg(0x04) >> 4; |
pvaibhav | 7:604a8369b801 | 61 | f.bytes[3] = read_reg(0x05); |
pvaibhav | 7:604a8369b801 | 62 | f.bytes[4] = read_reg(0x06) >> 4; |
pvaibhav | 7:604a8369b801 | 63 | f.bytes[5] = read_reg(0x07); |
pvaibhav | 7:604a8369b801 | 64 | |
pvaibhav | 7:604a8369b801 | 65 | frame.x = f.values.x; |
pvaibhav | 7:604a8369b801 | 66 | frame.y = f.values.y; |
pvaibhav | 7:604a8369b801 | 67 | frame.z = f.values.z; |
pvaibhav | 7:604a8369b801 | 68 | |
pvaibhav | 7:604a8369b801 | 69 | return frame; |
pvaibhav | 7:604a8369b801 | 70 | } |