WebSVN - FlightCtrl - Diff - Rev 2095 and 2160 - /branches/dongfang_FC


#include <inttypes.h>
#include "analog.h"
#include "attitude.h"
#include "configuration.h"
#include "controlMixer.h"
// for digital / LED debug.
#include "output.h"
 
// For scope debugging only!
#include "rc.h"
 
#define INTEGRAL_LIMIT 100000
 
#define LATCH_TIME 3
 
// This will translate a height value to a target height in centimeters.
// Currently: One step is 50 cm and zero is 5 meters below the takeoff altitude. With max. 255 steps the max.
// target height is then about 120m.
#define HEIGHT_GAIN 50L
#define HEIGHT_FORMULA(x) ((x) * HEIGHT_GAIN - 500L)
 
int32_t setHeight;
int32_t targetHeight;
 
uint16_t hc_testOscPrescaler = 0;
uint8_t hc_testOscTimer = 0;
 
int32_t maxHeightThisFlight;
int32_t iHeight;
 
void HC_setGround(void) {
  analog_setGround();
  // This should also happen when height control is enabled in-flight.
  setHeight = targetHeight = analog_getHeight();
  hc_testOscTimer = 0;
  maxHeightThisFlight = 0;
  iHeight = 0;
}
 
uint8_t HC_isSwitchOn(void) {
  return (dynamicParams.heightSetting >= 255/3);
}
 
void HC_periodicTask(void) {
  int32_t height = analog_getHeight();
  static uint8_t setHeightLatch = 0;
 
  if (height > maxHeightThisFlight)
    maxHeightThisFlight = height;
 
  // debugOut.analog[25] = dynamicParams.heightSetting;
 
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
    if (HC_isSwitchOn()) {
      // Switch is ON
      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.
          setHeight = height;
          hc_testOscTimer = 0;
          iHeight = 0;
        }
        // Time not yet reached.
        setHeightLatch++;
      }
    } else {
      // Switch is OFF.
      setHeightLatch = 0;
    }
  } else {
    // Switch is not activated; take the "max-height" as the target height.
    setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it)
  }
 
  /*
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
    heightRampingTimer = 0;
    if (rampedTargetHeight < targetHeight) {
      // climbing
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
        rampedTargetHeight += staticParams.heightSlewRate;
      } else {
        rampedTargetHeight = targetHeight;
      }
    } else {
      // descending
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
        rampedTargetHeight -= staticParams.heightSlewRate;
      } else {
        rampedTargetHeight = targetHeight;
      }
    }
  }
  */
  //      uint8_t heightControlTestOscPeriod;
  //      uint8_t heightControlTestOscAmplitude;
 
  hc_testOscPrescaler++;
  if (hc_testOscPrescaler == 488) {
          hc_testOscPrescaler = 0;
          hc_testOscTimer++;
          if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
                  hc_testOscTimer = 0;
                  if (staticParams.heightControlTestOscAmplitude)
                          iHeight = 0;
          } else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) {
                  if (staticParams.heightControlTestOscAmplitude)
                          iHeight = 0;
          }
  }
 
  if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2)
          targetHeight = setHeight;
  else
          targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * HEIGHT_GAIN;
 
  //if (staticParams.)
  // height, in meters (so the division factor is: 100)
  // debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
  // Calculated 0 alt number: 108205
  // Experimental 0 alt number: 117100
}
 
#define LOG_PHEIGHT_SCALE 10
#define LOG_IHEIGHT_SCALE 24
#define LOG_DHEIGHT_SCALE 6
 
// takes 180-200 usec (with integral term). That is too heavy!!!
// takes 100 usec without integral term.
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
  HC_periodicTask();
  int16_t throttle = PRTY[CONTROL_THROTTLE];
  int32_t height = analog_getHeight();
  int32_t heightError = targetHeight - height;
  int16_t dHeight = analog_getDHeight();
 
  debugOut.analog[22] = height/10L;
  debugOut.analog[23] = dHeight;
 
  if (heightError > 0) {
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
  } else {
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
  }
 
  if (dHeight > 0) {
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
  } else {
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
  }
 
  int32_t heightErrorForIntegral = heightError;
  int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE;
 
  if (heightErrorForIntegral > heightErrorForIntegralLimit) {
    heightErrorForIntegral = heightErrorForIntegralLimit;
  } else if (heightErrorForIntegral < -heightErrorForIntegralLimit) {
    heightErrorForIntegral =- heightErrorForIntegralLimit;
  }
 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
  iHeight += heightErrorForIntegral;
 
