WebSVN - FlightCtrl - Diff - Rev 1986 and 2026 - /branches/dongfang_FC


#include <inttypes.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 DEBUGINTEGRAL 0
 #define DEBUGDIFFERENTIAL 0
 #define DEBUGHOVERTHROTTLE 0
 #define DEBUGHEIGHTSWITCH 0
 */
 
#define LATCH_TIME 40
 
int32_t groundPressure;
int32_t targetHeight;
int32_t rampedTargetHeight;
 
uint8_t heightRampingTimer = 0;
int32_t maxHeightThisFlight;
int32_t iHeight;
 
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();
  maxHeightThisFlight = 0;
  iHeight = 0;
}
 
void HC_update(void) {
  int32_t height = 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.
          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 * 5L - 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] = filteredAirPressure / 10;
  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 = 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 (heightError > 0) {
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
  } else if (heightError < 0) {
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
    debugOut.digital[1] |= 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) >> 9)
 
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */-((dHeight * staticParams.heightD) >> 7);
 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
    dThrottle = staticParams.heightControlMaxThrottleChange;
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
    dThrottle = -staticParams.heightControlMaxThrottleChange;
 
  debugOut.analog[19] = rampedTargetHeight;
  debugOut.analog[21] = dThrottle;
  debugOut.analog[26] = height;
 
  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;
    // DebugOut.Analog[18] = hoverThrottle;
  } else
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
 
  //debugOut.analog[20] = dThrottle;
 
  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?
 */
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/heightControl.c @ 1989 - Rev 1986 → 2026