Rev 17 | Rev 19 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 17 | Rev 18 | ||
|---|---|---|---|
| Line 19... | Line 19... | ||
| 19 | #define BREAKER_COUNT_MIN (1E6/(MICROSECS_PULSE * 300)) |
19 | #define BREAKER_COUNT_MIN (1E6/(MICROSECS_PULSE * 300)) |
| 20 | 20 | ||
| 21 | #define COUNT_FROM_RPM(RPM) ((1E6/(MICROSECS_PULSE * 30 / (RPM ) ))) |
21 | #define COUNT_FROM_RPM(RPM) ((1E6/(MICROSECS_PULSE * 30 / (RPM ) ))) |
| 22 | 22 | ||
| 23 | 23 | ||
| 24 | - | ||
| 25 | - | ||
| 26 | #define SAMPLE_BUFF_SIZE 16 |
- | |
| 27 | uint16_t nominal = 0; |
24 | uint16_t nominal = 0; |
| 28 | uint16_t halfRot; |
25 | uint16_t halfRot; |
| 29 | uint16_t phase10 = 100; // 10 degrees |
26 | uint16_t phase10 = 100; // 10 degrees |
| 30 | volatile uint16_t sampleRefCount = 0; |
27 | volatile uint16_t sampleRefCount = 0; |
| 31 | uint16_t outSampleRefCount = 0; |
- | |
| 32 | volatile uint16_t sampleVar; |
28 | volatile uint16_t sampleVar; |
| 33 | volatile uint16_t sampleRef; |
29 | volatile uint16_t sampleRef; |
| 34 | volatile uint16_t sampleDiffBuff[SAMPLE_BUFF_SIZE]; |
- | |
| 35 | volatile uint16_t samplePeriodBuff[SAMPLE_BUFF_SIZE]; |
- | |
| 36 | volatile uint8_t refCountBuff[SAMPLE_BUFF_SIZE]; |
- | |
| 37 | volatile uint8_t varCountBuff[SAMPLE_BUFF_SIZE]; |
- | |
| 38 | 30 | ||
| 39 | 31 | ||
| 40 | int gainControl = 1000 ; |
32 | int gainControl = 1000 ; |
| 41 | 33 | ||
| 42 | volatile uint8_t haveSlowPulse = 0; |
34 | volatile uint8_t haveSlowPulse = 0; |
| Line 111... | Line 103... | ||
| 111 | if(rpm < 600) |
103 | if(rpm < 600) |
| 112 | rpm = 600; |
104 | rpm = 600; |
| 113 | if(rpm > 5000) |
105 | if(rpm > 5000) |
| 114 | rpm = 5000; |
106 | rpm = 5000; |
| 115 | 107 | ||
| 116 | - | ||
| 117 | } |
108 | } |
| 118 | 109 | ||
| 119 | uint16_t setRPM(uint16_t rpm_ ) |
110 | uint16_t setRPM(uint16_t rpm_ ) |
| 120 | { |
111 | { |
| 121 | if(rpm_ >= 600 && rpm_ < 6000) |
112 | if(rpm_ >= 600 && rpm_ < 6000) |
| Line 150... | Line 141... | ||
| 150 | uint8_t slowPulse(void) |
141 | uint8_t slowPulse(void) |
| 151 | { |
142 | { |
| 152 | return haveSlowPulse; |
143 | return haveSlowPulse; |
| 153 | } |
144 | } |
| 154 | 145 | ||
| 155 | // waits for ignition pulse reading in the buffer, returns |
- | |
| 156 | // the sample index of the next sample. |
- | |
| 157 | 146 | ||
| 158 | uint16_t getNextRefPulseIndex(void) |
- | |
| 159 | { |
- | |
| 160 | while (outSampleRefCount == sampleRefCount) |
- | |
| 161 | chThdSleep(10); |
- | |
| 162 | - | ||
| 163 | uint16_t sampleIndex = outSampleRefCount; |
- | |
| 164 | - | ||
| 165 | if(++outSampleRefCount == SAMPLE_BUFF_SIZE) |
- | |
| 166 | outSampleRefCount = 0; |
- | |
| 167 | return sampleIndex; |
- | |
| 168 | } |
- | |
| 169 | - | ||
| 170 | - | ||
| 171 | void processNextPulse(uint16_t index) |
147 | void processPhase (int refCount,int varCount, int diff) |
| 172 | { |
148 | { |
| 173 | 149 | ||
| 174 | // scale it up by 32 |
150 | // scale it up by 32 |
| 175 | static int32_t periodEstimate = 2000 * 256 ; |
- | |
| 176 | // at this point we should try to phase lock |
151 | // at this point we should try to phase lock |
| 177 | 152 | ||
| 178 | uint16_t sampleDiff = sampleDiffBuff[index]; |
- | |
| 179 | 153 | ||
| 180 | signed pd = 0; |
154 | signed pd = 0; |
| 181 | bool lock = false; |
155 | bool lock = false; |
| 182 | // basically locked |
156 | // basically locked |
| 183 | if(refCountBuff[index]==1 && varCountBuff[index]==1 ) |
157 | if(refCount==1 && varCount==1 ) |
| 184 | { |
158 | { |
| 185 | lock = true; |
159 | lock = true; |
| 186 | pd = (sampleDiff < 32768 ? sampleDiff : sampleDiff - 65536L) ; |
160 | pd = (diff < 32768 ? diff : diff - 65536L) ; |
| 187 | } |
161 | } |
| - | 162 | // frequency detector |
|
| 188 | else if (refCountBuff[index] > 1) |
163 | else if (refCount > 1) |
| 189 | { |
164 | { |
| 190 | pd = (refCountBuff [index]*32768L); |
165 | pd = (refCount*32768L); |
| 191 | } |
166 | } |
| 192 | else if (varCountBuff[index] > 1) |
167 | else if (varCount > 1) |
| 193 | { |
168 | { |
| 194 | pd = (-varCountBuff[index] * 32768L); |
169 | pd = (-varCount * 32768L); |
| 195 | } |
170 | } |
| 196 | float delta_phi = pd/ (gainControl * 1.0); |
171 | float delta_phi = pd/ (gainControl * 1.0); |
| 197 | 172 | ||
| 198 | 173 | ||
| 199 | delta = delta_phi; |
174 | delta = pd; |
| 200 | 175 | ||
| 201 | 176 | ||
| 202 | 177 | ||
| 203 | // phase detector returns +/ |
178 | // phase detector returns +/ |
| 204 | 179 | ||
| 205 | 180 | ||
| 206 | 181 | ||
| 207 | float const wn = 0.03f; |
182 | float const wn = 0.01; |
| 208 | float const zeta = 0.707f; |
183 | float const zeta = 0.707f; // was 0.707 |
| 209 | float const K = 1; |
184 | float const K = 1000; |
| 210 | 185 | ||
| 211 | float const t1 = K/(wn*wn); |
186 | float const t1 = K/(wn*wn); |
| 212 | float const t2 = 2 * zeta/wn; |
187 | float const t2 = 2 * zeta/wn; |
| 213 | float const b0 = (4*K/t1)*(1.+t2/2.0f); |
188 | float const b0 = (4*K/t1)*(1.+t2/2.0f); |
| 214 | float const b1 = (8*K / t1); |
189 | float const b1 = (8*K / t1); |
| Line 217... | Line 192... | ||
| 217 | float const a1 = -2.0f; |
192 | float const a1 = -2.0f; |
| 218 | float const a2 = 1.0f; |
193 | float const a2 = 1.0f; |
| 219 | static float v0=0, v1 = 0, v2 = 0 ; |
194 | static float v0=0, v1 = 0, v2 = 0 ; |
| 220 | 195 | ||
| 221 | 196 | ||
| 222 | static float phi_hat = 0.0f; |
197 | static float phi_hat = 100.0f; |
| 223 | 198 | ||
| 224 | 199 | ||
| 225 | v2=v1; v1=v0; |
200 | v2=v1; v1=v0; |
| 226 | v0 = delta_phi -v1 *a1 -v2 *a2; |
201 | v0 = delta_phi -v1 *a1 -v2 *a2; |
| 227 | 202 | ||
| 228 | 203 | ||
| 229 | phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ; |
204 | phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ; |
| 230 | 205 | ||
| - | 206 | ||
| - | 207 | ||
| 231 | // 6.283 = 1.0 Hz |
208 | // 6.283 = 1.0 Hz |
| 232 | 209 | ||
| 233 | // 62.2 = 10Hz |
210 | // 62.2 = 10Hz |
| 234 | // decide on whether to go for forcing loop or to track |
211 | // decide on whether to go for forcing loop or to track |
| 235 | 212 | ||
| 236 | int32_t arr; |
213 | int32_t arr; |
| 237 | 214 | ||
| 238 | // if(lock) |
215 | // if(lock) |
| 239 | 216 | ||
| 240 | 217 | ||
| 241 | arr = (6283000L/MICROSECS_PULSE)/ phi_hat; |
218 | arr = (6283000L/MICROSECS_PULSE)/ (phi_hat+60); |
| 242 | 219 | ||
| 243 | if(arr > 5000) |
220 | if(arr > 20000) |
| 244 | arr = 5000; |
221 | arr = 20000; |
| 245 | if(arr < 500) |
222 | if(arr < 100) |
| 246 | arr = 500; |
223 | arr = 100; |
| 247 | 224 | ||
| 248 | // compute period error] |
- | |
| 249 | // periodErr += periodEstimate - (arr * 256); |
- | |
| 250 | 225 | ||
| 251 | 226 | ||
| 252 | count = arr; |
227 | count = arr; |
| 253 | 228 | ||
| 254 | TIM2->ARR = arr ; |
229 | TIM2->ARR = arr ; |
| 255 | recalcPhase(); |
230 | recalcPhase(); |
| 256 | 231 | ||
| 257 | 232 | ||
| 258 | 233 | ||
| 259 | // calculate RPM |
- | |
| 260 | float nomRPM = 30E6 / (MICROSECS_PULSE * (periodEstimate/256)); |
234 | float nomRPM = 30E6 / (MICROSECS_PULSE * arr); |
| 261 | 235 | ||
| 262 | rpm = nomRPM ; |
236 | rpm = nomRPM ; |
| 263 | 237 | ||
| 264 | 238 | ||
| 265 | - | ||
| 266 | // rpm += delta / 256; |
- | |
| 267 | - | ||
| 268 | adjustRPM(); |
239 | adjustRPM(); |
| 269 | } |
240 | } |
| 270 | 241 | ||
| 271 | 242 | ||
| 272 | 243 | ||
| Line 315... | Line 286... | ||
| 315 | 286 | ||
| 316 | if(refCount != 0 && varCount != 0 ) /*we have an R,V pair */ |
287 | if(refCount != 0 && varCount != 0 ) /*we have an R,V pair */ |
| 317 | { |
288 | { |
| 318 | 289 | ||
| 319 | // |
290 | // |
| 320 | - | ||
| 321 | refCountBuff[sampleRefCount] = refCount; |
- | |
| 322 | varCountBuff[sampleRefCount] = varCount; |
291 | processPhase (refCount,varCount,sampleRef-sampleVar); |
| 323 | - | ||
| 324 | haveSlowPulse = (varCount > 20); |
292 | haveSlowPulse = (varCount > 20); |
| 325 | - | ||
| 326 | sampleDiffBuff[sampleRefCount] = sampleRef - sampleVar; |
- | |
| 327 | - | ||
| 328 | samplePeriodBuff[sampleRefCount] = samplePeriod; |
- | |
| 329 | - | ||
| 330 | - | ||
| 331 | - | ||
| 332 | if(++sampleRefCount == SAMPLE_BUFF_SIZE) |
- | |
| 333 | sampleRefCount = 0; |
- | |
| 334 | - | ||
| 335 | refCount = 0; |
293 | refCount = 0; |
| 336 | varCount = 0; |
294 | varCount = 0; |
| 337 | } |
295 | } |
| 338 | 296 | ||
| 339 | 297 | ||