diff --git a/config.c b/config.c index 055b30b8a..e550377f1 100755 --- a/config.c +++ b/config.c @@ -5,7 +5,7 @@ #define FLASH_PAGE_SIZE ((uint16_t)0x400) #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage -static uint8_t checkNewConf = 150; +static uint8_t checkNewConf = 151; typedef struct eep_entry_t { void *var; @@ -121,9 +121,9 @@ void checkFirstTime(void) P8[YAW] = 85; I8[YAW] = 0; D8[YAW] = 0; - P8[PIDALT] = 47; - I8[PIDALT] = 0; - D8[PIDALT] = 30; + P8[PIDALT] = 16; + I8[PIDALT] = 15; + D8[PIDALT] = 7; P8[PIDGPS] = 10; I8[PIDGPS] = 0; D8[PIDGPS] = 0; diff --git a/drv_bmp085.c b/drv_bmp085.c index 202cfe745..b94033d40 100755 --- a/drv_bmp085.c +++ b/drv_bmp085.c @@ -130,7 +130,7 @@ bool bmp085Init(void) i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); p_bmp085->number_of_samples = 1; - p_bmp085->oversampling_setting = 3; + p_bmp085->oversampling_setting = 2; if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */ p_bmp085->sensortype = BOSCH_PRESSURE_BMP085; diff --git a/imu.c b/imu.c index a8e9a083e..911cd440d 100755 --- a/imu.c +++ b/imu.c @@ -6,9 +6,12 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; int16_t acc_25deg = 0; int32_t pressure = 0; -int16_t BaroAlt = 0; -int16_t EstAlt = 0; // in cm -int16_t zVelocity = 0; +int32_t BaroAlt; +int32_t EstAlt; // in cm +int16_t BaroPID = 0; +int32_t AltHold; +int16_t errorAltitudeI = 0; +int32_t EstVelocity; // ************** // gyro+acc IMU @@ -21,7 +24,6 @@ int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0. int8_t smallAngle25 = 1; static void getEstimatedAttitude(void); -static void getEstimatedAltitude(void); void imuInit(void) { @@ -39,20 +41,11 @@ void computeIMU(void) int16_t gyroADCp[3]; int16_t gyroADCinter[3]; static uint32_t timeInterleave = 0; -#if defined(TRI) static int16_t gyroYawSmooth = 0; -#endif - - if (sensors(SENSOR_MAG)) - Mag_getADC(); - if (sensors(SENSOR_BARO)) - Baro_update(); if (sensors(SENSOR_ACC)) { ACC_getADC(); getEstimatedAttitude(); - if (sensors(SENSOR_BARO)) - getEstimatedAltitude(); } Gyro_getADC(); @@ -293,58 +286,59 @@ int32_t isq(int32_t x) #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) #define INIT_DELAY 4000000 // 4 sec initialization delay +#define BARO_TAB_SIZE 40 #define Kp1 5.5f // PI observer velocity gain #define Kp2 10.0f // PI observer position gain #define Ki 0.01f // PI observer integral gain (bias cancellation) #define dt (UPDATE_INTERVAL / 1000000.0f) -static void getEstimatedAltitude(void) +void getEstimatedAltitude(void) { static uint8_t inited = 0; - static int16_t AltErrorI = 0; - static float AccScale; static uint32_t deadLine = INIT_DELAY; - int16_t AltError; - int16_t InstAcc; - static int32_t tmpAlt; - static int16_t EstVelocity = 0; - static uint32_t velTimer; - static int16_t lastAlt; + static int16_t BaroHistTab[BARO_TAB_SIZE]; + static int8_t BaroHistIdx = 0; + int32_t BaroHigh, BaroLow; + int32_t temp32; if (currentTime < deadLine) return; deadLine = currentTime + UPDATE_INTERVAL; - // Soft start if (!inited) { inited = 1; - tmpAlt = BaroAlt * 10; - AccScale = 100 * 9.80665f / acc_1G; + EstAlt = BaroAlt; } - // Estimation Error - AltError = BaroAlt - EstAlt; - AltErrorI += AltError; - AltErrorI = constrain(AltErrorI, -2500, +2500); - // Gravity vector correction and projection to the local Z - //InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI; -#if defined(TRUSTED_ACCZ) - InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + AltErrorI / 100; -#else - InstAcc = AltErrorI / 100; -#endif - // Integrators - tmpAlt += EstVelocity * (dt * dt) + (Kp2 * dt) * AltError; - EstVelocity += InstAcc + Kp1 * AltError; - EstVelocity = constrain(EstVelocity, -10000, +10000); - - EstAlt = tmpAlt / 10; - - if (currentTime < velTimer) - return; - velTimer = currentTime + 500000; - zVelocity = tmpAlt - lastAlt; - lastAlt = tmpAlt; - - debug4 = zVelocity; + //**** Alt. Set Point stabilization PID **** + //calculate speed for D calculation + BaroHistTab[BaroHistIdx] = BaroAlt; + BaroHigh = 0; + BaroLow = 0; + BaroPID = 0; + for (temp32 = 0; temp32 < BARO_TAB_SIZE / 2; temp32++) { + BaroHigh += BaroHistTab[(BaroHistIdx - temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum last half samples + BaroLow += BaroHistTab[(BaroHistIdx + temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum older samples + } + BaroHistIdx++; + if (BaroHistIdx >= BARO_TAB_SIZE) + BaroHistIdx = 0; + + temp32 = D8[PIDALT] * (BaroHigh - BaroLow) / 400; + BaroPID -= temp32; + + EstAlt = BaroHigh / (BARO_TAB_SIZE / 2); + + temp32 = constrain(AltHold - EstAlt, -1000, 1000); + if (abs(temp32) < 10 && BaroPID < 10) + BaroPID = 0; // remove small D parametr to reduce noise near zoro position + // P + BaroPID += P8[PIDALT] * constrain(temp32, (-2) * P8[PIDALT], 2 * P8[PIDALT]) / 100; + BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150 + + // I + errorAltitudeI += temp32 * I8[PIDALT] / 50; + errorAltitudeI = constrain(errorAltitudeI, -30000, 30000); + temp32 = errorAltitudeI / 500; // I in range +/-60 + BaroPID += temp32; } diff --git a/mixer.c b/mixer.c index 31284e092..f12ae49ab 100755 --- a/mixer.c +++ b/mixer.c @@ -19,8 +19,8 @@ #define PRI_SERVO_TO 6 #elif defined(TRI) #define NUMBER_MOTOR 3 -#define PRI_SERVO_FROM 5 // use only servo 5 -#define PRI_SERVO_TO 5 +#define PRI_SERVO_FROM 6 // use only servo 5 +#define PRI_SERVO_TO 6 #elif defined(QUADP) || defined(QUADX) || defined(Y4) #define NUMBER_MOTOR 4 #elif defined(Y6) || defined(HEX6) || defined(HEX6X) @@ -78,7 +78,8 @@ void mixTable(void) motor[0] = PIDMIX(0, +4 / 3, 0); //REAR motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT - servo[4] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR + servo[5] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR + #endif #ifdef QUADP motor[0] = PIDMIX(0, +1, -1); //REAR @@ -152,15 +153,25 @@ void mixTable(void) motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L #endif +#ifdef VTAIL4 + motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R + motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R + motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L + motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L +#endif + #ifdef SERVO_TILT - if ((rcOptions1 & activate1[BOXCAMSTAB]) || (rcOptions2 & activate2[BOXCAMSTAB])) { - servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); - servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX); - } else { - servo[0] = constrain(TILT_PITCH_MIDDLE + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); - servo[1] = constrain(TILT_ROLL_MIDDLE + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX); + servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500; + servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500; + + if (rcOptions[BOXCAMSTAB]) { + servo[0] += TILT_PITCH_PROP * angle[PITCH] / 16; + servo[1] += TILT_ROLL_PROP * angle[ROLL] / 16; } + + servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX); + servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX); #endif #ifdef GIMBAL servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX); @@ -168,13 +179,15 @@ void mixTable(void) #endif #ifdef FLYING_WING motor[0] = rcCommand[THROTTLE]; - if (passThruMode) { // use raw stick values to drive output - servo[0] = constrain(wing_left_mid + PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC), WING_LEFT_MIN, WING_LEFT_MAX); //LEFT - servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC), WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT + if (passThruMode) {// do not use sensors for correction, simple 2 channel mixing + servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC); + servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC); } else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration - servo[0] = constrain(wing_left_mid + PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL], WING_LEFT_MIN, WING_LEFT_MAX); //LEFT - servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL], WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT + servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL]; + servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL]; } + servo[0] = constrain(servo[0] + wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX); + servo[1] = constrain(servo[1] + wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); #endif #if defined(CAMTRIG) if (camCycle == 1) { @@ -195,7 +208,7 @@ void mixTable(void) } } } - if ((rcOptions1 & activate1[BOXCAMTRIG]) || (rcOptions1 & activate2[BOXCAMTRIG])) + if (rcOptions[BOXCAMTRIG]) camCycle = 1; #endif @@ -225,7 +238,7 @@ void mixTable(void) 0, 2, 6, 15, 30, 52, 82, 123, 175, 240, 320, 415, 528, 659, 811, 984, 1181, 1402, 1648, 1923, 2226, 2559, 2924, 3322, 3755, 4224, 4730, 5276, 5861, 6489, 7160, 7875, 8637, 9446, 10304, 11213, 12173, 13187, 14256, 15381, 16564, 17805, 19108, 20472, 21900, 23392, 24951, 26578, 28274, 30041, 31879, 33792, 35779, 37843, 39984, 42205, 44507, 46890, 49358, 51910, 54549, 57276, 60093, 63000}; if (vbat) { // by all means - must avoid division by zero - for (uint8_t i = 0; i < NUMBER_MOTOR; i++) { + for (i = 0; i < NUMBER_MOTOR; i++) { amp = amperes[((motor[i] - 1000) >> 4)] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp #if (LOG_VALUES == 2) pMeter[i] += amp; // sum up over time the mapped ESC input diff --git a/mw.c b/mw.c index b76176928..fcfbc47b2 100755 --- a/mw.c +++ b/mw.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +// February 2012 V1.dev + #define CHECKBOXITEMS 11 #define PIDITEMS 8 @@ -33,8 +35,8 @@ uint8_t yawRate; uint8_t dynThrPID; uint8_t activate1[CHECKBOXITEMS]; uint8_t activate2[CHECKBOXITEMS]; +uint8_t rcOptions[CHECKBOXITEMS]; uint8_t okToArm = 0; -uint8_t rcOptions1, rcOptions2; uint8_t accMode = 0; // if level mode is a activated uint8_t magMode = 0; // if compass heading hold is a activated uint8_t baroMode = 0; // if altitude hold is activated @@ -62,6 +64,14 @@ uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position upda int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction +//Automatic ACC Offset Calibration +// ********************** +uint16_t InflightcalibratingA = 0; +int16_t AccInflightCalibrationArmed; +uint16_t AccInflightCalibrationMeasurementDone = 0; +uint16_t AccInflightCalibrationSavetoEEProm = 0; +uint16_t AccInflightCalibrationActive = 0; + // ********************** // power meter // ********************** @@ -181,7 +191,7 @@ void annexCode(void) vbatRaw += vbatRawArray[i]; vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps } - if ((rcOptions1 & activate1[BOXBEEPERON]) || (rcOptions2 & activate2[BOXBEEPERON])) { // unconditional beeper on via AUXn switch + if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch buzzerFreq = 7; } else if (((vbat > VBATLEVEL1_3S) #if defined(POWERMETER) @@ -267,7 +277,7 @@ void annexCode(void) } #endif -#if defined(GPS) +#if GPS static uint32_t GPSLEDTime; if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) { GPSLEDTime = currentTime + 150000; @@ -325,9 +335,6 @@ void loop(void) static int16_t errorAngleI[2] = { 0, 0 }; static uint32_t rcTime = 0; static int16_t initialThrottleHold; - static int16_t errorAltitudeI = 0; - int16_t AltPID = 0; - static int16_t AltHold; #if defined(SPEKTRUM) if (rcFrameComplete) @@ -392,8 +399,7 @@ void loop(void) } #endif else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) { - if (((rcOptions1 & activate1[BOXARM]) - || (rcOptions2 & activate2[BOXARM])) && okToArm) { + if (rcOptions[BOXARM] && okToArm) { armed = 1; headFreeModeHold = heading; } else if (armed) @@ -466,11 +472,11 @@ void loop(void) #endif #if defined(InflightAccCalibration) - if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !((rcOptions1 & activate1[BOXARM]) || (rcOptions2 & activate2[BOXARM]))) { // Copter is airborne and you are turning it off via boxarm : start measurement + if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = 0; } - if ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored + if (rcOptions[BOXPASSTHRU]) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored if (!AccInflightCalibrationArmed) { AccInflightCalibrationArmed = 1; InflightcalibratingA = 50; @@ -482,11 +488,12 @@ void loop(void) } #endif - rcOptions1 = (rcData[AUX1] < 1300) + (1300 < rcData[AUX1] && rcData[AUX1] < 1700) * 2 + (rcData[AUX1] > 1700) * 4 + (rcData[AUX2] < 1300) * 8 + (1300 < rcData[AUX2] && rcData[AUX2] < 1700) * 16 + (rcData[AUX2] > 1700) * 32; - rcOptions2 = (rcData[AUX3] < 1300) + (1300 < rcData[AUX3] && rcData[AUX3] < 1700) * 2 + (rcData[AUX3] > 1700) * 4 + (rcData[AUX4] < 1300) * 8 + (1300 < rcData[AUX4] && rcData[AUX4] < 1700) * 16 + (rcData[AUX4] > 1700) * 32; + for(i = 0; i < CHECKBOXITEMS; i++) { + rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & activate2[i]); + } //note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false - if (((rcOptions1 & activate1[BOXACC]) || (rcOptions2 & activate2[BOXACC]) || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) { + if ((rcOptions[BOXACC] || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode if (!accMode) { errorAngleI[ROLL] = 0; @@ -496,7 +503,7 @@ void loop(void) } else accMode = 0; // modified by MIS for failsave support - if ((rcOptions1 & activate1[BOXARM]) == 0 || (rcOptions2 & activate2[BOXARM]) == 0) + if ((rcOptions[BOXARM]) == 0) okToArm = 1; if (accMode == 1) { LED1_ON; @@ -505,25 +512,27 @@ void loop(void) } if (sensors(SENSOR_BARO)) { - if ((rcOptions1 & activate1[BOXBARO]) || (rcOptions2 & activate2[BOXBARO])) { + if (rcOptions[BOXBARO]) { if (baroMode == 0) { baroMode = 1; AltHold = EstAlt; initialThrottleHold = rcCommand[THROTTLE]; errorAltitudeI = 0; + EstVelocity = 0; + BaroPID = 0; } } else baroMode = 0; } if (sensors(SENSOR_MAG)) { - if ((rcOptions1 & activate1[BOXMAG]) || (rcOptions2 & activate2[BOXMAG])) { + if (rcOptions[BOXMAG]) { if (magMode == 0) { magMode = 1; magHold = heading; } } else magMode = 0; - if ((rcOptions1 & activate1[BOXHEADFREE]) || (rcOptions2 & activate2[BOXHEADFREE])) { + if (rcOptions[BOXHEADFREE]) { if (headFreeMode == 0) { headFreeMode = 1; } @@ -531,19 +540,39 @@ void loop(void) headFreeMode = 0; } #if defined(GPS) - if ((rcOptions1 & activate1[BOXGPSHOME]) || (rcOptions2 & activate2[BOXGPSHOME])) { + if (rcOptions[BOXGPSHOME]) { GPSModeHome = 1; } else GPSModeHome = 0; - if ((rcOptions1 & activate1[BOXGPSHOLD]) || (rcOptions2 & activate2[BOXGPSHOLD])) { + if (rcOptions[BOXGPSHOLD]) { GPSModeHold = 1; } else GPSModeHold = 0; #endif - if ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { + if (rcOptions[BOXPASSTHRU]) { passThruMode = 1; } else passThruMode = 0; + } else { // not in rc loop + static int8_t taskOrder = 0; //never call all function in the same loop + switch (taskOrder) { + case 0: + taskOrder++; + if (sensors(SENSOR_MAG)) + Mag_getADC(); + break; + case 1: + taskOrder++; + if (sensors(SENSOR_BARO)) + Baro_update(); + case 2: + taskOrder++; + if (sensors(SENSOR_BARO)) + getEstimatedAltitude(); + default: + taskOrder = 0; + break; + } } computeIMU(); @@ -572,29 +601,13 @@ void loop(void) AltHold = EstAlt; initialThrottleHold = rcCommand[THROTTLE]; errorAltitudeI = 0; + EstVelocity = 0; + BaroPID = 0; } - //**** Alt. Set Point stabilization PID **** - error = constrain(AltHold - EstAlt, -100, 100); // +/-10m, 1 decimeter accuracy - errorAltitudeI += error; - errorAltitudeI = constrain(errorAltitudeI, -5000, 5000); - - PTerm = P8[PIDALT] * error / 10; // 16 bits is ok here - - if (abs(error) > 5) // under 50cm error, we neutralize Iterm - ITerm = (int32_t) I8[PIDALT] * errorAltitudeI / 4000; - else - ITerm = 0; - - AltPID = PTerm + ITerm; - - //AltPID is reduced, depending of the zVelocity magnitude - AltPID = AltPID * (D8[PIDALT] - min(abs(zVelocity), D8[PIDALT] * 4 / 5)) / (D8[PIDALT] + 1); - debug3 = AltPID; - - rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID, -100, +100); + rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } } -#if defined(GPS) +#if GPS if ((GPSModeHome == 1)) { float radDiff = (GPS_directionToHome - heading) * 0.0174533f; GPS_angle[ROLL] = constrain(P8[PIDGPS] * sinf(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // with P=5, 1 meter = 0.5deg inclination @@ -615,7 +628,7 @@ void loop(void) #else PTerm = (int32_t) errorAngle * P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result #endif - PTerm = constrain(PTerm, -D8[PIDLEVEL], +D8[PIDLEVEL]); + PTerm = constrain(PTerm, -D8[PIDLEVEL] * 5, +D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here ITerm = ((int32_t) errorAngleI[axis] * I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result @@ -647,7 +660,7 @@ void loop(void) writeServos(); writeMotors(); -#if defined(GPS) +#if GPS while (SerialAvailable(GPS_SERIAL)) { if (GPS_newFrame(SerialRead(GPS_SERIAL))) { if (GPS_update == 1) diff --git a/mw.h b/mw.h index 08c6afd99..1303deb54 100755 --- a/mw.h +++ b/mw.h @@ -99,6 +99,8 @@ #define MULTITYPE 11 //the GUI is the same for all 8 motor configs #elif defined(OCTOFLATX) #define MULTITYPE 11 //the GUI is the same for all 8 motor configs +#elif defined(VTAIL4) + #define MULTITYPE 15 #endif /*********** RC alias *****************/ @@ -147,7 +149,6 @@ extern int16_t gyroZero[3]; extern int16_t magZero[3]; extern int16_t gyroData[3]; extern int16_t angle[2]; -extern int16_t EstAlt; extern int16_t axisPID[3]; extern int16_t rcCommand[4]; @@ -165,7 +166,12 @@ extern uint16_t calibratingG; extern int16_t heading; extern int16_t annex650_overrun_count; extern int32_t pressure; -extern int16_t BaroAlt; +extern int32_t BaroAlt; +extern int32_t EstAlt; +extern int32_t AltHold; +extern int16_t errorAltitudeI; +extern int32_t EstVelocity; +extern int16_t BaroPID; extern uint8_t headFreeMode; extern int16_t headFreeModeHold; extern int8_t smallAngle25; @@ -208,6 +214,7 @@ void imuInit(void); void annexCode(void); void computeIMU(void); void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat); +void getEstimatedAltitude(void); // Sensors void sensorsAutodetect(void); diff --git a/sensors.c b/sensors.c index cf296c05c..e17d3cad9 100755 --- a/sensors.c +++ b/sensors.c @@ -71,6 +71,7 @@ static void ACC_Common(void) } //all values are measured if (InflightcalibratingA == 1) { + AccInflightCalibrationActive = 0; AccInflightCalibrationMeasurementDone = 1; blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight // recover saved values to maintain current flight behavior until new values are transferred @@ -135,16 +136,16 @@ void Baro_update(void) case 2: bmp085_start_up(); baroState++; - baroDeadline += 26000; + baroDeadline += 14000; break; case 3: baroUP = bmp085_get_up(); baroTemp = bmp085_get_temperature(baroUT); pressure = bmp085_get_pressure(baroUP); - BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 443300.0f; //decimeter + BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter baroState = 0; - baroDeadline += 20000; + baroDeadline += 5000; break; } } @@ -219,9 +220,9 @@ void Mag_init(void) Mag_getRawADC(); delay(10); - magCal[ROLL] = 1000.0 / magADC[ROLL]; - magCal[PITCH] = 1000.0 / magADC[PITCH]; - magCal[YAW] = -1000.0 / magADC[YAW]; + magCal[ROLL] = 1000.0 / abs(magADC[ROLL]); + magCal[PITCH] = 1000.0 / abs(magADC[PITCH]); + magCal[YAW] = 1000.0 / abs(magADC[YAW]); hmc5883lFinishCal(); magInit = 1; diff --git a/serial.c b/serial.c index d33d68fea..66e5afa79 100755 --- a/serial.c +++ b/serial.c @@ -152,6 +152,7 @@ void serialCom(void) #else serialize16(cycleTime); #endif + serialize16(i2cGetErrorCounter()); for (i = 0; i < 2; i++) serialize16(angle[i]); serialize8(MULTITYPE); @@ -177,10 +178,10 @@ void serialCom(void) serialize16(intPowerMeterSum); serialize16(intPowerTrigger1); serialize8(vbat); - serialize16(BaroAlt); // 4 variables are here for general monitoring purpose - serialize16(i2cGetErrorCounter()); // debug2 - serialize16(debug3); // debug3 - serialize16(debug4); // debug4 + serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose + serialize16(0); // debug2 + serialize16(debug3); // debug3 + serialize16(debug4); // debug4 serialize8('M'); // UartSendData(); break;