Rev 1400 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1400 | Rev 1568 | ||
---|---|---|---|
Line -... | Line 1... | ||
- | 1 | ||
1 | #include "main.h" |
2 | #include "main.h" |
2 | #include "spectrum.h" |
3 | #include "spectrum.h" |
- | 4 | #include "heli.h" |
|
Line 3... | Line 5... | ||
3 | 5 | ||
4 | volatile unsigned int CountMilliseconds = 0; |
6 | volatile unsigned int CountMilliseconds = 0; |
5 | volatile static unsigned int tim_main; |
7 | volatile static unsigned int tim_main; |
6 | volatile unsigned char UpdateMotor = 0; |
8 | volatile unsigned char UpdateMotor = 0; |
7 | volatile unsigned int cntKompass = 0; |
9 | volatile unsigned int cntKompass = 0; |
8 | volatile unsigned int beeptime = 0; |
10 | volatile unsigned int beeptime = 0; |
Line 9... | Line 11... | ||
9 | volatile unsigned char SendSPI = 0, ServoActive = 0; |
11 | volatile unsigned char SendSPI = 0; |
Line 10... | Line 12... | ||
10 | 12 | ||
11 | unsigned int BeepMuster = 0xffff; |
13 | unsigned int BeepMuster = 0xffff; |
Line 29... | Line 31... | ||
29 | SIGNAL (SIG_OVERFLOW0) // 9,7kHz |
31 | SIGNAL (SIG_OVERFLOW0) // 9,7kHz |
30 | { |
32 | { |
31 | static unsigned char cnt_1ms = 1,cnt = 0; |
33 | static unsigned char cnt_1ms = 1,cnt = 0; |
32 | unsigned char pieper_ein = 0; |
34 | unsigned char pieper_ein = 0; |
33 | if(SendSPI) SendSPI--; |
35 | if(SendSPI) SendSPI--; |
34 | if(SpektrumTimer) SpektrumTimer--; |
36 | if(SpektrumTimer) SpektrumTimer--; |
Line 35... | Line 37... | ||
35 | 37 | ||
36 | if(!cnt--) |
38 | if(!cnt--) |
37 | { |
39 | { |
38 | cnt = 9; |
40 | cnt = 9; |
Line 132... | Line 134... | ||
132 | { |
134 | { |
133 | uint8_t sreg = SREG; |
135 | uint8_t sreg = SREG; |
Line 134... | Line 136... | ||
134 | 136 | ||
135 | // disable all interrupts before reconfiguration |
137 | // disable all interrupts before reconfiguration |
136 | cli(); |
138 | cli(); |
137 | 139 | ||
Line 138... | Line 140... | ||
138 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
140 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
139 | 141 | ||
140 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
142 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
Line 214... | Line 216... | ||
214 | 216 | ||
215 | #define MULTIPLYER 4 |
217 | #define MULTIPLYER 4 |
216 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
218 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
Line -... | Line 219... | ||
- | 219 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
|
- | 220 | ||
- | 221 | //----------------------------------------------------- |
|
217 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
222 | // PPM state machine, onboard demultiplexed by HEF4017 |
218 | 223 | //----------------------------------------------------- |
|
219 | if(PlatinenVersion < 20) |
- | |
220 | { |
- | |
221 | //--------------------------- |
- | |
222 | // Nick servo state machine |
224 | if(!PulseOutput) // pulse output complete |
223 | //--------------------------- |
225 | { |
224 | if(!PulseOutput) // pulse output complete |
226 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
- | 227 | { |
|
- | 228 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
|
- | 229 | ||
- | 230 | if(ServoIndex == 0) // if we are at the sync gap |
|
- | 231 | { |
|
- | 232 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time |
|
- | 233 | ServoFrameTime = 0; // reset servo frame time |
|
- | 234 | HEF4017R_ON; // enable HEF4017 reset |
|
225 | { |
235 | } |
226 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
- | |
227 | { |
236 | else // servo channels |
228 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
- | |
229 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
- | |
230 | - | ||
231 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | |
232 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
233 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
234 | { // inverting movement of servo |
- | |
235 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
236 | } |
- | |
237 | else |
- | |
238 | { // non inverting movement of servo |
- | |
239 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
240 | } |
- | |
241 | // limit servo value to its parameter range definition |
- | |
242 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
- | |
243 | { |
- | |
244 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
- | |
245 | } |
- | |
246 | else |
- | |
247 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
- | |
248 | { |
- | |
249 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
- | |
250 | } |
237 | { |
251 | - | ||
252 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
- | |
Line 253... | Line 238... | ||
253 | 238 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
|
254 | ServoNickValue /= MULTIPLYER; |
239 | RemainingPulse += 2 * servoValues[ServoIndex - 1]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
255 | 240 | ||
256 | // range servo pulse width |
- | |
257 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
- | |
258 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
- | |
259 | // accumulate time for correct update rate |
- | |
260 | ServoFrameTime = RemainingPulse; |
- | |
261 | } |
- | |
262 | else // we had a high pulse |
- | |
263 | { |
- | |
264 | TCCR2A |= (1<<COM2A0); // make a low pulse |
- | |
265 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
- | |
266 | } |
- | |
267 | // set pulse output active |
- | |
268 | PulseOutput = 1; |
- | |
269 | } |
- | |
270 | } // EOF Nick servo state machine |
- | |
271 | else |
- | |
272 | { |
- | |
273 | //----------------------------------------------------- |
- | |
274 | // PPM state machine, onboard demultiplexed by HEF4017 |
- | |
275 | //----------------------------------------------------- |
- | |
276 | if(!PulseOutput) // pulse output complete |
- | |
277 | { |
- | |
278 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
- | |
279 | { |
- | |
280 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
- | |
281 | - | ||
282 | if(ServoIndex == 0) // if we are at the sync gap |
- | |
283 | { |
- | |
284 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time |
- | |
285 | ServoFrameTime = 0; // reset servo frame time |
- | |
286 | HEF4017R_ON; // enable HEF4017 reset |
- | |
287 | } |
- | |
288 | else // servo channels |
- | |
289 | { |
- | |
290 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
- | |
291 | switch(ServoIndex) // map servo channels |
- | |
292 | { |
- | |
293 | case 1: // Nick Compensation Servo |
- | |
294 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | |
295 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
296 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
297 | { // inverting movement of servo |
- | |
298 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
299 | } |
- | |
300 | else |
- | |
301 | { // non inverting movement of servo |
- | |
302 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
303 | } |
- | |
304 | // limit servo value to its parameter range definition |
- | |
305 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
- | |
306 | { |
- | |
307 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
- | |
308 | } |
- | |
309 | else |
- | |
310 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
- | |
311 | { |
- | |
312 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
- | |
313 | } |
- | |
314 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
- | |
315 | ServoNickValue /= MULTIPLYER; |
- | |
316 | break; |
- | |
317 | case 2: // Roll Compensation Servo |
- | |
318 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
- | |
319 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
320 | if(EE_Parameter.ServoCompInvert & 0x02) |
- | |
321 | { // inverting movement of servo |
- | |
322 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
- | |
323 | } |
- | |
324 | else |
- | |
325 | { // non inverting movement of servo |
- | |
326 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
- | |
327 | } |
- | |
328 | // limit servo value to its parameter range definition |
- | |
329 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
- | |
330 | { |
- | |
331 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
- | |
332 | } |
- | |
333 | else |
- | |
334 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
- | |
335 | { |
- | |
336 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
- | |
337 | } |
- | |
338 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
- | |
339 | ServoRollValue /= MULTIPLYER; |
- | |
340 | //DebugOut.Analog[20] = ServoRollValue; |
- | |
341 | break; |
- | |
342 | - | ||
343 | default: // other servo channels |
- | |
344 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
- | |
345 | break; |
- | |
346 | } |
- | |
347 | // range servo pulse width |
241 | // range servo pulse width |
348 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
242 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
349 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
- | |
350 | // substract stop pulse width |
- | |
351 | RemainingPulse -= PPM_STOPPULSE; |
- | |
352 | // accumulate time for correct sync gap |
- | |
353 | ServoFrameTime += RemainingPulse; |
- | |
354 | } |
- | |
355 | } |
- | |
356 | else // we had a high pulse |
- | |
357 | { |
- | |
358 | TCCR2A |= (1<<COM2A0); // make a low pulse |
243 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
359 | // set pulsewidth to stop pulse width |
244 | // substract stop pulse width |
360 | RemainingPulse = PPM_STOPPULSE; |
- | |
361 | // accumulate time for correct sync gap |
- | |
362 | ServoFrameTime += RemainingPulse; |
- | |
363 | if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset |
- | |
364 | else HEF4017R_ON; |
245 | RemainingPulse -= PPM_STOPPULSE; |
365 | ServoIndex++; // change to next servo channel |
- | |
366 | if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap |
- | |
367 | } |
246 | // accumulate time for correct sync gap |
- | 247 | ServoFrameTime += RemainingPulse; |
|
- | 248 | } |
|
- | 249 | } |
|
- | 250 | else // we had a high pulse |
|
- | 251 | { |
|
- | 252 | TCCR2A |= (1<<COM2A0); // make a low pulse |
|
- | 253 | // set pulsewidth to stop pulse width |
|
- | 254 | RemainingPulse = PPM_STOPPULSE; |
|
- | 255 | // accumulate time for correct sync gap |
|
- | 256 | ServoFrameTime += RemainingPulse; |
|
- | 257 | HEF4017R_OFF; // disable HEF4017 reset |
|
368 | // set pulse output active |
258 | ServoIndex++; // change to next servo channel |
- | 259 | if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap |
|
- | 260 | } |
|
Line 369... | Line 261... | ||
369 | PulseOutput = 1; |
261 | // set pulse output active |
370 | } |
262 | PulseOutput = 1; |
371 | } // EOF PPM state machine |
263 | } |
372 | 264 |