James Heavey / Mbed 2 deprecated 2665-Breakout-Game

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

FXOS8700CQ/FXOS8700CQ.cpp@0:92b180c8d407, 2021-01-05 (annotated)

Committer:
jamesheavey
Date:
Tue Jan 05 01:14:11 2021 +0000
Revision:
0:92b180c8d407
test

Who changed what in which revision?

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