WebSVN - FlightCtrl - Path Comparison - / Rev 2032 and / Rev 2033

Ignore whitespace Rev 2032 → Rev 2033

 /branches/dongfang_FC_rewrite/analog.c
 ,6 → 97,8
 int16_t gyroD[2];
 int16_t yawGyro;
+int32_t groundPressure;
 /*
  * Offset values. These are the raw gyro and acc. meter sums when the copter is
  * standing still. They are used for adjusting the gyro and acc. meter values
 ,6 → 174,7
 // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
 int32_t filteredAirPressure;
+int32_t lastFilteredAirPressure;
 #define MAX_AIRPRESSURE_WINDOW_LENGTH 64
 int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
 ,6 → 509,7
     // 2 samples were added.
     pressureMeasurementCount += 2;
     if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) {
+      lastFilteredAirPressure = filteredAirPressure;
       filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1)
                              + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER;
       pressureMeasurementCount = airPressureSum = 0;
 ,7 → 528,6
   // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
   // This is divided by 3 --> 10.34 counts per volt.
   UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
-  debugOut.analog[11] = UBat;
 }
 void analog_update(void) {
 ,3 → 636,16
   accOffset_writeToEEProm();
   startAnalogConversionCycle();
 }
+void analog_setGround() {
+  groundPressure = filteredAirPressure;
+}
+int32_t analog_getHeight(void) {
+  return groundPressure - filteredAirPressure;
+}
+int16_t analog_getDHeight(void) {
+  // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
+  return lastFilteredAirPressure - filteredAirPressure;
+}

 /branches/dongfang_FC_rewrite/analog.h
 ,4 → 293,11
  * Zero-offset accelerometers and write the calibration data to EEPROM.
  */
 void analog_calibrateAcc(void);
+void analog_setGround();
+int32_t analog_getHeight(void);
+int16_t analog_getDHeight(void);
 #endif //_ANALOG_H

 /branches/dongfang_FC_rewrite/attitude.c
 ,7 → 156,7
 int32_t getAngleEstimateFromAcc(uint8_t axis) {
   //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
-  return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm;
+  return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm;
   // return 342L * filteredAcc[axis];
 }
 ,7 → 332,7
      */
     for (axis = PITCH; axis <= ROLL; axis++) {
       accDerived = getAngleEstimateFromAcc(axis);
-      //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
+      debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
       // 1000 * the correction amount that will be added to the gyro angle in next line.
       temp = angle[axis];
       angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
 ,18 → 340,12
       correctionSum[axis] += angle[axis] - temp;
     }
   } else {
-    //debugOut.analog[9] = 0;
-    //debugOut.analog[10] = 0;
+    debugOut.analog[9] = 0;
+    debugOut.analog[10] = 0;
     // experiment: Kill drift compensation updates when not flying smooth.
     // correctionSum[PITCH] = correctionSum[ROLL] = 0;
     debugOut.digital[0] |= DEBUG_ACC0THORDER;
   }
