WebSVN - FlightCtrl - Diff - Rev 2052 and 2055 - /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 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_periodicTask(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[31] = (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 = rampedTargetHeight - height;
  int16_t dHeight = analog_getDHeight();
 
 
  debugOut.analog[14] = height/100L;
  debugOut.analog[15] = 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;
  }
 
  // 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;
 
    */
 
  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 - Rev 2052 → 2055

Rev 2052		Rev 2055
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 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_periodicTask(void) {	32	void HC_periodicTask(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[31] = (117100 - filteredAirPressure) / 100;	80	// debugOut.analog[31] = (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	void HC_periodicTaskAndPRTY(int16_t* PRTY) {	87	void HC_periodicTaskAndPRTY(int16_t* PRTY) {
88	HC_periodicTask();	88	HC_periodicTask();
89	int16_t throttle = PRTY[CONTROL_THROTTLE];	89	int16_t throttle = PRTY[CONTROL_THROTTLE];
90	int32_t height = analog_getHeight();	90	int32_t height = analog_getHeight();
91	int32_t heightError = rampedTargetHeight - height;	91	int32_t heightError = rampedTargetHeight - height;
92	int16_t dHeight = analog_getDHeight();	92	int16_t dHeight = analog_getDHeight();
93	//lastHeight = height;	-
-		93
-		94	debugOut.analog[14] = height/100L;
-		95	debugOut.analog[15] = dHeight;
94		96
95	if (heightError > 0) {	97	if (heightError > 0) {
96	debugOut.digital[0] \|= DEBUG_HEIGHT_DIFF;	98	debugOut.digital[0] \|= DEBUG_HEIGHT_DIFF;
97	} else {	99	} else {
98	debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;	100	debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
99	}	101	}
100		102
101	if (dHeight > 0) {	103	if (dHeight > 0) {
102	debugOut.digital[1] \|= DEBUG_HEIGHT_DIFF;	104	debugOut.digital[1] \|= DEBUG_HEIGHT_DIFF;
103	} else {	105	} else {
104	debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;	106	debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
105	}	107	}
106		108
107	// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....	109	// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
108	iHeight += heightError;	110	iHeight += heightError;
109		111
110	#define IHEIGHT_SCALE 24	112	#define IHEIGHT_SCALE 24
111	// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256	113	// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256
112	int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);	114	int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE);
113		115
114	if (dThrottleI > staticParams.heightControlMaxIntegral) {	116	if (dThrottleI > staticParams.heightControlMaxIntegral) {
115	dThrottleI = staticParams.heightControlMaxIntegral;	117	dThrottleI = staticParams.heightControlMaxIntegral;
116	iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;	118	iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
117	} else if (dThrottleI < -staticParams.heightControlMaxIntegral) {	119	} else if (dThrottleI < -staticParams.heightControlMaxIntegral) {
118	dThrottleI = -staticParams.heightControlMaxIntegral;	120	dThrottleI = -staticParams.heightControlMaxIntegral;
119	iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;	121	iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI;
120	}	122	}
121		123
122	int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;	124	int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
123	int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;	125	int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
124		126
125	debugOut.analog[24] = dThrottleP;	127	//debugOut.analog[24] = dThrottleP;
126	debugOut.analog[25] = dThrottleI;	128	//debugOut.analog[25] = dThrottleI;
127	debugOut.analog[26] = dThrottleD;	129	//debugOut.analog[26] = dThrottleD;
128		130
129	//debugOut.analog[27] = dynamicParams.heightP;	131	//debugOut.analog[27] = dynamicParams.heightP;
130	//debugOut.analog[28] = dynamicParams.heightI;	132	//debugOut.analog[28] = dynamicParams.heightI;
131	//debugOut.analog[29] = dynamicParams.heightD;	133	//debugOut.analog[29] = dynamicParams.heightD;
132		134
133	int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;	135	int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;
134		136
135	if (dThrottle > staticParams.heightControlMaxThrottleChange)	137	if (dThrottle > staticParams.heightControlMaxThrottleChange)
136	dThrottle = staticParams.heightControlMaxThrottleChange;	138	dThrottle = staticParams.heightControlMaxThrottleChange;
137	else if (dThrottle < -staticParams.heightControlMaxThrottleChange)	139	else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
138	dThrottle = -staticParams.heightControlMaxThrottleChange;	140	dThrottle = -staticParams.heightControlMaxThrottleChange;
-		141
139		142	/*
140	debugOut.analog[19] = throttle;	143	debugOut.analog[19] = throttle;
141	debugOut.analog[20] = dThrottle;	144	debugOut.analog[20] = dThrottle;
142	debugOut.analog[21] = height;	145	debugOut.analog[21] = height;
143	debugOut.analog[22] = rampedTargetHeight;	146	debugOut.analog[22] = rampedTargetHeight;
144	debugOut.analog[23] = heightError;	147	debugOut.analog[23] = heightError;
-		148	*/
145		149
146	if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {	150	if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
147	if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)	151	if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
148	\|\| (dynamicParams.heightSetting < 40 \|\| dynamicParams.heightSetting > 255 - 40)) {	152	\|\| (dynamicParams.heightSetting < 40 \|\| dynamicParams.heightSetting > 255 - 40)) {
149	// If switch is not in use --> Just apply height control.	153	// 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.	154	// If switch is in use --> only apply height control when switch is also ON.
151	throttle += dThrottle;	155	throttle += dThrottle;
152	}	156	}
153	}	157	}
154		158
155	/* Experiment: Find hover-throttle */	159	/* Experiment: Find hover-throttle */
156		160
157	#define DEFAULT_HOVERTHROTTLE 50	161	#define DEFAULT_HOVERTHROTTLE 50
158	int32_t stronglyFilteredHeightDiff = 0;	162	int32_t stronglyFilteredHeightDiff = 0;
159	// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;	163	// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
160	uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;	164	uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
161	#define HOVERTHROTTLEFILTER 25	165	#define HOVERTHROTTLEFILTER 25
162		166
163	stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff	167	stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff
164	* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;	168	* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
165	stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER	169	stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER
166	- 1) + throttle) / HOVERTHROTTLEFILTER;	170	- 1) + throttle) / HOVERTHROTTLEFILTER;
167		171
168	/*	172	/*
169	if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3	173	if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3
170	&& stronglyFilteredHeightDiff > -3) {	174	&& stronglyFilteredHeightDiff > -3) {
171	hoverThrottle = stronglyFilteredThrottle;	175	hoverThrottle = stronglyFilteredThrottle;
172	debugOut.digital[0] \|= DEBUG_HOVERTHROTTLE;	176	debugOut.digital[0] \|= DEBUG_HOVERTHROTTLE;
173	} else	177	} else
174	debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;	178	debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
175		179
176	*/	180	*/
177		181
178	PRTY[CONTROL_THROTTLE] = throttle;	182	PRTY[CONTROL_THROTTLE] = throttle;
179	}	183	}
180		184
181	/*	185	/*
182	For a variometer thingy:	186	For a variometer thingy:
183	When switch is thrown on, freeze throttle (capture it into variable)	187	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.	188	For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
185	Output = frozen throttle + whatever is computed +/-. Integral?	189	Output = frozen throttle + whatever is computed +/-. Integral?
186	*/	190	*/
187		191