Max Houghton / Mbed 2 deprecated MazeGame_el15mh

el15mh 200929957

Dependencies:   mbed

FXOS8700CQ/FXOS8700CQ.cpp@10:989e5dbd12ee, 2017-05-04 (annotated)

Committer:
el15mh
Date:
Thu May 04 17:39:23 2017 +0000
Revision:
10:989e5dbd12ee
Parent:
9:960dfc71c224 
Documented and final revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
el15mh 9:960dfc71c224 1 /* FXOS8700CQ Library
el15mh 9:960dfc71c224 2
el15mh 9:960dfc71c224 3 Sample code from ELEC2645 - demonstrates how to create a library
el15mh 9:960dfc71c224 4 for the K64F on-board accelerometer and magnetometer
el15mh 9:960dfc71c224 5
el15mh 9:960dfc71c224 6 (c) Craig A. Evans, University of Leeds, Jan 2017
el15mh 9:960dfc71c224 7
el15mh 9:960dfc71c224 8 */
el15mh 9:960dfc71c224 9
el15mh 9:960dfc71c224 10 #include "FXOS8700CQ.h"
el15mh 9:960dfc71c224 11
el15mh 9:960dfc71c224 12 // constructor is called when the object is created - use it to set pins and frequency
el15mh 9:960dfc71c224 13 FXOS8700CQ::FXOS8700CQ(PinName sda, PinName scl)
el15mh 9:960dfc71c224 14 {
el15mh 9:960dfc71c224 15 i2c = new I2C(sda,scl); // create new I2C instance and initialise
el15mh 9:960dfc71c224 16 }
el15mh 9:960dfc71c224 17
el15mh 9:960dfc71c224 18 // destructor is called when the object goes out of scope
el15mh 9:960dfc71c224 19 FXOS8700CQ::~FXOS8700CQ()
el15mh 9:960dfc71c224 20 {
el15mh 9:960dfc71c224 21 delete i2c; // free memory
el15mh 9:960dfc71c224 22 }
el15mh 9:960dfc71c224 23
el15mh 9:960dfc71c224 24 // based on 13.4 in datasheet - 200 Hz hybrid mode (both acc and mag)
el15mh 9:960dfc71c224 25 void FXOS8700CQ::init()
el15mh 9:960dfc71c224 26 {
el15mh 9:960dfc71c224 27 // i2c fast-mode - 10.1.1 data sheet
el15mh 9:960dfc71c224 28 i2c->frequency(400000); // I2C Fast Mode - 400kHz
el15mh 9:960dfc71c224 29
el15mh 9:960dfc71c224 30 // the device has an ID number so we check the value to ensure the correct
el15mh 9:960dfc71c224 31 // drive is on the i2c bus
el15mh 9:960dfc71c224 32 char data = read_byte_from_reg(FXOS8700CQ_WHO_AM_I);
el15mh 9:960dfc71c224 33 if (data != FXOS8700CQ_WHO_AM_I_VAL) { // if correct ID not found, hang and flash error message
el15mh 9:960dfc71c224 34 error("Incorrect ID!");
el15mh 9:960dfc71c224 35 }
el15mh 9:960dfc71c224 36
el15mh 9:960dfc71c224 37 // write 0000 0000 = 0x00 to accelerometer control register 1 to place
el15mh 9:960dfc71c224 38 // FXOS8700CQ into standby
el15mh 9:960dfc71c224 39 // [7-1] = 0000 000
el15mh 9:960dfc71c224 40 // [0]: active=0
el15mh 9:960dfc71c224 41 data = 0x00;
el15mh 9:960dfc71c224 42 send_byte_to_reg(data,FXOS8700CQ_CTRL_REG1);
el15mh 9:960dfc71c224 43
el15mh 9:960dfc71c224 44 // write 0001 1111 = 0x1F to magnetometer control register 1
el15mh 9:960dfc71c224 45 // [7]: m_acal=0: auto calibration disabled
el15mh 9:960dfc71c224 46 // [6]: m_rst=0: no one-shot magnetic reset
el15mh 9:960dfc71c224 47 // [5]: m_ost=0: no one-shot magnetic measurement
el15mh 9:960dfc71c224 48 // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise
el15mh 9:960dfc71c224 49 // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active
el15mh 9:960dfc71c224 50 data = 0x1F;
el15mh 9:960dfc71c224 51 send_byte_to_reg(data,FXOS8700CQ_M_CTRL_REG1);
el15mh 9:960dfc71c224 52
el15mh 9:960dfc71c224 53 // write 0010 0000 = 0x20 to magnetometer control register 2
el15mh 9:960dfc71c224 54 // [7]: reserved
el15mh 9:960dfc71c224 55 // [6]: reserved
el15mh 9:960dfc71c224 56 // [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow
el15mh 9:960dfc71c224 57 // the accelerometer registers
el15mh 9:960dfc71c224 58 // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
el15mh 9:960dfc71c224 59 // [3]: m_maxmin_dis_ths=0
el15mh 9:960dfc71c224 60 // [2]: m_maxmin_rst=0
el15mh 9:960dfc71c224 61 // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle
el15mh 9:960dfc71c224 62 data = 0x20;
el15mh 9:960dfc71c224 63 send_byte_to_reg(data,FXOS8700CQ_M_CTRL_REG2);
el15mh 9:960dfc71c224 64
el15mh 9:960dfc71c224 65 // write 0000 0001= 0x01 to XYZ_DATA_CFG register
el15mh 9:960dfc71c224 66 // [7]: reserved
el15mh 9:960dfc71c224 67 // [6]: reserved
el15mh 9:960dfc71c224 68 // [5]: reserved
el15mh 9:960dfc71c224 69 // [4]: hpf_out=0
el15mh 9:960dfc71c224 70 // [3]: reserved
el15mh 9:960dfc71c224 71 // [2]: reserved
el15mh 9:960dfc71c224 72 // [1-0]: fs=01 for accelerometer range of +/-4g range with 0.488mg/LSB
el15mh 9:960dfc71c224 73 data = 0x01;
el15mh 9:960dfc71c224 74 send_byte_to_reg(data,FXOS8700CQ_XYZ_DATA_CFG);
el15mh 9:960dfc71c224 75
el15mh 9:960dfc71c224 76 // write 0000 1101 = 0x0D to accelerometer control register 1
el15mh 9:960dfc71c224 77 // [7-6]: aslp_rate=00
el15mh 9:960dfc71c224 78 // [5-3]: dr=001 for 200Hz data rate (when in hybrid mode)
el15mh 9:960dfc71c224 79 // [2]: lnoise=1 for low noise mode
el15mh 9:960dfc71c224 80 // [1]: f_read=0 for normal 16 bit reads
el15mh 9:960dfc71c224 81 // [0]: active=1 to take the part out of standby and enable sampling
el15mh 9:960dfc71c224 82 data = 0x0D;
el15mh 9:960dfc71c224 83 send_byte_to_reg(data,FXOS8700CQ_CTRL_REG1);
el15mh 9:960dfc71c224 84
el15mh 9:960dfc71c224 85 }
el15mh 9:960dfc71c224 86
el15mh 9:960dfc71c224 87 Data FXOS8700CQ::get_values()
el15mh 9:960dfc71c224 88 {
el15mh 9:960dfc71c224 89 // 13 bytes - status plus 6 channels (2 bytes each)
el15mh 9:960dfc71c224 90 // x,y,z for accelerometer and magnetometer
el15mh 9:960dfc71c224 91 char data[FXOS8700CQ_READ_LEN];
el15mh 9:960dfc71c224 92 read_bytes_from_reg(FXOS8700CQ_STATUS,FXOS8700CQ_READ_LEN,data);
el15mh 9:960dfc71c224 93
el15mh 9:960dfc71c224 94 // copy the 14 bit accelerometer byte data into 16 bit words
el15mh 9:960dfc71c224 95 int acc_x = (int16_t)(((data[1] << 8) | data[2]))>> 2;
el15mh 9:960dfc71c224 96 int acc_y = (int16_t)(((data[3] << 8) | data[4]))>> 2;
el15mh 9:960dfc71c224 97 int acc_z = (int16_t)(((data[5] << 8) | data[6]))>> 2;
el15mh 9:960dfc71c224 98
el15mh 9:960dfc71c224 99 // copy the magnetometer byte data into 16 bit words
el15mh 9:960dfc71c224 100 int mag_x = (int16_t) (data[7] << 8) | data[8];
el15mh 9:960dfc71c224 101 int mag_y = (int16_t) (data[9] << 8) | data[10];
el15mh 9:960dfc71c224 102 int mag_z = (int16_t) (data[11] << 8) | data[12];
el15mh 9:960dfc71c224 103
el15mh 9:960dfc71c224 104 Data values; // struct to hold values
el15mh 9:960dfc71c224 105
el15mh 9:960dfc71c224 106 // 0.488 mg/LSB in 4 g mode (8.1 data sheet)
el15mh 9:960dfc71c224 107 values.ax = 0.488e-3*acc_x;
el15mh 9:960dfc71c224 108 values.ay = 0.488e-3*acc_y;
el15mh 9:960dfc71c224 109 values.az = 0.488e-3*acc_z;
el15mh 9:960dfc71c224 110
el15mh 9:960dfc71c224 111 // the magnetometer sensitivity is fixed at 0.1 μT/LSB
el15mh 9:960dfc71c224 112 values.mx = 0.1e-6*mag_x;
el15mh 9:960dfc71c224 113 values.my = 0.1e-6*mag_y;
el15mh 9:960dfc71c224 114 values.mz = 0.1e-6*mag_z;
el15mh 9:960dfc71c224 115
el15mh 9:960dfc71c224 116 return values;
el15mh 9:960dfc71c224 117 }
el15mh 9:960dfc71c224 118
el15mh 9:960dfc71c224 119 float FXOS8700CQ::getPitchAngle()
el15mh 9:960dfc71c224 120 {
el15mh 9:960dfc71c224 121 Data values = get_values();
el15mh 9:960dfc71c224 122
el15mh 9:960dfc71c224 123 return atan2(values.ay,values.az) * RAD2DEG;
el15mh 9:960dfc71c224 124 }
el15mh 9:960dfc71c224 125
el15mh 9:960dfc71c224 126 float FXOS8700CQ::getRollAngle()
el15mh 9:960dfc71c224 127 {
el15mh 9:960dfc71c224 128 Data values = get_values();
el15mh 9:960dfc71c224 129
el15mh 9:960dfc71c224 130 float size = sqrt(values.ay*values.ay + values.az*values.az);
el15mh 9:960dfc71c224 131
el15mh 9:960dfc71c224 132 return atan2(-values.ax,size) * RAD2DEG;
el15mh 9:960dfc71c224 133 }
el15mh 9:960dfc71c224 134 /*
el15mh 9:960dfc71c224 135 Angle FXOS8700CQ::getRotationDirection()
el15mh 9:960dfc71c224 136 {
el15mh 9:960dfc71c224 137 Data values = get_values();
el15mh 9:960dfc71c224 138
el15mh 9:960dfc71c224 139 float roll = getRollAngle();
el15mh 9:960dfc71c224 140 float pitch = getPitchAngle();
el15mh 9:960dfc71c224 141
el15mh 9:960dfc71c224 142 float angle = atan2(pitch, angle);
el15mh 9:960dfc71c224 143
el15mh 9:960dfc71c224 144 Angle d;
el15mh 9:960dfc71c224 145 // partition 360 into segments and check which segment the angle is in
el15mh 9:960dfc71c224 146 if (angle < 0.0f) {
el15mh 9:960dfc71c224 147 d = CENTRE; // check for -1.0 angle
el15mh 9:960dfc71c224 148 } else if (angle < 22.5f) { // then keep going in 45 degree increments
el15mh 9:960dfc71c224 149 d = N;
el15mh 9:960dfc71c224 150 } else if (angle < 67.5f) {
el15mh 9:960dfc71c224 151 d = NE;
el15mh 9:960dfc71c224 152 } else if (angle < 112.5f) {
el15mh 9:960dfc71c224 153 d = E;
el15mh 9:960dfc71c224 154 } else if (angle < 157.5f) {
el15mh 9:960dfc71c224 155 d = SE;
el15mh 9:960dfc71c224 156 } else if (angle < 202.5f) {
el15mh 9:960dfc71c224 157 d = S;
el15mh 9:960dfc71c224 158 } else if (angle < 247.5f) {
el15mh 9:960dfc71c224 159 d = SW;
el15mh 9:960dfc71c224 160 } else if (angle < 292.5f) {
el15mh 9:960dfc71c224 161 d = W;
el15mh 9:960dfc71c224 162 } else if (angle < 337.5f) {
el15mh 9:960dfc71c224 163 d = NW;
el15mh 9:960dfc71c224 164 } else {
el15mh 9:960dfc71c224 165 d = N;
el15mh 9:960dfc71c224 166 }
el15mh 9:960dfc71c224 167
el15mh 9:960dfc71c224 168 return d;
el15mh 9:960dfc71c224 169 }
el15mh 9:960dfc71c224 170 */
el15mh 9:960dfc71c224 171 void FXOS8700CQ::send_byte_to_reg(char byte,char reg)
el15mh 9:960dfc71c224 172 {
el15mh 9:960dfc71c224 173 char data[2];
el15mh 9:960dfc71c224 174 data[0] = reg;
el15mh 9:960dfc71c224 175 data[1] = byte;
el15mh 9:960dfc71c224 176 // send the register address, followed by the data
el15mh 9:960dfc71c224 177 int nack = i2c->write(FXOS8700CQ_ADDR,data,2);
el15mh 9:960dfc71c224 178 if (nack)
el15mh 9:960dfc71c224 179 error("No acknowledgement received!"); // if we don't receive acknowledgement, send error message
el15mh 9:960dfc71c224 180
el15mh 9:960dfc71c224 181 }
el15mh 9:960dfc71c224 182
el15mh 9:960dfc71c224 183 // reads a byte from a specific register
el15mh 9:960dfc71c224 184 char FXOS8700CQ::read_byte_from_reg(char reg)
el15mh 9:960dfc71c224 185 {
el15mh 9:960dfc71c224 186 int nack = i2c->write(FXOS8700CQ_ADDR,&reg,1,true); // send the register address to the slave
el15mh 9:960dfc71c224 187 // true as need to send repeated start condition (5.10.1 datasheet)
el15mh 9:960dfc71c224 188 // http://www.i2c-bus.org/repeated-start-condition/
el15mh 9:960dfc71c224 189 if (nack)
el15mh 9:960dfc71c224 190 error("No acknowledgement received!"); // if we don't receive acknowledgement, send error message
el15mh 9:960dfc71c224 191
el15mh 9:960dfc71c224 192 char rx;
el15mh 9:960dfc71c224 193 nack = i2c->read(FXOS8700CQ_ADDR,&rx,1); // read a byte from the register and store in buffer
el15mh 9:960dfc71c224 194 if (nack)
el15mh 9:960dfc71c224 195 error("No acknowledgement received!"); // if we don't receive acknowledgement, send error message
el15mh 9:960dfc71c224 196
el15mh 9:960dfc71c224 197 return rx;
el15mh 9:960dfc71c224 198 }
el15mh 9:960dfc71c224 199
el15mh 9:960dfc71c224 200 // reads a series of bytes, starting from a specific register
el15mh 9:960dfc71c224 201 void FXOS8700CQ::read_bytes_from_reg(char reg,int number_of_bytes,char bytes[])
el15mh 9:960dfc71c224 202 {
el15mh 9:960dfc71c224 203 int nack = i2c->write(FXOS8700CQ_ADDR,&reg,1,true); // send the slave write address and the configuration register address
el15mh 9:960dfc71c224 204 // true as need to send repeated start condition (5.10.1 datasheet)
el15mh 9:960dfc71c224 205 // http://www.i2c-bus.org/repeated-start-condition/
el15mh 9:960dfc71c224 206
el15mh 9:960dfc71c224 207 if (nack)
el15mh 9:960dfc71c224 208 error("No acknowledgement received!"); // if we don't receive acknowledgement, send error message
el15mh 9:960dfc71c224 209
el15mh 9:960dfc71c224 210 nack = i2c->read(FXOS8700CQ_ADDR,bytes,number_of_bytes); // read bytes
el15mh 9:960dfc71c224 211 if (nack)
el15mh 9:960dfc71c224 212 error("No acknowledgement received!"); // if we don't receive acknowledgement, send error message
el15mh 9:960dfc71c224 213
el15mh 9:960dfc71c224 214 }
el15mh 9:960dfc71c224 215
el15mh 9:960dfc71c224 216
el15mh 9:960dfc71c224 217
el15mh 9:960dfc71c224 218