Rev 11 | Rev 13 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 11 | Rev 12 | ||
|---|---|---|---|
| Line 28... | Line 28... | ||
| 28 | typedef enum { WAIT_GAP, SKIP_BOUNCE, HAVE_SAMPLE } sampleState_t ; |
28 | typedef enum { WAIT_GAP, SKIP_BOUNCE, HAVE_SAMPLE } sampleState_t ; |
| 29 | sampleState_t sampleState = WAIT_GAP; |
29 | sampleState_t sampleState = WAIT_GAP; |
| 30 | // difference between samples |
30 | // difference between samples |
| 31 | volatile uint16_t deltaTime; |
31 | volatile uint16_t deltaTime; |
| 32 | 32 | ||
| - | 33 | static signed pdI = 0; |
|
| - | 34 | ||
| 33 | 35 | ||
| 34 | uint16_t rpm; |
36 | uint16_t rpm; |
| 35 | 37 | ||
| 36 | void initTimer2() |
38 | void initTimer2() |
| 37 | { |
39 | { |
| Line 97... | Line 99... | ||
| 97 | rpm = 600; |
99 | rpm = 600; |
| 98 | if(rpm > 5000) |
100 | if(rpm > 5000) |
| 99 | rpm = 5000; |
101 | rpm = 5000; |
| 100 | 102 | ||
| 101 | 103 | ||
| 102 | float pulseSec = rpm /30; |
- | |
| 103 | - | ||
| 104 | halfRot = 1e6 / (pulseSec * MICROSECS_PULSE) ; |
- | |
| 105 | - | ||
| 106 | TIM2->ARR = halfRot; |
- | |
| 107 | recalcPhase(); |
- | |
| 108 | } |
104 | } |
| 109 | 105 | ||
| 110 | uint16_t setRPM(uint16_t rpm_ ) |
106 | uint16_t setRPM(uint16_t rpm_ ) |
| 111 | { |
107 | { |
| 112 | if(rpm_ >= 600 && rpm_ < 5000) |
108 | if(rpm_ >= 600 && rpm_ < 6000) |
| 113 | { |
109 | { |
| 114 | rpm = rpm_; |
110 | rpm = rpm_; |
| 115 | adjustRPM(); |
111 | adjustRPM(); |
| 116 | } |
112 | } |
| 117 | return halfRot; |
113 | return halfRot; |
| Line 122... | Line 118... | ||
| 122 | return rpm; |
118 | return rpm; |
| 123 | } |
119 | } |
| 124 | 120 | ||
| 125 | uint16_t getDelta(void) |
121 | uint16_t getDelta(void) |
| 126 | { |
122 | { |
| 127 | return deltaTime; |
123 | return pdI&0xFFFF; |
| 128 | } |
124 | } |
| 129 | 125 | ||
| 130 | uint16_t wrapIndex(uint16_t index) |
126 | uint16_t wrapIndex(uint16_t index) |
| 131 | { |
127 | { |
| 132 | if (index >= SAMPLE_BUFF_SIZE) |
128 | if (index >= SAMPLE_BUFF_SIZE) |
| 133 | index -= SAMPLE_BUFF_SIZE; |
129 | index -= SAMPLE_BUFF_SIZE; |
| 134 | return index; |
130 | return index; |
| 135 | } |
131 | } |
| 136 | 132 | ||
| 137 | - | ||
| 138 | // allows for wrapping |
133 | // allows for wrapping |
| 139 | uint16_t getSampleBuff(uint16_t index) |
134 | uint16_t getSampleBuff(uint16_t index) |
| 140 | { |
135 | { |
| 141 | chSysLock(); |
136 | chSysLock(); |
| 142 | return sampleBuff[wrapIndex(index)]; |
137 | return sampleBuff[wrapIndex(index)]; |
| Line 177... | Line 172... | ||
| 177 | 172 | ||
| 178 | 173 | ||
| 179 | void processNextPulse(uint16_t retVal) |
174 | void processNextPulse(uint16_t retVal) |
| 180 | { |
175 | { |
| 181 | 176 | ||
| - | 177 | static uint32_t VperiodAccumulator = 0; |
|
| - | 178 | // scale it up by 32 |
|
| - | 179 | static uint32_t periodEstimate = 2000 * 256 ; |
|
| 182 | static uint16_t lastVal = 0; |
180 | static uint16_t lastVal = 0; |
| 183 | // at this point we should try to phase lock |
181 | // at this point we should try to phase lock |
| 184 | deltaTime = retVal - lastVal; |
182 | deltaTime = retVal - lastVal; |
| 185 | 183 | ||
| 186 | 184 | ||
| Line 190... | Line 188... | ||
| 190 | __asm(" BKPT #0"); |
188 | __asm(" BKPT #0"); |
| 191 | } |
189 | } |
| 192 | 190 | ||
| 193 | lastVal = retVal; |
191 | lastVal = retVal; |
| 194 | 192 | ||
| - | 193 | // look at the values and try to pull them togwther |
|
| - | 194 | ||
| - | 195 | // accumulate phase |
|
| - | 196 | VperiodAccumulator += periodEstimate; |
|
| - | 197 | VperiodAccumulator &= 0xFFFFFFF; |
|
| - | 198 | ||
| - | 199 | uint16_t accum_low = (VperiodAccumulator) >> 8; |
|
| - | 200 | #define WINDOW 1000 |
|
| - | 201 | ||
| - | 202 | #define LIMIT 2048 |
|
| - | 203 | ||
| - | 204 | #define STEP 32 |
|
| - | 205 | ||
| - | 206 | uint16_t diff = accum_low - retVal; |
|
| - | 207 | if(diff < WINDOW && diff != 0) |
|
| - | 208 | pdI += diff/10; |
|
| - | 209 | if(diff > 65536-WINDOW ) |
|
| - | 210 | pdI -= (65536-diff)/10; |
|
| - | 211 | ||
| - | 212 | if(pdI > LIMIT) |
|
| - | 213 | pdI = LIMIT; |
|
| - | 214 | if(pdI < -(LIMIT)) |
|
| - | 215 | pdI = -(LIMIT); |
|
| - | 216 | ||
| - | 217 | ||
| - | 218 | ||
| - | 219 | signed pd = (signed)(periodEstimate)- (signed)(deltaTime*256) + pdI; |
|
| 195 | 220 | ||
| 196 | 221 | ||
| - | 222 | periodEstimate -= pd / 100; |
|
| 197 | 223 | ||
| 198 | 224 | ||
| 199 | float nomRPM = 30E6 / (MICROSECS_PULSE * deltaTime) ; |
- | |
| 200 | 225 | ||
| 201 | rpm = rpm + (nomRPM -rpm)/10; |
226 | TIM2->ARR = (periodEstimate+128)/256 ; |
| - | 227 | recalcPhase(); |
|
| 202 | 228 | ||
| 203 | 229 | ||
| 204 | 230 | ||
| - | 231 | // calculate RPM |
|
| - | 232 | float nomRPM = 30E6 / (MICROSECS_PULSE * (periodEstimate/256)); |
|
| 205 | 233 | ||
| 206 | uint16_t skew = 32768 - nominal; |
234 | rpm = nomRPM ; |
| 207 | 235 | ||
| 208 | long delta = (retVal+skew) - (nominal+skew); |
- | |
| 209 | 236 | ||
| 210 | if(delta > 10) |
- | |
| 211 | rpm = rpm - 1; |
- | |
| 212 | if(delta -10) |
- | |
| 213 | rpm = rpm + 1; |
- | |
| 214 | 237 | ||
| 215 | // rpm += delta / 256; |
238 | // rpm += delta / 256; |
| 216 | 239 | ||
| 217 | adjustRPM(); |
240 | adjustRPM(); |
| 218 | } |
241 | } |