diff options
Diffstat (limited to 'debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino')
| -rw-r--r-- | debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino | 159 |
1 files changed, 159 insertions, 0 deletions
diff --git a/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino b/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino new file mode 100644 index 0000000..5ca58e2 --- /dev/null +++ b/debug/IMU_comp/minimu-9-ahrs-arduino-master/MinIMU9AHRS/I2C.ino @@ -0,0 +1,159 @@ +/* + +MinIMU-9-Arduino-AHRS +Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) + +Copyright (c) 2011-2016 Pololu Corporation. +http://www.pololu.com/ + +MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: +http://code.google.com/p/sf9domahrs/ + +sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose +Julio and Doug Weibel: +http://code.google.com/p/ardu-imu/ + +MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it +under the terms of the GNU Lesser General Public License as published by the +Free Software Foundation, either version 3 of the License, or (at your option) +any later version. + +MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but +WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for +more details. + +You should have received a copy of the GNU Lesser General Public License along +with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#ifdef IMU_V5 + +#include <LSM6.h> +#include <LIS3MDL.h> + +LSM6 gyro_acc; +LIS3MDL mag; + +#else // older IMUs through v4 + +#include <L3G.h> +#include <LSM303.h> + +L3G gyro; +LSM303 compass; + +#endif + + +void I2C_Init() +{ + Wire.begin(); +} + +void Gyro_Init() +{ +#ifdef IMU_V5 + // Accel_Init() should have already called gyro_acc.init() and enableDefault() + gyro_acc.writeReg(LSM6::CTRL2_G, 0x4C); // 104 Hz, 2000 dps full scale +#else + gyro.init(); + gyro.enableDefault(); + gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale + gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz +#endif +} + +void Read_Gyro() +{ +#ifdef IMU_V5 + gyro_acc.readGyro(); + + AN[0] = gyro_acc.g.x; + AN[1] = gyro_acc.g.y; + AN[2] = gyro_acc.g.z; +#else + gyro.read(); + + AN[0] = gyro.g.x; + AN[1] = gyro.g.y; + AN[2] = gyro.g.z; +#endif + + gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]); + gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]); + gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]); +} + +void Accel_Init() +{ +#ifdef IMU_V5 + gyro_acc.init(); + gyro_acc.enableDefault(); + gyro_acc.writeReg(LSM6::CTRL1_XL, 0x3C); // 52 Hz, 8 g full scale +#else + compass.init(); + compass.enableDefault(); + switch (compass.getDeviceType()) + { + case LSM303::device_D: + compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011 + break; + case LSM303::device_DLHC: + compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode + break; + default: // DLM, DLH + compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11 + } +#endif +} + +// Reads x,y and z accelerometer registers +void Read_Accel() +{ +#ifdef IMU_V5 + gyro_acc.readAcc(); + + AN[3] = gyro_acc.a.x >> 4; // shift right 4 bits to use 12-bit representation (1 g = 256) + AN[4] = gyro_acc.a.y >> 4; + AN[5] = gyro_acc.a.z >> 4; +#else + compass.readAcc(); + + AN[3] = compass.a.x >> 4; // shift right 4 bits to use 12-bit representation (1 g = 256) + AN[4] = compass.a.y >> 4; + AN[5] = compass.a.z >> 4; +#endif + accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]); + accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]); + accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]); +} + +void Compass_Init() +{ +#ifdef IMU_V5 + mag.init(); + mag.enableDefault(); +#else + // LSM303: doesn't need to do anything because Accel_Init() should have already called compass.enableDefault() +#endif +} + +void Read_Compass() +{ +#ifdef IMU_V5 + mag.read(); + + magnetom_x = SENSOR_SIGN[6] * mag.m.x; + magnetom_y = SENSOR_SIGN[7] * mag.m.y; + magnetom_z = SENSOR_SIGN[8] * mag.m.z; +#else + compass.readMag(); + + magnetom_x = SENSOR_SIGN[6] * compass.m.x; + magnetom_y = SENSOR_SIGN[7] * compass.m.y; + magnetom_z = SENSOR_SIGN[8] * compass.m.z; +#endif +} + |