WebSVN - FlightCtrl - Diff - Rev 1775 and 1796 - /branches/dongfang_FC


#include <inttypes.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 DEBUGINTEGRAL 0
#define DEBUGDIFFERENTIAL 0
#define DEBUGHOVERTHROTTLE 0
#define DEBUGHEIGHTSWITCH 0
*/
 
#define LATCH_TIME 40
 
int32_t groundPressure;
 
int32_t targetHeight;
int32_t rampedTargetHeight;
 
#define DEFAULT_HOVERTHROTTLE 50
int32_t stronglyFilteredHeightDiff = 0;
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
#define HOVERTHROTTLEFILTER 25
 
uint8_t heightRampingTimer = 0;
int32_t maxHeight;
int32_t iHeight;
/*
  These parameters are free to take:
        uint8_t HeightMinGas;           // Value : 0-100
        uint8_t HeightD;                // Value : 0-250
        uint8_t MaxHeight;              // Value : 0-32
        uint8_t HeightP;                // Value : 0-32
        uint8_t Height_Gain;            // Value : 0-50
        uint8_t Height_ACC_Effect;      // Value : 0-250
 
 */
 
int32_t getHeight(void) {
  return groundPressure - filteredAirPressure;
}
 
void HC_setGround(void) {
  groundPressure = filteredAirPressure;
  // This should also happen when height control is enabled in-flight.
  rampedTargetHeight = getHeight();
  maxHeight = 0;
  iHeight = 0;
}
 
void HC_update(void) {
  int32_t height = getHeight();
  static uint8_t setHeightLatch = 0;
 
  if (height > maxHeight)
    maxHeight = height;
 
  if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
    // If switch is activated in config
    DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
    if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 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.
          targetHeight = height;
          DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH;
        }
        // Time not yet reached.
        setHeightLatch++;
      }
    } else {
      // Switch is OFF.
      setHeightLatch = 0;
      DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
    }
  } else {
    // Switch is not activated; take the "max-height" as the target height.
    DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
    targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
  }
 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) {
    heightRampingTimer = 0;
    if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
      rampedTargetHeight += staticParams.Height_Gain;
    } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
      rampedTargetHeight -= staticParams.Height_Gain;
    }
  }
 
  // height, in meters (so the division factor is: 100)
  DebugOut.Analog[30] = height / 100;
}
 
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH
// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH
 
// 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 = getHeight();
  int32_t heightError = rampedTargetHeight - height;
 
  static int32_t lastHeight;
 
  int16_t dHeight = height - lastHeight;
  lastHeight = height;
 
  // DebugOut.Analog[20] = dHeight;
  // DebugOut.Analog[21] = dynamicParams.MaxHeight;
 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... 
  // iHeight += heightError;
 
  if (dHeight > 0) {
    DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF;
    DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF;
  } else if (dHeight < 0) {
    DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF;
    DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF;
  }
 
  /*
  if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
  else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
  else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
  else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
  */
 
  int16_t dThrottle = heightError * staticParams.HeightP / 1000 /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */ - dHeight * staticParams.HeightD;
 
  // the "minGas" is now a limit for how much up / down the throttle can be varied
  if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas;
  else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas;
 
  //DebugOut.Analog[18] = dThrottle;
  //DebugOut.Analog[19] = iHeight / 10000L;
 
  // TODO: Eliminate repitition.
  if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
    if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 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 */
  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;
    // DebugOut.Analog[18] = 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 @ 1991 - Rev 1775 → 1796

