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


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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c @ 2218 - Rev 2160 → 2189