#define IHEIGHT_SCALE 24
  // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
  int16_t dThrottleI =  (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
 
  if (dThrottleI > staticParams.heightControlMaxIntegralOut) {
    dThrottleI = staticParams.heightControlMaxIntegralOut;
    iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
  } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) {
    dThrottleI = -staticParams.heightControlMaxIntegralOut;
    iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
  }
 
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;
 
  //debugOut.analog[10] = dThrottleP;
  //debugOut.analog[11] = dThrottleI;
  //debugOut.analog[12] = dThrottleD;
  //debugOut.analog[13] = heightError/10;
 
  int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD;
 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
    dThrottle = staticParams.heightControlMaxThrottleChange;
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
    dThrottle = -staticParams.heightControlMaxThrottleChange;
 
  /*
  debugOut.analog[19] = throttle;
  debugOut.analog[20] = dThrottle;
  debugOut.analog[21] = height;
  debugOut.analog[22] = rampedTargetHeight;
  debugOut.analog[23] = heightError;
  */
 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) || HC_isSwitchOn()) {
      // If switch is not in use --> Just apply height control.
      // If switch is in use     --> only apply height control when switch is also ON.
      throttle += dThrottle;
    }
  }
 
  /* Experiment: Find hover-throttle */
 
#define DEFAULT_HOVERTHROTTLE 50
int32_t stronglyFilteredHeightDiff = 0;
// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
#define HOVERTHROTTLEFILTER 25
 
  stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
                                * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
  stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
                                                          - 1) + throttle) / HOVERTHROTTLEFILTER;
 
  /*
  if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
      && stronglyFilteredHeightDiff > -3) {
    hoverThrottle = stronglyFilteredThrottle;
    debugOut.digital[0] |= DEBUG_HOVERTHROTTLE;
  } else
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
 
    */
 
  PRTY[CONTROL_THROTTLE] = throttle;
}
 
/*
 For a variometer thingy:
 When switch is thrown on, freeze throttle (capture it into variable)
 For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
 Output = frozen throttle + whatever is computed +/-. Integral?
 */
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c - Rev 2095 → 2160

