Subversion Repositories FlightCtrl

Rev

Rev 2160 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2160 Rev 2189
Line 1... Line 1...
1 
#include <inttypes.h>
 1 
#include <inttypes.h>
2 
#include "analog.h"
 - 
 
3 
#include "attitude.h"
 2 
#include "attitude.h"
4 
#include "configuration.h"
 3 
#include "configuration.h"
5 
#include "controlMixer.h"
 4 
#include "definitions.h"
6 
// for digital / LED debug.
 - 
 
7 
#include "output.h"
 5 
#include "debug.h"
8 
 
 - 
 
9 
// For scope debugging only!
 - 
 
10 
#include "rc.h"
 - 
 
Line 11... Line 6...
11 
 
 6 
 
Line 12... Line 7...
12 
#define INTEGRAL_LIMIT 100000
 7 
#define INTEGRAL_LIMIT 100000
Line 67... Line 62...
67 
      // Switch is OFF.
 62 
      // Switch is OFF.
68 
      setHeightLatch = 0;
 63 
      setHeightLatch = 0;
69 
    }
 64 
    }
70 
  } else {
 65 
  } else {
71 
    // Switch is not activated; take the "max-height" as the target height.
 66 
    // Switch is not activated; take the "max-height" as the target height.
72 
    setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it)
 67 
    setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting));
73 
  }
 68 
  }
Line 74... Line 69...
74 
 
 69 
 
75 
  /*
 70 
  /*
76 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
 71 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
Line 94... Line 89...
94 
  */
 89 
  */
95 
  //      uint8_t heightControlTestOscPeriod;
 90 
  //      uint8_t heightControlTestOscPeriod;
96 
  //      uint8_t heightControlTestOscAmplitude;
 91 
  //      uint8_t heightControlTestOscAmplitude;
Line 97... Line 92...
97 
 
 92 
 
- 
 
 93 
  hc_testOscPrescaler++;
98 
  hc_testOscPrescaler++;
 94 
 
99 
  if (hc_testOscPrescaler == 488) {
 95 
  if (hc_testOscPrescaler == 250) {
100 
          hc_testOscPrescaler = 0;
 96 
          hc_testOscPrescaler = 0;
101 
          hc_testOscTimer++;
 97 
          hc_testOscTimer++;
102 
          if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
 98 
          if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) {
103 
                  hc_testOscTimer = 0;
 99 
                  hc_testOscTimer = 0;
Line 125... Line 121...
125 
#define LOG_IHEIGHT_SCALE 24
 121 
#define LOG_IHEIGHT_SCALE 24
126 
#define LOG_DHEIGHT_SCALE 6
 122 
#define LOG_DHEIGHT_SCALE 6
Line 127... Line 123...
127 
 
 123 
 
128 
// takes 180-200 usec (with integral term). That is too heavy!!!
 124 
// takes 180-200 usec (with integral term). That is too heavy!!!
129 
// takes 100 usec without integral term.
 125 
// takes 100 usec without integral term.
130 
void HC_periodicTaskAndPRTY(int16_t* PRTY) {
 126 
void HC_periodicTaskAndRPTY(int16_t* RPTY) {
131 
  HC_periodicTask();
 127 
  HC_periodicTask();
132 
  int16_t throttle = PRTY[CONTROL_THROTTLE];
 128 
  int16_t throttle = RPTY[CONTROL_THROTTLE];
133 
  int32_t height = analog_getHeight();
 129 
  int32_t height = analog_getHeight();
134 
  int32_t heightError = targetHeight - height;
 130 
  int32_t heightError = targetHeight - height;
Line 135... Line 131...
135 
  int16_t dHeight = analog_getDHeight();
 131 
  int16_t dHeight = analog_getDHeight();
Line 204... Line 200...
204 
    }
 200 
    }
205 
  }
 201 
  }
Line 206... Line 202...
206 
 
 202 
 
Line 207... Line 203...
207 
  /* Experiment: Find hover-throttle */
 203 
  /* Experiment: Find hover-throttle */
208 
 
 204 
 
209 
#define DEFAULT_HOVERTHROTTLE 50
 205 
#define DEFAULT_HOVERTHROTTLE 200
210 
int32_t stronglyFilteredHeightDiff = 0;
 206 
int32_t stronglyFilteredHeightDiff = 0;
211 
// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
 207 
// uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE;
Line 225... Line 221...
225 
  } else
 221 
  } else
226 
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
 222 
    debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE;
Line 227... Line 223...
227 
 
 223 
 
Line 228... Line 224...
228 
    */
 224 
    */
229 
 
 225 
 
Line 230... Line 226...
230 
  PRTY[CONTROL_THROTTLE] = throttle;
 226 
  RPTY[CONTROL_THROTTLE] = throttle;
231 
}
 227 
}
232 
 
 228