WebSVN - FlightCtrl - Diff - Rev 2073 and 2074 - /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() {
  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;
 
  //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?
 */
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c - Rev 2073 → 2074