FXOS8700CQ class modified for el17yfk
FXOS8700CQ.cpp@4:5a6ef0d027c4, 2019-04-18 (annotated)
- Committer:
- yfkwok
- Date:
- Thu Apr 18 04:52:31 2019 +0000
- Revision:
- 4:5a6ef0d027c4
- Parent:
- 3:eedcae2bd4aa
18/04/2019 - Added functions for getting direction and magnitude
Who changed what in which revision?
User | Revision | Line number | New 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,®,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,®,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; |
yfkwok | 4:5a6ef0d027c4 | 179 | } |
yfkwok | 4:5a6ef0d027c4 | 180 | |
yfkwok | 4:5a6ef0d027c4 | 181 | Direction FXOS8700CQ::get_direction() |
yfkwok | 4:5a6ef0d027c4 | 182 | { |
yfkwok | 4:5a6ef0d027c4 | 183 | float pitch_angle = get_pitch_angle(); // -180 to 180 degree |
yfkwok | 4:5a6ef0d027c4 | 184 | float roll_angle = get_roll_angle(); //-180 to 180 degree |
yfkwok | 4:5a6ef0d027c4 | 185 | |
yfkwok | 4:5a6ef0d027c4 | 186 | Direction d; |
yfkwok | 4:5a6ef0d027c4 | 187 | // partition 360 into segments and check which segment the angle is in |
yfkwok | 4:5a6ef0d027c4 | 188 | if ((pitch_angle <= 10) && (roll_angle <= 10) && (pitch_angle >= -10) && (roll_angle >= -10)) { |
yfkwok | 4:5a6ef0d027c4 | 189 | d = CENTRE; // check for -1.0 angle |
yfkwok | 4:5a6ef0d027c4 | 190 | } else if ((pitch_angle > 10) && (roll_angle < 10) && (roll_angle > -10)) { // then keep going in 45 degree increments |
yfkwok | 4:5a6ef0d027c4 | 191 | d = N; |
yfkwok | 4:5a6ef0d027c4 | 192 | } else if ((pitch_angle > 10) && (roll_angle < -10)) { |
yfkwok | 4:5a6ef0d027c4 | 193 | d = NE; |
yfkwok | 4:5a6ef0d027c4 | 194 | } else if ((pitch_angle < 10) && (roll_angle < -10) && (pitch_angle > -10)) { |
yfkwok | 4:5a6ef0d027c4 | 195 | d = E; |
yfkwok | 4:5a6ef0d027c4 | 196 | } else if ((pitch_angle < -10) && (roll_angle < -10)) { |
yfkwok | 4:5a6ef0d027c4 | 197 | d = SE; |
yfkwok | 4:5a6ef0d027c4 | 198 | } else if ((pitch_angle < -10) && (roll_angle < 10) && (roll_angle > -10)) { |
yfkwok | 4:5a6ef0d027c4 | 199 | d = S; |
yfkwok | 4:5a6ef0d027c4 | 200 | } else if ((pitch_angle < -10) && (roll_angle > 10)) { |
yfkwok | 4:5a6ef0d027c4 | 201 | d = SW; |
yfkwok | 4:5a6ef0d027c4 | 202 | } else if ((pitch_angle < 10) && (pitch_angle > -10) && (roll_angle > 10)) { |
yfkwok | 4:5a6ef0d027c4 | 203 | d = W; |
yfkwok | 4:5a6ef0d027c4 | 204 | } else if ((pitch_angle > 10) && (roll_angle > 10)) { |
yfkwok | 4:5a6ef0d027c4 | 205 | d = NW; |
yfkwok | 4:5a6ef0d027c4 | 206 | } |
yfkwok | 4:5a6ef0d027c4 | 207 | |
yfkwok | 4:5a6ef0d027c4 | 208 | return d; |
yfkwok | 4:5a6ef0d027c4 | 209 | } |
yfkwok | 4:5a6ef0d027c4 | 210 | |
yfkwok | 4:5a6ef0d027c4 | 211 | float FXOS8700CQ::get_mag() |
yfkwok | 4:5a6ef0d027c4 | 212 | { |
yfkwok | 4:5a6ef0d027c4 | 213 | Data values = get_values(); |
yfkwok | 4:5a6ef0d027c4 | 214 | float mag = sqrt(values.ay*values.ay + values.az*values.az + values.ax*values.ax); |
yfkwok | 4:5a6ef0d027c4 | 215 | return mag; |
eencae | 0:f66eda6a492a | 216 | } |