Rev 23 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 23 | Rev 24 | ||
|---|---|---|---|
| Line 33... | Line 33... | ||
| 33 | volatile uint16_t samplePeriod = 0; |
33 | volatile uint16_t samplePeriod = 0; |
| 34 | 34 | ||
| 35 | static signed phaseSamp = 0; |
35 | static signed phaseSamp = 0; |
| 36 | static uint8_t validPhaseSamp = 0; |
36 | static uint8_t validPhaseSamp = 0; |
| 37 | 37 | ||
| - | 38 | static uint8_t locked = 0; // in lock if true |
|
| 38 | int gainControl = 1000; |
39 | int gainControl = 1000; |
| 39 | 40 | ||
| 40 | uint16_t rpm; |
41 | uint16_t rpm; |
| 41 | signed count; |
42 | signed count; |
| 42 | signed delta; |
43 | signed delta; |
| Line 48... | Line 49... | ||
| 48 | } |
49 | } |
| 49 | 50 | ||
| 50 | void |
51 | void |
| 51 | adjustRPM (void) |
52 | adjustRPM (void) |
| 52 | { |
53 | { |
| 53 | if (rpm < 600) |
54 | if (rpm < 500) |
| 54 | rpm = 600; |
55 | rpm = 500; |
| 55 | if (rpm > 5000) |
56 | if (rpm > 10000) |
| 56 | rpm = 5000; |
57 | rpm = 10000; |
| 57 | 58 | ||
| 58 | } |
59 | } |
| 59 | 60 | ||
| 60 | uint16_t |
61 | uint16_t |
| 61 | setRPM (uint16_t rpm_) |
62 | setRPM (uint16_t rpm_) |
| 62 | { |
63 | { |
| 63 | if (rpm_ >= 600 && rpm_ < 6000) |
64 | if (rpm_ >= 500 && rpm_ < 10000) |
| 64 | { |
65 | { |
| 65 | rpm = rpm_; |
66 | rpm = rpm_; |
| 66 | adjustRPM (); |
67 | adjustRPM (); |
| 67 | } |
68 | } |
| 68 | return halfRot; |
69 | return halfRot; |
| Line 94... | Line 95... | ||
| 94 | 95 | ||
| 95 | void |
96 | void |
| 96 | processPhase (void) |
97 | processPhase (void) |
| 97 | { |
98 | { |
| 98 | // lpcl |
99 | // lpcl |
| 99 | const signed pdClip = 10000; |
100 | const signed pdClip = 1000; |
| 100 | static signed pd; |
101 | static signed pd; |
| 101 | if (validPhaseSamp) |
102 | if (validPhaseSamp) |
| 102 | { |
103 | { |
| 103 | chSysLock (); |
104 | chSysLock (); |
| 104 | 105 | ||
| Line 119... | Line 120... | ||
| 119 | delta = pd; |
120 | delta = pd; |
| 120 | 121 | ||
| 121 | static int sampleAverage = 0; |
122 | static int sampleAverage = 0; |
| 122 | 123 | ||
| 123 | static int phaseAverage = 0; |
124 | static int phaseAverage = 0; |
| 124 | const int freqScale = 16; |
125 | const int freqScale = 1024; // scale up calculations |
| 125 | - | ||
| 126 | const int periodScale= 4; // 1/periodScale of difference between period measured and average is added to period. |
- | |
| 127 | const int phaseScale = 50; |
- | |
| 128 | 126 | ||
| - | 127 | const int periodScale = 8; // 1/periodScale of difference between period measured and average is added to period. |
|
| - | 128 | const int phaseScale = 500; |
|
| - | 129 | const int phaseLim = 1; // total contribution of phase accumulator |
|
| 129 | // measure sample period devi |
130 | // measure sample period devi |
| - | 131 | sampleAverage = sampleAverage |
|
| 130 | sampleAverage = sampleAverage + (samplePeriod *freqScale - sampleAverage) / periodScale; |
132 | + (samplePeriod * freqScale - sampleAverage) / periodScale; |
| 131 | 133 | ||
| 132 | int32_t arr; |
134 | int32_t arr; |
| 133 | 135 | ||
| 134 | // if(lock) |
136 | // if(lock) |
| 135 | 137 | ||
| 136 | int intSample = sampleAverage / freqScale; |
138 | int intSample = sampleAverage; |
| 137 | - | ||
| 138 | int deltaPd = pd / phaseScale; |
- | |
| 139 | 139 | ||
| - | 140 | // dont phase lock until r and v in lock |
|
| 140 | if (deltaPd == 0) |
141 | if (locked == 1) |
| 141 | { |
142 | { |
| 142 | if (pd > 0) |
143 | phaseAverage += pd; |
| 143 | deltaPd = 1; |
144 | } |
| 144 | 145 | else |
|
| 145 | if (pd < 0) |
146 | { |
| 146 | deltaPd = -1; |
147 | phaseAverage = 0; |
| 147 | } |
148 | } |
| 148 | 149 | ||
| - | 150 | if(phaseAverage > phaseScale * phaseLim) |
|
| 149 | arr = intSample + deltaPd; |
151 | phaseAverage = phaseScale * phaseLim; |
| - | 152 | if (phaseAverage < -phaseScale * phaseLim) |
|
| - | 153 | phaseAverage = -phaseScale *phaseLim; |
|
| - | 154 | ||
| - | 155 | arr = sampleAverage / freqScale + phaseAverage / phaseScale; |
|
| 150 | 156 | ||
| 151 | // clamp values |
157 | // clamp values |
| 152 | 158 | ||
| 153 | if (arr > 65535) |
159 | if (arr > 65535) |
| 154 | arr = 65535; |
160 | arr = 65535; |
| Line 157... | Line 163... | ||
| 157 | 163 | ||
| 158 | count = arr; |
164 | count = arr; |
| 159 | 165 | ||
| 160 | TIM2->ARR = arr - 1; |
166 | TIM2->ARR = arr - 1; |
| 161 | 167 | ||
| 162 | nominal = intSample * (long) (phase10) / 3600; |
168 | nominal = count * (long) (phase10) / 3600; |
| 163 | 169 | ||
| 164 | float nomRPM = 30E6 / (MICROSECS_PULSE * arr); |
170 | float nomRPM = 30E6 / (MICROSECS_PULSE * arr); |
| 165 | 171 | ||
| 166 | rpm = nomRPM; |
172 | rpm = nomRPM; |
| 167 | 173 | ||
| Line 275... | Line 281... | ||
| 275 | } |
281 | } |
| 276 | 282 | ||
| 277 | validPhaseSamp = 1; |
283 | validPhaseSamp = 1; |
| 278 | refCount = 0; |
284 | refCount = 0; |
| 279 | varCount = 0; |
285 | varCount = 0; |
| 280 | - | ||
| - | 286 | locked = 1; |
|
| 281 | } |
287 | } |
| 282 | 288 | ||
| 283 | // frequency error, should deal with by direct period measurement |
- | |
| - | 289 | ||
| 284 | if (refCount > 1 || varCount > 1) |
290 | if (refCount > 1 || varCount > 1) |
| 285 | { |
291 | { |
| 286 | refCount = 0; |
292 | refCount = 0; |
| 287 | varCount = 0; |
293 | varCount = 0; |
| 288 | - | ||
| - | 294 | locked = 0; |
|
| 289 | } |
295 | } |
| 290 | 296 | ||
| 291 | } |
297 | } |
| 292 | 298 | ||