diff --git a/Sensors.cpp b/Sensors.cpp index 88ea67a2..faba55b0 100644 --- a/Sensors.cpp +++ b/Sensors.cpp @@ -1117,6 +1117,43 @@ void Device_Mag_getADC() { } #endif #endif + + +// ************************************************************************************************************ +// I2C Compass QMC5883L +// ************************************************************************************************************ +// I2C adress: 0x0D (7bit) +// ************************************************************************************************************ +#if defined(QMC5883) + +#define MAG_ADDRESS 0x0D +#define MAG_DATA_REGISTER 0x00 + +//REG CONTROL + +#define MAG_CTRL_REG1 0x09 +#define QMC_MODE 0xC1 // 11 00 00 01OSR=64(11); Range=2G(00); ODR=10Hz(00); MODE=Cont(01) +#define MAG_CTRL_REG2 0x0A + +void Mag_init() { + i2c_writeReg(MAG_ADDRESS,0x0B,0x01); + i2c_writeReg(MAG_ADDRESS,MAG_CTRL_REG1,QMC_MODE); +} + + +static void getADC() { + i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER); + MAG_ORIENTATION( ((rawADC[1]<<8) | rawADC[0]) , + ((rawADC[3]<<8) | rawADC[2]) , + ((rawADC[5]<<8) | rawADC[4]) ); +} + +#if !defined(MPU6050_I2C_AUX_MASTER) +static void Device_Mag_getADC() { + getADC(); +} +#endif +#endif // ************************************************************************************************************ @@ -1217,6 +1254,11 @@ void ACC_getADC () { ((rawADC[2]<<8) | rawADC[3]) , ((rawADC[4]<<8) | rawADC[5]) ); #endif + #if defined(QMC5883) + MAG_ORIENTATION( ((rawADC[1]<<8) | rawADC[0]) , + ((rawADC[3]<<8) | rawADC[2]) , + ((rawADC[5]<<8) | rawADC[4]) ); + #endif } #endif #endif @@ -1536,4 +1578,4 @@ void initSensors() { initS(); if (i2c_errors_count == 0) break; // no error during init => init ok } -} +} diff --git a/config.h b/config.h index dffdfa1e..656c80bf 100644 --- a/config.h +++ b/config.h @@ -133,6 +133,7 @@ //#define GY_80 // Chinese 10 DOF with L3G4200D ADXL345 HMC5883L BMP085, LLC //#define GY_85 // Chinese 9 DOF with ITG3205 ADXL345 HMC5883L LLC //#define GY_86 // Chinese 10 DOF with MPU6050 HMC5883L MS5611, LLC + //#define GY_87 // Chinese 10 DOF with MPU6050 & QMC5883L //#define GY_88 // Chinese 10 DOF with MPU6050 HMC5883L BMP085, LLC //#define GY_521 // Chinese 6 DOF with MPU6050, LLC //#define INNOVWORKS_10DOF // with ITG3200, BMA180, HMC5883, BMP085 available here http://www.diymulticopter.com @@ -1228,4 +1229,4 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /*************************************************************************************************/ #endif /* CONFIG_H_ */ - + diff --git a/def.h b/def.h index 04a93dcb..f9b3ba2f 100644 --- a/def.h +++ b/def.h @@ -1329,6 +1329,18 @@ #undef INTERNAL_I2C_PULLUPS #endif +#if defined(GY_87) + //https://forum.arduino.cc/index.php?topic=556590.0 + #define MPU6050 + #define QMC5883 + #define BMP085 + #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = -X; imu.accADC[PITCH] = -Y; imu.accADC[YAW] = Z;} + #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = Y; imu.gyroADC[PITCH] = -X; imu.gyroADC[YAW] = -Z;} + #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = -Z;} + #define MPU6050_I2C_AUX_MASTER // MAG connected to the AUX I2C bus of MPU6050 + #undef INTERNAL_I2C_PULLUPS +#endif + #if defined(GY_88) #define MPU6050 #define HMC5883 @@ -1674,8 +1686,8 @@ #define ACC 0 #endif -#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) || defined(MAG3110) - #define MAG 1 +#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) || defined(MAG3110) || defined(QMC5883) + #define MAG 1 #else #define MAG 0 #endif @@ -2029,4 +2041,4 @@ #error "you use one feature that is no longer supported or has undergone a name change" #endif -#endif /* DEF_H_ */ +#endif /* DEF_H_ */