Joe Shotton / Mbed 2 deprecated ELEC2645_Project_ll16j23s

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 ll16j23s_test_docs

FXOS8700CQ/FXOS8700CQ.cpp@3:fcd6d70e9694, 2020-05-20 (annotated)

Committer:
JoeShotton
Date:
Wed May 20 21:25:40 2020 +0000
Revision:
3:fcd6d70e9694
test

Who changed what in which revision?

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