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;	
}