Subversion Repositories FlightCtrl

Rev

Rev 2059 | Rev 2071 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2059 Rev 2069
Line 10... Line 10...
10 
// For scope debugging only!
 10 
// For scope debugging only!
11 
#include "rc.h"
 11 
#include "rc.h"
Line 12... Line 12...
12 
 
 12 
 
Line 13... Line 13...
13 
#define INTEGRAL_LIMIT 100000
 13 
#define INTEGRAL_LIMIT 100000
Line 14... Line 14...
14 
 
 14 
 
15 
#define LATCH_TIME 40
 15 
#define LATCH_TIME 3
Line 16... Line 16...
16 
 
 16 
 
17 
int32_t targetHeight;
 17 
int32_t targetHeight;
18 
int32_t rampedTargetHeight;
 18 
//int32_t rampedTargetHeight;
Line 19... Line 19...
19 
 
 19 
 
20 
uint8_t heightRampingTimer = 0;
 20 
//rampinguint8_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();
Line -... Line 26...
- 
 
 26 
  // This should also happen when height control is enabled in-flight.
- 
 
 27 
  /*rampedTargetHeight = */ targetHeight = analog_getHeight();
- 
 
 28 
  maxHeightThisFlight = 0;
- 
 
 29 
  iHeight = 0;
26 
  // This should also happen when height control is enabled in-flight.
 30 
}
27 
  rampedTargetHeight = targetHeight = analog_getHeight();
 31 
 
28 
  maxHeightThisFlight = 0;
 32 
uint8_t HC_isSwitchOn() {
Line 29... Line 33...
29 
  iHeight = 0;
 33 
  return (dynamicParams.heightSetting >= 255/3);
30 
}
 34 
}
Line 31... Line 35...
31 
 
 35 
 
Line 32... Line 36...
32 
void HC_periodicTask(void) {
 36 
void HC_periodicTask(void) {
33 
  int32_t height = analog_getHeight();
 - 
 
34 
  static uint8_t setHeightLatch = 0;
 - 
 
35 
 
 37 
  int32_t height = analog_getHeight();
36 
  if (height > maxHeightThisFlight)
 38 
  static uint8_t setHeightLatch = 0;
37 
    maxHeightThisFlight = height;
 39 
 
38 
 
 40 
  if (height > maxHeightThisFlight)
39 
  // debugOut.analog[25] = dynamicParams.heightSetting;
 41 
    maxHeightThisFlight = height;
40 
 
 42 
 
41 
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
 43 
  // debugOut.analog[25] = dynamicParams.heightSetting;
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.
 44 
 
43 
    // if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
 45 
  if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) {
44 
    if (dynamicParams.heightSetting >= 255/3) {
 46 
    if (HC_isSwitchOn()) {
45 
      // Switch is ON
 47 
      // Switch is ON
46 
      if (setHeightLatch <= LATCH_TIME) {
 48 
      if (setHeightLatch <= LATCH_TIME) {
47 
        if (setHeightLatch == LATCH_TIME) {
 49 
        if (setHeightLatch == LATCH_TIME) {
48 
          // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
 50 
          // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON.
49 
          rampedTargetHeight = targetHeight = height;
 51 
          /* rampedTargetHeight = */ targetHeight = height;
50 
        }
 52 
        }
51 
        // Time not yet reached.
 53 
        // Time not yet reached.
52 
        setHeightLatch++;
 54 
        setHeightLatch++;
Line -... Line 55...
- 
 
 55 
      }
53 
      }
 56 
    } else {
54 
    } else {
 57 
      // Switch is OFF.
55 
      // Switch is OFF.
 58 
      setHeightLatch = 0;
56 
      setHeightLatch = 0;
 59 
    }
57 
    }
 60 
  } else {
Line 76... Line 79...
76 
      } else {
 79 
      } else {
77 
        rampedTargetHeight = targetHeight;
 80 
        rampedTargetHeight = targetHeight;
78 
      }
 81 
      }
79 
    }
 82 
    }
80 
  }
 83 
  }
- 
 
 84 
  */
Line 81... Line 85...
81 
 
 85 
 
82 
  // height, in meters (so the division factor is: 100)
 86 
  // height, in meters (so the division factor is: 100)
83 
  // debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
 87 
  // debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
84 
  // Calculated 0 alt number: 108205
 88 
  // Calculated 0 alt number: 108205
Line 89... Line 93...
89 
// takes 100 usec without integral term.
 93 
// takes 100 usec without integral term.
90 
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
 94 
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
91 
  HC_periodicTask();
 95 
  HC_periodicTask();
92 
  int16_t throttle = PRTY[CONTROL_THROTTLE];
 96 
  int16_t throttle = PRTY[CONTROL_THROTTLE];
93 
  int32_t height = analog_getHeight();
 97 
  int32_t height = analog_getHeight();
94 
  int32_t heightError = rampedTargetHeight - height;
 98 
  int32_t heightError = /*ramped*/ targetHeight - height;
95 
  int16_t dHeight = analog_getDHeight();
 99 
  int16_t dHeight = analog_getDHeight();
Line 96... Line 100...
96 
 
 100 
 
97 
  debugOut.analog[22] = height/10L;
 101 
  debugOut.analog[22] = height/10L;
Line 149... Line 153...
149 
  debugOut.analog[22] = rampedTargetHeight;
 153 
  debugOut.analog[22] = rampedTargetHeight;
150 
  debugOut.analog[23] = heightError;
 154 
  debugOut.analog[23] = heightError;
151 
  */
 155 
  */
Line 152... Line 156...
152 
 
 156 
 
153 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
 157 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
154 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
 - 
 
155 
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
 158 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) || HC_isSwitchOn()) {
156 
      // If switch is not in use --> Just apply height control.
 159 
      // If switch is not in use --> Just apply height control.
157 
      // If switch is in use     --> only apply height control when switch is also ON.
 160 
      // If switch is in use     --> only apply height control when switch is also ON.
158 
      throttle += dThrottle;
 161 
      throttle += dThrottle;
159 
    }
 162 
    }