Subversion Repositories FlightCtrl

Rev

Rev 1224 | Rev 1320 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1224 Rev 1232
Line 39... Line 39...
39 
     cnt_1ms %= 2;
 39 
     cnt_1ms %= 2;
40 
     if(!cnt_1ms) UpdateMotor = 1;
 40 
     if(!cnt_1ms) UpdateMotor = 1;
41 
     CountMilliseconds++;
 41 
     CountMilliseconds++;
42 
    }
 42 
    }
Line 43... Line 43...
43 
 
 43 
 
44 
     if(beeptime > 1)
 44 
     if(beeptime >= 1)
45 
        {
 45 
        {
46 
        beeptime--;
 46 
        beeptime--;
47 
        if(beeptime & BeepMuster)
 47 
        if(beeptime & BeepMuster)
48 
         {
 48 
         {
Line 54... Line 54...
54 
      {
 54 
      {
55 
       pieper_ein = 0;
 55 
       pieper_ein = 0;
56 
       BeepMuster = 0xffff;
 56 
       BeepMuster = 0xffff;
57 
      }
 57 
      }
Line 58... Line -...
58 
 
 - 
 
59 
 
 58 
 
60 
     if(pieper_ein)
 59 
     if(pieper_ein)
61 
        {
 60 
        {
62 
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
 61 
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
63 
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
 62 
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
Line 131... Line 130...
131 
{
 130 
{
132 
        uint8_t sreg = SREG;
 131 
        uint8_t sreg = SREG;
Line 133... Line 132...
133 
 
 132 
 
134 
        // disable all interrupts before reconfiguration
 133 
        // disable all interrupts before reconfiguration
135 
        cli();
 134 
        cli();
136 
 
 - 
 
137 
        // set PD7 as output of the PWM for nick servo
 - 
 
138 
        DDRD  |= (1<<DDD7);
 135 
       
Line 139... Line 136...
139 
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
 136 
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
140 
 
 137 
 
141 
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
 138 
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
Line 192... Line 189...
192 
/*              Control Servo Position               */
 189 
/*              Control Servo Position               */
193 
/*****************************************************/
 190 
/*****************************************************/
Line 194... Line 191...
194 
 
 191 
 
195 
ISR(TIMER2_COMPA_vect)
 192 
ISR(TIMER2_COMPA_vect)
196 
{
 - 
 
197 
 
 193 
{
198 
        // frame len 22.5 ms = 14063 * 1.6 us
 194 
        // frame len 22.5 ms = 14063 * 1.6 us
199 
        // stop pulse: 0.3 ms = 188 * 1.6 us
 195 
        // stop pulse: 0.3 ms = 188 * 1.6 us
200 
        // min servo pulse: 0.6 ms =  375 * 1.6 us
 196 
        // min servo pulse: 0.6 ms =  375 * 1.6 us
201 
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
 197 
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
Line 230... Line 226...
230 
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
 226 
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
231 
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 227 
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
Line 232... Line 228...
232 
 
 228 
 
233 
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
 229 
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
234 
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
 230 
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
235 
                                if(EE_Parameter.ServoNickCompInvert & 0x01)
 231 
                                if(EE_Parameter.ServoCompInvert & 0x01)
236 
                                {       // inverting movement of servo
 232 
                                {       // inverting movement of servo
237 
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 233 
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
238 
                                }
 234 
                                }
239 
                                else
 235 
                                else
Line 294... Line 290...
294 
                                        switch(ServoIndex) // map servo channels
 290 
                                        switch(ServoIndex) // map servo channels
295 
                                        {
 291 
                                        {
296 
                                                case 1: // Nick Compensation Servo
 292 
                                                case 1: // Nick Compensation Servo
297 
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
 293 
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
298 
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
 294 
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
299 
                                                        if(EE_Parameter.ServoNickCompInvert & 0x01)
 295 
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
300 
                                                        {       // inverting movement of servo
 296 
                                                        {       // inverting movement of servo
301 
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 297 
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
302 
                                                        }
 298 
                                                        }
303 
                                                        else
 299 
                                                        else
304 
                                                        {       // non inverting movement of servo
 300 
                                                        {       // non inverting movement of servo
Line 317... Line 313...
317 
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 313 
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
318 
                                                        ServoNickValue /= MULTIPLYER;
 314 
                                                        ServoNickValue /= MULTIPLYER;
319 
                                                        DebugOut.Analog[20] = ServoNickValue;
 315 
                                                        DebugOut.Analog[20] = ServoNickValue;
320 
                                                        break;
 316 
                                                        break;
321 
                                         case 2: // Roll Compensation Servo
 317 
                                         case 2: // Roll Compensation Servo
322 
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset
 318 
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
323 
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
 319 
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
324 
                                                        //if(EE_Parameter.ServoRollCompInvert & 0x01)
 320 
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
325 
                                                        {       // inverting movement of servo
 321 
                                                        {       // inverting movement of servo
326 
                                                                ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
 - 
 
327 
                                                        }
 - 
 
328 
/*                                                      else
 - 
 
329 
                                                        {       // non inverting movement of servo
 - 
 
330 
                                                                ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
 - 
 
331 
                                                        }
 - 
 
332 
*/                                                      // limit servo value to its parameter range definition
 - 
 
333 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
 - 
 
334 
                                                        {
 - 
 
335 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
 - 
 
336 
                                                        }
 - 
 
337 
                                                        else
 - 
 
338 
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
 - 
 
339 
                                                        {
 - 
 
340 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
 - 
 
341 
                                                        }
 - 
 
342 
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 - 
 
343 
                                                        ServoRollValue /= MULTIPLYER;
 - 
 
344 
                                                        //DebugOut.Analog[20] = ServoRollValue;
 - 
 
345 
 
 - 
 
346 
/*                                                      ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
 - 
 
347 
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
348 
                                                        if(EE_Parameter.ServoRollCompInvert & 0x01)
 - 
 
349 
                                                        {       // inverting movement of servo
 - 
 
350 
                                                                ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 322 
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
351 
                                                        }
 323 
                                                        }
352 
                                                        else
 324 
                                                        else
353 
                                                        {       // non inverting movement of servo
 325 
                                                        {       // non inverting movement of servo
354 
                                                                ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 326 
                                                                ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
355 
                                                        }
 327 
                                                        }
356 
                                                        // limit servo value to its parameter range definition
 328 
                                                    // limit servo value to its parameter range definition
357 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
 329 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
358 
                                                        {
 330 
                                                        {
359 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
 331 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
360 
                                                        }
 332 
                                                        }
361 
                                                        else
 333 
                                                        else
Line 364... Line 336...
364 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
 336 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
365 
                                                        }
 337 
                                                        }
366 
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 338 
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
367 
                                                        ServoRollValue /= MULTIPLYER;
 339 
                                                        ServoRollValue /= MULTIPLYER;
368 
                                                        //DebugOut.Analog[20] = ServoRollValue;
 340 
                                                        //DebugOut.Analog[20] = ServoRollValue;
369 
*/                                                      break;
 341 
                                                        break;
Line 370... Line 342...
370 
 
 342 
 
371 
                                                default: // other servo channels
 343 
                                                default: // other servo channels
372 
                                                        RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
 344 
                                                        RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
373 
                                                        break;
 345 
                                                        break;
Line 387... Line 359...
387 
                                // set pulsewidth to stop pulse width
 359 
                                // set pulsewidth to stop pulse width
388 
                                RemainingPulse = PPM_STOPPULSE;
 360 
                                RemainingPulse = PPM_STOPPULSE;
389 
                                // accumulate time for correct sync gap
 361 
                                // accumulate time for correct sync gap
390 
                                ServoFrameTime += RemainingPulse;
 362 
                                ServoFrameTime += RemainingPulse;
391 
                                if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
 363 
                                if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
- 
 
 364 
                                else HEF4017R_ON;
392 
                                ServoIndex++; // change to next servo channel
 365 
                                ServoIndex++; // change to next servo channel
393 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
 366 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
394 
                        }
 367 
                        }
395 
                        // set pulse output active
 368 
                        // set pulse output active
396 
                        PulseOutput = 1;
 369 
                        PulseOutput = 1;