WebSVN - FlightCtrl - Diff - Rev 2074 and 2075 - /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
 
int32_t setHeight;
int32_t targetHeight;
 
uint16_t testOscPrescaler = 0;
uint8_t 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();
  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;
          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) dynamicParams.heightSetting * 25L - 500L; // 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;
 
  testOscPrescaler++;
  if (testOscPrescaler == 488) {
          testOscPrescaler = 0;
          testOscTimer++;
          if (testOscTimer == staticParams.heightControlTestOscPeriod) {
                  testOscTimer = 0;
                  iHeight = 0;
          } else if (testOscTimer == staticParams.heightControlTestOscPeriod/2) {
                  iHeight = 0;
          }
  }
 
  if (testOscTimer < staticParams.heightControlTestOscPeriod/2)
          targetHeight = setHeight;
  else
          targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * 25L;
 
  debugOut.analog[31] = targetHeight/100L;
 
  //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;
 
  //debugOut.analog[27] = dynamicParams.heightP;
  //debugOut.analog[28] = dynamicParams.heightI;
  //debugOut.analog[29] = dynamicParams.heightD;
 
  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 @ 1801 - Rev 2074 → 2075

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