Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Gyroscope.cpp@6:c12cea26842d, 2015-02-12 (annotated)

Committer:
pvaibhav
Date:
Thu Feb 12 19:00:28 2015 +0000
Revision:
6:c12cea26842d
Parent:
5:b9f2f62a8f90
Child:
7:604a8369b801
New "Sensor" base class and delegation mechanism for sensor updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 4:e759b8c756da 1 #include "Gyroscope.h"
pvaibhav 4:e759b8c756da 2 #define DEBUG "BMX055-Gyr"
pvaibhav 4:e759b8c756da 3 #include "Logger.h"
pvaibhav 4:e759b8c756da 4
pvaibhav 5:b9f2f62a8f90 5 Gyroscope::Gyroscope(I2C &i2c) : I2CPeripheral(i2c, 0x69 << 1 /* address */), int1(p3) {
pvaibhav 6:c12cea26842d 6 if (powerOn()) {
pvaibhav 5:b9f2f62a8f90 7 INFO("Bosch Sensortec BMX055-Gyro found");
pvaibhav 4:e759b8c756da 8 } else {
pvaibhav 6:c12cea26842d 9 WARN("Bosch Sensortec BMX055-Gyro not found");
pvaibhav 4:e759b8c756da 10 }
pvaibhav 4:e759b8c756da 11 }
pvaibhav 4:e759b8c756da 12
pvaibhav 6:c12cea26842d 13 void Gyroscope::powerOff() {
pvaibhav 5:b9f2f62a8f90 14 write_reg(0x11, 1);
pvaibhav 6:c12cea26842d 15 LOG("deep sleep");
pvaibhav 5:b9f2f62a8f90 16 }
pvaibhav 5:b9f2f62a8f90 17
pvaibhav 5:b9f2f62a8f90 18 void Gyroscope::handleInterrupt(void) {
pvaibhav 6:c12cea26842d 19 static uint32_t ticks = 0;
pvaibhav 6:c12cea26842d 20 Sensor::Data frame = read();
pvaibhav 6:c12cea26842d 21
pvaibhav 6:c12cea26842d 22 if (ticks % 100 == 0)
pvaibhav 6:c12cea26842d 23 sendData(frame);
pvaibhav 5:b9f2f62a8f90 24 }
pvaibhav 5:b9f2f62a8f90 25
pvaibhav 6:c12cea26842d 26 bool Gyroscope::powerOn() {
pvaibhav 5:b9f2f62a8f90 27 write_reg(0x14, 0xB6); // softreset
pvaibhav 5:b9f2f62a8f90 28 wait_ms(30);
pvaibhav 6:c12cea26842d 29 LOG("powered on");
pvaibhav 6:c12cea26842d 30 return read_reg(0x00) == 0x0f; // verify Chip ID
pvaibhav 5:b9f2f62a8f90 31 }
pvaibhav 5:b9f2f62a8f90 32
pvaibhav 6:c12cea26842d 33 void Gyroscope::start() {
pvaibhav 5:b9f2f62a8f90 34 write_reg(0x10, 5); // set capture rate: 200 Hz / filter: 23 Hz
pvaibhav 5:b9f2f62a8f90 35 write_reg(0x16, 5); // interrupts active high, push-pull
pvaibhav 5:b9f2f62a8f90 36 write_reg(0x18, 1 << 0); // map new data interrupt to INT3 pin (1st interrupt for gyro)
pvaibhav 5:b9f2f62a8f90 37 int1.rise(this, &Gyroscope::handleInterrupt);
pvaibhav 6:c12cea26842d 38 timer.reset();
pvaibhav 6:c12cea26842d 39 timer.start();
pvaibhav 5:b9f2f62a8f90 40 write_reg(0x15, 1 << 7); // new data interrupt enabled
pvaibhav 6:c12cea26842d 41 }
pvaibhav 6:c12cea26842d 42
pvaibhav 6:c12cea26842d 43 void Gyroscope::stop() {
pvaibhav 6:c12cea26842d 44 timer.stop();
pvaibhav 6:c12cea26842d 45 write_reg(0x15, 0); // turn off new data interrupt
pvaibhav 6:c12cea26842d 46 }
pvaibhav 6:c12cea26842d 47
pvaibhav 6:c12cea26842d 48 Sensor::Data Gyroscope::read() {
pvaibhav 6:c12cea26842d 49 Sensor::Data frame;
pvaibhav 6:c12cea26842d 50
pvaibhav 6:c12cea26842d 51 read_reg(0x3F, (uint8_t*)&frame, sizeof frame);
pvaibhav 6:c12cea26842d 52 frame.timestamp = timer.read();
pvaibhav 6:c12cea26842d 53
pvaibhav 6:c12cea26842d 54 return frame;
pvaibhav 6:c12cea26842d 55 }