Diff: CMPS03.cpp

Revision:

0:41ff46c0f65a

Child:

1:2507a3379f17

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CMPS03.cpp	Fri May 18 16:36:50 2018 +0000
@@ -0,0 +1,71 @@
+#include "CMPS03.h"
+
+CMPS03::CMPS03(PinName pwm, PinName sda, PinName scl, int address) : _boussole(pwm)
+{
+    _boussole.rise(this, &CMPS03::rise);
+    _boussole.fall(this, &CMPS03::fall);
+    _tim.start();
+    _i2c = new I2C(sda, scl);
+    //Compass designed to work at 100KHz. See datasheet for details.
+    _i2c->frequency(100000);
+    _i2cAddress = address;
+}
+
+char CMPS03::readSoftwareRevision(void)
+{
+
+    char registerNumber   = SOFTWARE_REVISION_REG;
+    char registerContents = 0;
+
+    //First, send the number of register we wish to read,
+    //in this case, command register, number 0.
+    _i2c->write(_i2cAddress, &registerNumber, 1);
+
+    //Now, read one byte, which will be the contents of the command register.
+    _i2c->read(_i2cAddress, &registerContents, 1);
+
+    return registerContents;
+
+}
+
+float CMPS03::readBearing(void)
+{
+
+    char registerNumber = COMPASS_BEARING_WORD_REG;
+    char registerContents[2] = {0x00, 0x00};
+
+    //First, send the number of register we wish to read,
+    //in this case, register numbers 2, 3, which hold the
+    //compass bearing as a 16-bit word.
+    _i2c->write(_i2cAddress, &registerNumber, 1);
+
+    //Now read two bytes which will be the contents of
+    //these registers.
+    _i2c->read(_i2cAddress, registerContents, 2);
+
+    //Register 2 [read first], was the high byte, followed by
+    //register 3 [read second], which was the low byte.
+    float _i2cbearing = (((int)registerContents[0] << 8) | ((int)registerContents[1]))/10.0;
+
+    return _i2cbearing;
+
+}
+
+void CMPS03::rise(void)
+{
+    _startTime = _tim.read_us();
+}
+
+void CMPS03::fall(void)
+{
+
+    _stopTime = _tim.read_us();
+    _pwmBearing = (_stopTime - _startTime)/100.0;
+}
+
+
+float CMPS03::getBearing(void)
+{
+    return _pwmBearing;
+}
+