WebSVN - FlightCtrl - Diff - Rev 2058 and 2059 - /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;
 
  // debugOut.analog[25] = dynamicParams.heightSetting;
 
  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) {
    if (dynamicParams.heightSetting >= 255/3) {
      // 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[24] = (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[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;
  }
 
  // 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 @ 1965 - Rev 2058 → 2059

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