Ghiglione Max
libreria aggiornata
Diff: LSM6DS0.cpp
- Revision:
- 0:30cd7984fb5b
- Child:
- 1:0b38c9475429
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM6DS0.cpp Mon Jan 23 22:27:11 2017 +0000
@@ -0,0 +1,250 @@
+#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;
+}