Subversion Repositories FlightCtrl

Rev

Rev 2160 | Go to most recent revision | 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