FXOS8700CQ class modified for el17yfk
Embed:
(wiki syntax)
Show/hide line numbers
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,®,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,®,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 }
Generated on Sun Jul 24 2022 22:56:13 by
1.7.2