Subversion Repositories FlightCtrl

Rev

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

Rev 1104 Rev 1107
Line 80... Line 80...
80
                switch(ServoState)
80
                switch(ServoState)
81
                {
81
                {
82
                        case 4:
82
                        case 4:
83
                                // recalculate new ServoValue
83
                                // recalculate new ServoValue
84
                                ServoValue = 0x0030; // Offset (part 1)
84
                                ServoValue = 0x0030; // Offset (part 1)
85
                                DebugOut.Analog[16] = ServoValue;
-
 
86
                                FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
85
                                FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
87
                                ServoValue += FilterServo; // add filtered static offset
86
                                ServoValue += FilterServo; // add filtered static offset
88
 
-
 
89
                                if(ParamSet.ServoNickCompInvert & 0x01)
87
                                if(ParamSet.ServoNickCompInvert & 0x01)
90
                                {       // inverting movement of servo
88
                                {       // inverting movement of servo
91
                                        ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
89
                                        ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
92
                                }
90
                                }
93
                                else
91
                                else
94
                                {       // non inverting movement of servo
92
                                {       // non inverting movement of servo
95
                                        ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
93
                                        ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
96
                                }
94
                                }
97
 
-
 
98
                                // limit servo value to its parameter range definition
95
                                // limit servo value to its parameter range definition
99
                                if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
96
                                if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
100
                                {
97
                                {
101
                                        ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
98
                                        ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
102
                                }
99
                                }
103
                                else
100
                                else
104
                                if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
101
                                if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
105
                                {
102
                                {
106
                                        ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
103
                                        ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
107
                                }
104
                                }
108
 
-
 
109
                                DebugOut.Analog[20] = ServoValue;
105
                                DebugOut.Analog[20] = ServoValue;
-
 
106
 
110
                                // determine prepulse width (remaining part of ServoValue/Timer Cycle)
107
                                // determine prepulse width (remaining part of ServoValue/Timer Cycle)
111
                                if ((ServoValue % 255) < 45)
108
                                if ((ServoValue % 255) < 45)
112
                                {       // if prepulse width is to short the execution time of this ISR is longer than the next compare match
109
                                {       // if prepulse width is to short the execution time of this ISR is longer than the next compare match
113
                                        // so balance with postpulse width
110
                                        // so balance with postpulse width
114
                                        ServoValue += 77;
111
                                        ServoValue += 77;