Ghiglione Max / LSM6DS0

libreria aggiornata

LSM6DS0.cpp@1:0b38c9475429, 2017-01-23 (annotated)

Committer:
max_mbed
Date:
Mon Jan 23 22:51:58 2017 +0000
Revision:
1:0b38c9475429
Parent:
0:30cd7984fb5b 
Tolto il passa alto all'accelerazione che come previsto da fastidio perch? troppo basso.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
max_mbed 0:30cd7984fb5b 1 #include "LSM6DS0.h"
max_mbed 0:30cd7984fb5b 2
max_mbed 0:30cd7984fb5b 3 Serial pcLSM(SERIAL_TX, SERIAL_RX);
max_mbed 0:30cd7984fb5b 4
max_mbed 0:30cd7984fb5b 5 LSM6DS0::LSM6DS0(PinName sda, PinName scl) : i2c(sda, scl) {
max_mbed 0:30cd7984fb5b 6 currentGyroRange = 0;
max_mbed 0:30cd7984fb5b 7 currentAcceleroRange=0;
max_mbed 0:30cd7984fb5b 8 gyro_offset.x = 0;
max_mbed 0:30cd7984fb5b 9 gyro_offset.y = 0;
max_mbed 0:30cd7984fb5b 10 gyro_offset.z = 0;
max_mbed 0:30cd7984fb5b 11 }
max_mbed 0:30cd7984fb5b 12
max_mbed 0:30cd7984fb5b 13 //--------------------------------------------------
max_mbed 0:30cd7984fb5b 14 //-------------------General------------------------
max_mbed 0:30cd7984fb5b 15 //--------------------------------------------------
max_mbed 0:30cd7984fb5b 16
max_mbed 0:30cd7984fb5b 17 int LSM6DS0::write(char addr_reg, char to_write) {
max_mbed 0:30cd7984fb5b 18 char data_write[2];
max_mbed 0:30cd7984fb5b 19 data_write[0]=addr_reg;
max_mbed 0:30cd7984fb5b 20 data_write[1]=to_write;
max_mbed 0:30cd7984fb5b 21 return i2c.write(LSM6DS0_ADDRESS, data_write, 2);
max_mbed 0:30cd7984fb5b 22 }
max_mbed 0:30cd7984fb5b 23
max_mbed 0:30cd7984fb5b 24 int LSM6DS0::read(char addr_reg, char *byte_returned) {
max_mbed 0:30cd7984fb5b 25 if(i2c.write(LSM6DS0_ADDRESS, &addr_reg, 1, true)==0){
max_mbed 0:30cd7984fb5b 26 return i2c.read(LSM6DS0_ADDRESS, byte_returned, 1);
max_mbed 0:30cd7984fb5b 27 }
max_mbed 0:30cd7984fb5b 28 else{
max_mbed 0:30cd7984fb5b 29 return 100; //ERROR 100 -> Error in writing pre reading
max_mbed 0:30cd7984fb5b 30 }
max_mbed 0:30cd7984fb5b 31 }
max_mbed 0:30cd7984fb5b 32
max_mbed 0:30cd7984fb5b 33 int LSM6DS0::read(char address_first_reg, char *data, int length) {
max_mbed 0:30cd7984fb5b 34 if(i2c.write(LSM6DS0_ADDRESS, &address_first_reg, 1, true)==0){
max_mbed 0:30cd7984fb5b 35 return i2c.read(LSM6DS0_ADDRESS , data, length);
max_mbed 0:30cd7984fb5b 36 }
max_mbed 0:30cd7984fb5b 37 else{
max_mbed 0:30cd7984fb5b 38 return 100; //ERROR 100 -> Error in writing pre reading
max_mbed 0:30cd7984fb5b 39 }
max_mbed 0:30cd7984fb5b 40 }
max_mbed 0:30cd7984fb5b 41
max_mbed 0:30cd7984fb5b 42 bool LSM6DS0::testConnection( void ) {
max_mbed 0:30cd7984fb5b 43 char temp;
max_mbed 0:30cd7984fb5b 44 if(this->read(WHO_AM_I, &temp) == 0){
max_mbed 0:30cd7984fb5b 45 return !(temp == 0x68);
max_mbed 0:30cd7984fb5b 46 }
max_mbed 0:30cd7984fb5b 47 else{
max_mbed 0:30cd7984fb5b 48 return 101; //ERROR 101 -> Error in reading WHO_AM_I Register
max_mbed 0:30cd7984fb5b 49 }
max_mbed 0:30cd7984fb5b 50 }
max_mbed 0:30cd7984fb5b 51
max_mbed 0:30cd7984fb5b 52 int LSM6DS0::turnOnAccelerometer(char Odr, char range, char bw){
max_mbed 0:30cd7984fb5b 53 char tmp = 0x00;
max_mbed 0:30cd7984fb5b 54 Odr &= 0x07;
max_mbed 0:30cd7984fb5b 55 range &= 0x03;
max_mbed 0:30cd7984fb5b 56 bw &= 0x03;
max_mbed 0:30cd7984fb5b 57
max_mbed 0:30cd7984fb5b 58 tmp = (Odr << ODR_XL);
max_mbed 0:30cd7984fb5b 59 tmp |= (range << FS_XL);
max_mbed 0:30cd7984fb5b 60 tmp |= (1 << BW_SCAL_ODR);
max_mbed 0:30cd7984fb5b 61 tmp |= (bw << BW_XL);
max_mbed 0:30cd7984fb5b 62
max_mbed 0:30cd7984fb5b 63 return this->write(CTRL_REG6_XL, tmp); //Return 0 if the operation successes!
max_mbed 0:30cd7984fb5b 64 }
max_mbed 0:30cd7984fb5b 65
max_mbed 0:30cd7984fb5b 66 int LSM6DS0::turnOnAccGyro(char Odr, char range, char bw){
max_mbed 0:30cd7984fb5b 67 char tmp = 0x00;
max_mbed 0:30cd7984fb5b 68 Odr &= 0x07; //NB: controllare se è giusto sembra sbagliato
max_mbed 0:30cd7984fb5b 69 range &= 0x03;
max_mbed 0:30cd7984fb5b 70 bw &= 0x03;
max_mbed 0:30cd7984fb5b 71 currentAcceleroRange=LSM6DS0_ACCELERO_RANGE_2G;
max_mbed 0:30cd7984fb5b 72 currentGyroRange = range;
max_mbed 0:30cd7984fb5b 73
max_mbed 0:30cd7984fb5b 74 tmp = (Odr << ODR_G);
max_mbed 0:30cd7984fb5b 75 tmp |= (range << FS_G);
max_mbed 0:30cd7984fb5b 76 tmp |= (bw << BW_G);
max_mbed 0:30cd7984fb5b 77 return write(CTRL_REG1_G, tmp); //Return 0 on succes!
max_mbed 0:30cd7984fb5b 78
max_mbed 0:30cd7984fb5b 79 }
max_mbed 0:30cd7984fb5b 80
max_mbed 0:30cd7984fb5b 81 int LSM6DS0::turnOnTestSlow(void){
max_mbed 0:30cd7984fb5b 82 char tmp = 0b00100000;
max_mbed 0:30cd7984fb5b 83
max_mbed 0:30cd7984fb5b 84 return this->write(CTRL_REG1_G, tmp); //Return 0 if the operation successes!
max_mbed 0:30cd7984fb5b 85 }
max_mbed 0:30cd7984fb5b 86
max_mbed 0:30cd7984fb5b 87 int LSM6DS0::turnOnTestFast(void){
max_mbed 0:30cd7984fb5b 88 bool state = 0;
max_mbed 0:30cd7984fb5b 89
max_mbed 0:30cd7984fb5b 90 char tmp = 0b11000011 ; // ODR 952(massimo) LPF1 100 Hz (massimo) LPF2 100 Hz (cmq da disabilitare)
max_mbed 0:30cd7984fb5b 91 state += this->write(CTRL_REG1_G, tmp); //Return 0 if the operation successes!
max_mbed 0:30cd7984fb5b 92
max_mbed 0:30cd7984fb5b 93 tmp = 0b01000011 ; // HPF_G enable, HPF_G 4 Hz cutoff
max_mbed 0:30cd7984fb5b 94 state += this->write(CTRL_REG3_G, tmp); //Return 0 if the operation successes!
max_mbed 0:30cd7984fb5b 95
max_mbed 1:0b38c9475429 96 /* //-> troppo alto non va bene
max_mbed 0:30cd7984fb5b 97 tmp = 0b11100010 ; // HPF_XL enable, HPF_G 952/400 Hz cutoff
max_mbed 0:30cd7984fb5b 98 state += this->write(CTRL_REG7_XL, tmp); //Return 0 if the operation successes!
max_mbed 1:0b38c9475429 99 */
max_mbed 0:30cd7984fb5b 100 return state;
max_mbed 0:30cd7984fb5b 101 }
max_mbed 0:30cd7984fb5b 102
max_mbed 0:30cd7984fb5b 103 /* Frequency in Hz */
max_mbed 0:30cd7984fb5b 104 int LSM6DS0::setI2CSpeed(int frequency){
max_mbed 0:30cd7984fb5b 105 i2c.frequency(frequency); // has no return value
max_mbed 0:30cd7984fb5b 106 return 0;
max_mbed 0:30cd7984fb5b 107 }
max_mbed 0:30cd7984fb5b 108
max_mbed 0:30cd7984fb5b 109 int LSM6DS0::enableAcceleroInterrupt(){
max_mbed 0:30cd7984fb5b 110 char tmp1 = 0x00;
max_mbed 0:30cd7984fb5b 111 char tmp2 = 0x00;
max_mbed 0:30cd7984fb5b 112 char tmp3 = 0x00;
max_mbed 0:30cd7984fb5b 113 tmp1 |= (1 << INT_DRDY_XL) |
max_mbed 0:30cd7984fb5b 114 (1 << INT_FSS5);
max_mbed 0:30cd7984fb5b 115 tmp2 |= (0 << AOI_XL)
max_mbed 0:30cd7984fb5b 116 |(1 << ZHIE_XL);
max_mbed 0:30cd7984fb5b 117 tmp3 |= (1 << IA_XL)
max_mbed 0:30cd7984fb5b 118 |(1 << XL_XL);
max_mbed 0:30cd7984fb5b 119 write(INT_CTRL,0b01010001);
max_mbed 0:30cd7984fb5b 120 //write(0x23,0x02);
max_mbed 0:30cd7984fb5b 121 //write(INT_GEN_CFG_XL, tmp2);
max_mbed 0:30cd7984fb5b 122 //write(INT_GEN_SRC_XL, tmp3);
max_mbed 0:30cd7984fb5b 123
max_mbed 0:30cd7984fb5b 124
max_mbed 0:30cd7984fb5b 125 return 0;
max_mbed 0:30cd7984fb5b 126 }
max_mbed 0:30cd7984fb5b 127
max_mbed 0:30cd7984fb5b 128
max_mbed 0:30cd7984fb5b 129 //--------------------------------------------------
max_mbed 0:30cd7984fb5b 130 //----------------Accelerometer---------------------
max_mbed 0:30cd7984fb5b 131 //--------------------------------------------------
max_mbed 0:30cd7984fb5b 132
max_mbed 0:30cd7984fb5b 133 char LSM6DS0::getAcceleroRange(void){ //For evaluation only, remove when complete everything!
max_mbed 0:30cd7984fb5b 134 return currentAcceleroRange;
max_mbed 0:30cd7984fb5b 135 }
max_mbed 0:30cd7984fb5b 136
max_mbed 0:30cd7984fb5b 137 int LSM6DS0::getAcceleroRaw(int16_t *data) {
max_mbed 0:30cd7984fb5b 138 char values[6];
max_mbed 0:30cd7984fb5b 139 if(this->read(OUT_X_XL, values, 6) == 0){
max_mbed 0:30cd7984fb5b 140 data[0] = (int16_t)((values[1]<<8) + values[0]);
max_mbed 0:30cd7984fb5b 141 data[1] = (int16_t)((values[3]<<8) + values[2]);
max_mbed 0:30cd7984fb5b 142 data[2] = (int16_t)((values[5]<<8) + values[4]);
max_mbed 0:30cd7984fb5b 143 return 0; //Return 0 on success
max_mbed 0:30cd7984fb5b 144 }
max_mbed 0:30cd7984fb5b 145 else{
max_mbed 0:30cd7984fb5b 146 return 102; //ERROR 102 -> Error in reading Acceleration
max_mbed 0:30cd7984fb5b 147 }
max_mbed 0:30cd7984fb5b 148 }
max_mbed 0:30cd7984fb5b 149
max_mbed 0:30cd7984fb5b 150 int LSM6DS0::getAccelero(Vector* v){
max_mbed 0:30cd7984fb5b 151 int16_t tmp[3];
max_mbed 0:30cd7984fb5b 152 if( this->getAcceleroRaw(tmp) == 0){
max_mbed 0:30cd7984fb5b 153 if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_2G){
max_mbed 0:30cd7984fb5b 154 /*v->x =(double)tmp[0]/(FULLSCALEBIT/2)*2*G;
max_mbed 0:30cd7984fb5b 155 v->y =(double)tmp[1]/(FULLSCALEBIT/2)*2*G;
max_mbed 0:30cd7984fb5b 156 v->z =(double)tmp[2]/(FULLSCALEBIT/2)*2*G;*/
max_mbed 0:30cd7984fb5b 157 v->x = (double)tmp[0] * 0.061e-3 * G;
max_mbed 0:30cd7984fb5b 158 v->y = (double)tmp[1] * 0.061e-3 * G;
max_mbed 0:30cd7984fb5b 159 v->z = (double)tmp[2] * 0.061e-3 * G;
max_mbed 0:30cd7984fb5b 160 }
max_mbed 0:30cd7984fb5b 161 if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_4G){
max_mbed 0:30cd7984fb5b 162 v->x=(double)tmp[0] * 0.122e-3 * G;
max_mbed 0:30cd7984fb5b 163 v->y=(double)tmp[1] * 0.122e-3 * G;
max_mbed 0:30cd7984fb5b 164 v->z=(double)tmp[2] * 0.122e-3 * G;
max_mbed 0:30cd7984fb5b 165 }
max_mbed 0:30cd7984fb5b 166 if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_8G){
max_mbed 0:30cd7984fb5b 167 v->x=(double)tmp[0] * 0.244e-3 * G;
max_mbed 0:30cd7984fb5b 168 v->y=(double)tmp[1] * 0.244e-3 * G;
max_mbed 0:30cd7984fb5b 169 v->z=(double)tmp[2] * 0.244e-3 * G;
max_mbed 0:30cd7984fb5b 170 }
max_mbed 0:30cd7984fb5b 171 if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_16G){
max_mbed 0:30cd7984fb5b 172 v->x=(double)tmp[0] * 0.732e-3 * G;
max_mbed 0:30cd7984fb5b 173 v->y=(double)tmp[1] * 0.732e-3 * G;
max_mbed 0:30cd7984fb5b 174 v->z=(double)tmp[2] * 0.732e-3 * G;
max_mbed 0:30cd7984fb5b 175 }
max_mbed 0:30cd7984fb5b 176 return 0; //RETURN 0 on success
max_mbed 0:30cd7984fb5b 177 }
max_mbed 0:30cd7984fb5b 178 else{
max_mbed 0:30cd7984fb5b 179 return 103; //ERROR 103 -> Error in Reading the acceleration
max_mbed 0:30cd7984fb5b 180 }
max_mbed 0:30cd7984fb5b 181 }
max_mbed 0:30cd7984fb5b 182
max_mbed 0:30cd7984fb5b 183 //--------------------------------------------------
max_mbed 0:30cd7984fb5b 184 //------------------Gyroscope-----------------------
max_mbed 0:30cd7984fb5b 185 //--------------------------------------------------
max_mbed 0:30cd7984fb5b 186 int LSM6DS0::getGyroRaw(int16_t *data) {
max_mbed 0:30cd7984fb5b 187 char tmp[6];
max_mbed 0:30cd7984fb5b 188 if(this->read(OUT_X_G, tmp, 6) == 0){
max_mbed 0:30cd7984fb5b 189 data[0] = (int16_t)(short)((tmp[1]<<8) + tmp[0]);
max_mbed 0:30cd7984fb5b 190 data[1] = (int16_t)(short)((tmp[3]<<8) + tmp[2]);
max_mbed 0:30cd7984fb5b 191 data[2] = (int16_t)(short)((tmp[5]<<8) + tmp[4]);
max_mbed 0:30cd7984fb5b 192 return 0;
max_mbed 0:30cd7984fb5b 193 }
max_mbed 0:30cd7984fb5b 194 else{
max_mbed 0:30cd7984fb5b 195 return 105; //ERROR 105 -> Error in reading Gyroscope
max_mbed 0:30cd7984fb5b 196 }
max_mbed 0:30cd7984fb5b 197 }
max_mbed 0:30cd7984fb5b 198
max_mbed 0:30cd7984fb5b 199 int LSM6DS0::getGyro(Vector* v, bool after_calibration) {
max_mbed 0:30cd7984fb5b 200 int16_t tmp[3];
max_mbed 0:30cd7984fb5b 201 if( this->getGyroRaw(tmp) == 0){
max_mbed 0:30cd7984fb5b 202 if (currentGyroRange == LSM6DS0_GYRO_RANGE_245) {
max_mbed 0:30cd7984fb5b 203 v->x=(double)tmp[0] * 8.75e-3;
max_mbed 0:30cd7984fb5b 204 v->y=(double)tmp[1] * 8.75e-3;
max_mbed 0:30cd7984fb5b 205 v->z=(double)tmp[2] * 8.75e-3;
max_mbed 0:30cd7984fb5b 206 }
max_mbed 0:30cd7984fb5b 207 if (currentGyroRange == LSM6DS0_GYRO_RANGE_500){
max_mbed 0:30cd7984fb5b 208 v->x=(double)tmp[0] * 17.50e-3;
max_mbed 0:30cd7984fb5b 209 v->y=(double)tmp[1] * 17.50e-3;
max_mbed 0:30cd7984fb5b 210 v->z=(double)tmp[2] * 17.50e-3;
max_mbed 0:30cd7984fb5b 211 }
max_mbed 0:30cd7984fb5b 212 if (currentGyroRange == LSM6DS0_GYRO_RANGE_2000){
max_mbed 0:30cd7984fb5b 213 v->x=(double)tmp[0] * 70e-3;
max_mbed 0:30cd7984fb5b 214 v->y=(double)tmp[1] * 70e-3;
max_mbed 0:30cd7984fb5b 215 v->z=(double)tmp[2] * 70e-3;
max_mbed 0:30cd7984fb5b 216 }
max_mbed 0:30cd7984fb5b 217 if(after_calibration){
max_mbed 0:30cd7984fb5b 218 v->x -= gyro_offset.x;
max_mbed 0:30cd7984fb5b 219 v->y -= gyro_offset.y;
max_mbed 0:30cd7984fb5b 220 v->z -= gyro_offset.z;
max_mbed 0:30cd7984fb5b 221 }
max_mbed 0:30cd7984fb5b 222 return 0;
max_mbed 0:30cd7984fb5b 223 }
max_mbed 0:30cd7984fb5b 224 else{
max_mbed 0:30cd7984fb5b 225 return 104; //ERROR 104 -> Error in reading Accelero!
max_mbed 0:30cd7984fb5b 226 }
max_mbed 0:30cd7984fb5b 227 }
max_mbed 0:30cd7984fb5b 228 int LSM6DS0::getAccGyro(Vector *acc, Vector *gyro, bool after_gyro_calibration){
max_mbed 0:30cd7984fb5b 229 if(this->getAccelero(acc) == 0) {
max_mbed 0:30cd7984fb5b 230 return getGyro(gyro, after_gyro_calibration);
max_mbed 0:30cd7984fb5b 231 }
max_mbed 0:30cd7984fb5b 232 else{
max_mbed 0:30cd7984fb5b 233 return 107; //ERROR 107 -> Error in reading Accelerometer!
max_mbed 0:30cd7984fb5b 234 }
max_mbed 0:30cd7984fb5b 235 }
max_mbed 0:30cd7984fb5b 236
max_mbed 0:30cd7984fb5b 237 int LSM6DS0::gyroCalibration(int max_iteration){
max_mbed 0:30cd7984fb5b 238 Vector v;
max_mbed 0:30cd7984fb5b 239 gyro_offset.x =0;
max_mbed 0:30cd7984fb5b 240 gyro_offset.y =0;
max_mbed 0:30cd7984fb5b 241 gyro_offset.z =0;
max_mbed 0:30cd7984fb5b 242 for(int i = 0; i < max_iteration; i++){
max_mbed 0:30cd7984fb5b 243 if(this->getGyro(&v,false)!= 0) return 108; //ERROR 108 -> Errore in reading gyroscope
max_mbed 0:30cd7984fb5b 244 gyro_offset.x += v.x/max_iteration;
max_mbed 0:30cd7984fb5b 245 gyro_offset.y += v.y/max_iteration;
max_mbed 0:30cd7984fb5b 246 gyro_offset.z += v.z/max_iteration;
max_mbed 0:30cd7984fb5b 247 }
max_mbed 0:30cd7984fb5b 248 //pcLSM.printf("%f\t%f\t%f\r\n", gyro_offset.x,gyro_offset.y,gyro_offset.z);
max_mbed 0:30cd7984fb5b 249 return 0;
max_mbed 0:30cd7984fb5b 250 }