-  int32_t accDerived = getAngleEstimateFromAcc(0);
-  debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
-  accDerived = getAngleEstimateFromAcc(1);
-  debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
 }
 /************************************************************************

 /branches/dongfang_FC_rewrite/configuration.h
 ,7 → 164,7
   uint8_t heightI;
   uint8_t heightD;
   uint8_t heightSetting;
-  uint8_t heightControlMaxIntegralThrottleChange;
+  uint8_t heightControlMaxIntegral;
   uint8_t heightControlMaxThrottleChange;
   uint8_t heightSlewRate;
 ,7 → 215,7
 #define ATMEGA644P      1
 #define SYSCLK F_CPU
-// Not really a part of configuration, but LEDs and HW version test are the same.
+// Not really a part of configuration, but LEDs and HW s test are the same.
 #define RED_OFF   {if((boardRelease == 10) || (boardRelease == 20)) PORTB &=~(1<<PORTB0); else  PORTB |= (1<<PORTB0);}
 #define RED_ON    {if((boardRelease == 10) || (boardRelease == 20)) PORTB |= (1<<PORTB0); else  PORTB &=~(1<<PORTB0);}
 #define RED_FLASH PORTB ^= (1<<PORTB0)

 /branches/dongfang_FC_rewrite/eeprom.h
 ,7 → 24,7
 #define EEPROM_ADR_PARAMSET_BEGIN   256
 #define CHANNELMAP_REVISION         0
-#define EEPARAM_REVISION                1
+#define EEPARAM_REVISION                2
 #define EEMIXER_REVISION                0
 #define SENSOROFFSET_REVISION       0

 /branches/dongfang_FC_rewrite/heightControl.c
 ,4 → 1,5
 #include <inttypes.h>
+#include "analog.h"
 #include "attitude.h"
 #include "uart0.h"
 #include "configuration.h"
 ,7 → 14,6
 #define LATCH_TIME 40
-int32_t groundPressure;
 int32_t targetHeight;
 int32_t rampedTargetHeight;
 ,20 → 21,16
 int32_t maxHeightThisFlight;
 int32_t iHeight;
-int32_t getHeight(void) {
-  return groundPressure - filteredAirPressure;
-}
 void HC_setGround(void) {
-  groundPressure = filteredAirPressure;
+  analog_setGround();
   // This should also happen when height control is enabled in-flight.
-  rampedTargetHeight = getHeight();
+  rampedTargetHeight = targetHeight = analog_getHeight();
   maxHeightThisFlight = 0;
   iHeight = 0;
 }
 void HC_update(void) {
-  int32_t height = getHeight();
+  int32_t height = analog_getHeight();
   static uint8_t setHeightLatch = 0;
   if (height > maxHeightThisFlight)
 ,7 → 43,7
       if (setHeightLatch <= LATCH_TIME) {
         if (setHeightLatch == LATCH_TIME) {
           // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
-          targetHeight = height;
+          rampedTargetHeight = targetHeight = height;
         }
         // Time not yet reached.
         setHeightLatch++;
 ,7 → 54,7
     }
   } else {
     // Switch is not activated; take the "max-height" as the target height.
-    targetHeight = (uint16_t) dynamicParams.heightSetting * 50L - 3000L; // should be: 100 (or make a param out of it)
+    targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it)
   }
   if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
 ,17 → 86,13
 // takes 180-200 usec (with integral term). That is too heavy!!!
 // takes 100 usec without integral term.
 uint16_t HC_getThrottle(uint16_t throttle) {
-  int32_t height = getHeight();
+  int32_t height = analog_getHeight();
   int32_t heightError = rampedTargetHeight - height;
-  static int32_t lastHeight;
+  //static int32_t lastHeight;
+  int16_t dHeight = analog_getDHeight();
+  //lastHeight = height;
-  int16_t dHeight = height - lastHeight;
-  lastHeight = height;
-  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
-  iHeight += heightError;
   if (heightError > 0) {
     debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
     debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
 ,14 → 100,28
     debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
     debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
   }
-  int16_t dThrottle = (iHeight * staticParams.heightI) / 10000L;
-  if (dThrottle > staticParams.heightControlMaxIntegralThrottleChange)
-    dThrottle = staticParams.heightControlMaxIntegralThrottleChange;
-  else if (dThrottle < -staticParams.heightControlMaxIntegralThrottleChange)
-    dThrottle = -staticParams.heightControlMaxIntegralThrottleChange;
+  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
+  iHeight += heightError;
+#define IHEIGHT_SCALE 24
+  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
+  int16_t dThrottle =  (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE);
+  // dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral
+  // (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE)
+  // iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI
+  if (dThrottle > staticParams.heightControlMaxIntegral) {
+    dThrottle = staticParams.heightControlMaxIntegral;
+    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI;
+  } else if (dThrottle < -staticParams.heightControlMaxIntegral) {
+    dThrottle = -staticParams.heightControlMaxIntegral;
+    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI;
+  }
+  debugOut.analog[27] = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE);
   dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7);
   if (dThrottle > staticParams.heightControlMaxThrottleChange)
 ,7 → 132,6
   debugOut.analog[19] = rampedTargetHeight;
   debugOut.analog[21] = dThrottle;
   debugOut.analog[26] = height;
-  debugOut.analog[27] = (iHeight * staticParams.heightI) / 10000L;
   if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
     if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)

/branches/dongfang_FC_rewrite/heightControl.h
1,3 → 1,2
void HC_setGround(void);
void HC_update(void);
uint16_t HC_getThrottle(uint16_t throttle);

 /branches/dongfang_FC_rewrite/makefile
 ,8 → 8,8
 VERSION_PATCH    =  0
 VERSION_SERIAL_MAJOR = 10       # Serial Protocol Major Version
-VERSION_SERIAL_MINOR = 2        # Serial Protocol Minor Version
-NC_SPI_COMPATIBLE =    6        # SPI Protocol Version
+VERSION_SERIAL_MINOR = 1        # Serial Protocol Minor Version
+NC_SPI_COMPATIBLE =    6        # SPI Protocol Version
 #-------------------------------------------------------------------
 #OPTIONS

 /branches/dongfang_FC_rewrite/uart0.c
 ,7 → 132,7
                 "AccZ            ",
                 "AccPitch (angle)",
                 "AccRoll (angle) ", //10
-                "UBat            ",
+                "-               ",
                 "Pitch Term      ",
                 "Roll Term       ",
                 "Yaw Term        ",

Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2032 → Rev 2033