FRC - Hackathon / CMPS03

CMPS03 Compass library with only PWM support. I2C support will be added shortly, while it will arrive, you may use MBED component library if you wish to use CMPS03 I2C interface

Dependents:   TestBoussole FRC_2018 0hackton_08_06_18 lib_FRC_2019 ... more

CMPS03.cpp@0:41ff46c0f65a, 2018-05-18 (annotated)

Committer:
haarkon
Date:
Fri May 18 16:36:50 2018 +0000
Revision:
0:41ff46c0f65a
Child:
1:2507a3379f17
Added the PWM output for CMPS03 (untested)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:41ff46c0f65a 1 #include "CMPS03.h"
haarkon 0:41ff46c0f65a 2
haarkon 0:41ff46c0f65a 3 CMPS03::CMPS03(PinName pwm, PinName sda, PinName scl, int address) : _boussole(pwm)
haarkon 0:41ff46c0f65a 4 {
haarkon 0:41ff46c0f65a 5 _boussole.rise(this, &CMPS03::rise);
haarkon 0:41ff46c0f65a 6 _boussole.fall(this, &CMPS03::fall);
haarkon 0:41ff46c0f65a 7 _tim.start();
haarkon 0:41ff46c0f65a 8 _i2c = new I2C(sda, scl);
haarkon 0:41ff46c0f65a 9 //Compass designed to work at 100KHz. See datasheet for details.
haarkon 0:41ff46c0f65a 10 _i2c->frequency(100000);
haarkon 0:41ff46c0f65a 11 _i2cAddress = address;
haarkon 0:41ff46c0f65a 12 }
haarkon 0:41ff46c0f65a 13
haarkon 0:41ff46c0f65a 14 char CMPS03::readSoftwareRevision(void)
haarkon 0:41ff46c0f65a 15 {
haarkon 0:41ff46c0f65a 16
haarkon 0:41ff46c0f65a 17 char registerNumber = SOFTWARE_REVISION_REG;
haarkon 0:41ff46c0f65a 18 char registerContents = 0;
haarkon 0:41ff46c0f65a 19
haarkon 0:41ff46c0f65a 20 //First, send the number of register we wish to read,
haarkon 0:41ff46c0f65a 21 //in this case, command register, number 0.
haarkon 0:41ff46c0f65a 22 _i2c->write(_i2cAddress, &registerNumber, 1);
haarkon 0:41ff46c0f65a 23
haarkon 0:41ff46c0f65a 24 //Now, read one byte, which will be the contents of the command register.
haarkon 0:41ff46c0f65a 25 _i2c->read(_i2cAddress, &registerContents, 1);
haarkon 0:41ff46c0f65a 26
haarkon 0:41ff46c0f65a 27 return registerContents;
haarkon 0:41ff46c0f65a 28
haarkon 0:41ff46c0f65a 29 }
haarkon 0:41ff46c0f65a 30
haarkon 0:41ff46c0f65a 31 float CMPS03::readBearing(void)
haarkon 0:41ff46c0f65a 32 {
haarkon 0:41ff46c0f65a 33
haarkon 0:41ff46c0f65a 34 char registerNumber = COMPASS_BEARING_WORD_REG;
haarkon 0:41ff46c0f65a 35 char registerContents[2] = {0x00, 0x00};
haarkon 0:41ff46c0f65a 36
haarkon 0:41ff46c0f65a 37 //First, send the number of register we wish to read,
haarkon 0:41ff46c0f65a 38 //in this case, register numbers 2, 3, which hold the
haarkon 0:41ff46c0f65a 39 //compass bearing as a 16-bit word.
haarkon 0:41ff46c0f65a 40 _i2c->write(_i2cAddress, &registerNumber, 1);
haarkon 0:41ff46c0f65a 41
haarkon 0:41ff46c0f65a 42 //Now read two bytes which will be the contents of
haarkon 0:41ff46c0f65a 43 //these registers.
haarkon 0:41ff46c0f65a 44 _i2c->read(_i2cAddress, registerContents, 2);
haarkon 0:41ff46c0f65a 45
haarkon 0:41ff46c0f65a 46 //Register 2 [read first], was the high byte, followed by
haarkon 0:41ff46c0f65a 47 //register 3 [read second], which was the low byte.
haarkon 0:41ff46c0f65a 48 float _i2cbearing = (((int)registerContents[0] << 8) | ((int)registerContents[1]))/10.0;
haarkon 0:41ff46c0f65a 49
haarkon 0:41ff46c0f65a 50 return _i2cbearing;
haarkon 0:41ff46c0f65a 51
haarkon 0:41ff46c0f65a 52 }
haarkon 0:41ff46c0f65a 53
haarkon 0:41ff46c0f65a 54 void CMPS03::rise(void)
haarkon 0:41ff46c0f65a 55 {
haarkon 0:41ff46c0f65a 56 _startTime = _tim.read_us();
haarkon 0:41ff46c0f65a 57 }
haarkon 0:41ff46c0f65a 58
haarkon 0:41ff46c0f65a 59 void CMPS03::fall(void)
haarkon 0:41ff46c0f65a 60 {
haarkon 0:41ff46c0f65a 61
haarkon 0:41ff46c0f65a 62 _stopTime = _tim.read_us();
haarkon 0:41ff46c0f65a 63 _pwmBearing = (_stopTime - _startTime)/100.0;
haarkon 0:41ff46c0f65a 64 }
haarkon 0:41ff46c0f65a 65
haarkon 0:41ff46c0f65a 66
haarkon 0:41ff46c0f65a 67 float CMPS03::getBearing(void)
haarkon 0:41ff46c0f65a 68 {
haarkon 0:41ff46c0f65a 69 return _pwmBearing;
haarkon 0:41ff46c0f65a 70 }
haarkon 0:41ff46c0f65a 71