libreria aggiornata
LSM6DS0.cpp
- Committer:
- max_mbed
- Date:
- 2017-01-23
- Revision:
- 1:0b38c9475429
- Parent:
- 0:30cd7984fb5b
File content as of revision 1:0b38c9475429:
#include "LSM6DS0.h" Serial pcLSM(SERIAL_TX, SERIAL_RX); LSM6DS0::LSM6DS0(PinName sda, PinName scl) : i2c(sda, scl) { currentGyroRange = 0; currentAcceleroRange=0; gyro_offset.x = 0; gyro_offset.y = 0; gyro_offset.z = 0; } //-------------------------------------------------- //-------------------General------------------------ //-------------------------------------------------- int LSM6DS0::write(char addr_reg, char to_write) { char data_write[2]; data_write[0]=addr_reg; data_write[1]=to_write; return i2c.write(LSM6DS0_ADDRESS, data_write, 2); } int LSM6DS0::read(char addr_reg, char *byte_returned) { if(i2c.write(LSM6DS0_ADDRESS, &addr_reg, 1, true)==0){ return i2c.read(LSM6DS0_ADDRESS, byte_returned, 1); } else{ return 100; //ERROR 100 -> Error in writing pre reading } } int LSM6DS0::read(char address_first_reg, char *data, int length) { if(i2c.write(LSM6DS0_ADDRESS, &address_first_reg, 1, true)==0){ return i2c.read(LSM6DS0_ADDRESS , data, length); } else{ return 100; //ERROR 100 -> Error in writing pre reading } } bool LSM6DS0::testConnection( void ) { char temp; if(this->read(WHO_AM_I, &temp) == 0){ return !(temp == 0x68); } else{ return 101; //ERROR 101 -> Error in reading WHO_AM_I Register } } int LSM6DS0::turnOnAccelerometer(char Odr, char range, char bw){ char tmp = 0x00; Odr &= 0x07; range &= 0x03; bw &= 0x03; tmp = (Odr << ODR_XL); tmp |= (range << FS_XL); tmp |= (1 << BW_SCAL_ODR); tmp |= (bw << BW_XL); return this->write(CTRL_REG6_XL, tmp); //Return 0 if the operation successes! } int LSM6DS0::turnOnAccGyro(char Odr, char range, char bw){ char tmp = 0x00; Odr &= 0x07; //NB: controllare se è giusto sembra sbagliato range &= 0x03; bw &= 0x03; currentAcceleroRange=LSM6DS0_ACCELERO_RANGE_2G; currentGyroRange = range; tmp = (Odr << ODR_G); tmp |= (range << FS_G); tmp |= (bw << BW_G); return write(CTRL_REG1_G, tmp); //Return 0 on succes! } int LSM6DS0::turnOnTestSlow(void){ char tmp = 0b00100000; return this->write(CTRL_REG1_G, tmp); //Return 0 if the operation successes! } int LSM6DS0::turnOnTestFast(void){ bool state = 0; char tmp = 0b11000011 ; // ODR 952(massimo) LPF1 100 Hz (massimo) LPF2 100 Hz (cmq da disabilitare) state += this->write(CTRL_REG1_G, tmp); //Return 0 if the operation successes! tmp = 0b01000011 ; // HPF_G enable, HPF_G 4 Hz cutoff state += this->write(CTRL_REG3_G, tmp); //Return 0 if the operation successes! /* //-> troppo alto non va bene tmp = 0b11100010 ; // HPF_XL enable, HPF_G 952/400 Hz cutoff state += this->write(CTRL_REG7_XL, tmp); //Return 0 if the operation successes! */ return state; } /* Frequency in Hz */ int LSM6DS0::setI2CSpeed(int frequency){ i2c.frequency(frequency); // has no return value return 0; } int LSM6DS0::enableAcceleroInterrupt(){ char tmp1 = 0x00; char tmp2 = 0x00; char tmp3 = 0x00; tmp1 |= (1 << INT_DRDY_XL) | (1 << INT_FSS5); tmp2 |= (0 << AOI_XL) |(1 << ZHIE_XL); tmp3 |= (1 << IA_XL) |(1 << XL_XL); write(INT_CTRL,0b01010001); //write(0x23,0x02); //write(INT_GEN_CFG_XL, tmp2); //write(INT_GEN_SRC_XL, tmp3); return 0; } //-------------------------------------------------- //----------------Accelerometer--------------------- //-------------------------------------------------- char LSM6DS0::getAcceleroRange(void){ //For evaluation only, remove when complete everything! return currentAcceleroRange; } int LSM6DS0::getAcceleroRaw(int16_t *data) { char values[6]; if(this->read(OUT_X_XL, values, 6) == 0){ data[0] = (int16_t)((values[1]<<8) + values[0]); data[1] = (int16_t)((values[3]<<8) + values[2]); data[2] = (int16_t)((values[5]<<8) + values[4]); return 0; //Return 0 on success } else{ return 102; //ERROR 102 -> Error in reading Acceleration } } int LSM6DS0::getAccelero(Vector* v){ int16_t tmp[3]; if( this->getAcceleroRaw(tmp) == 0){ if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_2G){ /*v->x =(double)tmp[0]/(FULLSCALEBIT/2)*2*G; v->y =(double)tmp[1]/(FULLSCALEBIT/2)*2*G; v->z =(double)tmp[2]/(FULLSCALEBIT/2)*2*G;*/ v->x = (double)tmp[0] * 0.061e-3 * G; v->y = (double)tmp[1] * 0.061e-3 * G; v->z = (double)tmp[2] * 0.061e-3 * G; } if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_4G){ v->x=(double)tmp[0] * 0.122e-3 * G; v->y=(double)tmp[1] * 0.122e-3 * G; v->z=(double)tmp[2] * 0.122e-3 * G; } if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_8G){ v->x=(double)tmp[0] * 0.244e-3 * G; v->y=(double)tmp[1] * 0.244e-3 * G; v->z=(double)tmp[2] * 0.244e-3 * G; } if (currentAcceleroRange == LSM6DS0_ACCELERO_RANGE_16G){ v->x=(double)tmp[0] * 0.732e-3 * G; v->y=(double)tmp[1] * 0.732e-3 * G; v->z=(double)tmp[2] * 0.732e-3 * G; } return 0; //RETURN 0 on success } else{ return 103; //ERROR 103 -> Error in Reading the acceleration } } //-------------------------------------------------- //------------------Gyroscope----------------------- //-------------------------------------------------- int LSM6DS0::getGyroRaw(int16_t *data) { char tmp[6]; if(this->read(OUT_X_G, tmp, 6) == 0){ data[0] = (int16_t)(short)((tmp[1]<<8) + tmp[0]); data[1] = (int16_t)(short)((tmp[3]<<8) + tmp[2]); data[2] = (int16_t)(short)((tmp[5]<<8) + tmp[4]); return 0; } else{ return 105; //ERROR 105 -> Error in reading Gyroscope } } int LSM6DS0::getGyro(Vector* v, bool after_calibration) { int16_t tmp[3]; if( this->getGyroRaw(tmp) == 0){ if (currentGyroRange == LSM6DS0_GYRO_RANGE_245) { v->x=(double)tmp[0] * 8.75e-3; v->y=(double)tmp[1] * 8.75e-3; v->z=(double)tmp[2] * 8.75e-3; } if (currentGyroRange == LSM6DS0_GYRO_RANGE_500){ v->x=(double)tmp[0] * 17.50e-3; v->y=(double)tmp[1] * 17.50e-3; v->z=(double)tmp[2] * 17.50e-3; } if (currentGyroRange == LSM6DS0_GYRO_RANGE_2000){ v->x=(double)tmp[0] * 70e-3; v->y=(double)tmp[1] * 70e-3; v->z=(double)tmp[2] * 70e-3; } if(after_calibration){ v->x -= gyro_offset.x; v->y -= gyro_offset.y; v->z -= gyro_offset.z; } return 0; } else{ return 104; //ERROR 104 -> Error in reading Accelero! } } int LSM6DS0::getAccGyro(Vector *acc, Vector *gyro, bool after_gyro_calibration){ if(this->getAccelero(acc) == 0) { return getGyro(gyro, after_gyro_calibration); } else{ return 107; //ERROR 107 -> Error in reading Accelerometer! } } int LSM6DS0::gyroCalibration(int max_iteration){ Vector v; gyro_offset.x =0; gyro_offset.y =0; gyro_offset.z =0; for(int i = 0; i < max_iteration; i++){ if(this->getGyro(&v,false)!= 0) return 108; //ERROR 108 -> Errore in reading gyroscope gyro_offset.x += v.x/max_iteration; gyro_offset.y += v.y/max_iteration; gyro_offset.z += v.z/max_iteration; } //pcLSM.printf("%f\t%f\t%f\r\n", gyro_offset.x,gyro_offset.y,gyro_offset.z); return 0; }