Rev 19 | Rev 21 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 19 | Rev 20 | ||
|---|---|---|---|
| Line 13... | Line 13... | ||
| 13 | 13 | ||
| 14 | 14 | ||
| 15 | // with a dwell angle of 45 degrees , 4 cylinders and a maximum RPM of 5000 |
15 | // with a dwell angle of 45 degrees , 4 cylinders and a maximum RPM of 5000 |
| 16 | // freq = 5000/60 * 2 = 166Hz. Because the breaker might bounce , we accept the |
16 | // freq = 5000/60 * 2 = 166Hz. Because the breaker might bounce , we accept the |
| 17 | // first pulse longer than 1/300 of a second as being a proper closure . |
17 | // first pulse longer than 1/300 of a second as being a proper closure . |
| 18 | // the TIM2 counter counts in 10uS increments, |
18 | // the TIM2 counter counts in 1uS increments, |
| 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 | uint16_t nominal = 0; |
24 | int16_t nominal = 0; |
| 25 | uint16_t halfRot; |
25 | uint16_t halfRot; |
| 26 | uint16_t phase10 = 100; // 10 degrees |
26 | int16_t phase10 = 100; // 10 degrees |
| 27 | volatile uint16_t sampleVar; |
27 | volatile uint16_t sampleVar; |
| 28 | volatile uint16_t sampleRef; |
28 | volatile uint16_t sampleRef; |
| 29 | 29 | ||
| 30 | volatile uint16_t lastSampleRef = 0; |
30 | volatile uint16_t lastSampleRef = 0; |
| 31 | 31 | ||
| Line 47... | Line 47... | ||
| 47 | signed count; |
47 | signed count; |
| 48 | signed delta; |
48 | signed delta; |
| 49 | 49 | ||
| 50 | 50 | ||
| 51 | 51 | ||
| 52 | void recalcPhase(void) |
52 | void recalcPhase() |
| 53 | { |
53 | { |
| 54 | nominal = halfRot * (long) (phase10)/ 1800; |
54 | nominal = halfRot * (long) (phase10)/ 3600; |
| 55 | } |
55 | } |
| 56 | 56 | ||
| 57 | void adjustRPM(void) |
57 | void adjustRPM(void) |
| 58 | { |
58 | { |
| 59 | if(rpm < 600) |
59 | if(rpm < 600) |
| 60 | rpm = 600; |
60 | rpm = 600; |
| 61 | if(rpm > 5000) |
61 | if(rpm > 5000) |
| 62 | rpm = 5000; |
62 | rpm = 5000; |
| 63 | 63 | ||
| - | 64 | ||
| 64 | } |
65 | } |
| 65 | 66 | ||
| 66 | uint16_t setRPM(uint16_t rpm_ ) |
67 | uint16_t setRPM(uint16_t rpm_ ) |
| 67 | { |
68 | { |
| 68 | if(rpm_ >= 600 && rpm_ < 6000) |
69 | if(rpm_ >= 600 && rpm_ < 6000) |
| Line 99... | Line 100... | ||
| 99 | void processPhase ( void ) |
100 | void processPhase ( void ) |
| 100 | { |
101 | { |
| 101 | // lpcl |
102 | // lpcl |
| 102 | chSysLock(); |
103 | chSysLock(); |
| 103 | 104 | ||
| 104 | signed pd; |
105 | const signed pdClip = 10000; |
| 105 | pd = 0; |
- | |
| 106 | static unsigned sp = 0; |
106 | static signed pd; |
| 107 | if(validPhaseSamp) |
107 | if(validPhaseSamp) |
| 108 | { |
108 | { |
| 109 | pd = phaseSamp; |
109 | pd = phaseSamp - nominal; |
| 110 | validPhaseSamp = 0; |
110 | validPhaseSamp = 0; |
| - | 111 | if(pd > pdClip) |
|
| - | 112 | pd = pdClip; |
|
| - | 113 | if(pd < -pdClip) |
|
| - | 114 | pd = -pdClip; |
|
| - | 115 | ||
| - | 116 | ||
| - | 117 | } |
|
| 111 | 118 | ||
| 112 | delta = phaseSamp; |
- | |
| 113 | 119 | ||
| 114 | sp = samplePeriod; |
- | |
| 115 | } |
- | |
| 116 | 120 | ||
| 117 | chSysUnlock(); |
121 | chSysUnlock(); |
| 118 | 122 | ||
| 119 | 123 | ||
| - | 124 | delta = pd; |
|
| - | 125 | ||
| - | 126 | ||
| 120 | static int sampleAverage = 0; |
127 | static int sampleAverage = 0; |
| 121 | 128 | ||
| 122 | static int phaseAverage = 0; |
129 | static int phaseAverage = 0; |
| 123 | const int freqScale = 40; |
130 | const int freqScale = 6; |
| 124 | 131 | ||
| 125 | const int phaseScale = 200; |
132 | const int phaseScale = 50; |
| 126 | 133 | ||
| 127 | sampleAverage = sampleAverage + (sp - sampleAverage/freqScale); |
134 | sampleAverage = sampleAverage + (samplePeriod - sampleAverage/freqScale); |
| 128 | 135 | ||
| 129 | 136 | ||
| 130 | 137 | ||
| 131 | 138 | ||
| 132 | 139 | ||
| Line 137... | Line 144... | ||
| 137 | int intSample = sampleAverage / freqScale; |
144 | int intSample = sampleAverage / freqScale; |
| 138 | 145 | ||
| 139 | static unsigned strange = 0; |
146 | static unsigned strange = 0; |
| 140 | 147 | ||
| 141 | 148 | ||
| - | 149 | int deltaPd= pd/ phaseScale; |
|
| 142 | 150 | ||
| 143 | int deltaPd= pd/10; |
- | |
| 144 | 151 | ||
| 145 | if(pd < -2000 || pd > 2000) |
152 | if(pd < -2000 || pd > 2000) |
| 146 | { |
153 | { |
| 147 | strange++; |
154 | strange++; |
| 148 | } |
155 | } |
| Line 161... | Line 168... | ||
| 161 | 168 | ||
| 162 | 169 | ||
| 163 | count = arr; |
170 | count = arr; |
| 164 | 171 | ||
| 165 | TIM2->ARR = arr -1; |
172 | TIM2->ARR = arr -1; |
| - | 173 | ||
| 166 | recalcPhase(); |
174 | nominal = intSample * (long) (phase10)/ 3600; |
| 167 | 175 | ||
| 168 | 176 | ||
| 169 | 177 | ||
| 170 | float nomRPM = 30E6 / (MICROSECS_PULSE * arr); |
178 | float nomRPM = 30E6 / (MICROSECS_PULSE * arr); |
| 171 | 179 | ||
| Line 176... | Line 184... | ||
| 176 | } |
184 | } |
| 177 | 185 | ||
| 178 | 186 | ||
| 179 | 187 | ||
| 180 | 188 | ||
| 181 | // set the timing advance from reference to |
189 | // set the timing advance from reference in 0.1 degrees units |
| 182 | void setAdvance(int16_t deg10) |
190 | void setAdvance(int16_t deg10) |
| 183 | { |
191 | { |
| 184 | phase10 = deg10; |
192 | phase10 = deg10; |
| 185 | recalcPhase(); |
- | |
| 186 | 193 | ||
| 187 | } |
194 | } |
| 188 | 195 | ||
| 189 | 196 | ||
| 190 | 197 | ||
| Line 278... | Line 285... | ||
| 278 | { |
285 | { |
| 279 | 286 | ||
| 280 | uint16_t refToVar = sampleRef - sampleVar; |
287 | uint16_t refToVar = sampleRef - sampleVar; |
| 281 | uint16_t varToRef = sampleVar - sampleRef; |
288 | uint16_t varToRef = sampleVar - sampleRef; |
| 282 | 289 | ||
| 283 | if(refToVar < 32768) |
290 | if(refToVar < varToRef) |
| 284 | phaseSamp = refToVar; |
291 | phaseSamp = refToVar; |
| 285 | else if(varToRef < 32768) |
292 | else if(varToRef <= refToVar) |
| 286 | phaseSamp = -varToRef; |
293 | phaseSamp = -varToRef; |
| 287 | 294 | ||
| 288 | } |
295 | } |
| 289 | 296 | ||
| 290 | 297 | ||