Rev 14 | Rev 18 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 14 | Rev 17 | ||
|---|---|---|---|
| Line 34... | Line 34... | ||
| 34 | volatile uint16_t sampleDiffBuff[SAMPLE_BUFF_SIZE]; |
34 | volatile uint16_t sampleDiffBuff[SAMPLE_BUFF_SIZE]; |
| 35 | volatile uint16_t samplePeriodBuff[SAMPLE_BUFF_SIZE]; |
35 | volatile uint16_t samplePeriodBuff[SAMPLE_BUFF_SIZE]; |
| 36 | volatile uint8_t refCountBuff[SAMPLE_BUFF_SIZE]; |
36 | volatile uint8_t refCountBuff[SAMPLE_BUFF_SIZE]; |
| 37 | volatile uint8_t varCountBuff[SAMPLE_BUFF_SIZE]; |
37 | volatile uint8_t varCountBuff[SAMPLE_BUFF_SIZE]; |
| 38 | 38 | ||
| - | 39 | ||
| - | 40 | int gainControl = 1000 ; |
|
| - | 41 | ||
| 39 | volatile uint8_t haveSlowPulse = 0; |
42 | volatile uint8_t haveSlowPulse = 0; |
| 40 | 43 | ||
| 41 | uint16_t rpm; |
44 | uint16_t rpm; |
| 42 | uint16_t count; |
45 | signed count; |
| 43 | uint16_t delta; |
46 | signed delta; |
| 44 | 47 | ||
| 45 | void initTimer2() |
48 | void initTimer2() |
| 46 | { |
49 | { |
| 47 | rccEnableTIM2(FALSE); |
50 | rccEnableTIM2(FALSE); |
| 48 | rccResetTIM2(); |
51 | rccResetTIM2(); |
| Line 126... | Line 129... | ||
| 126 | uint16_t getRPM(void) |
129 | uint16_t getRPM(void) |
| 127 | { |
130 | { |
| 128 | return rpm; |
131 | return rpm; |
| 129 | } |
132 | } |
| 130 | 133 | ||
| 131 | unsigned getDelta(void) |
134 | signed getDelta(void) |
| 132 | { |
135 | { |
| 133 | return delta; |
136 | return delta; |
| 134 | } |
137 | } |
| 135 | 138 | ||
| 136 | unsigned getCount(void) |
139 | signed getCount(void) |
| 137 | { |
140 | { |
| 138 | return count; |
141 | return count; |
| 139 | } |
142 | } |
| 140 | 143 | ||
| - | 144 | void setGain(int gain) |
|
| - | 145 | { |
|
| - | 146 | gainControl = gain; |
|
| - | 147 | } |
|
| - | 148 | ||
| 141 | 149 | ||
| 142 | uint8_t slowPulse(void) |
150 | uint8_t slowPulse(void) |
| 143 | { |
151 | { |
| 144 | return haveSlowPulse; |
152 | return haveSlowPulse; |
| 145 | } |
153 | } |
| Line 183... | Line 191... | ||
| 183 | } |
191 | } |
| 184 | else if (varCountBuff[index] > 1) |
192 | else if (varCountBuff[index] > 1) |
| 185 | { |
193 | { |
| 186 | pd = (-varCountBuff[index] * 32768L); |
194 | pd = (-varCountBuff[index] * 32768L); |
| 187 | } |
195 | } |
| 188 | float delta_phi = pd/ 2000; |
196 | float delta_phi = pd/ (gainControl * 1.0); |
| 189 | 197 | ||
| 190 | 198 | ||
| - | 199 | delta = delta_phi; |
|
| - | 200 | ||
| 191 | 201 | ||
| 192 | 202 | ||
| 193 | // phase detector returns +/ |
203 | // phase detector returns +/ |
| 194 | 204 | ||
| 195 | 205 | ||
| 196 | 206 | ||
| 197 | float const wn = 0.01f; |
207 | float const wn = 0.03f; |
| 198 | float const zeta = 0.707f; |
208 | float const zeta = 0.707f; |
| 199 | float const K = 1; |
209 | float const K = 1; |
| 200 | 210 | ||
| 201 | float const t1 = K/(wn*wn); |
211 | float const t1 = K/(wn*wn); |
| 202 | float const t2 = 2 * zeta/wn; |
212 | float const t2 = 2 * zeta/wn; |
| Line 215... | Line 225... | ||
| 215 | v2=v1; v1=v0; |
225 | v2=v1; v1=v0; |
| 216 | v0 = delta_phi -v1 *a1 -v2 *a2; |
226 | v0 = delta_phi -v1 *a1 -v2 *a2; |
| 217 | 227 | ||
| 218 | 228 | ||
| 219 | phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ; |
229 | phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ; |
| 220 | delta = phi_hat; |
- | |
| 221 | 230 | ||
| 222 | // 6.283 = 1.0 Hz |
231 | // 6.283 = 1.0 Hz |
| 223 | 232 | ||
| 224 | // 62.2 = 10Hz |
233 | // 62.2 = 10Hz |
| 225 | // decide on whether to go for forcing loop or to track |
234 | // decide on whether to go for forcing loop or to track |
| Line 310... | Line 319... | ||
| 310 | // |
319 | // |
| 311 | 320 | ||
| 312 | refCountBuff[sampleRefCount] = refCount; |
321 | refCountBuff[sampleRefCount] = refCount; |
| 313 | varCountBuff[sampleRefCount] = varCount; |
322 | varCountBuff[sampleRefCount] = varCount; |
| 314 | 323 | ||
| 315 | - | ||
| 316 | haveSlowPulse = (varCount > 20); |
324 | haveSlowPulse = (varCount > 20); |
| 317 | - | ||
| 318 | - | ||
| 319 | 325 | ||
| 320 | sampleDiffBuff[sampleRefCount] = sampleRef - sampleVar; |
326 | sampleDiffBuff[sampleRefCount] = sampleRef - sampleVar; |
| 321 | 327 | ||
| 322 | samplePeriodBuff[sampleRefCount] = samplePeriod; |
328 | samplePeriodBuff[sampleRefCount] = samplePeriod; |
| 323 | 329 | ||