Ben Evans / Mbed 2 deprecated Defender_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@48:e308067cfea5, 2020-05-19 (annotated)

Committer:
evanso
Date:
Tue May 19 15:13:47 2020 +0000
Revision:
48:e308067cfea5
Parent:
46:142a3914c3e3 
Added sd card class folder to project folder

Who changed what in which revision?

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