Subversion Repositories FlightCtrl

Rev

Rev 903 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 903 Rev 916
Line 8... Line 8...
8
 
8
 
9
/*****************************************************/
9
/*****************************************************/
10
/*              Initialize Timer 2                   */
10
/*              Initialize Timer 2                   */
11
/*****************************************************/
11
/*****************************************************/
12
// The timer 2 is used to generate the PWM at PD7 (J7)
12
// The timer 2 is used to generate the PWM at PD7 (J7)
13
// to control a camera servo for pitch compensation.
13
// to control a camera servo for nick compensation.
14
void TIMER2_Init(void)
14
void TIMER2_Init(void)
15
{
15
{
Line 16... Line 16...
16
        uint8_t sreg = SREG;
16
        uint8_t sreg = SREG;
17
 
17
 
Line 18... Line 18...
18
        // disable all interrupts before reconfiguration
18
        // disable all interrupts before reconfiguration
19
        cli();
19
        cli();
20
 
20
 
Line 21... Line 21...
21
        // set PD7 as output of the PWM for pitch servo
21
        // set PD7 as output of the PWM for nick servo
Line 67... Line 67...
67
    {
67
    {
68
                 // enable PWM on PD7 in non inverting mode
68
                 // enable PWM on PD7 in non inverting mode
69
                 TCCR2A &= ~(0<<COM2A0);
69
                 TCCR2A &= ~(0<<COM2A0);
70
                 TCCR2A |= (1<<COM2A1);
70
                 TCCR2A |= (1<<COM2A1);
Line 71... Line 71...
71
 
71
 
72
                 ServoValue =  FCParam.ServoPitchControl;
72
                 ServoValue =  FCParam.ServoNickControl;
73
                 // inverting movment of servo
73
                 // inverting movment of servo
74
                 if(ParamSet.ServoPitchCompInvert & 0x01)
74
                 if(ParamSet.ServoNickCompInvert & 0x01)
75
                 {
75
                 {
76
                         ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
76
                         ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
77
                 }
77
                 }
78
                 else // non inverting movement of servo
78
                 else // non inverting movement of servo
79
                 {
79
                 {
80
                         ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
80
                         ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
Line 81... Line 81...
81
                 }
81
                 }
82
 
82
 
83
                 // limit servo value to its parameter range definition
83
                 // limit servo value to its parameter range definition
84
                 if(ServoValue < ParamSet.ServoPitchMin)
84
                 if(ServoValue < ParamSet.ServoNickMin)
85
                 {
85
                 {
86
                         ServoValue = ParamSet.ServoPitchMin;
86
                         ServoValue = ParamSet.ServoNickMin;
87
                 }
87
                 }
88
                 else if(ServoValue > ParamSet.ServoPitchMax)
88
                 else if(ServoValue > ParamSet.ServoNickMax)
89
                 {
89
                 {
Line 90... Line 90...
90
                         ServoValue = ParamSet.ServoPitchMax;
90
                         ServoValue = ParamSet.ServoNickMax;
91
                 }
91
                 }
92
 
92
 
93
                 // update PWM
93
                 // update PWM
94
                 OCR2A = ServoValue;
94
                 OCR2A = ServoValue;
95
                 timer = ParamSet.ServoPitchRefresh;
95
                 timer = ParamSet.ServoNickRefresh;
96
    }
96
    }
97
    else
97
    else