From 69456ebd4349f3021856b531d9f319723981f00a Mon Sep 17 00:00:00 2001 From: Shantanu Bhadoria Date: Wed, 15 Mar 2017 09:21:47 +0800 Subject: [PATCH] Adding support for LSM303DLH and L3GD20 --- Sensors.cpp | 192 +++++++++++++++++---------- config.h | 85 ++++++------ def.h | 374 ++++++++++++++++++++++++++-------------------------- 3 files changed, 354 insertions(+), 297 deletions(-) diff --git a/Sensors.cpp b/Sensors.cpp index 88ea67a2..5ff8fcff 100644 --- a/Sensors.cpp +++ b/Sensors.cpp @@ -18,13 +18,13 @@ static void ACC_init(); // board orientation and setup // ************************************************************************************************************ //default board orientation -#if !defined(ACC_ORIENTATION) +#if !defined(ACC_ORIENTATION) #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = X; imu.accADC[PITCH] = Y; imu.accADC[YAW] = Z;} #endif -#if !defined(GYRO_ORIENTATION) +#if !defined(GYRO_ORIENTATION) #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = X; imu.gyroADC[PITCH] = Y; imu.gyroADC[YAW] = Z;} #endif -#if !defined(MAG_ORIENTATION) +#if !defined(MAG_ORIENTATION) #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = Z;} #endif @@ -155,7 +155,7 @@ void GYRO_Common() { static int32_t g[3]; uint8_t axis, tilt=0; - #if defined MMGYRO + #if defined MMGYRO // Moving Average Gyros by Magnetron1 //--------------------------------------------------- static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGTH]; @@ -196,7 +196,7 @@ void GYRO_Common() { #endif } - #ifdef MMGYRO + #ifdef MMGYRO mediaMobileGyroIDX = ++mediaMobileGyroIDX % conf.mmgyro; for (axis = 0; axis < 3; axis++) { imu.gyroADC[axis] -= gyroZero[axis]; @@ -210,7 +210,7 @@ void GYRO_Common() { imu.gyroADC[axis] -= gyroZero[axis]; //anti gyro glitch, limit the variation between two consecutive readings imu.gyroADC[axis] = constrain(imu.gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800); - #endif + #endif previousGyroADC[axis] = imu.gyroADC[axis]; } @@ -349,7 +349,7 @@ static struct { union {uint32_t val; uint8_t raw[4]; } up; //uncompensated P uint8_t state; uint32_t deadline; -} bmp085_ctx; +} bmp085_ctx; #define OSS 3 /* transform a series of bytes from big endian to little @@ -424,7 +424,7 @@ void i2c_BMP085_Calculate() { baroTemperature = (b5 * 10 + 8) >> 4; // in 0.01 degC (same as MS561101BA temperature) // Pressure calculations b6 = b5 - 4000; - x1 = (bmp085_ctx.b2 * (b6 * b6 >> 12)) >> 11; + x1 = (bmp085_ctx.b2 * (b6 * b6 >> 12)) >> 11; x2 = bmp085_ctx.ac2 * b6 >> 11; x3 = x1 + x2; tmp = bmp085_ctx.ac1; @@ -446,26 +446,26 @@ void Baro_init() { delay(10); i2c_BMP085_readCalibration(); delay(5); - i2c_BMP085_UT_Start(); + i2c_BMP085_UT_Start(); bmp085_ctx.deadline = currentTime+5000; } //return 0: no data available, no computation ; 1: new value available ; 2: no new value, but computation time uint8_t Baro_update() { // first UT conversion is started in init procedure - if (currentTime < bmp085_ctx.deadline) return 0; + if (currentTime < bmp085_ctx.deadline) return 0; bmp085_ctx.deadline = currentTime+6000; // 1.5ms margin according to the spec (4.5ms T convetion time) if (bmp085_ctx.state == 0) { - i2c_BMP085_UT_Read(); - i2c_BMP085_UP_Start(); - bmp085_ctx.state = 1; + i2c_BMP085_UT_Read(); + i2c_BMP085_UP_Start(); + bmp085_ctx.state = 1; Baro_Common(); bmp085_ctx.deadline += 21000; // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P convetion time with OSS=3) return 1; } else { - i2c_BMP085_UP_Read(); - i2c_BMP085_UT_Start(); - i2c_BMP085_Calculate(); - bmp085_ctx.state = 0; + i2c_BMP085_UP_Read(); + i2c_BMP085_UT_Start(); + i2c_BMP085_Calculate(); + bmp085_ctx.state = 0; return 2; } } @@ -478,7 +478,7 @@ uint8_t Baro_update() { // first UT conversion is started in i // specs are here: http://www.meas-spec.com/downloads/MS5611-01BA03.pdf // useful info on pages 7 -> 12 #if defined(MS561101BA) -#if !defined(MS561101BA_ADDRESS) +#if !defined(MS561101BA_ADDRESS) #define MS561101BA_ADDRESS 0x77 //CBR=0 0xEE I2C address when pin CSB is connected to LOW (GND) //#define MS561101BA_ADDRESS 0x76 //CBR=1 0xEC I2C address when pin CSB is connected to HIGH (VCC) #endif @@ -510,7 +510,7 @@ static void Baro_init() { //reset i2c_writeReg(MS561101BA_ADDRESS, MS561101BA_RESET, 0); delay(100); - + //read calibration data union {uint16_t val; uint8_t raw[2]; } data; for(uint8_t i=0;i<6;i++) { @@ -551,9 +551,9 @@ static void i2c_MS561101BA_Calculate() { float sens = ((uint32_t)ms561101ba_ctx.c[1] <<15) + ((dT * ms561101ba_ctx.c[3]) /((uint32_t)1<<8)); delt = (dT * ms561101ba_ctx.c[6])/((uint32_t)1<<23); baroTemperature = delt + 2000; - if (delt < 0) { // temperature lower than 20st.C + if (delt < 0) { // temperature lower than 20st.C delt *= 5 * delt; - off -= delt>>1; + off -= delt>>1; sens -= delt>>2; } baroPressure = (( (ms561101ba_ctx.up * sens ) /((uint32_t)1<<21)) - off)/((uint32_t)1<<15); @@ -570,7 +570,7 @@ uint8_t Baro_update() { // first UT conversion is start return 1; } if ((int16_t)(currentTime - ms561101ba_ctx.deadline)<0) return 0; // the initial timer is not initialized, but in any case, no more than 65ms to wait. - ms561101ba_ctx.deadline = currentTime+10000; // UT and UP conversion take 8.5ms so we do next reading after 10ms + ms561101ba_ctx.deadline = currentTime+10000; // UT and UP conversion take 8.5ms so we do next reading after 10ms if (ms561101ba_ctx.state == 0) { Baro_Common(); // moved here for less timecycle spike, goes after i2c_MS561101BA_Calculate rawValPointer = &ms561101ba_ctx.ut; @@ -587,7 +587,7 @@ uint8_t Baro_update() { // first UT conversion is start #endif // ************************************************************************************************************ -// I2C Accelerometer MMA7455 +// I2C Accelerometer MMA7455 // ************************************************************************************************************ #if defined(MMA7455) #if !defined(MMA7455_ADDRESS) @@ -610,7 +610,7 @@ void ACC_getADC () { #endif // ************************************************************************************************************ -// I2C Accelerometer MMA8451Q +// I2C Accelerometer MMA8451Q // ************************************************************************************************************ #if defined(MMA8451Q) @@ -637,7 +637,7 @@ void ACC_getADC () { #endif // ************************************************************************************************************ -// I2C Accelerometer ADXL345 +// I2C Accelerometer ADXL345 // ************************************************************************************************************ // I2C adress: 0x3A (8bit) 0x1D (7bit) // Resolution: 10bit (Full range - 14bit, but this is autoscaling 10bit ADC to the range +- 16g) @@ -648,7 +648,7 @@ void ACC_getADC () { // 4) bits b00001011 must be set on register 0x31 to select the data format (only once at the initialization) // ************************************************************************************************************ #if defined(ADXL345) -#if !defined(ADXL345_ADDRESS) +#if !defined(ADXL345_ADDRESS) #define ADXL345_ADDRESS 0x1D //#define ADXL345_ADDRESS 0x53 //WARNING: Conflicts with a Wii Motion plus! #endif @@ -673,7 +673,7 @@ void ACC_getADC () { // ************************************************************************************************************ // I2C Accelerometer BMA180 // ************************************************************************************************************ -// I2C adress: 0x80 (8bit) 0x40 (7bit) (SDO connection to VCC) +// I2C adress: 0x80 (8bit) 0x40 (7bit) (SDO connection to VCC) // I2C adress: 0x82 (8bit) 0x41 (7bit) (SDO connection to VDDIO) // Resolution: 14bit // @@ -687,7 +687,7 @@ void ACC_getADC () { // | xxxxxxxxxxxxx | 8G: 101 | xxxxxxxx | // ************************************************************************************************************ #if defined(BMA180) -#if !defined(BMA180_ADDRESS) +#if !defined(BMA180_ADDRESS) #define BMA180_ADDRESS 0x40 //#define BMA180_ADDRESS 0x41 #endif @@ -707,12 +707,12 @@ void ACC_init () { control = control & 0xFC; // save tco_z register control = control | 0x00; // set mode_config to 0 i2c_writeReg(BMA180_ADDRESS, 0x30, control); - delay(5); + delay(5); control = i2c_readReg(BMA180_ADDRESS, 0x35); control = control & 0xF1; // save offset_x and smp_skip register control = control | (0x05 << 1); // set range to 8G i2c_writeReg(BMA180_ADDRESS, 0x35, control); - delay(5); + delay(5); } void ACC_getADC () { @@ -729,7 +729,7 @@ void ACC_getADC () { // I2C Accelerometer BMA280 // ************************************************************************************************************ #if defined(BMA280) -#if !defined(BMA280_ADDRESS) +#if !defined(BMA280_ADDRESS) #define BMA280_ADDRESS 0x18 // SDO PIN on GND //#define BMA280_ADDRESS 0x19 // SDO PIN on Vddio #endif @@ -774,7 +774,7 @@ void ACC_init(){ control = control & 0xE0; // save bits 7,6,5 control = control | (0x02 << 3); // Range 8G (10) control = control | 0x00; // Bandwidth 25 Hz 000 - i2c_writeReg(0x38,0x14,control); + i2c_writeReg(0x38,0x14,control); } void ACC_getADC(){ @@ -793,7 +793,7 @@ void ACC_getADC(){ #define LIS3A 0x1D void ACC_init(){ - i2c_writeReg(LIS3A ,0x20 ,0xD7 ); // CTRL_REG1 1101 0111 Pwr on, 160Hz + i2c_writeReg(LIS3A ,0x20 ,0xD7 ); // CTRL_REG1 1101 0111 Pwr on, 160Hz i2c_writeReg(LIS3A ,0x21 ,0x50 ); // CTRL_REG2 0100 0000 Littl endian, 12 Bit, Boot } @@ -846,7 +846,7 @@ void ACC_getADC() { #endif // ************************************************************************************************************ -// I2C Gyroscope L3G4200D +// I2C Gyroscope L3G4200D // ************************************************************************************************************ #if defined(L3G4200D) #define L3G4200D_ADDRESS 0x69 @@ -869,6 +869,38 @@ void Gyro_getADC () { } #endif +// ************************************************************************************************************ +// I2C Gyroscope L3GD20 +// ************************************************************************************************************ +#if defined(L3GD20) +#define L3GD20_ADDRESS 0x6B +#define L3GD20_REG1 0x20 +#define L3GD20_REG4 0x23 +#define L3GD20_REG5 0x24 +void Gyro_init() { + delay(100); + /* Reset then switch to normal mode and enable all three channels */ + i2c_writeReg(L3GD20_ADDRESS ,L3GD20_REG1 ,0x00 ); + delay(5); + i2c_writeReg(L3GD20_ADDRESS ,L3GD20_REG1 ,0x8F ); + delay(5); + /* 2000 degrees per second with wire interface */ + i2c_writeReg(L3GD20_ADDRESS ,L3GD20_REG4 ,0x30 ); + delay(5); + /* Low pass filter enabled */ + i2c_writeReg(L3GD20_ADDRESS ,L3GD20_REG5 ,0x02 ); +} + +void Gyro_getADC () { + i2c_getSixRawADC(L3GD20_ADDRESS,0x80|0x28); + + GYRO_ORIENTATION( (rawADC[0] | (rawADC[1]<<8)), + (rawADC[2] | (rawADC[3]<<8)), + (rawADC[4] | (rawADC[5]<<8))); + GYRO_Common(); +} +#endif + // ************************************************************************************************************ // I2C Gyroscope ITG3200 / ITG3205 / ITG3050 / MPU3050 // ************************************************************************************************************ @@ -924,7 +956,7 @@ uint8_t Mag_getADC() { // return 1 when news values are available, 0 otherwise imu.magADC[axis] = imu.magADC[axis] * magGain[axis]; if (!f.CALIBRATE_MAG) imu.magADC[axis] -= global_conf.magZero[axis]; } - + if (f.CALIBRATE_MAG) { if (tCal == 0) // init mag calibration tCal = t; @@ -956,7 +988,7 @@ uint8_t Mag_getADC() { // return 1 when news values are available, 0 otherwise imu.magADC[ROLL] = ((imu.magADC[ROLL] - imu.magADC[PITCH])*7)/10; imu.magADC[PITCH] = temp; #endif - + return 1; } #endif @@ -983,13 +1015,37 @@ void Mag_init() { #if !defined(MPU6050_I2C_AUX_MASTER) void Device_Mag_getADC() { i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER); - MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , - ((rawADC[2]<<8) | rawADC[3]) , + MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , + ((rawADC[2]<<8) | rawADC[3]) , ((rawADC[4]<<8) | rawADC[5]) ); } #endif #endif +// ************************************************************************************************************ +// I2C Compass LSM303DLx +// ************************************************************************************************************ +// I2C adress: 0x1E (7bit) +// ************************************************************************************************************ +#if defined(LSM303DLx_MAG) +#define MAG_ADDRESS 0x1E +#define MAG_DATA_REGISTER 0x03 // Thats where the axis registers start +#define MAG_MR_REGISTER_M 0x02 + +void Mag_init() { + delay(100); + i2c_writeReg(MAG_ADDRESS,MAG_MR_REGISTER_M,0x00); //Automatic Magnetic Sensor Reset + delay(100); +} + +void Device_Mag_getADC() { + i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER); + MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , + ((rawADC[2]<<8) | rawADC[3]) , + ((rawADC[4]<<8) | rawADC[5]) ); +} +#endif + // ************************************************************************************************************ // I2C Compass HMC5883 @@ -1042,7 +1098,7 @@ static uint8_t bias_collect(uint8_t bias) { static void Mag_init() { bool bret=true; // Error indicator - // Note that the very first measurement after a gain change maintains the same gain as the previous setting. + // Note that the very first measurement after a gain change maintains the same gain as the previous setting. // The new gain setting is effective from the second measurement and on. i2c_writeReg(MAG_ADDRESS, HMC58X3_R_CONFB, 2 << 5); //Set the Gain i2c_writeReg(MAG_ADDRESS,HMC58X3_R_MODE, 1); @@ -1085,8 +1141,8 @@ void getADC() { ((rawADC[2]<<8) | rawADC[3]) , ((rawADC[4]<<8) | rawADC[5]) ); } - -void Mag_init() { + +void Mag_init() { delay(100); // force positiveBias i2c_writeReg(MAG_ADDRESS ,0x00 ,0x71 ); //Configuration Register A -- 0 11 100 01 num samples: 8 ; output rate: 15Hz ; positive bias @@ -1117,7 +1173,7 @@ void Device_Mag_getADC() { } #endif #endif - + // ************************************************************************************************************ // I2C Compass AK8975 @@ -1127,7 +1183,7 @@ void Device_Mag_getADC() { #if defined(AK8975) #define MAG_ADDRESS 0x0C #define MAG_DATA_REGISTER 0x03 - + void Mag_init() { delay(100); i2c_writeReg(MAG_ADDRESS,0x0a,0x01); //Start the first conversion @@ -1136,8 +1192,8 @@ void Device_Mag_getADC() { void Device_Mag_getADC() { i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER); - MAG_ORIENTATION( ((rawADC[1]<<8) | rawADC[0]) , - ((rawADC[3]<<8) | rawADC[2]) , + MAG_ORIENTATION( ((rawADC[1]<<8) | rawADC[0]) , + ((rawADC[3]<<8) | rawADC[2]) , ((rawADC[5]<<8) | rawADC[4]) ); //Start another meassurement i2c_writeReg(MAG_ADDRESS,0x0a,0x01); @@ -1207,14 +1263,14 @@ void ACC_getADC () { ((rawADC[2]<<8) | rawADC[3]) , ((rawADC[4]<<8) | rawADC[5]) ); #endif - #if defined (HMC5883) + #if defined (HMC5883) MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , ((rawADC[4]<<8) | rawADC[5]) , ((rawADC[2]<<8) | rawADC[3]) ); #endif #if defined (MAG3110) - MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , - ((rawADC[2]<<8) | rawADC[3]) , + MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) , + ((rawADC[2]<<8) | rawADC[3]) , ((rawADC[4]<<8) | rawADC[5]) ); #endif } @@ -1245,13 +1301,13 @@ void ACC_init () { i2c_writeReg(LSM330_ACC_ADDRESS ,0x20 ,0x37 ); // 25Hz //i2c_writeReg(LSM330_ACC_ADDRESS ,0x20 ,0x47 ); // 50Hz //i2c_writeReg(LSM330_ACC_ADDRESS ,0x20 ,0x57 ); // 100Hz - + delay(5); //i2c_writeReg(LSM330_ACC_ADDRESS ,0x23 ,0x08 ); // 2G //i2c_writeReg(LSM330_ACC_ADDRESS ,0x23 ,0x18 ); // 4G i2c_writeReg(LSM330_ACC_ADDRESS ,0x23 ,0x28 ); // 8G - //i2c_writeReg(LSM330_ACC_ADDRESS ,0x23 ,0x38 ); // 16G - + //i2c_writeReg(LSM330_ACC_ADDRESS ,0x23 ,0x38 ); // 16G + delay(5); i2c_writeReg(LSM330_ACC_ADDRESS,0x21,0x00);// no high-pass filter } @@ -1331,19 +1387,19 @@ void Gyro_getADC() { if (micros() < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state for (axis = 0; axis < 3; axis++) {imu.gyroADC[axis]=0;imu.accADC[axis]=0;} imu.accADC[YAW] = ACC_1G; - } + } // Wii Motion Plus Data if ( (rawADC[5]&0x03) == 0x02 ) { - // Assemble 14bit data + // Assemble 14bit data imu.gyroADC[ROLL] = - ( ((rawADC[5]>>2)<<8) | rawADC[2] ); //range: +/- 8192 imu.gyroADC[PITCH] = - ( ((rawADC[4]>>2)<<8) | rawADC[1] ); imu.gyroADC[YAW] = - ( ((rawADC[3]>>2)<<8) | rawADC[0] ); GYRO_Common(); // Check if slow bit is set and normalize to fast mode range - imu.gyroADC[ROLL] = (rawADC[3]&0x01) ? imu.gyroADC[ROLL]/5 : imu.gyroADC[ROLL]; //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification + imu.gyroADC[ROLL] = (rawADC[3]&0x01) ? imu.gyroADC[ROLL]/5 : imu.gyroADC[ROLL]; //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification imu.gyroADC[PITCH] = (rawADC[4]&0x02)>>1 ? imu.gyroADC[PITCH]/5 : imu.gyroADC[PITCH]; //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details) - imu.gyroADC[YAW] = (rawADC[3]&0x02)>>1 ? imu.gyroADC[YAW]/5 : imu.gyroADC[YAW]; // this step must be done after zero compensation + imu.gyroADC[YAW] = (rawADC[3]&0x02)>>1 ? imu.gyroADC[YAW]/5 : imu.gyroADC[YAW]; // this step must be done after zero compensation } } #endif @@ -1360,23 +1416,23 @@ void Gyro_getADC() { // the code will move new sonars to the next available sonar address in range of F0-FE so that another // sonar sensor can be added again. // Thus, add only 1 sonar sensor at a time, poweroff, then wire the next, power on, wait for flashing light and repeat -#if !defined(SRF08_DEFAULT_ADDRESS) +#if !defined(SRF08_DEFAULT_ADDRESS) #define SRF08_DEFAULT_ADDRESS (0xE0>>1) #endif -#if !defined(SRF08_RANGE_WAIT) +#if !defined(SRF08_RANGE_WAIT) #define SRF08_RANGE_WAIT 70000 // delay between Ping and Range Read commands (65ms is safe in any case) #endif -#if !defined(SRF08_RANGE_SLEEP) +#if !defined(SRF08_RANGE_SLEEP) #define SRF08_RANGE_SLEEP 5000 // sleep this long before starting another Ping #endif -#if !defined(SRF08_SENSOR_FIRST) +#if !defined(SRF08_SENSOR_FIRST) #define SRF08_SENSOR_FIRST (0xF0>>1) // the first sensor i2c address (after it has been moved) #endif -#if !defined(SRF08_MAX_SENSORS) +#if !defined(SRF08_MAX_SENSORS) #define SRF08_MAX_SENSORS 4 // maximum number of sensors we'll allow (can go up to 8) #endif @@ -1422,7 +1478,7 @@ uint16_t i2c_try_readReg(uint8_t add, uint8_t reg) { } uint8_t r = TWDR; i2c_stop(); - return r; + return r; } // read a 16bit unsigned int from the i2c bus @@ -1471,12 +1527,12 @@ void Sonar_update() { if ((int32_t)(currentTime - srf08_ctx.deadline)<0) return; srf08_ctx.deadline = currentTime; switch (srf08_ctx.state) { - case 0: + case 0: i2c_srf08_discover(); - if(srf08_ctx.sensors>0) srf08_ctx.state++; + if(srf08_ctx.sensors>0) srf08_ctx.state++; else srf08_ctx.deadline += 5000000; // wait 5 secs before trying search again break; - case 1: + case 1: srf08_ctx.current=0; srf08_ctx.state++; srf08_ctx.deadline += SRF08_RANGE_SLEEP; @@ -1488,7 +1544,7 @@ void Sonar_update() { srf08_ctx.state++; srf08_ctx.deadline += SRF08_RANGE_WAIT; break; - case 3: + case 3: srf08_ctx.range[srf08_ctx.current] = i2c_readReg16( SRF08_SENSOR_FIRST+srf08_ctx.current, SRF08_ECHO_RANGE); srf08_ctx.current++; if(srf08_ctx.current >= srf08_ctx.sensors) srf08_ctx.state=1; @@ -1500,14 +1556,14 @@ void Sonar_update() { srf08_ctx.state++; srf08_ctx.deadline += SRF08_RANGE_WAIT; break; - case 3: + case 3: srf08_ctx.range[srf08_ctx.current] = i2c_readReg16(SRF08_SENSOR_FIRST+srf08_ctx.current, SRF08_ECHO_RANGE); srf08_ctx.current++; if(srf08_ctx.current >= srf08_ctx.sensors) srf08_ctx.state=1; - else srf08_ctx.state=2; + else srf08_ctx.state=2; break; #endif - } + } sonarAlt = srf08_ctx.range[0]; // only one sensor considered for the moment } #else diff --git a/config.h b/config.h index dffdfa1e..85437883 100644 --- a/config.h +++ b/config.h @@ -151,7 +151,7 @@ //#define MEGAWAP_V2_STD // available here: http://www.multircshop.com <- confirmed by Alex //#define MEGAWAP_V2_ADV //#define HK_MultiWii_SE_V2 // Hobbyking board with MPU6050 + HMC5883L + BMP085 - //#define HK_MultiWii_328P // Also labeled "Hobbybro" on the back. ITG3205 + BMA180 + BMP085 + NMC5583L + DSM2 Connector (Spektrum Satellite) + //#define HK_MultiWii_328P // Also labeled "Hobbybro" on the back. ITG3205 + BMA180 + BMP085 + NMC5583L + DSM2 Connector (Spektrum Satellite) //#define RCNet_FC // RCNet FC with MPU6050 and MS561101BA http://www.rcnet.com //#define RCNet_FC_GPS // RCNet FC with MPU6050 + MS561101BA + HMC5883L + UBLOX GPS http://www.rcnet.com //#define FLYDU_ULTRA // MEGA+10DOF+MT3339 FC @@ -160,7 +160,7 @@ //#define MultiWii_32U4_SE_no_baro // Hextronik MultiWii_32U4_SE without the MS561101BA to free flash-memory for other functions //#define Flyduino9DOF // Flyduino 9DOF IMU MPU6050+HMC5883l //#define Nano_Plane // Multiwii Plane version with tail-front LSM330 sensor http://www.radiosait.ru/en/page_5324.html - + /*************************** independent sensors ********************************/ /* leave it commented if you already checked a specific board above */ /* I2C gyroscope */ @@ -171,7 +171,8 @@ //#define L3G4200D //#define MPU6050 //combo + ACC //#define LSM330 //combo + ACC - + //#define L3GD20 + /* I2C accelerometer */ //#define MMA7455 //#define ADXL345 @@ -191,6 +192,7 @@ //#define HMC5883 //#define AK8975 //#define MAG3110 + //#define LSM303DLx_MAG /* Sonar */ // for visualization purpose currently - no control code behind //#define SRF02 // use the Devantech SRF i2c sensors @@ -273,7 +275,7 @@ //#define FLAPPERONS AUX4 // Mix Flaps with Aileroins. #define FLAPPERON_EP { 1500, 1700 } // Endpooints for flaps on a 2 way switch else set {1020,2000} and program in radio. #define FLAPPERON_INVERT { -1, 1 } // Change direction om flapperons { Wing1, Wing2 } - + //#define FLAPS // Traditional Flaps on SERVO3. //#define FLAPSPEED 3 // Make flaps move slowm Higher value is Higher Speed. @@ -304,8 +306,8 @@ /* Servo mixing for heli 120 {Coll,Nick,Roll} */ #define SERVO_NICK { +10, -10, 0 } - #define SERVO_LEFT { +10, +5, +10 } - #define SERVO_RIGHT { +10, +5, -10 } + #define SERVO_LEFT { +10, +5, +10 } + #define SERVO_RIGHT { +10, +5, -10 } /* Limit Maximum controll for Roll & Nick in 0-100% */ #define CONTROL_RANGE { 100, 100 } // { ROLL,PITCH } @@ -316,7 +318,7 @@ /*********************** your individual mixing ***********************/ /* if you want to override an existing entry in the mixing table, you may want to avoid editing the - * mixTable() function for every version again and again. + * mixTable() function for every version again and again. * howto: http://www.multiwii.com/wiki/index.php?title=Config.h#Individual_Mixing */ //#define MY_PRIVATE_MIXING "filename.h" @@ -381,12 +383,12 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC //************************** // Defines that allow a "Bind" of a Spektrum or Compatible Remote Receiver (aka Satellite) via Configuration GUI. // Bind mode will be same as declared above, if your TX is capable. - // Ground, Power, and Signal must come from three adjacent pins. - // By default, these are Ground=4, Power=5, Signal=6. These pins are in a row on most MultiWii shield boards. Pins can be overriden below. - // Normally use 3.3V regulator is needed on the power pin!! If your satellite hangs during bind (blinks, but won't complete bind with a solid light), go direct 5V on all pins. + // Ground, Power, and Signal must come from three adjacent pins. + // By default, these are Ground=4, Power=5, Signal=6. These pins are in a row on most MultiWii shield boards. Pins can be overriden below. + // Normally use 3.3V regulator is needed on the power pin!! If your satellite hangs during bind (blinks, but won't complete bind with a solid light), go direct 5V on all pins. //************************** - // For Pro Mini, the connector for the Satellite that resides on the FTDI can be unplugged and moved to these three adjacent pins. - //#define SPEK_BIND //Un-Comment for Spektrum Satellie Bind Support. Code is ~420 bytes smaller without it. + // For Pro Mini, the connector for the Satellite that resides on the FTDI can be unplugged and moved to these three adjacent pins. + //#define SPEK_BIND //Un-Comment for Spektrum Satellie Bind Support. Code is ~420 bytes smaller without it. //#define SPEK_BIND_GROUND 4 //#define SPEK_BIND_POWER 5 //#define SPEK_BIND_DATA 6 @@ -444,7 +446,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC //#define A32U4ALLPINS /********************************** PWM Setup **********************************/ - /* activate all 6 hardware PWM outputs Motor 5 = D11 and 6 = D13. + /* activate all 6 hardware PWM outputs Motor 5 = D11 and 6 = D13. note: not possible on the sparkfun promicro (pin 11 & 13 are not broken out there) if activated: Motor 1-6 = 10-bit hardware PWM @@ -559,14 +561,14 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC Original idea by Kraut Rob, first implementation HAdrian */ //#define THROTTLE_ANGLE_CORRECTION 40 - + /*** HEADFREE : the copter can be controled by an absolute stick orientation, whatever the yaw orientation ***/ //#define HEADFREE - + /************************* Advanced Headfree Mode ********************/ - /* In Advanced Headfree mode when the copter is farther than ADV_HEADFREE_RANGE meters then - the bearing between home and copter position will become the control direction - IF copter come closer than ADV_HEADFREE_RANGE meters, then the control direction freezed to the + /* In Advanced Headfree mode when the copter is farther than ADV_HEADFREE_RANGE meters then + the bearing between home and copter position will become the control direction + IF copter come closer than ADV_HEADFREE_RANGE meters, then the control direction freezed to the bearing between home and copter at the point where it crosses the ADV_HEADFREE_RANGE meter distance first implementation by HAdrian, mods by EOSBandi */ @@ -583,7 +585,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC /*** FUNCTIONALITY TEMPORARY REMOVED ***/ /* Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks.*/ //#define AP_MODE 40 // Create a deadspan for GPS. - + /************************ Assisted AcroTrainer ************************************/ /* Train Acro with auto recovery. Value set the point where ANGLE_MODE takes over. Remember to activate ANGLE_MODE first!... @@ -592,16 +594,16 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC /******** Failsafe settings ********************/ - /* Failsafe check pulses on four main control channels CH1-CH4. If the pulse is missing or bellow 985us (on any of these four channels) + /* Failsafe check pulses on four main control channels CH1-CH4. If the pulse is missing or bellow 985us (on any of these four channels) the failsafe procedure is initiated. After FAILSAFE_DELAY time from failsafe detection, the level mode is on (if ACC is avaliable), PITCH, ROLL and YAW is centered and THROTTLE is set to FAILSAFE_THROTTLE value. You must set this value to descending about 1m/s or so - for best results. This value is depended from your configuration, AUW and some other params. Next, after FAILSAFE_OFF_DELAY the copter is disarmed, + for best results. This value is depended from your configuration, AUW and some other params. Next, after FAILSAFE_OFF_DELAY the copter is disarmed, and motors is stopped. If RC pulse coming back before reached FAILSAFE_OFF_DELAY time, after the small quard time the RC control is returned to normal. */ //#define FAILSAFE // uncomment to activate the failsafe function #define FAILSAFE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example #define FAILSAFE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example #define FAILSAFE_THROTTLE (MINTHROTTLE + 200) // (*) Throttle level used for landing - may be relative to MINTHROTTLE - as in this case - + #define FAILSAFE_DETECT_TRESHOLD 985 @@ -665,7 +667,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC note: only the RX PIN is used in case of NMEA mode, the GPS is not configured by multiwii in NMEA mode the GPS must be configured to output GGA and RMC NMEA sentences (which is generally the default conf for most GPS devices) at least 5Hz update rate. uncomment the first line to select the GPS serial port of the arduino */ - + //#define GPS_SERIAL 2 // should be 2 for flyduino v2. It's the serial port number on arduino MEGA // must be 0 for PRO_MINI (ex GPS_PRO_MINI) // note: Now a GPS can share MSP on the same port. The only constrain is to not use it simultaneously, and use the same port speed. @@ -673,13 +675,13 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC // avoid using 115200 baud because with 16MHz arduino the 115200 baudrate have more than 2% speed error (57600 have 0.8% error) #define GPS_BAUD 57600 // GPS_BAUD will override SERIALx_COM_SPEED for the selected port - /* GPS protocol + /* GPS protocol NMEA - Standard NMEA protocol GGA, GSA and RMC sentences are needed - UBLOX - U-Blox binary protocol, use the ublox config file (u-blox-config.ublox.txt) from the source tree + UBLOX - U-Blox binary protocol, use the ublox config file (u-blox-config.ublox.txt) from the source tree MTK_BINARY16 and MTK_BINARY19 - MTK3329 chipset based GPS with DIYDrones binary firmware (v1.6 or v1.9) With UBLOX and MTK_BINARY you don't have to use GPS_FILTERING in multiwii code !!! */ - + //#define NMEA //#define UBLOX //#define MTK_BINARY16 @@ -689,7 +691,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC /* I2C GPS device made with an independant arduino + GPS device including some navigation functions - contribution from EOSBandi http://code.google.com/p/i2c-gps-nav/ + contribution from EOSBandi http://code.google.com/p/i2c-gps-nav/ You have to use at least I2CGpsNav code r33 */ /* all functionnalities allowed by SERIAL_GPS are now available for I2C_GPS: all relevant navigation computations are gathered in the main FC */ @@ -698,7 +700,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC // If your I2C GPS board has Sonar support enabled //#define I2C_GPS_SONAR - /* indicate a valid GPS fix with at least 5 satellites by flashing the LED - Modified by MIS - Using stable LED (YELLOW on CRIUS AIO) led work as sat number indicator + /* indicate a valid GPS fix with at least 5 satellites by flashing the LED - Modified by MIS - Using stable LED (YELLOW on CRIUS AIO) led work as sat number indicator - No GPS FIX -> LED blink at speed of incoming GPS frames - Fix and sat no. bellow 5 -> LED off - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ... */ @@ -707,7 +709,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC //Enables the MSP_WP command set , which is used by WinGUI for displaying an setting up navigation //#define USE_MSP_WP - // HOME position is reset at every arm, uncomment it to prohibit it (you can set home position with GyroCalibration) + // HOME position is reset at every arm, uncomment it to prohibit it (you can set home position with GyroCalibration) //#define DONT_RESET_HOME_AT_ARM /* GPS navigation can control the heading */ @@ -729,7 +731,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e #define GPS_LEAD_FILTER //(**) // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency comment out to disable -// use it with NMEA gps only +// use it with NMEA gps only //#define GPS_FILTERING //(**) // if we are within this distance to a waypoint then we consider it reached (distance is in cm) @@ -782,7 +784,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /***************************** The type of LCD **********************************/ /* choice of LCD attached for configuration and telemetry, see notes below */ - //#define LCD_DUMMY // No Physical LCD attached. With this & LCD_CONF defined, TX sticks still work to set gains, by watching LED blink. + //#define LCD_DUMMY // No Physical LCD attached. With this & LCD_CONF defined, TX sticks still work to set gains, by watching LED blink. //#define LCD_SERIAL3W // Alex' initial variant with 3 wires, using rx-pin for transmission @9600 baud fixed //#define LCD_TEXTSTAR // SERIAL LCD: Cat's Whisker LCD_TEXTSTAR Module CW-LCD-02 (Which has 4 input keys for selecting menus) //#define LCD_VT100 // SERIAL LCD: vt100 compatible terminal emulation (blueterm, putty, etc.) @@ -840,7 +842,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /*********************** LCD telemetry **************************/ /**************************************************************************************/ - /* to monitor system values (battery level, loop time etc. with LCD + /* to monitor system values (battery level, loop time etc. with LCD * http://www.multiwii.com/wiki/index.php?title=LCD_Telemetry */ /******************************** Activation ***********************************/ @@ -881,7 +883,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /**** TELEMETRY ****/ /********************************************************************/ // select one of the two protocols depending on your receiver - //#define FRSKY_TELEMETRY // used for FRSKY twoway receivers with telemetry (D-series like D8R-II or D8R-XP) + //#define FRSKY_TELEMETRY // used for FRSKY twoway receivers with telemetry (D-series like D8R-II or D8R-XP) // VBAT, Baro, MAG, GPS and POWERMETER are helpful // VBAT_CELLS is optional for a forth screen on the display FLD-02 //#define SPORT_TELEMETRY // for FRSKY twoway receivers with S.PORT telemetry (S-series like X4R/X6R/X8R), not implemented yet - TO BE DONE @@ -894,7 +896,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e //#define OPENTX // send OpenTX specific data // FRSKY standard telemetry specific selections - //#define COORDFORMAT_DECIMALMINUTES // uncomment to get the format DD°MM.mmmm for the coordinates - comment out to get the format DD.dddddd° for the coordinates + //#define COORDFORMAT_DECIMALMINUTES // uncomment to get the format DD°MM.mmmm for the coordinates - comment out to get the format DD.dddddd° for the coordinates //#define KILOMETER_HOUR // send speed in kilometers per hour instead of knots (default) - requested by OPENTX #define TELEMETRY_ALT_BARO // send BARO based altitude, calibrated to 0 when arming, recommended if BARO available //#define TELEMETRY_ALT_GPS // send GPS based altitude (altitude above see level), for FLD-02 don't use together with TELEMETRY_ALT_BARO @@ -1034,7 +1036,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e //#define EXT_MOTOR_32KHZ //#define EXT_MOTOR_4KHZ //#define EXT_MOTOR_1KHZ - + // for 32u4 proc //#define EXT_MOTOR_64KHZ //#define EXT_MOTOR_32KHZ @@ -1062,8 +1064,8 @@ Also note, that maqgnetic declination changes with time, so recheck your value e for use with digital servos dont use it with analog servos! thay may get damage. (some will work but be careful) */ //#define SERVO_RFR_300HZ - - /*********************** HW PWM Servos ***********************/ + + /*********************** HW PWM Servos ***********************/ /* HW PWM Servo outputs for Arduino Mega.. moves: Pitch = pin 44 Roll = pin 45 @@ -1072,10 +1074,10 @@ Also note, that maqgnetic declination changes with time, so recheck your value e SERVO5 = pin 12 (aileron right for fixed wing) SERVO6 = pin 6 (rudder for fixed wing) SERVO7 = pin 7 (elevator for fixed wing) - SERVO8 = pin 8 (motor for fixed wing) */ + SERVO8 = pin 8 (motor for fixed wing) */ #define MEGA_HW_PWM_SERVOS - + /* HW PWM Servo outputs for 32u4 NanoWii, MicroWii etc. - works with either the variable SERVO_RFR_RATE or * one of the 3 fixed servo.refresh.rates * * Tested only for heli_120, i.e. 1 motor + 4 servos, moves.. @@ -1118,7 +1120,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e * reset in GUI will not work on PIDs */ //#define SUPPRESS_DEFAULTS_FROM_GUI - + //#define DISABLE_SETTINGS_TAB // Saves ~400bytes on ProMini /********************************************************************/ @@ -1188,7 +1190,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /********************************************************************/ /**** Dynamic Motor/Prop Balancing ****/ /********************************************************************/ - /* !!! No Fly Mode !!! */ + /* !!! No Fly Mode !!! */ //#define DYNBALANCE // (**) Dynamic balancing controlled from Gui @@ -1228,4 +1230,3 @@ 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..4e60858d 100644 --- a/def.h +++ b/def.h @@ -218,12 +218,12 @@ #define NUMBER_MOTOR 1 #define PRI_SERVO_TO 7 #endif - #if defined(FLAPS) - #define PRI_SERVO_FROM 3 // use servo from 3 to 8 + #if defined(FLAPS) + #define PRI_SERVO_FROM 3 // use servo from 3 to 8 #undef CAMTRIG // Disable Camtrig on A2 #else #define PRI_SERVO_FROM 4 // use servo from 4 to 8 - #endif + #endif #elif defined(BI) #define NUMBER_MOTOR 2 #define PRI_SERVO_FROM 5 // use servo from 5 to 6 @@ -281,7 +281,7 @@ #define LEDPIN_OFF PORTB &= ~(1<<5); #define LEDPIN_ON PORTB |= (1<<5); #endif - #if !defined(RCAUXPIN8) + #if !defined(RCAUXPIN8) #if !defined(MONGOOSE1_0) #define BUZZERPIN_PINMODE DDRB |= 1; // Arduino pin 8 #if NUMBER_MOTOR >4 @@ -293,7 +293,7 @@ #else #define BUZZERPIN_ON PORTB |= 1; #define BUZZERPIN_OFF PORTB &= ~1; - #endif + #endif #endif #else #define BUZZERPIN_PINMODE ; @@ -322,7 +322,7 @@ #define STABLEPIN_PINMODE ; #define STABLEPIN_ON ; #define STABLEPIN_OFF ; - #endif + #endif #define PPM_PIN_INTERRUPT attachInterrupt(0, rxInt, RISING); //PIN 0 #define RX_SERIAL_PORT 0 //RX PIN assignment inside the port //for PORTD @@ -332,9 +332,9 @@ #define YAWPIN 6 #define AUX1PIN 7 #define AUX2PIN 0 // optional PIN 8 or PIN 12 - #define AUX3PIN 1 // unused - #define AUX4PIN 3 // unused - + #define AUX3PIN 1 // unused + #define AUX4PIN 3 // unused + #define PCINT_PIN_COUNT 5 #define PCINT_RX_BITS (1<<2),(1<<4),(1<<5),(1<<6),(1<<7) #define PCINT_RX_PORT PORTD @@ -344,12 +344,12 @@ #define RX_PCINT_PIN_PORT PIND #define V_BATPIN A3 // Analog PIN 3 #define PSENSORPIN A2 // Analog PIN 2 - + #if defined(A0_A1_PIN_HEX) || (NUMBER_MOTOR > 6) #define SOFT_PWM_1_PIN_HIGH PORTC |= 1<<0; #define SOFT_PWM_1_PIN_LOW PORTC &= ~(1<<0); #define SOFT_PWM_2_PIN_HIGH PORTC |= 1<<1; - #define SOFT_PWM_2_PIN_LOW PORTC &= ~(1<<1); + #define SOFT_PWM_2_PIN_LOW PORTC &= ~(1<<1); #else #define SOFT_PWM_1_PIN_HIGH PORTD |= 1<<5; #define SOFT_PWM_1_PIN_LOW PORTD &= ~(1<<5); @@ -360,7 +360,7 @@ #define SOFT_PWM_3_PIN_LOW PORTC &= ~(1<<2); #define SOFT_PWM_4_PIN_HIGH PORTB |= 1<<4; #define SOFT_PWM_4_PIN_LOW PORTB &= ~(1<<4); - + #define SERVO_1_PINMODE DDRC |= 1<<0; // pin A0 // TILT_PITCH - WING left #define SERVO_1_PIN_HIGH PORTC |= 1<<0; #define SERVO_1_PIN_LOW PORTC &= ~(1<<0); @@ -392,14 +392,14 @@ /************************** atmega32u4 (Promicro) ***********************************/ #if defined(PROMICRO) #if defined(MICROWII) - #define A32U4ALLPINS + #define A32U4ALLPINS #endif #if !defined(TEENSY20) #define LEDPIN_PINMODE // #define LEDPIN_TOGGLE PIND |= 1<<5; //switch LEDPIN state (Port D5) #if !defined(PROMICRO10) #define LEDPIN_OFF PORTD |= (1<<5); - #define LEDPIN_ON PORTD &= ~(1<<5); + #define LEDPIN_ON PORTD &= ~(1<<5); #else #define LEDPIN_OFF PORTD &= ~(1<<5); #define LEDPIN_ON PORTD |= (1<<5); @@ -407,8 +407,8 @@ #else #define LEDPIN_PINMODE DDRD |= (1<<6); #define LEDPIN_OFF PORTD &= ~(1<<6); - #define LEDPIN_ON PORTD |= (1<<6); - #define LEDPIN_TOGGLE PIND |= 1<<6; //switch LEDPIN state (Port D6) + #define LEDPIN_ON PORTD |= (1<<6); + #define LEDPIN_TOGGLE PIND |= 1<<6; //switch LEDPIN state (Port D6) #endif #if defined(D8BUZZER) #define BUZZERPIN_PINMODE DDRB |= (1<<4); @@ -417,9 +417,9 @@ #define PL_PIN_OFF PORTB &= ~(1<<4); #else #define BUZZERPIN_ON PORTB |= 1<<4; - #define BUZZERPIN_OFF PORTB &= ~(1<<4); - #endif - + #define BUZZERPIN_OFF PORTB &= ~(1<<4); + #endif + #elif defined(A32U4ALLPINS) #define BUZZERPIN_PINMODE DDRD |= (1<<4); #if defined PILOTLAMP @@ -427,8 +427,8 @@ #define PL_PIN_OFF PORTD &= ~(1<<4); #else #define BUZZERPIN_ON PORTD |= 1<<4; - #define BUZZERPIN_OFF PORTD &= ~(1<<4); - #endif + #define BUZZERPIN_OFF PORTD &= ~(1<<4); + #endif #else #define BUZZERPIN_PINMODE DDRD |= (1<<3); #if defined PILOTLAMP @@ -436,7 +436,7 @@ #define PL_PIN_OFF PORTD &= ~(1<<3); #else #define BUZZERPIN_ON PORTD |= 1<<3; - #define BUZZERPIN_OFF PORTD &= ~(1<<3); + #define BUZZERPIN_OFF PORTD &= ~(1<<3); #endif #endif #define POWERPIN_PINMODE // @@ -456,8 +456,8 @@ #endif #define USB_CDC_TX 3 #define USB_CDC_RX 2 - - //soft PWM Pins + + //soft PWM Pins #define SOFT_PWM_1_PIN_HIGH PORTD |= 1<<4; #define SOFT_PWM_1_PIN_LOW PORTD &= ~(1<<4); #define SOFT_PWM_2_PIN_HIGH PORTF |= 1<<5; @@ -467,17 +467,17 @@ #define SOFT_PWM_3_PIN_LOW PORTF &= ~(1<<7); #define SOFT_PWM_4_PIN_HIGH PORTF |= 1<<6; #define SOFT_PWM_4_PIN_LOW PORTF &= ~(1<<6); - #define SW_PWM_P3 A1 + #define SW_PWM_P3 A1 #define SW_PWM_P4 A0 #else #define SOFT_PWM_3_PIN_HIGH PORTF |= 1<<4; #define SOFT_PWM_3_PIN_LOW PORTF &= ~(1<<4); #define SOFT_PWM_4_PIN_HIGH PORTF |= 1<<5; - #define SOFT_PWM_4_PIN_LOW PORTF &= ~(1<<5); - #define SW_PWM_P3 A2 - #define SW_PWM_P4 A3 + #define SOFT_PWM_4_PIN_LOW PORTF &= ~(1<<5); + #define SW_PWM_P3 A2 + #define SW_PWM_P4 A3 #endif - + // Servos #define SERVO_1_PINMODE DDRF |= (1<<7); // A0 #define SERVO_1_PIN_HIGH PORTF|= 1<<7; @@ -495,7 +495,7 @@ #else #define SERVO_4_PINMODE DDRF |= (1<<4); // A3 #define SERVO_4_PIN_HIGH PORTF |= 1<<4; - #define SERVO_4_PIN_LOW PORTF &= ~(1<<4); + #define SERVO_4_PIN_LOW PORTF &= ~(1<<4); #endif #define SERVO_5_PINMODE DDRC |= (1<<6); // 5 #define SERVO_5_PIN_HIGH PORTC|= 1<<6; @@ -509,7 +509,7 @@ #define SERVO_8_PINMODE DDRB |= (1<<5); // 9 #define SERVO_8_PIN_HIGH PORTB |= 1<<5; #define SERVO_8_PIN_LOW PORTB &= ~(1<<5); - + //Standart RX #define THROTTLEPIN 3 #if defined(A32U4ALLPINS) @@ -523,9 +523,9 @@ #define YAWPIN 2 #define AUX1PIN 6 #endif - #define AUX2PIN 7 - #define AUX3PIN 1 // unused - #define AUX4PIN 0 // unused + #define AUX2PIN 7 + #define AUX3PIN 1 // unused + #define AUX4PIN 0 // unused #if !defined(RCAUX2PIND17) #define PCINT_PIN_COUNT 4 #define PCINT_RX_BITS (1<<1),(1<<2),(1<<3),(1<<4) @@ -547,9 +547,9 @@ #define V_BATPIN A2 // Analog PIN 3 #endif #if !defined(TEENSY20) - #define PSENSORPIN A2 // Analog PIN 2 + #define PSENSORPIN A2 // Analog PIN 2 #else - #define PSENSORPIN A2 // Analog PIN 2 + #define PSENSORPIN A2 // Analog PIN 2 #endif #endif @@ -566,8 +566,8 @@ #else #define BUZZERPIN_ON PORTC |= 1<<5; #define BUZZERPIN_OFF PORTC &= ~(1<<5); - #endif - + #endif + #if !defined(DISABLE_POWER_PIN) #define POWERPIN_PINMODE DDRC |= 1<<0; // Arduino pin 37 #define POWERPIN_ON PORTC |= 1<<0; @@ -612,7 +612,7 @@ #define PCIR_PORT_BIT (1<<2) #define RX_PC_INTERRUPT PCINT2_vect #define RX_PCINT_PIN_PORT PINK - + #define SERVO_1_PINMODE DDRC |= 1<<3;DDRL |= 1<<5; // Arduino pin 34 , pin 44 // TILT_PITCH - WING left #define SERVO_1_PIN_HIGH PORTC |= 1<<3;PORTL |= 1<<5; #define SERVO_1_PIN_LOW PORTC &= ~(1<<3);PORTL &= ~(1<<5); @@ -641,7 +641,7 @@ #endif -// special defines for the Mongose IMU board +// special defines for the Mongose IMU board // note: that may be moved to the IMU Orientations because this are board defines .. not Proc #if defined(MONGOOSE1_0) // basically it's a PROMINI without some PINS => same code as a PROMINI board except PIN definition @@ -649,8 +649,8 @@ // http://www.multiwii.com/forum/viewtopic.php?f=6&t=627 #define LEDPIN_PINMODE pinMode (4, OUTPUT); #define LEDPIN_TOGGLE PIND |= 1<<4; //switch LEDPIN state (digital PIN 13) - #define LEDPIN_OFF PORTD &= ~(1<<4); - #define LEDPIN_ON PORTD |= (1<<4); + #define LEDPIN_OFF PORTD &= ~(1<<4); + #define LEDPIN_ON PORTD |= (1<<4); #define SPEK_BAUD_SET UCSR0A = (1<> 8; UBRR0L = ((F_CPU / 4 / 115200 -1) / 2); #define RX_SERIAL_PORT 0 @@ -663,12 +663,12 @@ #define POWERPIN_OFF ; #define STABLEPIN_PINMODE ; // #define STABLEPIN_ON ; - #define STABLEPIN_OFF ; + #define STABLEPIN_OFF ; #define PINMODE_LCD ; // #define LCDPIN_OFF ; - #define LCDPIN_ON ; - - + #define LCDPIN_ON ; + + #define SERVO_4_PINMODE ; // Not available #define SERVO_4_PIN_HIGH ; #define SERVO_4_PIN_LOW ; @@ -684,43 +684,43 @@ #define SERVO_1_LOW SERVO_1_PIN_LOW #define SERVO_1_ARR_POS 0 #endif -#if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2) +#if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2) #undef LAST_LOW #define LAST_LOW SERVO_2_PIN_LOW #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_2_PIN_HIGH - #define SERVO_1_LOW SERVO_2_PIN_LOW + #define SERVO_1_LOW SERVO_2_PIN_LOW #define SERVO_1_ARR_POS 1 #else #define SERVO_2_HIGH SERVO_2_PIN_HIGH - #define SERVO_2_LOW SERVO_2_PIN_LOW + #define SERVO_2_LOW SERVO_2_PIN_LOW #define SERVO_2_ARR_POS 1 #endif #endif -#if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3) +#if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3) #undef LAST_LOW #define LAST_LOW SERVO_3_PIN_LOW #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_3_PIN_HIGH #define SERVO_1_LOW SERVO_3_PIN_LOW - #define SERVO_1_ARR_POS 2 + #define SERVO_1_ARR_POS 2 #elif !defined(SERVO_2_HIGH) #define SERVO_2_HIGH SERVO_3_PIN_HIGH - #define SERVO_2_LOW SERVO_3_PIN_LOW - #define SERVO_2_ARR_POS 2 + #define SERVO_2_LOW SERVO_3_PIN_LOW + #define SERVO_2_ARR_POS 2 #else #define SERVO_3_HIGH SERVO_3_PIN_HIGH - #define SERVO_3_LOW SERVO_3_PIN_LOW - #define SERVO_3_ARR_POS 2 + #define SERVO_3_LOW SERVO_3_PIN_LOW + #define SERVO_3_ARR_POS 2 #endif #endif -#if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4) +#if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4) #undef LAST_LOW #define LAST_LOW SERVO_4_PIN_LOW #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_4_PIN_HIGH #define SERVO_1_LOW SERVO_4_PIN_LOW - #define SERVO_1_ARR_POS 3 + #define SERVO_1_ARR_POS 3 #elif !defined(SERVO_2_HIGH) #define SERVO_2_HIGH SERVO_4_PIN_HIGH #define SERVO_2_LOW SERVO_4_PIN_LOW @@ -728,11 +728,11 @@ #elif !defined(SERVO_3_HIGH) #define SERVO_3_HIGH SERVO_4_PIN_HIGH #define SERVO_3_LOW SERVO_4_PIN_LOW - #define SERVO_3_ARR_POS 3 + #define SERVO_3_ARR_POS 3 #else #define SERVO_4_HIGH SERVO_4_PIN_HIGH - #define SERVO_4_LOW SERVO_4_PIN_LOW - #define SERVO_4_ARR_POS 3 + #define SERVO_4_LOW SERVO_4_PIN_LOW + #define SERVO_4_ARR_POS 3 #endif #endif #if (PRI_SERVO_FROM <= 5 && PRI_SERVO_TO >= 5) || (SEC_SERVO_FROM <= 5 && SEC_SERVO_TO >= 5) @@ -741,23 +741,23 @@ #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_5_PIN_HIGH #define SERVO_1_LOW SERVO_5_PIN_LOW - #define SERVO_1_ARR_POS 4 + #define SERVO_1_ARR_POS 4 #elif !defined(SERVO_2_HIGH) #define SERVO_2_HIGH SERVO_5_PIN_HIGH #define SERVO_2_LOW SERVO_5_PIN_LOW - #define SERVO_2_ARR_POS 4 + #define SERVO_2_ARR_POS 4 #elif !defined(SERVO_3_HIGH) #define SERVO_3_HIGH SERVO_5_PIN_HIGH #define SERVO_3_LOW SERVO_5_PIN_LOW - #define SERVO_3_ARR_POS 4 + #define SERVO_3_ARR_POS 4 #elif !defined(SERVO_4_HIGH) #define SERVO_4_HIGH SERVO_5_PIN_HIGH #define SERVO_4_LOW SERVO_5_PIN_LOW - #define SERVO_4_ARR_POS 4 + #define SERVO_4_ARR_POS 4 #else #define SERVO_5_HIGH SERVO_5_PIN_HIGH - #define SERVO_5_LOW SERVO_5_PIN_LOW - #define SERVO_5_ARR_POS 4 + #define SERVO_5_LOW SERVO_5_PIN_LOW + #define SERVO_5_ARR_POS 4 #endif #endif #if (PRI_SERVO_FROM <= 6 && PRI_SERVO_TO >= 6) || (SEC_SERVO_FROM <= 6 && SEC_SERVO_TO >= 6) @@ -765,28 +765,28 @@ #define LAST_LOW SERVO_6_PIN_LOW #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_6_PIN_HIGH - #define SERVO_1_LOW SERVO_6_PIN_LOW - #define SERVO_1_ARR_POS 5 + #define SERVO_1_LOW SERVO_6_PIN_LOW + #define SERVO_1_ARR_POS 5 #elif !defined(SERVO_2_HIGH) #define SERVO_2_HIGH SERVO_6_PIN_HIGH #define SERVO_2_LOW SERVO_6_PIN_LOW - #define SERVO_2_ARR_POS 5 + #define SERVO_2_ARR_POS 5 #elif !defined(SERVO_3_HIGH) #define SERVO_3_HIGH SERVO_6_PIN_HIGH #define SERVO_3_LOW SERVO_6_PIN_LOW - #define SERVO_3_ARR_POS 5 + #define SERVO_3_ARR_POS 5 #elif !defined(SERVO_4_HIGH) #define SERVO_4_HIGH SERVO_6_PIN_HIGH - #define SERVO_4_LOW SERVO_6_PIN_LOW - #define SERVO_4_ARR_POS 5 + #define SERVO_4_LOW SERVO_6_PIN_LOW + #define SERVO_4_ARR_POS 5 #elif !defined(SERVO_5_HIGH) #define SERVO_5_HIGH SERVO_6_PIN_HIGH - #define SERVO_5_LOW SERVO_6_PIN_LOW - #define SERVO_5_ARR_POS 5 + #define SERVO_5_LOW SERVO_6_PIN_LOW + #define SERVO_5_ARR_POS 5 #else #define SERVO_6_HIGH SERVO_6_PIN_HIGH - #define SERVO_6_LOW SERVO_6_PIN_LOW - #define SERVO_6_ARR_POS 5 + #define SERVO_6_LOW SERVO_6_PIN_LOW + #define SERVO_6_ARR_POS 5 #endif #endif #if (PRI_SERVO_FROM <= 7 && PRI_SERVO_TO >= 7) || (SEC_SERVO_FROM <= 7 && SEC_SERVO_TO >= 7) @@ -794,41 +794,41 @@ #define LAST_LOW SERVO_7_PIN_LOW #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_7_PIN_HIGH - #define SERVO_1_LOW SERVO_7_PIN_LOW - #define SERVO_1_ARR_POS 6 + #define SERVO_1_LOW SERVO_7_PIN_LOW + #define SERVO_1_ARR_POS 6 #elif !defined(SERVO_2_HIGH) #define SERVO_2_HIGH SERVO_7_PIN_HIGH #define SERVO_2_LOW SERVO_7_PIN_LOW - #define SERVO_2_ARR_POS 6 + #define SERVO_2_ARR_POS 6 #elif !defined(SERVO_3_HIGH) #define SERVO_3_HIGH SERVO_7_PIN_HIGH #define SERVO_3_LOW SERVO_7_PIN_LOW - #define SERVO_3_ARR_POS 6 + #define SERVO_3_ARR_POS 6 #elif !defined(SERVO_4_HIGH) #define SERVO_4_HIGH SERVO_7_PIN_HIGH - #define SERVO_4_LOW SERVO_7_PIN_LOW - #define SERVO_4_ARR_POS 6 + #define SERVO_4_LOW SERVO_7_PIN_LOW + #define SERVO_4_ARR_POS 6 #elif !defined(SERVO_5_HIGH) #define SERVO_5_HIGH SERVO_7_PIN_HIGH - #define SERVO_5_LOW SERVO_7_PIN_LOW - #define SERVO_5_ARR_POS 6 + #define SERVO_5_LOW SERVO_7_PIN_LOW + #define SERVO_5_ARR_POS 6 #elif !defined(SERVO_6_HIGH) #define SERVO_6_HIGH SERVO_7_PIN_HIGH - #define SERVO_6_LOW SERVO_7_PIN_LOW - #define SERVO_6_ARR_POS 6 + #define SERVO_6_LOW SERVO_7_PIN_LOW + #define SERVO_6_ARR_POS 6 #else #define SERVO_7_HIGH SERVO_7_PIN_HIGH - #define SERVO_7_LOW SERVO_7_PIN_LOW - #define SERVO_7_ARR_POS 6 + #define SERVO_7_LOW SERVO_7_PIN_LOW + #define SERVO_7_ARR_POS 6 #endif #endif -#if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8) +#if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8) #undef LAST_LOW #define LAST_LOW SERVO_8_PIN_LOW #if !defined(SERVO_1_HIGH) #define SERVO_1_HIGH SERVO_8_PIN_HIGH - #define SERVO_1_LOW SERVO_8_PIN_LOW - #define SERVO_1_ARR_POS 7 + #define SERVO_1_LOW SERVO_8_PIN_LOW + #define SERVO_1_ARR_POS 7 #elif !defined(SERVO_2_HIGH) #define SERVO_2_HIGH SERVO_8_PIN_HIGH #define SERVO_2_LOW SERVO_8_PIN_LOW @@ -836,27 +836,27 @@ #elif !defined(SERVO_3_HIGH) #define SERVO_3_HIGH SERVO_8_PIN_HIGH #define SERVO_3_LOW SERVO_8_PIN_LOW - #define SERVO_3_ARR_POS 7 + #define SERVO_3_ARR_POS 7 #elif !defined(SERVO_4_HIGH) #define SERVO_4_HIGH SERVO_8_PIN_HIGH #define SERVO_4_LOW SERVO_8_PIN_LOW - #define SERVO_4_ARR_POS 7 + #define SERVO_4_ARR_POS 7 #elif !defined(SERVO_5_HIGH) #define SERVO_5_HIGH SERVO_8_PIN_HIGH - #define SERVO_5_LOW SERVO_8_PIN_LOW - #define SERVO_5_ARR_POS 7 + #define SERVO_5_LOW SERVO_8_PIN_LOW + #define SERVO_5_ARR_POS 7 #elif !defined(SERVO_6_HIGH) #define SERVO_6_HIGH SERVO_8_PIN_HIGH - #define SERVO_6_LOW SERVO_8_PIN_LOW - #define SERVO_6_ARR_POS 7 + #define SERVO_6_LOW SERVO_8_PIN_LOW + #define SERVO_6_ARR_POS 7 #elif !defined(SERVO_7_HIGH) #define SERVO_7_HIGH SERVO_8_PIN_HIGH - #define SERVO_7_LOW SERVO_8_PIN_LOW - #define SERVO_7_ARR_POS 7 + #define SERVO_7_LOW SERVO_8_PIN_LOW + #define SERVO_7_ARR_POS 7 #else #define SERVO_8_HIGH SERVO_8_PIN_HIGH - #define SERVO_8_LOW SERVO_8_PIN_LOW - #define SERVO_8_ARR_POS 7 + #define SERVO_8_LOW SERVO_8_PIN_LOW + #define SERVO_8_ARR_POS 7 #endif #endif @@ -964,7 +964,7 @@ #define SOFT_PWM_3_PIN_LOW PORTD &= ~(1<<4); #define SOFT_PWM_4_PIN_HIGH PORTF |= 1<<5; #define SOFT_PWM_4_PIN_LOW PORTF &= ~(1<<5); - #define SW_PWM_P3 4 + #define SW_PWM_P3 4 #define SW_PWM_P4 A2 #define HWPWM6 // move servo 3 & 4 to pin 13 & 11 @@ -989,7 +989,7 @@ #define LEDPIN_PINMODE DDRD |= (1<<4); //D4 to output #define LEDPIN_TOGGLE PIND |= (1<<5)|(1<<4); //switch LEDPIN state (Port D5) & pin D4 #define LEDPIN_OFF PORTD |= (1<<5); PORTD &= ~(1<<4); - #define LEDPIN_ON PORTD &= ~(1<<5); PORTD |= (1<<4); + #define LEDPIN_ON PORTD &= ~(1<<5); PORTD |= (1<<4); #endif #endif @@ -1053,7 +1053,7 @@ #define GPS_SERIAL 2 #define VENUS8 #define USE_MSP_WP - + #define LED1PIN_ON PORTB |= (1<<7); #define LED1PIN_OFF PORTB &= ~(1<<7); #define LED2PIN_ON PORTC |= (1<<7); @@ -1386,7 +1386,7 @@ #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] = -Y; imu.magADC[PITCH] = X; imu.magADC[YAW] = -Z;} + #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = -Y; imu.magADC[PITCH] = X; imu.magADC[YAW] = -Z;} #endif #if defined(Bobs_6DOF_V1) @@ -1419,44 +1419,44 @@ #undef INTERNAL_IC2_PULLUPS #endif -#if defined(HK_MultiWii_SE_V2 ) - #define MPU6050 - #define HMC5883 - #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_EN_I2C_BYPASS // MAG connected to the AUX I2C bus of MPU6050 - #undef INTERNAL_I2C_PULLUPS +#if defined(HK_MultiWii_SE_V2 ) + #define MPU6050 + #define HMC5883 + #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_EN_I2C_BYPASS // MAG connected to the AUX I2C bus of MPU6050 + #undef INTERNAL_I2C_PULLUPS #endif -#if defined(HK_MultiWii_328P ) +#if defined(HK_MultiWii_328P ) #define ITG3200 #define BMA180 - #define HMC5883 - #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;} - #undef INTERNAL_I2C_PULLUPS -#endif - -#if defined(CRIUS_AIO_PRO) - #define MPU6050 - #define HMC5883 - #define MS561101BA - #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 + #define HMC5883 + #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;} + #undef INTERNAL_I2C_PULLUPS +#endif + +#if defined(CRIUS_AIO_PRO) + #define MPU6050 + #define HMC5883 + #define MS561101BA + #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 #define I2C_SPEED 400000L //400kHz fast mode //servo pins on AIO board is at pins 44,45,46, then release pins 33,34,35 for other usage //eg. pin 33 on AIO can be used for LEDFLASHER output #define SERVO_1_PINMODE pinMode(44,OUTPUT); // TILT_PITCH #define SERVO_1_PIN_HIGH PORTL |= 1<<5; #define SERVO_1_PIN_LOW PORTL &= ~(1<<5); - #define SERVO_2_PINMODE pinMode(45,OUTPUT); // TILT_ROLL + #define SERVO_2_PINMODE pinMode(45,OUTPUT); // TILT_ROLL #define SERVO_2_PIN_HIGH PORTL |= 1<<4; #define SERVO_2_PIN_LOW PORTL &= ~(1<<4); #define SERVO_3_PINMODE pinMode(46,OUTPUT); // CAM TRIG @@ -1481,7 +1481,7 @@ #define MOTOR_STOP #endif -#if defined(MEGAWAP_V2_STD) +#if defined(MEGAWAP_V2_STD) #define ITG3200 #define BMA180 #define HMC5883 @@ -1491,39 +1491,39 @@ #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = -Z;} #endif -#if defined(MEGAWAP_V2_ADV) - #define MPU6050 - #define HMC5883 - #define MS561101BA - #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_EN_I2C_BYPASS // MAG connected to the AUX I2C bus of MPU6050 - #undef INTERNAL_I2C_PULLUPS +#if defined(MEGAWAP_V2_ADV) + #define MPU6050 + #define HMC5883 + #define MS561101BA + #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_EN_I2C_BYPASS // MAG connected to the AUX I2C bus of MPU6050 + #undef INTERNAL_I2C_PULLUPS #endif #if defined(RCNet_FC_GPS) #define RCNet_FC #define HMC5883 - #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = -X; imu.magADC[PITCH] = Y; imu.magADC[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 + #undef INTERNAL_I2C_PULLUPS #define GPS_SERIAL 2 #define GPS_BAUD 115200 - #define UBLOX + #define UBLOX #endif -#if defined(RCNet_FC) - #define MPU6050 - #define MS561101BA +#if defined(RCNet_FC) + #define MPU6050 + #define MS561101BA #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;} - #undef INTERNAL_I2C_PULLUPS + #undef INTERNAL_I2C_PULLUPS //servo pins on RCNet FC board are at pins 38,39,40 #define SERVO_1_PINMODE pinMode(40,OUTPUT); // TILT_PITCH #define SERVO_1_PIN_HIGH PORTL |= 1<<5; #define SERVO_1_PIN_LOW PORTL &= ~(1<<5); - #define SERVO_2_PINMODE pinMode(39,OUTPUT); // TILT_ROLL + #define SERVO_2_PINMODE pinMode(39,OUTPUT); // TILT_ROLL #define SERVO_2_PIN_HIGH PORTL |= 1<<4; #define SERVO_2_PIN_LOW PORTL &= ~(1<<4); #define SERVO_3_PINMODE pinMode(38,OUTPUT); // CAM TRIG @@ -1536,11 +1536,11 @@ #define MMA8451Q #define MS561101BA #define MAG3110 - + #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] = -Y; imu.magADC[PITCH] = X; imu.magADC[YAW] = Z;} - + #define GPS_SERIAL 2 #define GPS_BAUD 57600 #define MTK_BINARY19 @@ -1591,47 +1591,47 @@ #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 ADXL345_ADDRESS 0x53 - + #define SDO_pin A0 - #define SDI_pin A1 - #define SCLK_pin A2 + #define SDI_pin A1 + #define SCLK_pin A2 #define IRQ_pin 2 #define nSel_pin 4 #define IRQ_interrupt 0 - + #define nIRQ_1 (PIND & 0x04)==0x04 //D2 #define nIRQ_0 (PIND & 0x04)==0x00 //D2 - + #define nSEL_on PORTD |= 0x10 //D4 #define nSEL_off PORTD &= 0xEF //D4 - + #define SCK_on PORTC |= 0x04 //C2 #define SCK_off PORTC &= 0xFB //C2 - + #define SDI_on PORTC |= 0x02 //C1 #define SDI_off PORTC &= 0xFD //C1 - + #define SDO_1 (PINC & 0x01) == 0x01 //C0 #define SDO_0 (PINC & 0x01) == 0x00 //C0 - + //#### Other interface pinouts ### #define GREEN_LED_pin 13 #define RED_LED_pin A3 #define Red_LED_ON PORTC |= _BV(3); #define Red_LED_OFF PORTC &= ~_BV(3); - + #define Green_LED_ON PORTB |= _BV(5); #define Green_LED_OFF PORTB &= ~_BV(5); - - #define NOP() __asm__ __volatile__("nop") - - #define RF22B_PWRSTATE_READY 01 - #define RF22B_PWRSTATE_TX 0x09 - #define RF22B_PWRSTATE_RX 05 - #define RF22B_Rx_packet_received_interrupt 0x02 - #define RF22B_PACKET_SENT_INTERRUPT 04 - #define RF22B_PWRSTATE_POWERDOWN 00 + + #define NOP() __asm__ __volatile__("nop") + + #define RF22B_PWRSTATE_READY 01 + #define RF22B_PWRSTATE_TX 0x09 + #define RF22B_PWRSTATE_RX 05 + #define RF22B_Rx_packet_received_interrupt 0x02 + #define RF22B_PACKET_SENT_INTERRUPT 04 + #define RF22B_PWRSTATE_POWERDOWN 00 #endif @@ -1674,13 +1674,13 @@ #define ACC 0 #endif -#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) || defined(MAG3110) +#if defined(HMC5883) || defined(HMC5843) || defined(AK8975) || defined(MAG3110) || defined(LSM303DLx_MAG) #define MAG 1 #else #define MAG 0 #endif -#if defined(ITG3200) || defined(ITG3050) || defined(L3G4200D) || defined(MPU6050) || defined(LSM330) || defined(MPU3050) || defined(WMP) +#if defined(ITG3200) || defined(ITG3050) || defined(L3G4200D) || defined(MPU6050) || defined(LSM330) || defined(MPU3050) || defined(WMP) || defined(L3GD20) #define GYRO 1 #else #define GYRO 0 @@ -1746,18 +1746,18 @@ #elif defined(HEX6X) #define MULTITYPE 10 #elif defined(OCTOX8) - #define MULTITYPE 11 //the JAVA GUI is the same for all 8 motor configs + #define MULTITYPE 11 //the JAVA GUI is the same for all 8 motor configs #elif defined(OCTOFLATP) #define MULTITYPE 12 //12 for MultiWinGui #elif defined(OCTOFLATX) - #define MULTITYPE 13 //13 for MultiWinGui + #define MULTITYPE 13 //13 for MultiWinGui #elif defined(AIRPLANE) - #define MULTITYPE 14 + #define MULTITYPE 14 #define SERVO_RATES {30,30,100,100,-100,100,100,100} -#elif defined (HELI_120_CCPM) - #define MULTITYPE 15 -#elif defined (HELI_90_DEG) - #define MULTITYPE 16 +#elif defined (HELI_120_CCPM) + #define MULTITYPE 15 +#elif defined (HELI_90_DEG) + #define MULTITYPE 16 #define SERVO_RATES {30,30,100,-100,-100,100,100,100} #elif defined(VTAIL4) #define MULTITYPE 17 @@ -1774,7 +1774,7 @@ /*************** Some unsorted "chain" defines ********************/ /**************************************************************************************/ -#if defined (AIRPLANE) || defined(HELICOPTER)|| defined(SINGLECOPTER)|| defined(DUALCOPTER) && defined(PROMINI) +#if defined (AIRPLANE) || defined(HELICOPTER)|| defined(SINGLECOPTER)|| defined(DUALCOPTER) && defined(PROMINI) #if defined(D12_POWER) #define SERVO_4_PINMODE ; // D12 #define SERVO_4_PIN_HIGH ; @@ -1796,7 +1796,7 @@ #define PLEVELDIV 36000 #endif -#if defined PILOTLAMP +#if defined PILOTLAMP #define PL_CHANNEL OCR0B //use B since A can be used by camstab #define PL_ISR TIMER0_COMPB_vect #define PL_INIT TCCR0A=0;TIMSK0|=(1<