Subversion Repositories FlightCtrl

Rev

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

Rev 2052 Rev 2055
Line 88... Line 88...
88 
  HC_periodicTask();
 88 
  HC_periodicTask();
89 
  int16_t throttle = PRTY[CONTROL_THROTTLE];
 89 
  int16_t throttle = PRTY[CONTROL_THROTTLE];
90 
  int32_t height = analog_getHeight();
 90 
  int32_t height = analog_getHeight();
91 
  int32_t heightError = rampedTargetHeight - height;
 91 
  int32_t heightError = rampedTargetHeight - height;
92 
  int16_t dHeight = analog_getDHeight();
 92 
  int16_t dHeight = analog_getDHeight();
93 
  //lastHeight = height;
 - 
 
Line -... Line 93...
- 
 
 93 
 
- 
 
 94 
  debugOut.analog[14] = height/100L;
- 
 
 95 
  debugOut.analog[15] = dHeight;
94 
 
 96 
 
95 
  if (heightError > 0) {
 97 
  if (heightError > 0) {
96 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
 98 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
97 
  } else {
 99 
  } else {
98 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
 100 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
Line 120... Line 122...
120 
  }
 122 
  }
Line 121... Line 123...
121 
 
 123 
 
122 
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
 124 
  int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
Line 123... Line 125...
123 
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
 125 
  int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
124 
 
 126 
 
125 
  debugOut.analog[24] = dThrottleP;
 127 
  //debugOut.analog[24] = dThrottleP;
Line 126... Line 128...
126 
  debugOut.analog[25] = dThrottleI;
 128 
  //debugOut.analog[25] = dThrottleI;
127 
  debugOut.analog[26] = dThrottleD;
 129 
  //debugOut.analog[26] = dThrottleD;
128 
 
 130 
 
Line 135... Line 137...
135 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
 137 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
136 
    dThrottle = staticParams.heightControlMaxThrottleChange;
 138 
    dThrottle = staticParams.heightControlMaxThrottleChange;
137 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
 139 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
138 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
 140 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
Line -... Line 141...
- 
 
 141 
 
139 
 
 142 
  /*
140 
  debugOut.analog[19] = throttle;
 143 
  debugOut.analog[19] = throttle;
141 
  debugOut.analog[20] = dThrottle;
 144 
  debugOut.analog[20] = dThrottle;
142 
  debugOut.analog[21] = height;
 145 
  debugOut.analog[21] = height;
143 
  debugOut.analog[22] = rampedTargetHeight;
 146 
  debugOut.analog[22] = rampedTargetHeight;
- 
 
 147 
  debugOut.analog[23] = heightError;
Line 144... Line 148...
144 
  debugOut.analog[23] = heightError;
 148 
  */
145 
 
 149 
 
146 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
 150 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
147 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
 151 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)