Yiu Fai Kwok / FXOS8700CQ

FXOS8700CQ class modified for el17yfk

Dependents:   el17yfk el17yfk

FXOS8700CQ.cpp@3:eedcae2bd4aa, 2019-04-17 (annotated)

Committer:
yfkwok
Date:
Wed Apr 17 17:53:33 2019 +0000
Revision:
3:eedcae2bd4aa
Parent:
0:f66eda6a492a
Child:
4:5a6ef0d027c4
17/4/2019 - FXOS8700CQ inplementation to Game_two

Who changed what in which revision?

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