WebSVN - FlightCtrl - Diff - Rev 2045 and 2048 - /branches/dongfang_FC


#include <inttypes.h>
#include "analog.h"
#include "attitude.h"
#include "uart0.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();
  //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;
 
    */
 
  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 @ 1841 - Rev 2045 → 2048

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