WebSVN - FlightCtrl - Diff - Rev 2071 and 2073 - /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 targetHeight;
//int32_t rampedTargetHeight;
 
//rampinguint8_t heightRampingTimer = 0;
int32_t maxHeightThisFlight;
int32_t iHeight;
 
void HC_setGround(void) {
  analog_setGround();
  // This should also happen when height control is enabled in-flight.
  /*rampedTargetHeight = */ targetHeight = analog_getHeight();
  maxHeightThisFlight = 0;
  iHeight = 0;
}
 
uint8_t HC_isSwitchOn() {
  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.
          /* rampedTargetHeight = */ targetHeight = height;
          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.
    targetHeight = (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;
      }
    }
  }
  */
 
  // 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
}
 
// 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 << 10;
 
  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) >> 10;
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
 
  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?
 */
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c @ 1965 - Rev 2071 → 2073