Rev 2095		Rev 2160
1	#include <inttypes.h>	1	#include <inttypes.h>
2	#include "analog.h"	2	#include "analog.h"
3	#include "attitude.h"	3	#include "attitude.h"
4	#include "configuration.h"	4	#include "configuration.h"
5	#include "controlMixer.h"	5	#include "controlMixer.h"
6	// for digital / LED debug.	6	// for digital / LED debug.
7	#include "output.h"	7	#include "output.h"
8		8
9	// For scope debugging only!	9	// For scope debugging only!
10	#include "rc.h"	10	#include "rc.h"
11		11
12	#define INTEGRAL_LIMIT 100000	12	#define INTEGRAL_LIMIT 100000
13		13
14	#define LATCH_TIME 3	14	#define LATCH_TIME 3
15		15
16	// This will translate a height value to a target height in centimeters.	16	// This will translate a height value to a target height in centimeters.
17	// Currently: One step is 50 cm and zero is 5 meters below the takeoff altitude. With max. 255 steps the max.	17	// Currently: One step is 50 cm and zero is 5 meters below the takeoff altitude. With max. 255 steps the max.
18	// target height is then about 120m.	18	// target height is then about 120m.
19	#define HEIGHT_GAIN 50L	19	#define HEIGHT_GAIN 50L
20	#define HEIGHT_FORMULA(x) ((x) * HEIGHT_GAIN - 500L)	20	#define HEIGHT_FORMULA(x) ((x) * HEIGHT_GAIN - 500L)
21		21
22	int32_t setHeight;	22	int32_t setHeight;
23	int32_t targetHeight;	23	int32_t targetHeight;
24		24
25	uint16_t hc_testOscPrescaler = 0;	25	uint16_t hc_testOscPrescaler = 0;
26	uint8_t hc_testOscTimer = 0;	26	uint8_t hc_testOscTimer = 0;
27		27
28	int32_t maxHeightThisFlight;	28	int32_t maxHeightThisFlight;
29	int32_t iHeight;	29	int32_t iHeight;
30		30
31	void HC_setGround(void) {	31	void HC_setGround(void) {
32	analog_setGround();	32	analog_setGround();
33	// This should also happen when height control is enabled in-flight.	33	// This should also happen when height control is enabled in-flight.
34	setHeight = targetHeight = analog_getHeight();	34	setHeight = targetHeight = analog_getHeight();
35	hc_testOscTimer = 0;	35	hc_testOscTimer = 0;
36	maxHeightThisFlight = 0;	36	maxHeightThisFlight = 0;
37	iHeight = 0;	37	iHeight = 0;
38	}	38	}
39		39
40	uint8_t HC_isSwitchOn(void) {	40	uint8_t HC_isSwitchOn(void) {
41	return (dynamicParams.heightSetting >= 255/3);	41	return (dynamicParams.heightSetting >= 255/3);
42	}	42	}
43		43
44	void HC_periodicTask(void) {	44	void HC_periodicTask(void) {
45	int32_t height = analog_getHeight();	45	int32_t height = analog_getHeight();
46	static uint8_t setHeightLatch = 0;	46	static uint8_t setHeightLatch = 0;
47		47
48	if (height > maxHeightThisFlight)	48	if (height > maxHeightThisFlight)
49	maxHeightThisFlight = height;	49	maxHeightThisFlight = height;
50		50
51	// debugOut.analog[25] = dynamicParams.heightSetting;	51	// debugOut.analog[25] = dynamicParams.heightSetting;
52		52
53	if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {	53	if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
54	if (HC_isSwitchOn()) {	54	if (HC_isSwitchOn()) {
55	// Switch is ON	55	// Switch is ON
56	if (setHeightLatch <= LATCH_TIME) {	56	if (setHeightLatch <= LATCH_TIME) {
57	if (setHeightLatch == LATCH_TIME) {	57	if (setHeightLatch == LATCH_TIME) {
58	// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.	58	// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
59	setHeight = height;	59	setHeight = height;
60	hc_testOscTimer = 0;	60	hc_testOscTimer = 0;
61	iHeight = 0;	61	iHeight = 0;
62	}	62	}
63	// Time not yet reached.	63	// Time not yet reached.
64	setHeightLatch++;	64	setHeightLatch++;
65	}	65	}
66	} else {	66	} else {
67	// Switch is OFF.	67	// Switch is OFF.
68	setHeightLatch = 0;	68	setHeightLatch = 0;
69	}	69	}
70	} else {	70	} else {
71	// Switch is not activated; take the "max-height" as the target height.	71	// Switch is not activated; take the "max-height" as the target height.
72	setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it)	72	setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it)
73	}	73	}
74		74
75	/*	75	/*
76	if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {	76	if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
77	heightRampingTimer = 0;	77	heightRampingTimer = 0;
78	if (rampedTargetHeight < targetHeight) {	78	if (rampedTargetHeight < targetHeight) {
79	// climbing	79	// climbing
80	if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {	80	if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
81	rampedTargetHeight += staticParams.heightSlewRate;	81	rampedTargetHeight += staticParams.heightSlewRate;
82	} else {	82	} else {
83	rampedTargetHeight = targetHeight;	83	rampedTargetHeight = targetHeight;
84	}	84	}
85	} else {	85	} else {
86	// descending	86	// descending
87	if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {	87	if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
88	rampedTargetHeight -= staticParams.heightSlewRate;	88	rampedTargetHeight -= staticParams.heightSlewRate;
89	} else {	89	} else {
90	rampedTargetHeight = targetHeight;	90	rampedTargetHeight = targetHeight;
91	}	91	}
92	}	92	}
93	}	93	}
94	*/	94	*/
95	// uint8_t heightControlTestOscPeriod;	95	// uint8_t heightControlTestOscPeriod;
96	// uint8_t heightControlTestOscAmplitude;	96	// uint8_t heightControlTestOscAmplitude;
97		97
98	hc_testOscPrescaler++;	98	hc_testOscPrescaler++;
99	if (hc_testOscPrescaler == 488) {	99	if (hc_testOscPrescaler == 488) {
100	hc_testOscPrescaler = 0;	100	hc_testOscPrescaler = 0;
101	hc_testOscTimer++;	101	hc_testOscTimer++;
102	if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {	102	if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
103	hc_testOscTimer = 0;	103	hc_testOscTimer = 0;
104	if (staticParams.heightControlTestOscAmplitude)	104	if (staticParams.heightControlTestOscAmplitude)
105	iHeight = 0;	105	iHeight = 0;
106	} else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) {	106	} else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) {
107	if (staticParams.heightControlTestOscAmplitude)	107	if (staticParams.heightControlTestOscAmplitude)
108	iHeight = 0;	108	iHeight = 0;
109	}	109	}
110	}	110	}
111		111
112	if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2)	112	if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2)
113	targetHeight = setHeight;	113	targetHeight = setHeight;
114	else	114	else
115	targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * HEIGHT_GAIN;	115	targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * HEIGHT_GAIN;
116		116
117	//if (staticParams.)	117	//if (staticParams.)
118	// height, in meters (so the division factor is: 100)	118	// height, in meters (so the division factor is: 100)
119	// debugOut.analog[24] = (117100 - filteredAirPressure) / 100;	119	// debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
120	// Calculated 0 alt number: 108205	120	// Calculated 0 alt number: 108205
121	// Experimental 0 alt number: 117100	121	// Experimental 0 alt number: 117100
122	}	122	}
123		123
124	#define LOG_PHEIGHT_SCALE 10	124	#define LOG_PHEIGHT_SCALE 10
125	#define LOG_IHEIGHT_SCALE 24	125	#define LOG_IHEIGHT_SCALE 24
126	#define LOG_DHEIGHT_SCALE 6	126	#define LOG_DHEIGHT_SCALE 6
127		127
128	// takes 180-200 usec (with integral term). That is too heavy!!!	128	// takes 180-200 usec (with integral term). That is too heavy!!!
129	// takes 100 usec without integral term.	129	// takes 100 usec without integral term.
130	void HC_periodicTaskAndPRTY(int16_t* PRTY) {	130	void HC_periodicTaskAndPRTY(int16_t* PRTY) {
131	HC_periodicTask();	131	HC_periodicTask();
132	int16_t throttle = PRTY[CONTROL_THROTTLE];	132	int16_t throttle = PRTY[CONTROL_THROTTLE];
133	int32_t height = analog_getHeight();	133	int32_t height = analog_getHeight();
134	int32_t heightError = targetHeight - height;	134	int32_t heightError = targetHeight - height;
135	int16_t dHeight = analog_getDHeight();	135	int16_t dHeight = analog_getDHeight();
136		136
137	debugOut.analog[22] = height/10L;	137	debugOut.analog[22] = height/10L;
138	debugOut.analog[23] = dHeight;	138	debugOut.analog[23] = dHeight;
139		139
140	if (heightError > 0) {	140	if (heightError > 0) {
141	debugOut.digital[0] \|= DEBUG_HEIGHT_DIFF;	141	debugOut.digital[0] \|= DEBUG_HEIGHT_DIFF;
142	} else {	142	} else {
143	debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;	143	debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
144	}	144	}
145		145
146	if (dHeight > 0) {	146	if (dHeight > 0) {
147	debugOut.digital[1] \|= DEBUG_HEIGHT_DIFF;	147	debugOut.digital[1] \|= DEBUG_HEIGHT_DIFF;
148	} else {	148	} else {
149	debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;	149	debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
150	}	150	}
151		151
152	int32_t heightErrorForIntegral = heightError;	152	int32_t heightErrorForIntegral = heightError;
153	int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE;	153	int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE;
154		154
155	if (heightErrorForIntegral > heightErrorForIntegralLimit) {	155	if (heightErrorForIntegral > heightErrorForIntegralLimit) {
156	heightErrorForIntegral = heightErrorForIntegralLimit;	156	heightErrorForIntegral = heightErrorForIntegralLimit;
157	} else if (heightErrorForIntegral < -heightErrorForIntegralLimit) {	157	} else if (heightErrorForIntegral < -heightErrorForIntegralLimit) {
158	heightErrorForIntegral =- heightErrorForIntegralLimit;	158	heightErrorForIntegral =- heightErrorForIntegralLimit;
159	}	159	}
160		160
161	// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....	161	// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
162	iHeight += heightErrorForIntegral;	162	iHeight += heightErrorForIntegral;
163		163
164	#define IHEIGHT_SCALE 24	164	#define IHEIGHT_SCALE 24
165	// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256	165	// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
166	int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);	166	int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
167		167
168	if (dThrottleI > staticParams.heightControlMaxIntegralOut) {	168	if (dThrottleI > staticParams.heightControlMaxIntegralOut) {
169	dThrottleI = staticParams.heightControlMaxIntegralOut;	169	dThrottleI = staticParams.heightControlMaxIntegralOut;
170	iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;	170	iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
171	} else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) {	171	} else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) {
172	dThrottleI = -staticParams.heightControlMaxIntegralOut;	172	dThrottleI = -staticParams.heightControlMaxIntegralOut;
173	iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;	173	iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI;
174	}	174	}
175		175
176	int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;	176	int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE;
177	int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;	177	int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE;
178		178
179	debugOut.analog[10] = dThrottleP;	179	//debugOut.analog[10] = dThrottleP;
180	debugOut.analog[11] = dThrottleI;	180	//debugOut.analog[11] = dThrottleI;
181	debugOut.analog[12] = dThrottleD;	181	//debugOut.analog[12] = dThrottleD;
182	//debugOut.analog[13] = heightError/10;	182	//debugOut.analog[13] = heightError/10;
183		183
184	int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD;	184	int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD;
185		185
186	if (dThrottle > staticParams.heightControlMaxThrottleChange)	186	if (dThrottle > staticParams.heightControlMaxThrottleChange)
187	dThrottle = staticParams.heightControlMaxThrottleChange;	187	dThrottle = staticParams.heightControlMaxThrottleChange;
188	else if (dThrottle < -staticParams.heightControlMaxThrottleChange)	188	else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
189	dThrottle = -staticParams.heightControlMaxThrottleChange;	189	dThrottle = -staticParams.heightControlMaxThrottleChange;
190		190
191	/*	191	/*
192	debugOut.analog[19] = throttle;	192	debugOut.analog[19] = throttle;
193	debugOut.analog[20] = dThrottle;	193	debugOut.analog[20] = dThrottle;
194	debugOut.analog[21] = height;	194	debugOut.analog[21] = height;
195	debugOut.analog[22] = rampedTargetHeight;	195	debugOut.analog[22] = rampedTargetHeight;
196	debugOut.analog[23] = heightError;	196	debugOut.analog[23] = heightError;
197	*/	197	*/
198		198
199	if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {	199	if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
200	if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) \|\| HC_isSwitchOn()) {	200	if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) \|\| HC_isSwitchOn()) {
201	// If switch is not in use --> Just apply height control.	201	// If switch is not in use --> Just apply height control.
202	// If switch is in use --> only apply height control when switch is also ON.	202	// If switch is in use --> only apply height control when switch is also ON.
203	throttle += dThrottle;	203	throttle += dThrottle;
204	}	204	}
205	}	205	}
206		206
207	/* Experiment: Find hover-throttle */	207	/* Experiment: Find hover-throttle */
208		208
209	#define DEFAULT_HOVERTHROTTLE 50	209	#define DEFAULT_HOVERTHROTTLE 50
210	int32_t stronglyFilteredHeightDiff = 0;	210	int32_t stronglyFilteredHeightDiff = 0;
211	// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;	211	// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
212	uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;	212	uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
213	#define HOVERTHROTTLEFILTER 25	213	#define HOVERTHROTTLEFILTER 25
214		214
215	stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff	215	stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
216	* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;	216	* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
217	stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER	217	stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
218	- 1) + throttle) / HOVERTHROTTLEFILTER;	218	- 1) + throttle) / HOVERTHROTTLEFILTER;
219		219
220	/*	220	/*
221	if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3	221	if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
222	&& stronglyFilteredHeightDiff > -3) {	222	&& stronglyFilteredHeightDiff > -3) {
223	hoverThrottle = stronglyFilteredThrottle;	223	hoverThrottle = stronglyFilteredThrottle;
224	debugOut.digital[0] \|= DEBUG_HOVERTHROTTLE;	224	debugOut.digital[0] \|= DEBUG_HOVERTHROTTLE;
225	} else	225	} else
226	debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;	226	debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
227		227
228	*/	228	*/
229		229
230	PRTY[CONTROL_THROTTLE] = throttle;	230	PRTY[CONTROL_THROTTLE] = throttle;
231	}	231	}
232		232
233	/*	233	/*
234	For a variometer thingy:	234	For a variometer thingy:
235	When switch is thrown on, freeze throttle (capture it into variable)	235	When switch is thrown on, freeze throttle (capture it into variable)
236	For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.	236	For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
237	Output = frozen throttle + whatever is computed +/-. Integral?	237	Output = frozen throttle + whatever is computed +/-. Integral?
238	*/	238	*/
239		239