FXOS8700CQ.cpp

00001 /* FXOS8700CQ Library
00002 
00003 Sample code from ELEC2645 - demonstrates how to create a library
00004 for the K64F on-board accelerometer and magnetometer
00005 
00006 (c) Craig A. Evans, University of Leeds, Jan 2017
00007 
00008 */ 
00009 
00010 #include "FXOS8700CQ.h"
00011 #include <cmath>
00012 
00013 // constructor is called when the object is created - use it to set pins and frequency
00014 FXOS8700CQ::FXOS8700CQ(PinName sda, PinName scl)
00015 {
00016     i2c = new I2C(sda,scl);        // create new I2C instance and initialise
00017 }
00018 
00019 // destructor is called when the object goes out of scope
00020 FXOS8700CQ::~FXOS8700CQ()
00021 {
00022     delete i2c;        // free memory
00023 }
00024 
00025 // based on 13.4 in datasheet - 200 Hz hybrid mode (both acc and mag)
00026 void FXOS8700CQ::init()
00027 {
00028     // i2c fast-mode - 10.1.1 data sheet
00029     i2c->frequency(400000);       // I2C Fast Mode - 400kHz
00030 
00031     // the device has an ID number so we check the value to ensure the correct
00032     // drive is on the i2c bus
00033     char data = read_byte_from_reg(FXOS8700CQ_WHO_AM_I);
00034     if (data != FXOS8700CQ_WHO_AM_I_VAL) { // if correct ID not found, hang and flash error message
00035         error("Incorrect ID!");
00036     }
00037 
00038     // write 0000 0000 = 0x00 to accelerometer control register 1 to place
00039     // FXOS8700CQ into standby
00040     // [7-1] = 0000 000
00041     // [0]: active=0
00042     data = 0x00;
00043     send_byte_to_reg(data,FXOS8700CQ_CTRL_REG1);
00044 
00045     // write 0001 1111 = 0x1F to magnetometer control register 1
00046     // [7]: m_acal=0: auto calibration disabled
00047     // [6]: m_rst=0: no one-shot magnetic reset
00048     // [5]: m_ost=0: no one-shot magnetic measurement
00049     // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise
00050     // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active
00051     data = 0x1F;
00052     send_byte_to_reg(data,FXOS8700CQ_M_CTRL_REG1);
00053 
00054     // write 0010 0000 = 0x20 to magnetometer control register 2
00055     // [7]: reserved
00056     // [6]: reserved
00057     // [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow
00058     // the accelerometer registers
00059     // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
00060     // [3]: m_maxmin_dis_ths=0
00061     // [2]: m_maxmin_rst=0
00062     // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle
00063     data = 0x20;
00064     send_byte_to_reg(data,FXOS8700CQ_M_CTRL_REG2);
00065 
00066     // write 0000 0001= 0x01 to XYZ_DATA_CFG register
00067     // [7]: reserved
00068     // [6]: reserved
00069     // [5]: reserved
00070     // [4]: hpf_out=0
00071     // [3]: reserved
00072     // [2]: reserved
00073     // [1-0]: fs=01 for accelerometer range of +/-4g range with 0.488mg/LSB
00074     data = 0x01;
00075     send_byte_to_reg(data,FXOS8700CQ_XYZ_DATA_CFG);
00076 
00077     // write 0000 1101 = 0x0D to accelerometer control register 1
00078     // [7-6]: aslp_rate=00
00079     // [5-3]: dr=001 for 200Hz data rate (when in hybrid mode)
00080     // [2]: lnoise=1 for low noise mode
00081     // [1]: f_read=0 for normal 16 bit reads
00082     // [0]: active=1 to take the part out of standby and enable sampling
00083     data = 0x0D;
00084     send_byte_to_reg(data,FXOS8700CQ_CTRL_REG1);
00085 
00086 }
00087 
00088 Data FXOS8700CQ::get_values()
00089 {
00090     // 13 bytes - status plus 6 channels (2 bytes each)
00091     // x,y,z for accelerometer and magnetometer
00092     char data[FXOS8700CQ_READ_LEN];
00093     read_bytes_from_reg(FXOS8700CQ_STATUS,FXOS8700CQ_READ_LEN,data);
00094 
00095     // copy the 14 bit accelerometer byte data into 16 bit words
00096     int acc_x = (int16_t)(((data[1] << 8) | data[2]))>> 2;
00097     int acc_y = (int16_t)(((data[3] << 8) | data[4]))>> 2;
00098     int acc_z = (int16_t)(((data[5] << 8) | data[6]))>> 2;
00099 
00100     // copy the magnetometer byte data into 16 bit words
00101     int mag_x = (int16_t) (data[7] << 8) | data[8];
00102     int mag_y = (int16_t) (data[9] << 8) | data[10];
00103     int mag_z = (int16_t) (data[11] << 8) | data[12];
00104 
00105     Data values;  // struct to hold values
00106 
00107     // 0.488 mg/LSB in 4 g mode (8.1 data sheet)
00108     values.ax = 0.488e-3*acc_x;
00109     values.ay = 0.488e-3*acc_y;
00110     values.az = 0.488e-3*acc_z;
00111 
00112     // the magnetometer sensitivity is fixed at 0.1 μT/LSB
00113     values.mx = 0.1e-6*mag_x;
00114     values.my = 0.1e-6*mag_y;
00115     values.mz = 0.1e-6*mag_z;
00116 
00117     return values;
00118 }
00119 
00120 void FXOS8700CQ::send_byte_to_reg(char byte,char reg)
00121 {
00122     char data[2];
00123     data[0] = reg;
00124     data[1] = byte;
00125     // send the register address, followed by the data
00126     int nack = i2c->write(FXOS8700CQ_ADDR,data,2);
00127     if (nack)
00128         error("No acknowledgement received!");  // if we don't receive acknowledgement, send error message
00129 
00130 }
00131 
00132 // reads a byte from a specific register
00133 char FXOS8700CQ::read_byte_from_reg(char reg)
00134 {
00135     int nack = i2c->write(FXOS8700CQ_ADDR,&reg,1,true);  // send the register address to the slave
00136     // true as need to send repeated start condition (5.10.1 datasheet)
00137     // http://www.i2c-bus.org/repeated-start-condition/
00138     if (nack)
00139         error("No acknowledgement received!");  // if we don't receive acknowledgement, send error message
00140 
00141     char rx;
00142     nack = i2c->read(FXOS8700CQ_ADDR,&rx,1);  // read a byte from the register and store in buffer
00143     if (nack)
00144         error("No acknowledgement received!");  // if we don't receive acknowledgement, send error message
00145 
00146     return rx;
00147 }
00148 
00149 // reads a series of bytes, starting from a specific register
00150 void FXOS8700CQ::read_bytes_from_reg(char reg,int number_of_bytes,char bytes[])
00151 {
00152     int nack = i2c->write(FXOS8700CQ_ADDR,&reg,1,true);  // send the slave write address and the configuration register address
00153     // true as need to send repeated start condition (5.10.1 datasheet)
00154     // http://www.i2c-bus.org/repeated-start-condition/
00155 
00156     if (nack)
00157         error("No acknowledgement received!");  // if we don't receive acknowledgement, send error message
00158 
00159     nack = i2c->read(FXOS8700CQ_ADDR,bytes,number_of_bytes);  // read bytes
00160     if (nack)
00161         error("No acknowledgement received!");  // if we don't receive acknowledgement, send error message
00162 
00163 }
00164 
00165 float FXOS8700CQ::get_pitch_angle()
00166 {
00167     Data values = get_values();
00168     float pitch_angle = atan2(-values.ax,sqrt(values.ay*values.ay + values.az*values.az))*180.0/3.1416;
00169     
00170     return pitch_angle;
00171 }
00172 
00173 float FXOS8700CQ::get_roll_angle()
00174 {
00175     Data values = get_values();
00176     float roll_angle = atan2(values.ay,values.az)*180.0/3.1416;
00177     
00178     return roll_angle;
00179 }
00180 
00181 Direction FXOS8700CQ::get_direction()
00182 {
00183     float pitch_angle = get_pitch_angle();  // -180 to 180 degree
00184     float roll_angle = get_roll_angle(); //-180 to 180 degree
00185 
00186     Direction d;
00187     // partition 360 into segments and check which segment the angle is in
00188     if ((pitch_angle <= 10) && (roll_angle <= 10) && (pitch_angle >= -10) && (roll_angle >= -10)) {
00189         d = CENTRE;   // check for -1.0 angle
00190     } else if ((pitch_angle > 10) && (roll_angle < 10) && (roll_angle > -10)) {  // then keep going in 45 degree increments
00191         d = N;
00192     } else if ((pitch_angle > 10) && (roll_angle < -10)) {
00193         d = NE;
00194     } else if ((pitch_angle < 10) && (roll_angle < -10) && (pitch_angle > -10)) {
00195         d = E;
00196     } else if ((pitch_angle < -10) && (roll_angle < -10)) {
00197         d = SE;
00198     } else if ((pitch_angle < -10) && (roll_angle < 10) && (roll_angle > -10)) {
00199         d = S;
00200     } else if ((pitch_angle < -10) && (roll_angle > 10)) {
00201         d = SW;
00202     } else if ((pitch_angle < 10) && (pitch_angle > -10) && (roll_angle > 10)) {
00203         d = W;
00204     } else if ((pitch_angle > 10) && (roll_angle > 10)) {
00205         d = NW;
00206     }
00207 
00208     return d;
00209 }
00210 
00211 float FXOS8700CQ::get_mag()
00212 {
00213     Data values = get_values();
00214     float mag = sqrt(values.ay*values.ay + values.az*values.az + values.ax*values.ax);
00215     return mag;
00216 }