Rev 1775		Rev 1796
1	#include <inttypes.h>	1	#include <inttypes.h>
2	#include "attitude.h"	2	#include "attitude.h"
3	#include "uart0.h"	3	#include "uart0.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	/*	15	/*
16	#define DEBUGINTEGRAL 0	16	#define DEBUGINTEGRAL 0
17	#define DEBUGDIFFERENTIAL 0	17	#define DEBUGDIFFERENTIAL 0
18	#define DEBUGHOVERTHROTTLE 0	18	#define DEBUGHOVERTHROTTLE 0
19	#define DEBUGHEIGHTSWITCH 0	19	#define DEBUGHEIGHTSWITCH 0
20	*/	20	*/
21		21
22	#define LATCH_TIME 40	22	#define LATCH_TIME 40
23		23
24	int32_t groundPressure;	24	int32_t groundPressure;
25		25
26	int32_t targetHeight;	26	int32_t targetHeight;
27	int32_t rampedTargetHeight;	27	int32_t rampedTargetHeight;
28		28
29	#define DEFAULT_HOVERTHROTTLE 50	29	#define DEFAULT_HOVERTHROTTLE 50
30	int32_t stronglyFilteredHeightDiff = 0;	30	int32_t stronglyFilteredHeightDiff = 0;
31	uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;	31	uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
32	uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;	32	uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE;
33	#define HOVERTHROTTLEFILTER 25	33	#define HOVERTHROTTLEFILTER 25
34		34
35	uint8_t heightRampingTimer = 0;	35	uint8_t heightRampingTimer = 0;
36	int32_t maxHeight;	36	int32_t maxHeight;
37	int32_t iHeight;	37	int32_t iHeight;
38	/*	38	/*
39	These parameters are free to take:	39	These parameters are free to take:
40	uint8_t HeightMinGas; // Value : 0-100	40	uint8_t HeightMinGas; // Value : 0-100
41	uint8_t HeightD; // Value : 0-250	41	uint8_t HeightD; // Value : 0-250
42	uint8_t MaxHeight; // Value : 0-32	42	uint8_t MaxHeight; // Value : 0-32
43	uint8_t HeightP; // Value : 0-32	43	uint8_t HeightP; // Value : 0-32
44	uint8_t Height_Gain; // Value : 0-50	44	uint8_t Height_Gain; // Value : 0-50
45	uint8_t Height_ACC_Effect; // Value : 0-250	45	uint8_t Height_ACC_Effect; // Value : 0-250
46		-
47	*/	46	*/
48		47
49	int32_t getHeight(void) {	48	int32_t getHeight(void) {
50	return groundPressure - filteredAirPressure;	49	return groundPressure - filteredAirPressure;
51	}	50	}
52		51
53	void HC_setGround(void) {	52	void HC_setGround(void) {
54	groundPressure = filteredAirPressure;	53	groundPressure = filteredAirPressure;
55	// This should also happen when height control is enabled in-flight.	54	// This should also happen when height control is enabled in-flight.
56	rampedTargetHeight = getHeight();	55	rampedTargetHeight = getHeight();
57	maxHeight = 0;	56	maxHeight = 0;
58	iHeight = 0;	57	iHeight = 0;
59	}	58	}
60		59
61	void HC_update(void) {	60	void HC_update(void) {
62	int32_t height = getHeight();	61	int32_t height = getHeight();
63	static uint8_t setHeightLatch = 0;	62	static uint8_t setHeightLatch = 0;
64		63
65	if (height > maxHeight)	64	if (height > maxHeight)
66	maxHeight = height;	65	maxHeight = height;
67		66
68	if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {	67	if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
69	// If switch is activated in config	68	// If switch is activated in config
70	DebugOut.Digital[0] \|= DEBUG_HEIGHT_SWITCH;	69	DebugOut.Digital[0] \|= DEBUG_HEIGHT_SWITCH;
71	if (dynamicParams.MaxHeight < 40 \|\| dynamicParams.MaxHeight > 255-40) {	70	if (dynamicParams.MaxHeight < 40 \|\| dynamicParams.MaxHeight > 255-40) {
72	// Switch is ON	71	// Switch is ON
73	if (setHeightLatch <= LATCH_TIME) {	72	if (setHeightLatch <= LATCH_TIME) {
74	if (setHeightLatch == LATCH_TIME) {	73	if (setHeightLatch == LATCH_TIME) {
75	// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.	74	// Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
76	targetHeight = height;	75	targetHeight = height;
77	DebugOut.Digital[1] \|= DEBUG_HEIGHT_SWITCH;	76	DebugOut.Digital[1] \|= DEBUG_HEIGHT_SWITCH;
78	}	77	}
79	// Time not yet reached.	78	// Time not yet reached.
80	setHeightLatch++;	79	setHeightLatch++;
81	}	80	}
82	} else {	81	} else {
83	// Switch is OFF.	82	// Switch is OFF.
84	setHeightLatch = 0;	83	setHeightLatch = 0;
85	DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;	84	DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
86	}	85	}
87	} else {	86	} else {
88	// Switch is not activated; take the "max-height" as the target height.	87	// Switch is not activated; take the "max-height" as the target height.
89	DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;	88	DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
90	targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;	89	targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
91	}	90	}
92		91
93	if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) {	92	if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) {
94	heightRampingTimer = 0;	93	heightRampingTimer = 0;
95	if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {	94	if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
96	rampedTargetHeight += staticParams.Height_Gain;	95	rampedTargetHeight += staticParams.Height_Gain;
97	} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {	96	} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
98	rampedTargetHeight -= staticParams.Height_Gain;	97	rampedTargetHeight -= staticParams.Height_Gain;
99	}	98	}
100	}	99	}
101		100
102	//DebugOut.Analog[16] = (int16_t)(height / 10);	101	// height, in meters (so the division factor is: 100)
103	//DebugOut.Analog[17] = (int16_t)(rampedTargetHeight / 10);	102	DebugOut.Analog[30] = height / 100;
104	}	103	}
105		104
106	// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL	105	// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
107	// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH	106	// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH
108	// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH	107	// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH
109		108
110	// takes 180-200 usec (with integral term). That is too heavy!!!	109	// takes 180-200 usec (with integral term). That is too heavy!!!
111	// takes 100 usec without integral term.	110	// takes 100 usec without integral term.
112	uint16_t HC_getThrottle(uint16_t throttle) {	111	uint16_t HC_getThrottle(uint16_t throttle) {
113	int32_t height = getHeight();	112	int32_t height = getHeight();
114	int32_t heightError = rampedTargetHeight - height;	113	int32_t heightError = rampedTargetHeight - height;
115		114
116	static int32_t lastHeight;	115	static int32_t lastHeight;
117		116
118	int16_t dHeight = height - lastHeight;	117	int16_t dHeight = height - lastHeight;
119	lastHeight = height;	118	lastHeight = height;
120		119
121	// DebugOut.Analog[20] = dHeight;	120	// DebugOut.Analog[20] = dHeight;
122	// DebugOut.Analog[21] = dynamicParams.MaxHeight;	121	// DebugOut.Analog[21] = dynamicParams.MaxHeight;
123		122
124	// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....	123	// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
125	// iHeight += heightError;	124	// iHeight += heightError;
126		125
127	if (dHeight > 0) {	126	if (dHeight > 0) {
128	DebugOut.Digital[0] \|= DEBUG_HEIGHT_DIFF;	127	DebugOut.Digital[0] \|= DEBUG_HEIGHT_DIFF;
129	DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF;	128	DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF;
130	} else if (dHeight < 0) {	129	} else if (dHeight < 0) {
131	DebugOut.Digital[1] \|= DEBUG_HEIGHT_DIFF;	130	DebugOut.Digital[1] \|= DEBUG_HEIGHT_DIFF;
132	DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF;	131	DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF;
133	}	132	}
134		133
135	/*	134	/*
136	if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}	135	if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}}
137	else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}	136	else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
138	else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}	137	else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
139	else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}	138	else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
140	*/	139	*/
141		140
142	int16_t dThrottle = heightError * staticParams.HeightP / 1000 /+ iHeight / 10000L staticParams.Height_ACC_Effect / - dHeight staticParams.HeightD;	141	int16_t dThrottle = heightError * staticParams.HeightP / 1000 /+ iHeight / 10000L staticParams.Height_ACC_Effect / - dHeight staticParams.HeightD;
143		142
144	// the "minGas" is now a limit for how much up / down the throttle can be varied	143	// the "minGas" is now a limit for how much up / down the throttle can be varied
145	if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas;	144	if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas;
146	else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas;	145	else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas;
147		146
148	//DebugOut.Analog[18] = dThrottle;	147	//DebugOut.Analog[18] = dThrottle;
149	//DebugOut.Analog[19] = iHeight / 10000L;	148	//DebugOut.Analog[19] = iHeight / 10000L;
150		149
151	// TODO: Eliminate repitition.	150	// TODO: Eliminate repitition.
152	if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {	151	if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
153	if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) \|\| (dynamicParams.MaxHeight < 40 \|\| dynamicParams.MaxHeight > 255-40)) {	152	if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) \|\| (dynamicParams.MaxHeight < 40 \|\| dynamicParams.MaxHeight > 255-40)) {
154	// If switch is not in use --> Just apply height control.	153	// If switch is not in use --> Just apply height control.
155	// 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.
156	throttle += dThrottle;	155	throttle += dThrottle;
157	}	156	}
158	}	157	}
159		158
160	/* Experiment: Find hover-throttle */	159	/* Experiment: Find hover-throttle */
161	stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;	160	stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER;
162	stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER;	161	stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER;
163		162
164	if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) {	163	if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) {
165	hoverThrottle = stronglyFilteredThrottle;	164	hoverThrottle = stronglyFilteredThrottle;
166	DebugOut.Digital[0] \|= DEBUG_HOVERTHROTTLE;	165	DebugOut.Digital[0] \|= DEBUG_HOVERTHROTTLE;
167	// DebugOut.Analog[18] = hoverThrottle;	166	// DebugOut.Analog[18] = hoverThrottle;
168	} else	167	} else
169	DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE;	168	DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE;
170	return throttle;	169	return throttle;
171	}	170	}
172		171
173	/*	172	/*
174	For a variometer thingy:	173	For a variometer thingy:
175	When switch is thrown on, freeze throttle (capture it into variable)	174	When switch is thrown on, freeze throttle (capture it into variable)
176	For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.	175	For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping.
177	Output = frozen throttle + whatever is computed +/-. Integral?	176	Output = frozen throttle + whatever is computed +/-. Integral?
178	*/	177	*/
179		178