WebSVN - FlightCtrl - Diff - Rev 2035 and 2036 - /branches/dongfang_FC


#include <inttypes.h>
#include "analog.h"
#include "attitude.h"
#include "uart0.h"
#include "configuration.h"
 
// for digital / LED debug.
#include "output.h"
 
// For scope debugging only!
#include "rc.h"
 
#define INTEGRAL_LIMIT 100000
 
#define LATCH_TIME 40
 
int32_t targetHeight;
int32_t rampedTargetHeight;
 
uint8_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;
}
 
void HC_update(void) {
  int32_t height = analog_getHeight();
  static uint8_t setHeightLatch = 0;
 
  if (height > maxHeightThisFlight)
    maxHeightThisFlight = height;
 
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
    // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
      // 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;
        }
        // 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[30] = (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.
uint16_t HC_getThrottle(uint16_t throttle) {
  int32_t height = analog_getHeight();
  int32_t heightError = rampedTargetHeight - height;
 
  //static int32_t lastHeight;
  int16_t dHeight = analog_getDHeight();
  //lastHeight = height;
 
  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;
  }
 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
  iHeight += heightError;
 
#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.heightControlMaxIntegral) {
    dThrottleI = staticParams.heightControlMaxIntegral;
    iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
  } else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
    dThrottleI = -staticParams.heightControlMaxIntegral;
    iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
  }
 
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
 
  debugOut.analog[24] = dThrottleP;
  debugOut.analog[25] = dThrottleI;
  debugOut.analog[26] = dThrottleD;
 
  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)
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
      // 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;
 
    */
 
  return 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 2035 → 2036

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